diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..496ee2c --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +.DS_Store \ No newline at end of file diff --git a/0.0.0.18/pom.xml b/0.0.0.18/pom.xml new file mode 100644 index 0000000..c14b2d6 --- /dev/null +++ b/0.0.0.18/pom.xml @@ -0,0 +1,28 @@ + + 4.0.0 + platform.clients.fp9.libraries + AlternativaPhysics + swc + 0.0.0.18 + + platform.tools.maven + FlashBasePom + 1.0 + + + + platform.clients.fp9.libraries + AlternativaTypes + [1.1.0.1,2.0.0.0) + swc + external + + + platform.clients.fp9.libraries + AlternativaUtils + [1.2.0.0,2.0.0.0) + swc + external + + + \ No newline at end of file diff --git a/0.0.0.18/src/.svn/all-wcprops b/0.0.0.18/src/.svn/all-wcprops new file mode 100644 index 0000000..b7f99f9 --- /dev/null +++ b/0.0.0.18/src/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 74 +/!svn/ver/5193/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src +END diff --git a/0.0.0.18/src/.svn/entries b/0.0.0.18/src/.svn/entries new file mode 100644 index 0000000..b621a67 --- /dev/null +++ b/0.0.0.18/src/.svn/entries @@ -0,0 +1,31 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src +http://svndev.alternativaplatform.com + + + +2008-12-17T11:38:42.040147Z +5193 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +alternativa +dir + diff --git a/0.0.0.18/src/.svn/format b/0.0.0.18/src/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.0.18/src/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.0.18/src/alternativa/.svn/all-wcprops b/0.0.0.18/src/alternativa/.svn/all-wcprops new file mode 100644 index 0000000..5a876f1 --- /dev/null +++ b/0.0.0.18/src/alternativa/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 86 +/!svn/ver/5193/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa +END diff --git a/0.0.0.18/src/alternativa/.svn/entries b/0.0.0.18/src/alternativa/.svn/entries new file mode 100644 index 0000000..18c2959 --- /dev/null +++ b/0.0.0.18/src/alternativa/.svn/entries @@ -0,0 +1,31 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa +http://svndev.alternativaplatform.com + + + +2008-12-17T11:38:42.040147Z +5193 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +physics +dir + diff --git a/0.0.0.18/src/alternativa/.svn/format b/0.0.0.18/src/alternativa/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.0.18/src/alternativa/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.0.18/src/alternativa/physics/.svn/all-wcprops b/0.0.0.18/src/alternativa/physics/.svn/all-wcprops new file mode 100644 index 0000000..9ec4fdf --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 94 +/!svn/ver/5193/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics +END +altphysics.as +K 25 +svn:wc:ra_dav:version-url +V 108 +/!svn/ver/4196/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/altphysics.as +END diff --git a/0.0.0.18/src/alternativa/physics/.svn/entries b/0.0.0.18/src/alternativa/physics/.svn/entries new file mode 100644 index 0000000..5db5d29 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/.svn/entries @@ -0,0 +1,55 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics +http://svndev.alternativaplatform.com + + + +2008-12-17T11:38:42.040147Z +5193 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +particle +dir + +altphysics.as +file + + + + +2010-10-28T04:34:31.000000Z +44fa0a3979af103ba658ab05e4428212 +2008-11-24T06:53:44.272574Z +4196 +mike + +registry +dir + +collision +dir + +force +dir + +rigid +dir + diff --git a/0.0.0.18/src/alternativa/physics/.svn/format b/0.0.0.18/src/alternativa/physics/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.0.18/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base b/0.0.0.18/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base new file mode 100644 index 0000000..8252b4f --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base @@ -0,0 +1,3 @@ +package alternativa.physics { + public namespace altphysics = "http://alternativaplatform.com/en/altphysics"; +} diff --git a/0.0.0.18/src/alternativa/physics/altphysics.as b/0.0.0.18/src/alternativa/physics/altphysics.as new file mode 100644 index 0000000..8252b4f --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/altphysics.as @@ -0,0 +1,3 @@ +package alternativa.physics { + public namespace altphysics = "http://alternativaplatform.com/en/altphysics"; +} diff --git a/0.0.0.18/src/alternativa/physics/collision/.svn/all-wcprops b/0.0.0.18/src/alternativa/physics/collision/.svn/all-wcprops new file mode 100644 index 0000000..390e0e6 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/collision/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 104 +/!svn/ver/4398/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/collision +END +CollisionBox.as +K 25 +svn:wc:ra_dav:version-url +V 120 +/!svn/ver/4196/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/collision/CollisionBox.as +END +CollisionPrimitive.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/4375/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/collision/CollisionPrimitive.as +END +CollisionData.as +K 25 +svn:wc:ra_dav:version-url +V 121 +/!svn/ver/4196/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/collision/CollisionData.as +END +CollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 125 +/!svn/ver/4398/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/collision/CollisionDetector.as +END diff --git a/0.0.0.18/src/alternativa/physics/collision/.svn/entries b/0.0.0.18/src/alternativa/physics/collision/.svn/entries new file mode 100644 index 0000000..a0d5b41 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/collision/.svn/entries @@ -0,0 +1,76 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/collision +http://svndev.alternativaplatform.com + + + +2008-11-26T15:04:50.319495Z +4398 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +CollisionBox.as +file + + + + +2010-10-28T04:34:31.000000Z +777502bd075948da11606f32afba6679 +2008-11-24T06:53:44.272574Z +4196 +mike + +CollisionPrimitive.as +file + + + + +2010-10-28T04:34:31.000000Z +1e73f056a418e5c664977b4fafad70e5 +2008-11-26T12:04:03.108808Z +4375 +mike + +CollisionData.as +file + + + + +2010-10-28T04:34:31.000000Z +a8943fcaceb0836d59e7f4c2b2d3f4df +2008-11-24T06:53:44.272574Z +4196 +mike + +CollisionDetector.as +file + + + + +2010-10-28T04:34:31.000000Z +ca727662c3fe996a487323977c4a67bb +2008-11-26T15:04:50.319495Z +4398 +mike + diff --git a/0.0.0.18/src/alternativa/physics/collision/.svn/format b/0.0.0.18/src/alternativa/physics/collision/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/collision/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.0.18/src/alternativa/physics/collision/.svn/text-base/CollisionBox.as.svn-base b/0.0.0.18/src/alternativa/physics/collision/.svn/text-base/CollisionBox.as.svn-base new file mode 100644 index 0000000..be77bc3 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/collision/.svn/text-base/CollisionBox.as.svn-base @@ -0,0 +1,15 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.RigidBody; + import alternativa.types.Point3D; + + public class CollisionBox extends CollisionPrimitive { + + public var halfSize:Point3D = new Point3D(); + + public function CollisionBox(body:RigidBody, halfSize:Point3D) { + super(body); + this.halfSize.copy(halfSize); + } + + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/collision/.svn/text-base/CollisionData.as.svn-base b/0.0.0.18/src/alternativa/physics/collision/.svn/text-base/CollisionData.as.svn-base new file mode 100644 index 0000000..ce02511 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/collision/.svn/text-base/CollisionData.as.svn-base @@ -0,0 +1,18 @@ +package alternativa.physics.collision { + import alternativa.types.Point3D; + + public class CollisionData { + + public var primitive1:CollisionPrimitive; + public var primitive2:CollisionPrimitive; + + public var collisionNormal:Point3D = new Point3D(); + public var collisionPoint:Point3D = new Point3D(); + public var penetration:Number; + + public var pointAxisCode1:int; + public var pointAxisCode2:int; + + public var timeStamp:int; + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/collision/.svn/text-base/CollisionDetector.as.svn-base b/0.0.0.18/src/alternativa/physics/collision/.svn/text-base/CollisionDetector.as.svn-base new file mode 100644 index 0000000..9d9de35 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/collision/.svn/text-base/CollisionDetector.as.svn-base @@ -0,0 +1,342 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.types.Matrix3D; + import alternativa.types.Point3D; + + use namespace altphysics; + + public class CollisionDetector { + + private var epsilon:Number; + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var vectorToBox1:Point3D = new Point3D(); + private var axis:Point3D = new Point3D(); + private var axis10:Point3D = new Point3D(); + private var axis11:Point3D = new Point3D(); + private var axis12:Point3D = new Point3D(); + private var axis20:Point3D = new Point3D(); + private var axis21:Point3D = new Point3D(); + private var axis22:Point3D = new Point3D(); + private var collisionAxis:Point3D = new Point3D(); + private var tmpAxis:Point3D = new Point3D(); + private var point1:Point3D = new Point3D(); + private var point2:Point3D = new Point3D(); + private var vector:Point3D = new Point3D(); + + public function CollisionDetector(epsilon:Number = 0.01) { + this.epsilon = epsilon; + } + + /** + * Проверяет столкновение двух боксов, используя теорему о разделяющих плоскостях. + * + * @param box1 + * @param box2 + * @param collisionData + * + * @return + */ + public function boxToBoxCollision(box1:CollisionBox, box2:CollisionBox, collisionData:CollisionData):Boolean { + if (box1.body.inverseMass == 0 && box2.body.inverseMass == 0) { + // Нет необходимости проверять столкновения между неподвижными объектами + return false; + } + minOverlap = Number.MAX_VALUE; + // Вектор из центра второго бокса в центр первого + vectorToBox1.difference(box1.position, box2.position); + + // Проверка пересечения по основным осям + box1.transform.getAxis(0, axis10); + if (!testAxis(box1, box2, axis10, 0, vectorToBox1)) return false; + box1.transform.getAxis(1, axis11); + if (!testAxis(box1, box2, axis11, 1, vectorToBox1)) return false; + box1.transform.getAxis(2, axis12); + if (!testAxis(box1, box2, axis12, 2, vectorToBox1)) return false; + + box2.transform.getAxis(0, axis20); + if (!testAxis(box1, box2, axis20, 3, vectorToBox1)) return false; + box2.transform.getAxis(1, axis21); + if (!testAxis(box1, box2, axis21, 4, vectorToBox1)) return false; + box2.transform.getAxis(2, axis22); + if (!testAxis(box1, box2, axis22, 5, vectorToBox1)) return false; + + // Сохраняем ось с наименьшим перекрытием на случай обнаружения контакта ребро-грань + var bestMainAxis:int = bestAxisIndex; + + // Проверка производных осей + axis.cross2(axis10, axis20); + if (!testAxis(box1, box2, axis, 6, vectorToBox1)) return false; + axis.cross2(axis10, axis21); + if (!testAxis(box1, box2, axis, 7, vectorToBox1)) return false; + axis.cross2(axis10, axis22); + if (!testAxis(box1, box2, axis, 8, vectorToBox1)) return false; + + axis.cross2(axis11, axis20); + if (!testAxis(box1, box2, axis, 9, vectorToBox1)) return false; + axis.cross2(axis11, axis21); + if (!testAxis(box1, box2, axis, 10, vectorToBox1)) return false; + axis.cross2(axis11, axis22); + if (!testAxis(box1, box2, axis, 11, vectorToBox1)) return false; + + axis.cross2(axis12, axis20); + if (!testAxis(box1, box2, axis, 12, vectorToBox1)) return false; + axis.cross2(axis12, axis21); + if (!testAxis(box1, box2, axis, 13, vectorToBox1)) return false; + axis.cross2(axis12, axis22); + if (!testAxis(box1, box2, axis, 14, vectorToBox1)) return false; + + if (bestAxisIndex < 3) { + // Пересечение вершины второго бокса с гранью первого + fillPointBoxPenetrationData(box1, box2, vectorToBox1, bestAxisIndex, minOverlap, collisionData); + collisionData.primitive1 = box1; + collisionData.primitive2 = box2; + return true; + } else if (bestAxisIndex < 6) { + // Пересечение вершины первого бокса с гранью второго + vectorToBox1.invert(); + fillPointBoxPenetrationData(box2, box1, vectorToBox1, bestAxisIndex - 3, minOverlap, collisionData); + collisionData.primitive1 = box1; + collisionData.primitive2 = box2; + collisionData.collisionNormal.invert(); + var tmp:Number = collisionData.pointAxisCode1; + collisionData.pointAxisCode1 = collisionData.pointAxisCode2; + collisionData.pointAxisCode2 = tmp; + return true; + } else { + // Пересечение рёбер + bestAxisIndex -= 6; + var axisIndex1:int = int(bestAxisIndex/3); + var axisIndex2:int = bestAxisIndex%3; + + box1.transform.getAxis(axisIndex1, axis10); + box2.transform.getAxis(axisIndex2, axis20); + collisionAxis.cross2(axis10, axis20); + collisionAxis.normalize(); + // Разворот оси в сторону первого бокса + if (collisionAxis.dot(vectorToBox1) < 0) { + collisionAxis.invert(); + } + + var edgeHalfLength1:Number; + var edgeHalfLength2:Number; + var pointEdgeCode1:int; + var pointEdgeCode2:int; + + point1.copy(box1.halfSize); + point2.copy(box2.halfSize); + // x + if (axisIndex1 == 0) { + point1.x = 0; + edgeHalfLength1 = box1.halfSize.x; + pointEdgeCode1 |= 1; + } else { + box1.transform.getAxis(0, tmpAxis); + if (tmpAxis.dot(collisionAxis) > 0) { + point1.x = -point1.x; + pointEdgeCode1 |= 1; + } + } + if (axisIndex2 == 0) { + point2.x = 0; + edgeHalfLength2 = box2.halfSize.x; + pointEdgeCode2 |= 1; + } else { + box2.transform.getAxis(0, tmpAxis); + if (tmpAxis.dot(collisionAxis) < 0) { + point2.x = -point2.x; + pointEdgeCode2 |= 1; + } + } + // y + if (axisIndex1 == 1) { + point1.y = 0; + edgeHalfLength1 = box1.halfSize.y; + pointEdgeCode1 |= 2; + } else { + box1.transform.getAxis(1, tmpAxis); + if (tmpAxis.dot(collisionAxis) > 0) { + point1.y = -point1.y; + pointEdgeCode1 |= 2; + } + } + if (axisIndex2 == 1) { + point2.y = 0; + edgeHalfLength2 = box2.halfSize.y; + pointEdgeCode2 |= 2; + } else { + box2.transform.getAxis(1, tmpAxis); + if (tmpAxis.dot(collisionAxis) < 0) { + point2.y = -point2.y; + pointEdgeCode2 |= 2; + } + } + // z + if (axisIndex1 == 2) { + point1.z = 0; + edgeHalfLength1 = box1.halfSize.z; + pointEdgeCode1 |= 4; + } else { + box1.transform.getAxis(2, tmpAxis); + if (tmpAxis.dot(collisionAxis) > 0) { + point1.z = -point1.z; + pointEdgeCode1 |= 4; + } + } + if (axisIndex2 == 2) { + point2.z = 0; + edgeHalfLength2 = box2.halfSize.z; + pointEdgeCode2 |= 4; + } else { + box2.transform.getAxis(2, tmpAxis); + if (tmpAxis.dot(collisionAxis) < 0) { + point2.z = -point2.z; + pointEdgeCode2 |= 4; + } + } + + point1.transform(box1.transform); + point2.transform(box2.transform); + + // Находим точку пересечения рёбер + var k:Number = axis10.dot(axis20); + var det:Number = k*k - 1; + if (det > -0.0001 && det < 0.0001) { + vector.copy(bestMainAxis > 2 ? point1 : point2); + } else { + vector.difference(point2, point1); + var c1:Number = axis10.dot(vector); + var c2:Number = axis20.dot(vector); + var t1:Number = (c2*k - c1)/det; + if (t1 < -edgeHalfLength1 || t1 > edgeHalfLength1) { + vector.copy(bestMainAxis > 2 ? point1 : point2); + } else { + var t2:Number = (c2 - c1*k)/det; + if (t2 < -edgeHalfLength2 || t2 > edgeHalfLength2) { + vector.copy(bestMainAxis > 2 ? point1 : point2); + } else { + vector.x = 0.5*(point1.x + point2.x + axis10.x*t1 + axis20.x*t2); + vector.y = 0.5*(point1.y + point2.y + axis10.y*t1 + axis20.y*t2); + vector.z = 0.5*(point1.z + point2.z + axis10.z*t1 + axis20.z*t2); + } + } + } + collisionData.primitive1 = box1; + collisionData.primitive2 = box2; + collisionData.penetration = minOverlap; + collisionData.collisionPoint.copy(vector); + collisionData.collisionNormal.copy(collisionAxis); + collisionData.pointAxisCode1 = pointEdgeCode1 | (axisIndex1 << 3) | 32; + collisionData.pointAxisCode2 = pointEdgeCode2 | (axisIndex2 << 3) | 32; + } + return true; + } + + /** + * + * @param box1 + * @param box2 + * @param axis + * @param axisIndex + * @param vectorToBox1 + * @param bestAxis + * @return + * + */ + private function testAxis(box1:CollisionBox, box2:CollisionBox, axis:Point3D, axisIndex:int, vectorToBox1:Point3D):Boolean { + if (axis.lengthSqr < 0.0001) { + return true; + } + axis.normalize(); + + var overlap:Number = overlapOnAxis(box1, box2, axis, vectorToBox1); + if (overlap <= 0) { + return false; + } + if (overlap < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box1 + * @param box2 + * @param axis + * @param vectorToBox1 + * @return + * + */ + public function overlapOnAxis(box1:CollisionBox, box2:CollisionBox, axis:Point3D, vectorToBox1:Point3D):Number { + var m:Matrix3D = box1.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box1.halfSize.x; + var projection:Number = d < 0 ? -d : d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box1.halfSize.y; + projection += d < 0 ? -d : d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box1.halfSize.z; + projection += d < 0 ? -d : d; + + m = box2.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box2.halfSize.x; + projection += d < 0 ? -d : d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box2.halfSize.y; + projection += d < 0 ? -d : d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box2.halfSize.z; + projection += d < 0 ? -d : d; + + d = vectorToBox1.x*axis.x + vectorToBox1.y*axis.y + vectorToBox1.z*axis.z; + if (d < 0) { + d = -d; + } + + return projection - d; + } + + /** + * Заполняет данные о контакте грани первого бокса с вершиной второго. + * + * @param box1 + * @param box2 + * @param vectorToBox1 + * @param axisIndex + * @param overlap + * @param collisionData + * @param swapBoxes поменять примитивы местами при записи данных + */ + private function fillPointBoxPenetrationData(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Point3D, axisIndex:int, overlap:Number, collisionData:CollisionData):void { + box1.transform.getAxis(axisIndex, collisionData.collisionNormal); + if (collisionData.collisionNormal.dot(vectorToBox1) < 0) { + collisionData.collisionNormal.invert(); + } + + var pointCode:int; + axis10.copy(box2.halfSize); + box2.transform.getAxis(0, axis20); + if (axis20.dot(collisionData.collisionNormal) < 0) { + axis10.x = -axis10.x; + pointCode |= 1; + } + box2.transform.getAxis(1, axis20); + if (axis20.dot(collisionData.collisionNormal) < 0) { + axis10.y = -axis10.y; + pointCode |= 2; + } + box2.transform.getAxis(2, axis20); + if (axis20.dot(collisionData.collisionNormal) < 0) { + axis10.z = -axis10.z; + pointCode |= 4; + } + axis10.transform(box2.transform); + + collisionData.collisionPoint.copy(axis10); + collisionData.penetration = overlap; + collisionData.pointAxisCode1 = (axisIndex << 3) | 32; + collisionData.pointAxisCode2 = pointCode; + } + + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/collision/.svn/text-base/CollisionPrimitive.as.svn-base b/0.0.0.18/src/alternativa/physics/collision/.svn/text-base/CollisionPrimitive.as.svn-base new file mode 100644 index 0000000..d473792 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/collision/.svn/text-base/CollisionPrimitive.as.svn-base @@ -0,0 +1,31 @@ +package alternativa.physics.collision { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.RigidBody; + import alternativa.types.Matrix3D; + import alternativa.types.Point3D; + + use namespace altphysics; + + public class CollisionPrimitive { + + public var position:Point3D = new Point3D(); + public var transform:Matrix3D = new Matrix3D(); + public var body:RigidBody; + + public function CollisionPrimitive(body:RigidBody) { + this.body = body; + } + + public function updateTransform():void { + transform.copy(body.transformMatrix); + position.x = body.position.x; + position.y = body.position.y; + position.z = body.position.z; + transform.d = position.x; + transform.h = position.y; + transform.l = position.z; + } + + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/collision/CollisionBox.as b/0.0.0.18/src/alternativa/physics/collision/CollisionBox.as new file mode 100644 index 0000000..be77bc3 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/collision/CollisionBox.as @@ -0,0 +1,15 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.RigidBody; + import alternativa.types.Point3D; + + public class CollisionBox extends CollisionPrimitive { + + public var halfSize:Point3D = new Point3D(); + + public function CollisionBox(body:RigidBody, halfSize:Point3D) { + super(body); + this.halfSize.copy(halfSize); + } + + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/collision/CollisionData.as b/0.0.0.18/src/alternativa/physics/collision/CollisionData.as new file mode 100644 index 0000000..ce02511 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/collision/CollisionData.as @@ -0,0 +1,18 @@ +package alternativa.physics.collision { + import alternativa.types.Point3D; + + public class CollisionData { + + public var primitive1:CollisionPrimitive; + public var primitive2:CollisionPrimitive; + + public var collisionNormal:Point3D = new Point3D(); + public var collisionPoint:Point3D = new Point3D(); + public var penetration:Number; + + public var pointAxisCode1:int; + public var pointAxisCode2:int; + + public var timeStamp:int; + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/collision/CollisionDetector.as b/0.0.0.18/src/alternativa/physics/collision/CollisionDetector.as new file mode 100644 index 0000000..9d9de35 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/collision/CollisionDetector.as @@ -0,0 +1,342 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.types.Matrix3D; + import alternativa.types.Point3D; + + use namespace altphysics; + + public class CollisionDetector { + + private var epsilon:Number; + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var vectorToBox1:Point3D = new Point3D(); + private var axis:Point3D = new Point3D(); + private var axis10:Point3D = new Point3D(); + private var axis11:Point3D = new Point3D(); + private var axis12:Point3D = new Point3D(); + private var axis20:Point3D = new Point3D(); + private var axis21:Point3D = new Point3D(); + private var axis22:Point3D = new Point3D(); + private var collisionAxis:Point3D = new Point3D(); + private var tmpAxis:Point3D = new Point3D(); + private var point1:Point3D = new Point3D(); + private var point2:Point3D = new Point3D(); + private var vector:Point3D = new Point3D(); + + public function CollisionDetector(epsilon:Number = 0.01) { + this.epsilon = epsilon; + } + + /** + * Проверяет столкновение двух боксов, используя теорему о разделяющих плоскостях. + * + * @param box1 + * @param box2 + * @param collisionData + * + * @return + */ + public function boxToBoxCollision(box1:CollisionBox, box2:CollisionBox, collisionData:CollisionData):Boolean { + if (box1.body.inverseMass == 0 && box2.body.inverseMass == 0) { + // Нет необходимости проверять столкновения между неподвижными объектами + return false; + } + minOverlap = Number.MAX_VALUE; + // Вектор из центра второго бокса в центр первого + vectorToBox1.difference(box1.position, box2.position); + + // Проверка пересечения по основным осям + box1.transform.getAxis(0, axis10); + if (!testAxis(box1, box2, axis10, 0, vectorToBox1)) return false; + box1.transform.getAxis(1, axis11); + if (!testAxis(box1, box2, axis11, 1, vectorToBox1)) return false; + box1.transform.getAxis(2, axis12); + if (!testAxis(box1, box2, axis12, 2, vectorToBox1)) return false; + + box2.transform.getAxis(0, axis20); + if (!testAxis(box1, box2, axis20, 3, vectorToBox1)) return false; + box2.transform.getAxis(1, axis21); + if (!testAxis(box1, box2, axis21, 4, vectorToBox1)) return false; + box2.transform.getAxis(2, axis22); + if (!testAxis(box1, box2, axis22, 5, vectorToBox1)) return false; + + // Сохраняем ось с наименьшим перекрытием на случай обнаружения контакта ребро-грань + var bestMainAxis:int = bestAxisIndex; + + // Проверка производных осей + axis.cross2(axis10, axis20); + if (!testAxis(box1, box2, axis, 6, vectorToBox1)) return false; + axis.cross2(axis10, axis21); + if (!testAxis(box1, box2, axis, 7, vectorToBox1)) return false; + axis.cross2(axis10, axis22); + if (!testAxis(box1, box2, axis, 8, vectorToBox1)) return false; + + axis.cross2(axis11, axis20); + if (!testAxis(box1, box2, axis, 9, vectorToBox1)) return false; + axis.cross2(axis11, axis21); + if (!testAxis(box1, box2, axis, 10, vectorToBox1)) return false; + axis.cross2(axis11, axis22); + if (!testAxis(box1, box2, axis, 11, vectorToBox1)) return false; + + axis.cross2(axis12, axis20); + if (!testAxis(box1, box2, axis, 12, vectorToBox1)) return false; + axis.cross2(axis12, axis21); + if (!testAxis(box1, box2, axis, 13, vectorToBox1)) return false; + axis.cross2(axis12, axis22); + if (!testAxis(box1, box2, axis, 14, vectorToBox1)) return false; + + if (bestAxisIndex < 3) { + // Пересечение вершины второго бокса с гранью первого + fillPointBoxPenetrationData(box1, box2, vectorToBox1, bestAxisIndex, minOverlap, collisionData); + collisionData.primitive1 = box1; + collisionData.primitive2 = box2; + return true; + } else if (bestAxisIndex < 6) { + // Пересечение вершины первого бокса с гранью второго + vectorToBox1.invert(); + fillPointBoxPenetrationData(box2, box1, vectorToBox1, bestAxisIndex - 3, minOverlap, collisionData); + collisionData.primitive1 = box1; + collisionData.primitive2 = box2; + collisionData.collisionNormal.invert(); + var tmp:Number = collisionData.pointAxisCode1; + collisionData.pointAxisCode1 = collisionData.pointAxisCode2; + collisionData.pointAxisCode2 = tmp; + return true; + } else { + // Пересечение рёбер + bestAxisIndex -= 6; + var axisIndex1:int = int(bestAxisIndex/3); + var axisIndex2:int = bestAxisIndex%3; + + box1.transform.getAxis(axisIndex1, axis10); + box2.transform.getAxis(axisIndex2, axis20); + collisionAxis.cross2(axis10, axis20); + collisionAxis.normalize(); + // Разворот оси в сторону первого бокса + if (collisionAxis.dot(vectorToBox1) < 0) { + collisionAxis.invert(); + } + + var edgeHalfLength1:Number; + var edgeHalfLength2:Number; + var pointEdgeCode1:int; + var pointEdgeCode2:int; + + point1.copy(box1.halfSize); + point2.copy(box2.halfSize); + // x + if (axisIndex1 == 0) { + point1.x = 0; + edgeHalfLength1 = box1.halfSize.x; + pointEdgeCode1 |= 1; + } else { + box1.transform.getAxis(0, tmpAxis); + if (tmpAxis.dot(collisionAxis) > 0) { + point1.x = -point1.x; + pointEdgeCode1 |= 1; + } + } + if (axisIndex2 == 0) { + point2.x = 0; + edgeHalfLength2 = box2.halfSize.x; + pointEdgeCode2 |= 1; + } else { + box2.transform.getAxis(0, tmpAxis); + if (tmpAxis.dot(collisionAxis) < 0) { + point2.x = -point2.x; + pointEdgeCode2 |= 1; + } + } + // y + if (axisIndex1 == 1) { + point1.y = 0; + edgeHalfLength1 = box1.halfSize.y; + pointEdgeCode1 |= 2; + } else { + box1.transform.getAxis(1, tmpAxis); + if (tmpAxis.dot(collisionAxis) > 0) { + point1.y = -point1.y; + pointEdgeCode1 |= 2; + } + } + if (axisIndex2 == 1) { + point2.y = 0; + edgeHalfLength2 = box2.halfSize.y; + pointEdgeCode2 |= 2; + } else { + box2.transform.getAxis(1, tmpAxis); + if (tmpAxis.dot(collisionAxis) < 0) { + point2.y = -point2.y; + pointEdgeCode2 |= 2; + } + } + // z + if (axisIndex1 == 2) { + point1.z = 0; + edgeHalfLength1 = box1.halfSize.z; + pointEdgeCode1 |= 4; + } else { + box1.transform.getAxis(2, tmpAxis); + if (tmpAxis.dot(collisionAxis) > 0) { + point1.z = -point1.z; + pointEdgeCode1 |= 4; + } + } + if (axisIndex2 == 2) { + point2.z = 0; + edgeHalfLength2 = box2.halfSize.z; + pointEdgeCode2 |= 4; + } else { + box2.transform.getAxis(2, tmpAxis); + if (tmpAxis.dot(collisionAxis) < 0) { + point2.z = -point2.z; + pointEdgeCode2 |= 4; + } + } + + point1.transform(box1.transform); + point2.transform(box2.transform); + + // Находим точку пересечения рёбер + var k:Number = axis10.dot(axis20); + var det:Number = k*k - 1; + if (det > -0.0001 && det < 0.0001) { + vector.copy(bestMainAxis > 2 ? point1 : point2); + } else { + vector.difference(point2, point1); + var c1:Number = axis10.dot(vector); + var c2:Number = axis20.dot(vector); + var t1:Number = (c2*k - c1)/det; + if (t1 < -edgeHalfLength1 || t1 > edgeHalfLength1) { + vector.copy(bestMainAxis > 2 ? point1 : point2); + } else { + var t2:Number = (c2 - c1*k)/det; + if (t2 < -edgeHalfLength2 || t2 > edgeHalfLength2) { + vector.copy(bestMainAxis > 2 ? point1 : point2); + } else { + vector.x = 0.5*(point1.x + point2.x + axis10.x*t1 + axis20.x*t2); + vector.y = 0.5*(point1.y + point2.y + axis10.y*t1 + axis20.y*t2); + vector.z = 0.5*(point1.z + point2.z + axis10.z*t1 + axis20.z*t2); + } + } + } + collisionData.primitive1 = box1; + collisionData.primitive2 = box2; + collisionData.penetration = minOverlap; + collisionData.collisionPoint.copy(vector); + collisionData.collisionNormal.copy(collisionAxis); + collisionData.pointAxisCode1 = pointEdgeCode1 | (axisIndex1 << 3) | 32; + collisionData.pointAxisCode2 = pointEdgeCode2 | (axisIndex2 << 3) | 32; + } + return true; + } + + /** + * + * @param box1 + * @param box2 + * @param axis + * @param axisIndex + * @param vectorToBox1 + * @param bestAxis + * @return + * + */ + private function testAxis(box1:CollisionBox, box2:CollisionBox, axis:Point3D, axisIndex:int, vectorToBox1:Point3D):Boolean { + if (axis.lengthSqr < 0.0001) { + return true; + } + axis.normalize(); + + var overlap:Number = overlapOnAxis(box1, box2, axis, vectorToBox1); + if (overlap <= 0) { + return false; + } + if (overlap < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box1 + * @param box2 + * @param axis + * @param vectorToBox1 + * @return + * + */ + public function overlapOnAxis(box1:CollisionBox, box2:CollisionBox, axis:Point3D, vectorToBox1:Point3D):Number { + var m:Matrix3D = box1.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box1.halfSize.x; + var projection:Number = d < 0 ? -d : d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box1.halfSize.y; + projection += d < 0 ? -d : d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box1.halfSize.z; + projection += d < 0 ? -d : d; + + m = box2.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box2.halfSize.x; + projection += d < 0 ? -d : d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box2.halfSize.y; + projection += d < 0 ? -d : d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box2.halfSize.z; + projection += d < 0 ? -d : d; + + d = vectorToBox1.x*axis.x + vectorToBox1.y*axis.y + vectorToBox1.z*axis.z; + if (d < 0) { + d = -d; + } + + return projection - d; + } + + /** + * Заполняет данные о контакте грани первого бокса с вершиной второго. + * + * @param box1 + * @param box2 + * @param vectorToBox1 + * @param axisIndex + * @param overlap + * @param collisionData + * @param swapBoxes поменять примитивы местами при записи данных + */ + private function fillPointBoxPenetrationData(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Point3D, axisIndex:int, overlap:Number, collisionData:CollisionData):void { + box1.transform.getAxis(axisIndex, collisionData.collisionNormal); + if (collisionData.collisionNormal.dot(vectorToBox1) < 0) { + collisionData.collisionNormal.invert(); + } + + var pointCode:int; + axis10.copy(box2.halfSize); + box2.transform.getAxis(0, axis20); + if (axis20.dot(collisionData.collisionNormal) < 0) { + axis10.x = -axis10.x; + pointCode |= 1; + } + box2.transform.getAxis(1, axis20); + if (axis20.dot(collisionData.collisionNormal) < 0) { + axis10.y = -axis10.y; + pointCode |= 2; + } + box2.transform.getAxis(2, axis20); + if (axis20.dot(collisionData.collisionNormal) < 0) { + axis10.z = -axis10.z; + pointCode |= 4; + } + axis10.transform(box2.transform); + + collisionData.collisionPoint.copy(axis10); + collisionData.penetration = overlap; + collisionData.pointAxisCode1 = (axisIndex << 3) | 32; + collisionData.pointAxisCode2 = pointCode; + } + + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/collision/CollisionPrimitive.as b/0.0.0.18/src/alternativa/physics/collision/CollisionPrimitive.as new file mode 100644 index 0000000..d473792 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/collision/CollisionPrimitive.as @@ -0,0 +1,31 @@ +package alternativa.physics.collision { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.RigidBody; + import alternativa.types.Matrix3D; + import alternativa.types.Point3D; + + use namespace altphysics; + + public class CollisionPrimitive { + + public var position:Point3D = new Point3D(); + public var transform:Matrix3D = new Matrix3D(); + public var body:RigidBody; + + public function CollisionPrimitive(body:RigidBody) { + this.body = body; + } + + public function updateTransform():void { + transform.copy(body.transformMatrix); + position.x = body.position.x; + position.y = body.position.y; + position.z = body.position.z; + transform.d = position.x; + transform.h = position.y; + transform.l = position.z; + } + + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/force/.svn/all-wcprops b/0.0.0.18/src/alternativa/physics/force/.svn/all-wcprops new file mode 100644 index 0000000..71adea8 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/force/.svn/all-wcprops @@ -0,0 +1,77 @@ +K 25 +svn:wc:ra_dav:version-url +V 100 +/!svn/ver/4732/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/force +END +ParticleAnchoredBungee.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/4196/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/force/ParticleAnchoredBungee.as +END +ParticleBuoyancy.as +K 25 +svn:wc:ra_dav:version-url +V 120 +/!svn/ver/4196/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/force/ParticleBuoyancy.as +END +RigidBodySpring.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/4196/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/force/RigidBodySpring.as +END +ParticleDrag.as +K 25 +svn:wc:ra_dav:version-url +V 116 +/!svn/ver/4196/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/force/ParticleDrag.as +END +IParticelForceGenerator.as +K 25 +svn:wc:ra_dav:version-url +V 127 +/!svn/ver/4196/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/force/IParticelForceGenerator.as +END +RigidBodyGravity.as +K 25 +svn:wc:ra_dav:version-url +V 120 +/!svn/ver/4196/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/force/RigidBodyGravity.as +END +ParticleSpring.as +K 25 +svn:wc:ra_dav:version-url +V 118 +/!svn/ver/4196/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/force/ParticleSpring.as +END +RigidBodyAnchoredBungee.as +K 25 +svn:wc:ra_dav:version-url +V 127 +/!svn/ver/4732/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/force/RigidBodyAnchoredBungee.as +END +ParticleBungee.as +K 25 +svn:wc:ra_dav:version-url +V 118 +/!svn/ver/4196/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/force/ParticleBungee.as +END +ParticleGravity.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/4196/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/force/ParticleGravity.as +END +ParticleAnchoredSpring.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/4196/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/force/ParticleAnchoredSpring.as +END +IRigidBodyForceGenerator.as +K 25 +svn:wc:ra_dav:version-url +V 128 +/!svn/ver/4196/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/force/IRigidBodyForceGenerator.as +END diff --git a/0.0.0.18/src/alternativa/physics/force/.svn/entries b/0.0.0.18/src/alternativa/physics/force/.svn/entries new file mode 100644 index 0000000..ed6e268 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/force/.svn/entries @@ -0,0 +1,172 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/force +http://svndev.alternativaplatform.com + + + +2008-12-03T05:15:16.843707Z +4732 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +ParticleAnchoredBungee.as +file + + + + +2010-10-28T04:34:31.000000Z +ed65cd81f1a99df1cc3c04e061cc15fd +2008-11-24T06:53:44.272574Z +4196 +mike + +ParticleBuoyancy.as +file + + + + +2010-10-28T04:34:31.000000Z +4479f6ebbce6c7326410485e5c62b9a9 +2008-11-24T06:53:44.272574Z +4196 +mike + +RigidBodySpring.as +file + + + + +2010-10-28T04:34:31.000000Z +3f0de100aebf0d3250e608872e6f3863 +2008-11-24T06:53:44.272574Z +4196 +mike + +ParticleDrag.as +file + + + + +2010-10-28T04:34:31.000000Z +1a8e8b326a47d4771d157259e43deb53 +2008-11-24T06:53:44.272574Z +4196 +mike + +IParticelForceGenerator.as +file + + + + +2010-10-28T04:34:31.000000Z +645a997b618f0708a8311054767340a7 +2008-11-24T06:53:44.272574Z +4196 +mike + +RigidBodyGravity.as +file + + + + +2010-10-28T04:34:31.000000Z +b206b142e125e9fe4a1775f8c4768b17 +2008-11-24T06:53:44.272574Z +4196 +mike + +ParticleSpring.as +file + + + + +2010-10-28T04:34:31.000000Z +fb5cb524f90e27812074bc2668c548d1 +2008-11-24T06:53:44.272574Z +4196 +mike + +RigidBodyAnchoredBungee.as +file + + + + +2010-10-28T04:34:31.000000Z +a64b67f4f850a73a3ffc226ab8fb0774 +2008-12-03T05:15:16.843707Z +4732 +mike + +ParticleBungee.as +file + + + + +2010-10-28T04:34:31.000000Z +d79408ff66b4c9d92f9a2be73417ba3b +2008-11-24T06:53:44.272574Z +4196 +mike + +ParticleGravity.as +file + + + + +2010-10-28T04:34:31.000000Z +3b0d1310430d785b3ec7ffebf8ea7ea9 +2008-11-24T06:53:44.272574Z +4196 +mike + +ParticleAnchoredSpring.as +file + + + + +2010-10-28T04:34:31.000000Z +2a0eee0933c8973ec42bb28a8b0b7160 +2008-11-24T06:53:44.272574Z +4196 +mike + +IRigidBodyForceGenerator.as +file + + + + +2010-10-28T04:34:31.000000Z +824ea9b7a68dda215974882d6f32f892 +2008-11-24T06:53:44.272574Z +4196 +mike + diff --git a/0.0.0.18/src/alternativa/physics/force/.svn/format b/0.0.0.18/src/alternativa/physics/force/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/force/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.0.18/src/alternativa/physics/force/.svn/text-base/IParticelForceGenerator.as.svn-base b/0.0.0.18/src/alternativa/physics/force/.svn/text-base/IParticelForceGenerator.as.svn-base new file mode 100644 index 0000000..06a4eec --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/force/.svn/text-base/IParticelForceGenerator.as.svn-base @@ -0,0 +1,7 @@ +package alternativa.physics.force { + import alternativa.physics.particle.Particle; + + public interface IParticelForceGenerator { + function updateForce(particle:Particle, time:Number):void; + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/force/.svn/text-base/IRigidBodyForceGenerator.as.svn-base b/0.0.0.18/src/alternativa/physics/force/.svn/text-base/IRigidBodyForceGenerator.as.svn-base new file mode 100644 index 0000000..d49cc8c --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/force/.svn/text-base/IRigidBodyForceGenerator.as.svn-base @@ -0,0 +1,10 @@ +package alternativa.physics.force { + import alternativa.physics.rigid.RigidBody; + + /** + * + */ + public interface IRigidBodyForceGenerator { + function updateForce(body:RigidBody, time:Number):void; + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/force/.svn/text-base/ParticleAnchoredBungee.as.svn-base b/0.0.0.18/src/alternativa/physics/force/.svn/text-base/ParticleAnchoredBungee.as.svn-base new file mode 100644 index 0000000..399dac4 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/force/.svn/text-base/ParticleAnchoredBungee.as.svn-base @@ -0,0 +1,37 @@ +package alternativa.physics.force { + import alternativa.physics.particle.Particle; + import alternativa.types.Point3D; + + public class ParticleAnchoredBungee implements IParticelForceGenerator { + + private var force:Point3D = new Point3D(); + + private var anchor:Point3D; + private var springCoeff:Number; + private var restLength:Number; + public var relaxed:Boolean; + + public function ParticleAnchoredBungee(anchor:Point3D, springCoeff:Number, restLength:Number) { + this.anchor = anchor; + this.springCoeff = springCoeff; + this.restLength = restLength; + } + + public function updateForce(particle:Particle, time:Number):void { + force.x = particle.position.x - anchor.x; + force.y = particle.position.y - anchor.y; + force.z = particle.position.z - anchor.z; + var len:Number = force.length; + if (len < restLength) { + relaxed = true; + return; + } + relaxed = false; + var k:Number = (restLength - len)*springCoeff/len; + force.x *= k; + force.y *= k; + force.z *= k; + particle.addForce(force); + } + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/force/.svn/text-base/ParticleAnchoredSpring.as.svn-base b/0.0.0.18/src/alternativa/physics/force/.svn/text-base/ParticleAnchoredSpring.as.svn-base new file mode 100644 index 0000000..ed9263d --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/force/.svn/text-base/ParticleAnchoredSpring.as.svn-base @@ -0,0 +1,31 @@ +package alternativa.physics.force { + import alternativa.physics.particle.Particle; + import alternativa.types.Point3D; + + public class ParticleAnchoredSpring implements IParticelForceGenerator { + + private var force:Point3D = new Point3D(); + + private var anchor:Point3D; + private var springCoeff:Number; + private var restLength:Number; + + public function ParticleAnchoredSpring(anchor:Point3D, springCoeff:Number, restLength:Number) { + this.anchor = anchor; + this.springCoeff = springCoeff; + this.restLength = restLength; + } + + public function updateForce(particle:Particle, time:Number):void { + force.x = particle.position.x - anchor.x; + force.y = particle.position.y - anchor.y; + force.z = particle.position.z - anchor.z; + var len:Number = force.length; + var k:Number = (restLength - len)*springCoeff/len; + force.x *= k; + force.y *= k; + force.z *= k; + particle.addForce(force); + } + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/force/.svn/text-base/ParticleBungee.as.svn-base b/0.0.0.18/src/alternativa/physics/force/.svn/text-base/ParticleBungee.as.svn-base new file mode 100644 index 0000000..41cf3c0 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/force/.svn/text-base/ParticleBungee.as.svn-base @@ -0,0 +1,38 @@ +package alternativa.physics.force { + import alternativa.physics.particle.Particle; + import alternativa.types.Point3D; + + public class ParticleBungee implements IParticelForceGenerator { + + private var force:Point3D = new Point3D(); + + private var otherParticle:Particle; + private var springCoeff:Number; + private var restLength:Number; + + public var relaxed:Boolean; + + public function ParticleBungee(otherParticle:Particle, springCoeff:Number, restLength:Number) { + this.otherParticle = otherParticle; + this.springCoeff = springCoeff; + this.restLength = restLength; + } + + public function updateForce(particle:Particle, time:Number):void { + force.x = particle.position.x - otherParticle.position.x; + force.y = particle.position.y - otherParticle.position.y; + force.z = particle.position.z - otherParticle.position.z; + var len:Number = force.length; + if (len < restLength) { + relaxed = true; + return; + } + relaxed = false; + var k:Number = (restLength - len)*springCoeff/len; + force.x *= k; + force.y *= k; + force.z *= k; + particle.addForce(force); + } + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/force/.svn/text-base/ParticleBuoyancy.as.svn-base b/0.0.0.18/src/alternativa/physics/force/.svn/text-base/ParticleBuoyancy.as.svn-base new file mode 100644 index 0000000..45c0de3 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/force/.svn/text-base/ParticleBuoyancy.as.svn-base @@ -0,0 +1,37 @@ +package alternativa.physics.force { + import alternativa.physics.particle.Particle; + import alternativa.types.Point3D; + + public class ParticleBuoyancy implements IParticelForceGenerator { + + private var force:Point3D = new Point3D(); + + private var maxDepth:Number; + private var waterHeight:Number; + private var volume:Number; + private var liquidDensity:Number; + + public function ParticleBuoyancy(maxDepth:Number, waterHeight:Number, volume:Number, liquidDensity:Number = 1000) { + this.maxDepth = maxDepth; + this.waterHeight = waterHeight; + this.volume = volume; + this.liquidDensity = liquidDensity; + } + + public function updateForce(particle:Particle, time:Number):void { + var depth:Number = particle.position.y; + if (depth > waterHeight + maxDepth) { + return; + } + + force.x = 0; + force.y = 0; + if (depth <= waterHeight - maxDepth) { + force.z = liquidDensity*volume; + } else { + force.z = liquidDensity*volume*(waterHeight + maxDepth - depth)/maxDepth*0.5; + } + particle.addForce(force); + } + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/force/.svn/text-base/ParticleDrag.as.svn-base b/0.0.0.18/src/alternativa/physics/force/.svn/text-base/ParticleDrag.as.svn-base new file mode 100644 index 0000000..e9de457 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/force/.svn/text-base/ParticleDrag.as.svn-base @@ -0,0 +1,29 @@ +package alternativa.physics.force { + import alternativa.physics.particle.Particle; + import alternativa.types.Point3D; + + public class ParticleDrag implements IParticelForceGenerator { + private var force:Point3D = new Point3D(); + + private var k1:Number; + private var k2:Number; + + public function ParticleDrag(k1:Number, k2:Number) { + this.k1 = k1; + this.k2 = k2; + } + + public function updateForce(particle:Particle, time:Number):void { + force.x = particle.velocity.x; + force.y = particle.velocity.y; + force.z = particle.velocity.z; + var len:Number = force.length; + var coeff:Number = len; + coeff = -(k1*coeff + k2*coeff*coeff)/len; + force.x *= coeff; + force.y *= coeff; + force.z *= coeff; + particle.addForce(force); + } + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/force/.svn/text-base/ParticleGravity.as.svn-base b/0.0.0.18/src/alternativa/physics/force/.svn/text-base/ParticleGravity.as.svn-base new file mode 100644 index 0000000..ff400d2 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/force/.svn/text-base/ParticleGravity.as.svn-base @@ -0,0 +1,24 @@ +package alternativa.physics.force { + import alternativa.physics.particle.Particle; + import alternativa.types.Point3D; + + public class ParticleGravity implements IParticelForceGenerator { + private var force:Point3D = new Point3D(); + + private var _gravity:Point3D; + + public function ParticleGravity(gravity:Point3D) { + _gravity = gravity.clone(); + } + + public function updateForce(particle:Particle, time:Number):void { + if (!particle.hasFiniteMass()) { + return; + } + force.x = _gravity.x*particle.mass; + force.y = _gravity.y*particle.mass; + force.z = _gravity.z*particle.mass; + particle.addForce(force); + } + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/force/.svn/text-base/ParticleSpring.as.svn-base b/0.0.0.18/src/alternativa/physics/force/.svn/text-base/ParticleSpring.as.svn-base new file mode 100644 index 0000000..aee5732 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/force/.svn/text-base/ParticleSpring.as.svn-base @@ -0,0 +1,31 @@ +package alternativa.physics.force { + import alternativa.physics.particle.Particle; + import alternativa.types.Point3D; + + public class ParticleSpring implements IParticelForceGenerator { + + private var force:Point3D = new Point3D(); + + private var otherParticle:Particle; + private var springCoeff:Number; + private var restLength:Number; + + public function ParticleSpring(otherParticle:Particle, springCoeff:Number, restLength:Number) { + this.otherParticle = otherParticle; + this.springCoeff = springCoeff; + this.restLength = restLength; + } + + public function updateForce(particle:Particle, time:Number):void { + force.x = particle.position.x - otherParticle.position.x; + force.y = particle.position.y - otherParticle.position.y; + force.z = particle.position.z - otherParticle.position.z; + var len:Number = force.length; + var k:Number = (restLength - len)*springCoeff/len; + force.x *= k; + force.y *= k; + force.z *= k; + particle.addForce(force); + } + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/force/.svn/text-base/RigidBodyAnchoredBungee.as.svn-base b/0.0.0.18/src/alternativa/physics/force/.svn/text-base/RigidBodyAnchoredBungee.as.svn-base new file mode 100644 index 0000000..85921c4 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/force/.svn/text-base/RigidBodyAnchoredBungee.as.svn-base @@ -0,0 +1,45 @@ +package alternativa.physics.force { + import alternativa.physics.*; + import alternativa.physics.rigid.RigidBody; + import alternativa.types.Point3D; + + use namespace altphysics; + + public class RigidBodyAnchoredBungee implements IRigidBodyForceGenerator { + + private var vector1:Point3D = new Point3D(); + private var vector2:Point3D = new Point3D(); + + private var _connectionPoint:Point3D = new Point3D(); + private var _anchorPoint:Point3D = new Point3D(); + private var _springConstant:Number; + private var _restLength:Number; + + public function RigidBodyAnchoredBungee(connectionPoint:Point3D, anchorPoint:Point3D, springConstant:Number, restLength:Number) { + _connectionPoint.copy(connectionPoint); + _anchorPoint.copy(anchorPoint); + _springConstant = springConstant; + _restLength = restLength; + } + + public function updateForce(body:RigidBody, time:Number):void { + // TODO: Оптимизировать!!! + vector1.copy(_connectionPoint); + vector1.transform(body.transformMatrix); + vector2.copy(_anchorPoint); + vector2.subtract(vector1); + var len:Number = vector2.length; + var magnitude:Number = len - _restLength; + if (magnitude > 0) { + magnitude *= _springConstant; + vector2.multiply(magnitude/len); + body.addForceAtPoint(vector2, vector1); + } + } + + public function set anchorPoint(value:Point3D):void { + _anchorPoint.copy(value); + } + + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/force/.svn/text-base/RigidBodyGravity.as.svn-base b/0.0.0.18/src/alternativa/physics/force/.svn/text-base/RigidBodyGravity.as.svn-base new file mode 100644 index 0000000..e99aed1 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/force/.svn/text-base/RigidBodyGravity.as.svn-base @@ -0,0 +1,27 @@ +package alternativa.physics.force { + import alternativa.physics.*; + import alternativa.physics.rigid.RigidBody; + import alternativa.types.Point3D; + + use namespace altphysics; + + /** + * + */ + public class RigidBodyGravity implements IRigidBodyForceGenerator { + + private var _gravity:Point3D = new Point3D(); + + public function RigidBodyGravity(g:Point3D) { + _gravity.copy(g); + } + + public function updateForce(body:RigidBody, time:Number):void { + if (body.inverseMass == 0) { + return; + } + body.addForceComponents(_gravity.x*body.mass, _gravity.y*body.mass, _gravity.z*body.mass); + } + + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/force/.svn/text-base/RigidBodySpring.as.svn-base b/0.0.0.18/src/alternativa/physics/force/.svn/text-base/RigidBodySpring.as.svn-base new file mode 100644 index 0000000..1a5d44a --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/force/.svn/text-base/RigidBodySpring.as.svn-base @@ -0,0 +1,45 @@ +package alternativa.physics.force { + import alternativa.physics.*; + import alternativa.physics.rigid.RigidBody; + import alternativa.types.Point3D; + + use namespace altphysics; + + public class RigidBodySpring implements IRigidBodyForceGenerator { + + private var vector1:Point3D = new Point3D(); + private var vector2:Point3D = new Point3D(); + + private var _connectionPoint:Point3D = new Point3D(); + private var _otherConnectionPoint:Point3D = new Point3D(); + private var _springConstant:Number; + private var _restLength:Number; + private var _otherBody:RigidBody; + + public function RigidBodySpring(connectionPoint:Point3D, otherBody:RigidBody, otherConnectionPoint:Point3D, springConstant:Number, restLength:Number) { + _connectionPoint.copy(connectionPoint); + _otherConnectionPoint = otherConnectionPoint; + _otherBody = otherBody; + _springConstant = springConstant; + _restLength = restLength; + } + + public function updateForce(body:RigidBody, time:Number):void { + // TODO: Оптимизировать!!! + vector1.copy(_connectionPoint); + vector1.transform(body.transformMatrix); + vector2.copy(_otherConnectionPoint); + vector2.transform(_otherBody.transformMatrix); + vector2.subtract(vector1); + var len:Number = vector2.length; + if (len == 0) { + trace("[RigidBodySpring::updateForce] spring length is zero"); + } + var magnitude:Number = len - _restLength; + magnitude *= _springConstant; + vector2.multiply(magnitude/len); + body.addForceAtPoint(vector2, vector1); + } + + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/force/IParticelForceGenerator.as b/0.0.0.18/src/alternativa/physics/force/IParticelForceGenerator.as new file mode 100644 index 0000000..06a4eec --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/force/IParticelForceGenerator.as @@ -0,0 +1,7 @@ +package alternativa.physics.force { + import alternativa.physics.particle.Particle; + + public interface IParticelForceGenerator { + function updateForce(particle:Particle, time:Number):void; + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/force/IRigidBodyForceGenerator.as b/0.0.0.18/src/alternativa/physics/force/IRigidBodyForceGenerator.as new file mode 100644 index 0000000..d49cc8c --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/force/IRigidBodyForceGenerator.as @@ -0,0 +1,10 @@ +package alternativa.physics.force { + import alternativa.physics.rigid.RigidBody; + + /** + * + */ + public interface IRigidBodyForceGenerator { + function updateForce(body:RigidBody, time:Number):void; + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/force/ParticleAnchoredBungee.as b/0.0.0.18/src/alternativa/physics/force/ParticleAnchoredBungee.as new file mode 100644 index 0000000..399dac4 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/force/ParticleAnchoredBungee.as @@ -0,0 +1,37 @@ +package alternativa.physics.force { + import alternativa.physics.particle.Particle; + import alternativa.types.Point3D; + + public class ParticleAnchoredBungee implements IParticelForceGenerator { + + private var force:Point3D = new Point3D(); + + private var anchor:Point3D; + private var springCoeff:Number; + private var restLength:Number; + public var relaxed:Boolean; + + public function ParticleAnchoredBungee(anchor:Point3D, springCoeff:Number, restLength:Number) { + this.anchor = anchor; + this.springCoeff = springCoeff; + this.restLength = restLength; + } + + public function updateForce(particle:Particle, time:Number):void { + force.x = particle.position.x - anchor.x; + force.y = particle.position.y - anchor.y; + force.z = particle.position.z - anchor.z; + var len:Number = force.length; + if (len < restLength) { + relaxed = true; + return; + } + relaxed = false; + var k:Number = (restLength - len)*springCoeff/len; + force.x *= k; + force.y *= k; + force.z *= k; + particle.addForce(force); + } + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/force/ParticleAnchoredSpring.as b/0.0.0.18/src/alternativa/physics/force/ParticleAnchoredSpring.as new file mode 100644 index 0000000..ed9263d --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/force/ParticleAnchoredSpring.as @@ -0,0 +1,31 @@ +package alternativa.physics.force { + import alternativa.physics.particle.Particle; + import alternativa.types.Point3D; + + public class ParticleAnchoredSpring implements IParticelForceGenerator { + + private var force:Point3D = new Point3D(); + + private var anchor:Point3D; + private var springCoeff:Number; + private var restLength:Number; + + public function ParticleAnchoredSpring(anchor:Point3D, springCoeff:Number, restLength:Number) { + this.anchor = anchor; + this.springCoeff = springCoeff; + this.restLength = restLength; + } + + public function updateForce(particle:Particle, time:Number):void { + force.x = particle.position.x - anchor.x; + force.y = particle.position.y - anchor.y; + force.z = particle.position.z - anchor.z; + var len:Number = force.length; + var k:Number = (restLength - len)*springCoeff/len; + force.x *= k; + force.y *= k; + force.z *= k; + particle.addForce(force); + } + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/force/ParticleBungee.as b/0.0.0.18/src/alternativa/physics/force/ParticleBungee.as new file mode 100644 index 0000000..41cf3c0 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/force/ParticleBungee.as @@ -0,0 +1,38 @@ +package alternativa.physics.force { + import alternativa.physics.particle.Particle; + import alternativa.types.Point3D; + + public class ParticleBungee implements IParticelForceGenerator { + + private var force:Point3D = new Point3D(); + + private var otherParticle:Particle; + private var springCoeff:Number; + private var restLength:Number; + + public var relaxed:Boolean; + + public function ParticleBungee(otherParticle:Particle, springCoeff:Number, restLength:Number) { + this.otherParticle = otherParticle; + this.springCoeff = springCoeff; + this.restLength = restLength; + } + + public function updateForce(particle:Particle, time:Number):void { + force.x = particle.position.x - otherParticle.position.x; + force.y = particle.position.y - otherParticle.position.y; + force.z = particle.position.z - otherParticle.position.z; + var len:Number = force.length; + if (len < restLength) { + relaxed = true; + return; + } + relaxed = false; + var k:Number = (restLength - len)*springCoeff/len; + force.x *= k; + force.y *= k; + force.z *= k; + particle.addForce(force); + } + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/force/ParticleBuoyancy.as b/0.0.0.18/src/alternativa/physics/force/ParticleBuoyancy.as new file mode 100644 index 0000000..45c0de3 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/force/ParticleBuoyancy.as @@ -0,0 +1,37 @@ +package alternativa.physics.force { + import alternativa.physics.particle.Particle; + import alternativa.types.Point3D; + + public class ParticleBuoyancy implements IParticelForceGenerator { + + private var force:Point3D = new Point3D(); + + private var maxDepth:Number; + private var waterHeight:Number; + private var volume:Number; + private var liquidDensity:Number; + + public function ParticleBuoyancy(maxDepth:Number, waterHeight:Number, volume:Number, liquidDensity:Number = 1000) { + this.maxDepth = maxDepth; + this.waterHeight = waterHeight; + this.volume = volume; + this.liquidDensity = liquidDensity; + } + + public function updateForce(particle:Particle, time:Number):void { + var depth:Number = particle.position.y; + if (depth > waterHeight + maxDepth) { + return; + } + + force.x = 0; + force.y = 0; + if (depth <= waterHeight - maxDepth) { + force.z = liquidDensity*volume; + } else { + force.z = liquidDensity*volume*(waterHeight + maxDepth - depth)/maxDepth*0.5; + } + particle.addForce(force); + } + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/force/ParticleDrag.as b/0.0.0.18/src/alternativa/physics/force/ParticleDrag.as new file mode 100644 index 0000000..e9de457 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/force/ParticleDrag.as @@ -0,0 +1,29 @@ +package alternativa.physics.force { + import alternativa.physics.particle.Particle; + import alternativa.types.Point3D; + + public class ParticleDrag implements IParticelForceGenerator { + private var force:Point3D = new Point3D(); + + private var k1:Number; + private var k2:Number; + + public function ParticleDrag(k1:Number, k2:Number) { + this.k1 = k1; + this.k2 = k2; + } + + public function updateForce(particle:Particle, time:Number):void { + force.x = particle.velocity.x; + force.y = particle.velocity.y; + force.z = particle.velocity.z; + var len:Number = force.length; + var coeff:Number = len; + coeff = -(k1*coeff + k2*coeff*coeff)/len; + force.x *= coeff; + force.y *= coeff; + force.z *= coeff; + particle.addForce(force); + } + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/force/ParticleGravity.as b/0.0.0.18/src/alternativa/physics/force/ParticleGravity.as new file mode 100644 index 0000000..ff400d2 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/force/ParticleGravity.as @@ -0,0 +1,24 @@ +package alternativa.physics.force { + import alternativa.physics.particle.Particle; + import alternativa.types.Point3D; + + public class ParticleGravity implements IParticelForceGenerator { + private var force:Point3D = new Point3D(); + + private var _gravity:Point3D; + + public function ParticleGravity(gravity:Point3D) { + _gravity = gravity.clone(); + } + + public function updateForce(particle:Particle, time:Number):void { + if (!particle.hasFiniteMass()) { + return; + } + force.x = _gravity.x*particle.mass; + force.y = _gravity.y*particle.mass; + force.z = _gravity.z*particle.mass; + particle.addForce(force); + } + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/force/ParticleSpring.as b/0.0.0.18/src/alternativa/physics/force/ParticleSpring.as new file mode 100644 index 0000000..aee5732 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/force/ParticleSpring.as @@ -0,0 +1,31 @@ +package alternativa.physics.force { + import alternativa.physics.particle.Particle; + import alternativa.types.Point3D; + + public class ParticleSpring implements IParticelForceGenerator { + + private var force:Point3D = new Point3D(); + + private var otherParticle:Particle; + private var springCoeff:Number; + private var restLength:Number; + + public function ParticleSpring(otherParticle:Particle, springCoeff:Number, restLength:Number) { + this.otherParticle = otherParticle; + this.springCoeff = springCoeff; + this.restLength = restLength; + } + + public function updateForce(particle:Particle, time:Number):void { + force.x = particle.position.x - otherParticle.position.x; + force.y = particle.position.y - otherParticle.position.y; + force.z = particle.position.z - otherParticle.position.z; + var len:Number = force.length; + var k:Number = (restLength - len)*springCoeff/len; + force.x *= k; + force.y *= k; + force.z *= k; + particle.addForce(force); + } + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/force/RigidBodyAnchoredBungee.as b/0.0.0.18/src/alternativa/physics/force/RigidBodyAnchoredBungee.as new file mode 100644 index 0000000..85921c4 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/force/RigidBodyAnchoredBungee.as @@ -0,0 +1,45 @@ +package alternativa.physics.force { + import alternativa.physics.*; + import alternativa.physics.rigid.RigidBody; + import alternativa.types.Point3D; + + use namespace altphysics; + + public class RigidBodyAnchoredBungee implements IRigidBodyForceGenerator { + + private var vector1:Point3D = new Point3D(); + private var vector2:Point3D = new Point3D(); + + private var _connectionPoint:Point3D = new Point3D(); + private var _anchorPoint:Point3D = new Point3D(); + private var _springConstant:Number; + private var _restLength:Number; + + public function RigidBodyAnchoredBungee(connectionPoint:Point3D, anchorPoint:Point3D, springConstant:Number, restLength:Number) { + _connectionPoint.copy(connectionPoint); + _anchorPoint.copy(anchorPoint); + _springConstant = springConstant; + _restLength = restLength; + } + + public function updateForce(body:RigidBody, time:Number):void { + // TODO: Оптимизировать!!! + vector1.copy(_connectionPoint); + vector1.transform(body.transformMatrix); + vector2.copy(_anchorPoint); + vector2.subtract(vector1); + var len:Number = vector2.length; + var magnitude:Number = len - _restLength; + if (magnitude > 0) { + magnitude *= _springConstant; + vector2.multiply(magnitude/len); + body.addForceAtPoint(vector2, vector1); + } + } + + public function set anchorPoint(value:Point3D):void { + _anchorPoint.copy(value); + } + + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/force/RigidBodyGravity.as b/0.0.0.18/src/alternativa/physics/force/RigidBodyGravity.as new file mode 100644 index 0000000..e99aed1 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/force/RigidBodyGravity.as @@ -0,0 +1,27 @@ +package alternativa.physics.force { + import alternativa.physics.*; + import alternativa.physics.rigid.RigidBody; + import alternativa.types.Point3D; + + use namespace altphysics; + + /** + * + */ + public class RigidBodyGravity implements IRigidBodyForceGenerator { + + private var _gravity:Point3D = new Point3D(); + + public function RigidBodyGravity(g:Point3D) { + _gravity.copy(g); + } + + public function updateForce(body:RigidBody, time:Number):void { + if (body.inverseMass == 0) { + return; + } + body.addForceComponents(_gravity.x*body.mass, _gravity.y*body.mass, _gravity.z*body.mass); + } + + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/force/RigidBodySpring.as b/0.0.0.18/src/alternativa/physics/force/RigidBodySpring.as new file mode 100644 index 0000000..1a5d44a --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/force/RigidBodySpring.as @@ -0,0 +1,45 @@ +package alternativa.physics.force { + import alternativa.physics.*; + import alternativa.physics.rigid.RigidBody; + import alternativa.types.Point3D; + + use namespace altphysics; + + public class RigidBodySpring implements IRigidBodyForceGenerator { + + private var vector1:Point3D = new Point3D(); + private var vector2:Point3D = new Point3D(); + + private var _connectionPoint:Point3D = new Point3D(); + private var _otherConnectionPoint:Point3D = new Point3D(); + private var _springConstant:Number; + private var _restLength:Number; + private var _otherBody:RigidBody; + + public function RigidBodySpring(connectionPoint:Point3D, otherBody:RigidBody, otherConnectionPoint:Point3D, springConstant:Number, restLength:Number) { + _connectionPoint.copy(connectionPoint); + _otherConnectionPoint = otherConnectionPoint; + _otherBody = otherBody; + _springConstant = springConstant; + _restLength = restLength; + } + + public function updateForce(body:RigidBody, time:Number):void { + // TODO: Оптимизировать!!! + vector1.copy(_connectionPoint); + vector1.transform(body.transformMatrix); + vector2.copy(_otherConnectionPoint); + vector2.transform(_otherBody.transformMatrix); + vector2.subtract(vector1); + var len:Number = vector2.length; + if (len == 0) { + trace("[RigidBodySpring::updateForce] spring length is zero"); + } + var magnitude:Number = len - _restLength; + magnitude *= _springConstant; + vector2.multiply(magnitude/len); + body.addForceAtPoint(vector2, vector1); + } + + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/particle/.svn/all-wcprops b/0.0.0.18/src/alternativa/physics/particle/.svn/all-wcprops new file mode 100644 index 0000000..c72fffa --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/particle/.svn/all-wcprops @@ -0,0 +1,53 @@ +K 25 +svn:wc:ra_dav:version-url +V 103 +/!svn/ver/4378/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/particle +END +ParticleContact.as +K 25 +svn:wc:ra_dav:version-url +V 122 +/!svn/ver/4196/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/particle/ParticleContact.as +END +ParticleContactResolver.as +K 25 +svn:wc:ra_dav:version-url +V 130 +/!svn/ver/4196/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/particle/ParticleContactResolver.as +END +ParticleLink.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/4196/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/particle/ParticleLink.as +END +Particle.as +K 25 +svn:wc:ra_dav:version-url +V 115 +/!svn/ver/4196/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/particle/Particle.as +END +ParticleContactGenerator.as +K 25 +svn:wc:ra_dav:version-url +V 131 +/!svn/ver/4196/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/particle/ParticleContactGenerator.as +END +ParticleRod.as +K 25 +svn:wc:ra_dav:version-url +V 118 +/!svn/ver/4196/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/particle/ParticleRod.as +END +ParticleCable.as +K 25 +svn:wc:ra_dav:version-url +V 120 +/!svn/ver/4196/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/particle/ParticleCable.as +END +ParticleWorld.as +K 25 +svn:wc:ra_dav:version-url +V 120 +/!svn/ver/4378/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/particle/ParticleWorld.as +END diff --git a/0.0.0.18/src/alternativa/physics/particle/.svn/entries b/0.0.0.18/src/alternativa/physics/particle/.svn/entries new file mode 100644 index 0000000..cc2b166 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/particle/.svn/entries @@ -0,0 +1,124 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/particle +http://svndev.alternativaplatform.com + + + +2008-11-26T12:12:57.063201Z +4378 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +ParticleContact.as +file + + + + +2010-10-28T04:34:31.000000Z +af51b49cb3918bc31be32dca4a9ddff2 +2008-11-24T06:53:44.272574Z +4196 +mike + +ParticleContactResolver.as +file + + + + +2010-10-28T04:34:31.000000Z +09a37756afe614b640d36b5e474ae709 +2008-11-24T06:53:44.272574Z +4196 +mike + +ParticleLink.as +file + + + + +2010-10-28T04:34:31.000000Z +6313230f57ccdba2b80c3ab4306d9d02 +2008-11-24T06:53:44.272574Z +4196 +mike + +Particle.as +file + + + + +2010-10-28T04:34:31.000000Z +36a3a8de3fce54e29395a591388b0fd9 +2008-11-24T06:53:44.272574Z +4196 +mike + +ParticleContactGenerator.as +file + + + + +2010-10-28T04:34:31.000000Z +1d585c4b2678e36ed0d44403ed35bbac +2008-11-24T06:53:44.272574Z +4196 +mike + +ParticleRod.as +file + + + + +2010-10-28T04:34:31.000000Z +a95a229bcce5f16576e8aa3bbee4d4b9 +2008-11-24T06:53:44.272574Z +4196 +mike + +ParticleCable.as +file + + + + +2010-10-28T04:34:31.000000Z +b289fc57f98c5170eb964493250a309d +2008-11-24T06:53:44.272574Z +4196 +mike + +ParticleWorld.as +file + + + + +2010-10-28T04:34:31.000000Z +29ebff641b6d9cc9def3022dd679889f +2008-11-26T12:12:57.063201Z +4378 +mike + diff --git a/0.0.0.18/src/alternativa/physics/particle/.svn/format b/0.0.0.18/src/alternativa/physics/particle/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/particle/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.0.18/src/alternativa/physics/particle/.svn/text-base/Particle.as.svn-base b/0.0.0.18/src/alternativa/physics/particle/.svn/text-base/Particle.as.svn-base new file mode 100644 index 0000000..3ac9a25 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/particle/.svn/text-base/Particle.as.svn-base @@ -0,0 +1,78 @@ +package alternativa.physics.particle { + import alternativa.types.Point3D; + + public class Particle { + + private static var resultingAcceleration:Point3D = new Point3D(); + + public var position:Point3D = new Point3D(); + public var velocity:Point3D = new Point3D(); + public var acceleration:Point3D = new Point3D(); + public var accumulatedForce:Point3D = new Point3D(); + public var mass:Number; + public var inverseMass:Number; + public var damping:Number = 0.995; + + public var next:Particle; + + /** + * @param mass масса частицы. Значение меньшее или равное нулю означает бесконечную массу. + */ + public function Particle(mass:Number = 0) { + if (mass <= 0) { + this.mass = 0; + inverseMass = 0; + } else { + this.mass = mass; + inverseMass = 1/mass; + } + } + + /** + * + * @param time время в секундах + */ + public function integrate(time:Number):void { + if (time <= 0) { + return; + } + + position.x += velocity.x*time; + position.y += velocity.y*time; + position.z += velocity.z*time; + + resultingAcceleration.x = acceleration.x + inverseMass*accumulatedForce.x; + resultingAcceleration.y = acceleration.y + inverseMass*accumulatedForce.y; + resultingAcceleration.z = acceleration.z + inverseMass*accumulatedForce.z; + + velocity.x += resultingAcceleration.x*time; + velocity.y += resultingAcceleration.y*time; + velocity.z += resultingAcceleration.z*time; + + var d:Number = Math.pow(damping, time); + velocity.x *= d; + velocity.y *= d; + velocity.z *= d; + + accumulatedForce.x = 0; + accumulatedForce.y = 0; + accumulatedForce.z = 0; + } + + public function clearForce():void { + accumulatedForce.x = 0; + accumulatedForce.y = 0; + accumulatedForce.z = 0; + } + + public function addForce(force:Point3D):void { + accumulatedForce.x += force.x; + accumulatedForce.y += force.y; + accumulatedForce.z += force.z; + } + + public function hasFiniteMass():Boolean { + return inverseMass > 0; + } + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/particle/.svn/text-base/ParticleCable.as.svn-base b/0.0.0.18/src/alternativa/physics/particle/.svn/text-base/ParticleCable.as.svn-base new file mode 100644 index 0000000..61d494b --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/particle/.svn/text-base/ParticleCable.as.svn-base @@ -0,0 +1,46 @@ +package alternativa.physics.particle { + import alternativa.types.Point3D; + + public class ParticleCable extends ParticleLink { + + private static var vector:Point3D = new Point3D(); + + public var maxLength:Number; + public var restitution:Number; + + public function ParticleCable(particle1:Particle, particle2:Particle, maxLength:Number, restitution:Number) { + super(particle1, particle2); + this.maxLength = maxLength; + this.restitution = restitution; + } + + override protected function getCurrentLength():Number { + var x:Number = particle1.position.x - particle2.position.x; + var y:Number = particle1.position.y - particle2.position.y; + var z:Number = particle1.position.z - particle2.position.z; + return Math.sqrt(x*x + y*y + z*z); + } + + override public function fillContact(contact:ParticleContact):Boolean { + // Для кабеля нормаль контакта направлена в сторону второй частицы + vector.x = particle2.position.x - particle1.position.x; + vector.y = particle2.position.y - particle1.position.y; + vector.z = particle2.position.z - particle1.position.z; + var len:Number = Math.sqrt(vector.x*vector.x + vector.y*vector.y + vector.z*vector.z); + + if (len < maxLength) { + return false; + } + + contact.particle1 = particle1; + contact.particle2 = particle2; + contact.contactNormal.x = vector.x; + contact.contactNormal.y = vector.y; + contact.contactNormal.z = vector.z; + contact.penetration = len - maxLength; + contact.restitution = restitution; + + return true; + } + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/particle/.svn/text-base/ParticleContact.as.svn-base b/0.0.0.18/src/alternativa/physics/particle/.svn/text-base/ParticleContact.as.svn-base new file mode 100644 index 0000000..3701fba --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/particle/.svn/text-base/ParticleContact.as.svn-base @@ -0,0 +1,175 @@ +package alternativa.physics.particle { + import alternativa.types.Point3D; + + public class ParticleContact { + + private static var vector:Point3D = new Point3D(); + + public var particle1:Particle; + public var particle2:Particle; + public var restitution:Number; + public var penetration:Number; + public var contactNormal:Point3D = new Point3D(); + public var isCollision:Boolean; + public var frictionCoeff:Number = 0; + + public var next:ParticleContact; + public var index:int; + + /** + * + * @param particle1 + * @param particle2 + * @param restitution + * @param contactNormal + */ + public function ParticleContact(index:int, particle1:Particle = null, particle2:Particle = null, contactNormal:Point3D = null, penetration:Number = 0, restitution:Number = 0) { + this.index = index; + this.particle1 = particle1; + this.particle2 = particle2; + if (contactNormal != null) { + this.contactNormal.x = contactNormal.x; + this.contactNormal.y = contactNormal.y; + this.contactNormal.z = contactNormal.z; + } + this.penetration = penetration; + this.restitution = restitution; + } + + /** + * + * @param time + */ + public function resolve(time:Number):void { + resolveVelocity(time); + resolveInterpenetration(time); + } + + /** + * + * @return + */ + public function calculateSeparatingVelocity():Number { + vector.x = particle1.velocity.x; + vector.y = particle1.velocity.y; + vector.z = particle1.velocity.z; + if (particle2 != null) { + vector.x -= particle2.velocity.x; + vector.y -= particle2.velocity.y; + vector.z -= particle2.velocity.z; + } + return vector.x*contactNormal.x + vector.y*contactNormal.y + vector.z*contactNormal.z; + } + + /** + * + * @param time + */ + private function resolveVelocity(time:Number):void { + var separatingVelocity:Number = calculateSeparatingVelocity(); + if (separatingVelocity > 0) { + return; + } + var newSeparatingVelocity:Number = -restitution*separatingVelocity; + + var totalInverseMass:Number = particle1.inverseMass; + if (particle2 != null) { + totalInverseMass += particle2.inverseMass; + } + if (totalInverseMass <= 0) { + // Пропускаем, т.к. обе частицы бесконечной массы + return; + } + + // Обработка покоящихся контактов + // Проверяем, не была ли относительная скорость приобретена только в течении одного кадра + if (particle2 != null) { + vector.x = particle1.acceleration.x - particle2.acceleration.x; + vector.y = particle1.acceleration.y - particle2.acceleration.y; + vector.z = particle1.acceleration.z - particle2.acceleration.z; + } else { + vector.x = particle1.acceleration.x; + vector.y = particle1.acceleration.y; + vector.z = particle1.acceleration.z; + } + var accCausedSepVelocity:Number = (vector.x*contactNormal.x + vector.y*contactNormal.y + vector.z*contactNormal.z)*time; + + if (accCausedSepVelocity < 0) { + // Вычтем скорость сближения, полученную под действием сил во время последнего кадра, из скорости разделения контакта + newSeparatingVelocity += restitution*accCausedSepVelocity; + if (newSeparatingVelocity < 0) { + // Скорость разделения не может быть отрицательной + newSeparatingVelocity = 0; + } + } + + var impulse:Number = (newSeparatingVelocity - separatingVelocity)/totalInverseMass; + vector.x = impulse*contactNormal.x; + vector.y = impulse*contactNormal.y; + vector.z = impulse*contactNormal.z; + + particle1.velocity.x += vector.x*particle1.inverseMass; + particle1.velocity.y += vector.y*particle1.inverseMass; + particle1.velocity.z += vector.z*particle1.inverseMass; + + if (particle2 != null) { + particle2.velocity.x -= vector.x*particle2.inverseMass; + particle2.velocity.y -= vector.y*particle2.inverseMass; + particle2.velocity.z -= vector.z*particle2.inverseMass; + } + +// if (isCollision) { +// var tangentSpeed:Number = particle1.velocity.length; +// var deltaSpeed:Number = frictionCoeff*particle1.inverseMass*impulse; +// if (tangentSpeed < deltaSpeed) { +// particle1.velocity.x = 0; +// particle1.velocity.y = 0; +// particle1.velocity.z = 0; +// } else { +// var k:Number = (tangentSpeed - deltaSpeed)/tangentSpeed; +// particle1.velocity.x *= k; +// particle1.velocity.y *= k; +// particle1.velocity.z *= k; +// } +// } + } + + /** + * + * @param time + */ + private function resolveInterpenetration(time:Number):void { + if (penetration <= 0) { + return; + } + + var totalInverseMass:Number = particle1.inverseMass; + if (particle2 != null) { + totalInverseMass += particle2.inverseMass; + } + if (totalInverseMass <= 0) { + // Пропускаем, т.к. обе частицы бесконечной массы + return; + } + + var k:Number = penetration/totalInverseMass; + + vector.x = k*contactNormal.x; + vector.y = k*contactNormal.y; + vector.z = k*contactNormal.z; + +// trace("\nparticle1.position", particle1.position.z, particle1.velocity.z); + particle1.position.x += vector.x*particle1.inverseMass; + particle1.position.y += vector.y*particle1.inverseMass; + particle1.position.z += vector.z*particle1.inverseMass; +// trace("particle1.position", particle1.position.z, particle1.velocity.z); + + if (particle2 != null) { + particle2.position.x -= vector.x*particle2.inverseMass; + particle2.position.y -= vector.y*particle2.inverseMass; + particle2.position.z -= vector.z*particle2.inverseMass; + } + penetration = 0; + } + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/particle/.svn/text-base/ParticleContactGenerator.as.svn-base b/0.0.0.18/src/alternativa/physics/particle/.svn/text-base/ParticleContactGenerator.as.svn-base new file mode 100644 index 0000000..65ec7b8 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/particle/.svn/text-base/ParticleContactGenerator.as.svn-base @@ -0,0 +1,26 @@ +package alternativa.physics.particle { + + /** + * Базовй класс для генераторов контактов. Реализует поддержку однонаправленного списока генераторов. + */ + public class ParticleContactGenerator { + + /** + * Следующий генератор в списке. + */ + public var next:ParticleContactGenerator; + + /** + * Метод создаёт информацию о контактах. + * + * @param contact первый свободный для записи контакт в списке контактов мира + * + * @return следующий свободный для записи контакт после того, как генератор записал свою информацию. Если возвращено значение null, + * то значит список доступных контактов исчерпан. + */ + public function addContacts(contact:ParticleContact):ParticleContact { + return null; + } + + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/particle/.svn/text-base/ParticleContactResolver.as.svn-base b/0.0.0.18/src/alternativa/physics/particle/.svn/text-base/ParticleContactResolver.as.svn-base new file mode 100644 index 0000000..b65a7f5 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/particle/.svn/text-base/ParticleContactResolver.as.svn-base @@ -0,0 +1,87 @@ +package alternativa.physics.particle { + public class ParticleContactResolver { + + private var iterations:int; + private var iterationsUsed:int; + + public function ParticleContactResolver(iterations:int) { + this.iterations = iterations; + } + + public function setIterations(value:int):void { + iterations = value; + } + + public function resolveContacts(contacts:ParticleContact, contactsUsed:int, time:Number):void { + resolveAllContacts(contacts, contactsUsed, time); +// resolveCollisionContacts(contacts, contactsUsed, time); +// resolveNonCollisionContacts(contacts, contactsUsed, time); + } + + private function resolveAllContacts(contacts:ParticleContact, contactsUsed:int, time:Number):void { + iterationsUsed = 0; + iterations = 5; + while (iterationsUsed < iterations) { + var maxSepVelocity:Number = 0; + var maxPenetration:Number = 0; + var mostSevereContact:ParticleContact = null; + var contact:ParticleContact = contacts; + while (contact.index < contactsUsed) { + var sepVelocity:Number = contact.calculateSeparatingVelocity(); + if (sepVelocity < maxSepVelocity) { + maxSepVelocity = sepVelocity; + mostSevereContact = contact; + } + contact = contact.next; + } + if (mostSevereContact != null) { + mostSevereContact.resolve(time); + } + iterationsUsed++; + } + } + + private function resolveNonCollisionContacts(contacts:ParticleContact, contactsUsed:int, time:Number):void { + iterationsUsed = 0; + while (iterationsUsed < iterations) { + var maxSepVelocity:Number = 0; + var maxPenetration:Number = 0; + var mostSevereContact:ParticleContact = null; + var contact:ParticleContact = contacts; + while (contact.index < contactsUsed) { + if (!contact.isCollision) { + var sepVelocity:Number = contact.calculateSeparatingVelocity(); + if (sepVelocity < maxSepVelocity) { + maxSepVelocity = sepVelocity; + mostSevereContact = contact; + } + } + contact = contact.next; + } + if (mostSevereContact != null) { + mostSevereContact.resolve(time); + } + iterationsUsed++; + } + } + + private function resolveCollisionContacts(contacts:ParticleContact, contactsUsed:int, time:Number):void { + var maxPenetration:Number = 0; + var mostSevereContact:ParticleContact = null; + var contact:ParticleContact = contacts; + while (contact.index < contactsUsed) { + if (contact.isCollision && contact.penetration > maxPenetration) { + maxPenetration = contact.penetration; + mostSevereContact = contact; + } + contact = contact.next; + } + if (mostSevereContact != null) { + if (mostSevereContact.calculateSeparatingVelocity() < 0) { + mostSevereContact.resolve(time); + } + } + } + + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/particle/.svn/text-base/ParticleLink.as.svn-base b/0.0.0.18/src/alternativa/physics/particle/.svn/text-base/ParticleLink.as.svn-base new file mode 100644 index 0000000..d4b999a --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/particle/.svn/text-base/ParticleLink.as.svn-base @@ -0,0 +1,21 @@ +package alternativa.physics.particle { + + public class ParticleLink { + + public var particle1:Particle; + public var particle2:Particle; + + public function ParticleLink(particle1:Particle, particle2:Particle) { + this.particle1 = particle1; + this.particle2 = particle2; + } + + protected function getCurrentLength():Number { + return 0; + } + + public function fillContact(contact:ParticleContact):Boolean { + return false; + } + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/particle/.svn/text-base/ParticleRod.as.svn-base b/0.0.0.18/src/alternativa/physics/particle/.svn/text-base/ParticleRod.as.svn-base new file mode 100644 index 0000000..e24fa5e --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/particle/.svn/text-base/ParticleRod.as.svn-base @@ -0,0 +1,58 @@ +package alternativa.physics.particle { + import alternativa.types.Point3D; + + public class ParticleRod extends ParticleLink { + + private static var vector:Point3D = new Point3D(); + + public var length:Number; + + public function ParticleRod(particle1:Particle, particle2:Particle, length:Number = 0) { + super(particle1, particle2); + if (length <= 0) { + this.length = Point3D.difference(particle1.position, particle2.position).length; + } else { + this.length = length; + } + } + + override protected function getCurrentLength():Number { + var x:Number = particle1.position.x - particle2.position.x; + var y:Number = particle1.position.y - particle2.position.y; + var z:Number = particle1.position.z - particle2.position.z; + return Math.sqrt(x*x + y*y + z*z); + } + + override public function fillContact(contact:ParticleContact):Boolean { + vector.x = particle1.position.x - particle2.position.x; + vector.y = particle1.position.y - particle2.position.y; + vector.z = particle1.position.z - particle2.position.z; + var len:Number = Math.sqrt(vector.x*vector.x + vector.y*vector.y + vector.z*vector.z); + + if (len == length) { + return false; + } + + var penetration:Number; + if (len > length) { + vector.x = -vector.x; + vector.y = -vector.y; + vector.z = -vector.z; + penetration = len - length; + } else { + penetration = length - len; + } + + contact.particle1 = particle1; + contact.particle2 = particle2; + contact.contactNormal.x = vector.x/len; + contact.contactNormal.y = vector.y/len; + contact.contactNormal.z = vector.z/len; + contact.penetration = penetration; + contact.restitution = 0; + contact.isCollision = false; + + return true; + } + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/particle/.svn/text-base/ParticleWorld.as.svn-base b/0.0.0.18/src/alternativa/physics/particle/.svn/text-base/ParticleWorld.as.svn-base new file mode 100644 index 0000000..ebbf5a4 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/particle/.svn/text-base/ParticleWorld.as.svn-base @@ -0,0 +1,113 @@ +package alternativa.physics.particle { + import alternativa.physics.particle.Particle; + import alternativa.physics.particle.ParticleContact; + import alternativa.physics.particle.ParticleContactGenerator; + import alternativa.physics.particle.ParticleContactResolver; + import alternativa.physics.registry.ParticleForceRegistry; + + public class ParticleWorld { + + public var particles:Particle; + public var last:Particle; + + public var contacts:ParticleContact; + public var contactGenerators:ParticleContactGenerator; + public var forceRegistry:ParticleForceRegistry; + + public var resolver:ParticleContactResolver; + + public var maxContacts:int; + public var iterations:int; + + public function ParticleWorld(maxContacts:int, iterations:int = 0) { + this.maxContacts = maxContacts; + this.iterations = iterations; + + // Создаём контакты + contacts = new ParticleContact(0); + var contact:ParticleContact = contacts; + for (var i:int = 1; i < maxContacts; i++) { + contact = contact.next = new ParticleContact(i); + } + + resolver = new ParticleContactResolver(0); + forceRegistry = new ParticleForceRegistry(); + } + + public function addParticle(particle:Particle):void { + if (particles == null) { + last = particles = particle; + } else { + last.next = particle; + last = particle; + } + } + + public function addContactGenerator(generator:ParticleContactGenerator):void { + if (contactGenerators == null) { + contactGenerators = generator; + } else { + var gen:ParticleContactGenerator = contactGenerators; + while (gen.next != null) { + gen = gen.next; + } + gen.next = generator; + } + } + + public function startFrame():void { + var particle:Particle = particles; + while (particle != null) { + particle.clearForce(); + particle = particle.next; + } + } + + /** + * Последоавтельно запускает все зарегистрированные генераторы контактов. + * + * @return количество использованных контактов + */ + public function generateContacts():int { + var contact:ParticleContact = contacts; + var generator:ParticleContactGenerator = contactGenerators; + while (generator != null) { + contact = generator.addContacts(contact); + if (contact == null) { + // Доступные контакты исчерпаны + return maxContacts; + } + generator = generator.next; + } + return contact.index; + } + + public function intergrate(time:Number):void { + var particle:Particle = particles; + while (particle != null) { + particle.integrate(time); + particle = particle.next; + } + } + + /** + * Запускает физическую симуляцию. + * + * @param time + */ + public function runPhysics(time:Number):void { + // Применение генераторов сил + forceRegistry.updateForces(time); + // Запуск интеграторов для всех частиц + intergrate(time); + // Создание контактов + var contactsUsed:int = generateContacts(); + // Обработка полученных контактов + if (iterations == 0) { + resolver.setIterations(3*contactsUsed); + } + resolver.resolveContacts(contacts, contactsUsed, time); + } + + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/particle/Particle.as b/0.0.0.18/src/alternativa/physics/particle/Particle.as new file mode 100644 index 0000000..3ac9a25 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/particle/Particle.as @@ -0,0 +1,78 @@ +package alternativa.physics.particle { + import alternativa.types.Point3D; + + public class Particle { + + private static var resultingAcceleration:Point3D = new Point3D(); + + public var position:Point3D = new Point3D(); + public var velocity:Point3D = new Point3D(); + public var acceleration:Point3D = new Point3D(); + public var accumulatedForce:Point3D = new Point3D(); + public var mass:Number; + public var inverseMass:Number; + public var damping:Number = 0.995; + + public var next:Particle; + + /** + * @param mass масса частицы. Значение меньшее или равное нулю означает бесконечную массу. + */ + public function Particle(mass:Number = 0) { + if (mass <= 0) { + this.mass = 0; + inverseMass = 0; + } else { + this.mass = mass; + inverseMass = 1/mass; + } + } + + /** + * + * @param time время в секундах + */ + public function integrate(time:Number):void { + if (time <= 0) { + return; + } + + position.x += velocity.x*time; + position.y += velocity.y*time; + position.z += velocity.z*time; + + resultingAcceleration.x = acceleration.x + inverseMass*accumulatedForce.x; + resultingAcceleration.y = acceleration.y + inverseMass*accumulatedForce.y; + resultingAcceleration.z = acceleration.z + inverseMass*accumulatedForce.z; + + velocity.x += resultingAcceleration.x*time; + velocity.y += resultingAcceleration.y*time; + velocity.z += resultingAcceleration.z*time; + + var d:Number = Math.pow(damping, time); + velocity.x *= d; + velocity.y *= d; + velocity.z *= d; + + accumulatedForce.x = 0; + accumulatedForce.y = 0; + accumulatedForce.z = 0; + } + + public function clearForce():void { + accumulatedForce.x = 0; + accumulatedForce.y = 0; + accumulatedForce.z = 0; + } + + public function addForce(force:Point3D):void { + accumulatedForce.x += force.x; + accumulatedForce.y += force.y; + accumulatedForce.z += force.z; + } + + public function hasFiniteMass():Boolean { + return inverseMass > 0; + } + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/particle/ParticleCable.as b/0.0.0.18/src/alternativa/physics/particle/ParticleCable.as new file mode 100644 index 0000000..61d494b --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/particle/ParticleCable.as @@ -0,0 +1,46 @@ +package alternativa.physics.particle { + import alternativa.types.Point3D; + + public class ParticleCable extends ParticleLink { + + private static var vector:Point3D = new Point3D(); + + public var maxLength:Number; + public var restitution:Number; + + public function ParticleCable(particle1:Particle, particle2:Particle, maxLength:Number, restitution:Number) { + super(particle1, particle2); + this.maxLength = maxLength; + this.restitution = restitution; + } + + override protected function getCurrentLength():Number { + var x:Number = particle1.position.x - particle2.position.x; + var y:Number = particle1.position.y - particle2.position.y; + var z:Number = particle1.position.z - particle2.position.z; + return Math.sqrt(x*x + y*y + z*z); + } + + override public function fillContact(contact:ParticleContact):Boolean { + // Для кабеля нормаль контакта направлена в сторону второй частицы + vector.x = particle2.position.x - particle1.position.x; + vector.y = particle2.position.y - particle1.position.y; + vector.z = particle2.position.z - particle1.position.z; + var len:Number = Math.sqrt(vector.x*vector.x + vector.y*vector.y + vector.z*vector.z); + + if (len < maxLength) { + return false; + } + + contact.particle1 = particle1; + contact.particle2 = particle2; + contact.contactNormal.x = vector.x; + contact.contactNormal.y = vector.y; + contact.contactNormal.z = vector.z; + contact.penetration = len - maxLength; + contact.restitution = restitution; + + return true; + } + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/particle/ParticleContact.as b/0.0.0.18/src/alternativa/physics/particle/ParticleContact.as new file mode 100644 index 0000000..3701fba --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/particle/ParticleContact.as @@ -0,0 +1,175 @@ +package alternativa.physics.particle { + import alternativa.types.Point3D; + + public class ParticleContact { + + private static var vector:Point3D = new Point3D(); + + public var particle1:Particle; + public var particle2:Particle; + public var restitution:Number; + public var penetration:Number; + public var contactNormal:Point3D = new Point3D(); + public var isCollision:Boolean; + public var frictionCoeff:Number = 0; + + public var next:ParticleContact; + public var index:int; + + /** + * + * @param particle1 + * @param particle2 + * @param restitution + * @param contactNormal + */ + public function ParticleContact(index:int, particle1:Particle = null, particle2:Particle = null, contactNormal:Point3D = null, penetration:Number = 0, restitution:Number = 0) { + this.index = index; + this.particle1 = particle1; + this.particle2 = particle2; + if (contactNormal != null) { + this.contactNormal.x = contactNormal.x; + this.contactNormal.y = contactNormal.y; + this.contactNormal.z = contactNormal.z; + } + this.penetration = penetration; + this.restitution = restitution; + } + + /** + * + * @param time + */ + public function resolve(time:Number):void { + resolveVelocity(time); + resolveInterpenetration(time); + } + + /** + * + * @return + */ + public function calculateSeparatingVelocity():Number { + vector.x = particle1.velocity.x; + vector.y = particle1.velocity.y; + vector.z = particle1.velocity.z; + if (particle2 != null) { + vector.x -= particle2.velocity.x; + vector.y -= particle2.velocity.y; + vector.z -= particle2.velocity.z; + } + return vector.x*contactNormal.x + vector.y*contactNormal.y + vector.z*contactNormal.z; + } + + /** + * + * @param time + */ + private function resolveVelocity(time:Number):void { + var separatingVelocity:Number = calculateSeparatingVelocity(); + if (separatingVelocity > 0) { + return; + } + var newSeparatingVelocity:Number = -restitution*separatingVelocity; + + var totalInverseMass:Number = particle1.inverseMass; + if (particle2 != null) { + totalInverseMass += particle2.inverseMass; + } + if (totalInverseMass <= 0) { + // Пропускаем, т.к. обе частицы бесконечной массы + return; + } + + // Обработка покоящихся контактов + // Проверяем, не была ли относительная скорость приобретена только в течении одного кадра + if (particle2 != null) { + vector.x = particle1.acceleration.x - particle2.acceleration.x; + vector.y = particle1.acceleration.y - particle2.acceleration.y; + vector.z = particle1.acceleration.z - particle2.acceleration.z; + } else { + vector.x = particle1.acceleration.x; + vector.y = particle1.acceleration.y; + vector.z = particle1.acceleration.z; + } + var accCausedSepVelocity:Number = (vector.x*contactNormal.x + vector.y*contactNormal.y + vector.z*contactNormal.z)*time; + + if (accCausedSepVelocity < 0) { + // Вычтем скорость сближения, полученную под действием сил во время последнего кадра, из скорости разделения контакта + newSeparatingVelocity += restitution*accCausedSepVelocity; + if (newSeparatingVelocity < 0) { + // Скорость разделения не может быть отрицательной + newSeparatingVelocity = 0; + } + } + + var impulse:Number = (newSeparatingVelocity - separatingVelocity)/totalInverseMass; + vector.x = impulse*contactNormal.x; + vector.y = impulse*contactNormal.y; + vector.z = impulse*contactNormal.z; + + particle1.velocity.x += vector.x*particle1.inverseMass; + particle1.velocity.y += vector.y*particle1.inverseMass; + particle1.velocity.z += vector.z*particle1.inverseMass; + + if (particle2 != null) { + particle2.velocity.x -= vector.x*particle2.inverseMass; + particle2.velocity.y -= vector.y*particle2.inverseMass; + particle2.velocity.z -= vector.z*particle2.inverseMass; + } + +// if (isCollision) { +// var tangentSpeed:Number = particle1.velocity.length; +// var deltaSpeed:Number = frictionCoeff*particle1.inverseMass*impulse; +// if (tangentSpeed < deltaSpeed) { +// particle1.velocity.x = 0; +// particle1.velocity.y = 0; +// particle1.velocity.z = 0; +// } else { +// var k:Number = (tangentSpeed - deltaSpeed)/tangentSpeed; +// particle1.velocity.x *= k; +// particle1.velocity.y *= k; +// particle1.velocity.z *= k; +// } +// } + } + + /** + * + * @param time + */ + private function resolveInterpenetration(time:Number):void { + if (penetration <= 0) { + return; + } + + var totalInverseMass:Number = particle1.inverseMass; + if (particle2 != null) { + totalInverseMass += particle2.inverseMass; + } + if (totalInverseMass <= 0) { + // Пропускаем, т.к. обе частицы бесконечной массы + return; + } + + var k:Number = penetration/totalInverseMass; + + vector.x = k*contactNormal.x; + vector.y = k*contactNormal.y; + vector.z = k*contactNormal.z; + +// trace("\nparticle1.position", particle1.position.z, particle1.velocity.z); + particle1.position.x += vector.x*particle1.inverseMass; + particle1.position.y += vector.y*particle1.inverseMass; + particle1.position.z += vector.z*particle1.inverseMass; +// trace("particle1.position", particle1.position.z, particle1.velocity.z); + + if (particle2 != null) { + particle2.position.x -= vector.x*particle2.inverseMass; + particle2.position.y -= vector.y*particle2.inverseMass; + particle2.position.z -= vector.z*particle2.inverseMass; + } + penetration = 0; + } + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/particle/ParticleContactGenerator.as b/0.0.0.18/src/alternativa/physics/particle/ParticleContactGenerator.as new file mode 100644 index 0000000..65ec7b8 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/particle/ParticleContactGenerator.as @@ -0,0 +1,26 @@ +package alternativa.physics.particle { + + /** + * Базовй класс для генераторов контактов. Реализует поддержку однонаправленного списока генераторов. + */ + public class ParticleContactGenerator { + + /** + * Следующий генератор в списке. + */ + public var next:ParticleContactGenerator; + + /** + * Метод создаёт информацию о контактах. + * + * @param contact первый свободный для записи контакт в списке контактов мира + * + * @return следующий свободный для записи контакт после того, как генератор записал свою информацию. Если возвращено значение null, + * то значит список доступных контактов исчерпан. + */ + public function addContacts(contact:ParticleContact):ParticleContact { + return null; + } + + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/particle/ParticleContactResolver.as b/0.0.0.18/src/alternativa/physics/particle/ParticleContactResolver.as new file mode 100644 index 0000000..b65a7f5 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/particle/ParticleContactResolver.as @@ -0,0 +1,87 @@ +package alternativa.physics.particle { + public class ParticleContactResolver { + + private var iterations:int; + private var iterationsUsed:int; + + public function ParticleContactResolver(iterations:int) { + this.iterations = iterations; + } + + public function setIterations(value:int):void { + iterations = value; + } + + public function resolveContacts(contacts:ParticleContact, contactsUsed:int, time:Number):void { + resolveAllContacts(contacts, contactsUsed, time); +// resolveCollisionContacts(contacts, contactsUsed, time); +// resolveNonCollisionContacts(contacts, contactsUsed, time); + } + + private function resolveAllContacts(contacts:ParticleContact, contactsUsed:int, time:Number):void { + iterationsUsed = 0; + iterations = 5; + while (iterationsUsed < iterations) { + var maxSepVelocity:Number = 0; + var maxPenetration:Number = 0; + var mostSevereContact:ParticleContact = null; + var contact:ParticleContact = contacts; + while (contact.index < contactsUsed) { + var sepVelocity:Number = contact.calculateSeparatingVelocity(); + if (sepVelocity < maxSepVelocity) { + maxSepVelocity = sepVelocity; + mostSevereContact = contact; + } + contact = contact.next; + } + if (mostSevereContact != null) { + mostSevereContact.resolve(time); + } + iterationsUsed++; + } + } + + private function resolveNonCollisionContacts(contacts:ParticleContact, contactsUsed:int, time:Number):void { + iterationsUsed = 0; + while (iterationsUsed < iterations) { + var maxSepVelocity:Number = 0; + var maxPenetration:Number = 0; + var mostSevereContact:ParticleContact = null; + var contact:ParticleContact = contacts; + while (contact.index < contactsUsed) { + if (!contact.isCollision) { + var sepVelocity:Number = contact.calculateSeparatingVelocity(); + if (sepVelocity < maxSepVelocity) { + maxSepVelocity = sepVelocity; + mostSevereContact = contact; + } + } + contact = contact.next; + } + if (mostSevereContact != null) { + mostSevereContact.resolve(time); + } + iterationsUsed++; + } + } + + private function resolveCollisionContacts(contacts:ParticleContact, contactsUsed:int, time:Number):void { + var maxPenetration:Number = 0; + var mostSevereContact:ParticleContact = null; + var contact:ParticleContact = contacts; + while (contact.index < contactsUsed) { + if (contact.isCollision && contact.penetration > maxPenetration) { + maxPenetration = contact.penetration; + mostSevereContact = contact; + } + contact = contact.next; + } + if (mostSevereContact != null) { + if (mostSevereContact.calculateSeparatingVelocity() < 0) { + mostSevereContact.resolve(time); + } + } + } + + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/particle/ParticleLink.as b/0.0.0.18/src/alternativa/physics/particle/ParticleLink.as new file mode 100644 index 0000000..d4b999a --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/particle/ParticleLink.as @@ -0,0 +1,21 @@ +package alternativa.physics.particle { + + public class ParticleLink { + + public var particle1:Particle; + public var particle2:Particle; + + public function ParticleLink(particle1:Particle, particle2:Particle) { + this.particle1 = particle1; + this.particle2 = particle2; + } + + protected function getCurrentLength():Number { + return 0; + } + + public function fillContact(contact:ParticleContact):Boolean { + return false; + } + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/particle/ParticleRod.as b/0.0.0.18/src/alternativa/physics/particle/ParticleRod.as new file mode 100644 index 0000000..e24fa5e --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/particle/ParticleRod.as @@ -0,0 +1,58 @@ +package alternativa.physics.particle { + import alternativa.types.Point3D; + + public class ParticleRod extends ParticleLink { + + private static var vector:Point3D = new Point3D(); + + public var length:Number; + + public function ParticleRod(particle1:Particle, particle2:Particle, length:Number = 0) { + super(particle1, particle2); + if (length <= 0) { + this.length = Point3D.difference(particle1.position, particle2.position).length; + } else { + this.length = length; + } + } + + override protected function getCurrentLength():Number { + var x:Number = particle1.position.x - particle2.position.x; + var y:Number = particle1.position.y - particle2.position.y; + var z:Number = particle1.position.z - particle2.position.z; + return Math.sqrt(x*x + y*y + z*z); + } + + override public function fillContact(contact:ParticleContact):Boolean { + vector.x = particle1.position.x - particle2.position.x; + vector.y = particle1.position.y - particle2.position.y; + vector.z = particle1.position.z - particle2.position.z; + var len:Number = Math.sqrt(vector.x*vector.x + vector.y*vector.y + vector.z*vector.z); + + if (len == length) { + return false; + } + + var penetration:Number; + if (len > length) { + vector.x = -vector.x; + vector.y = -vector.y; + vector.z = -vector.z; + penetration = len - length; + } else { + penetration = length - len; + } + + contact.particle1 = particle1; + contact.particle2 = particle2; + contact.contactNormal.x = vector.x/len; + contact.contactNormal.y = vector.y/len; + contact.contactNormal.z = vector.z/len; + contact.penetration = penetration; + contact.restitution = 0; + contact.isCollision = false; + + return true; + } + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/particle/ParticleWorld.as b/0.0.0.18/src/alternativa/physics/particle/ParticleWorld.as new file mode 100644 index 0000000..ebbf5a4 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/particle/ParticleWorld.as @@ -0,0 +1,113 @@ +package alternativa.physics.particle { + import alternativa.physics.particle.Particle; + import alternativa.physics.particle.ParticleContact; + import alternativa.physics.particle.ParticleContactGenerator; + import alternativa.physics.particle.ParticleContactResolver; + import alternativa.physics.registry.ParticleForceRegistry; + + public class ParticleWorld { + + public var particles:Particle; + public var last:Particle; + + public var contacts:ParticleContact; + public var contactGenerators:ParticleContactGenerator; + public var forceRegistry:ParticleForceRegistry; + + public var resolver:ParticleContactResolver; + + public var maxContacts:int; + public var iterations:int; + + public function ParticleWorld(maxContacts:int, iterations:int = 0) { + this.maxContacts = maxContacts; + this.iterations = iterations; + + // Создаём контакты + contacts = new ParticleContact(0); + var contact:ParticleContact = contacts; + for (var i:int = 1; i < maxContacts; i++) { + contact = contact.next = new ParticleContact(i); + } + + resolver = new ParticleContactResolver(0); + forceRegistry = new ParticleForceRegistry(); + } + + public function addParticle(particle:Particle):void { + if (particles == null) { + last = particles = particle; + } else { + last.next = particle; + last = particle; + } + } + + public function addContactGenerator(generator:ParticleContactGenerator):void { + if (contactGenerators == null) { + contactGenerators = generator; + } else { + var gen:ParticleContactGenerator = contactGenerators; + while (gen.next != null) { + gen = gen.next; + } + gen.next = generator; + } + } + + public function startFrame():void { + var particle:Particle = particles; + while (particle != null) { + particle.clearForce(); + particle = particle.next; + } + } + + /** + * Последоавтельно запускает все зарегистрированные генераторы контактов. + * + * @return количество использованных контактов + */ + public function generateContacts():int { + var contact:ParticleContact = contacts; + var generator:ParticleContactGenerator = contactGenerators; + while (generator != null) { + contact = generator.addContacts(contact); + if (contact == null) { + // Доступные контакты исчерпаны + return maxContacts; + } + generator = generator.next; + } + return contact.index; + } + + public function intergrate(time:Number):void { + var particle:Particle = particles; + while (particle != null) { + particle.integrate(time); + particle = particle.next; + } + } + + /** + * Запускает физическую симуляцию. + * + * @param time + */ + public function runPhysics(time:Number):void { + // Применение генераторов сил + forceRegistry.updateForces(time); + // Запуск интеграторов для всех частиц + intergrate(time); + // Создание контактов + var contactsUsed:int = generateContacts(); + // Обработка полученных контактов + if (iterations == 0) { + resolver.setIterations(3*contactsUsed); + } + resolver.resolveContacts(contacts, contactsUsed, time); + } + + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/registry/.svn/all-wcprops b/0.0.0.18/src/alternativa/physics/registry/.svn/all-wcprops new file mode 100644 index 0000000..d24107d --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/registry/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 103 +/!svn/ver/4196/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/registry +END +ParticleForceRegistration.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/4196/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/registry/ParticleForceRegistration.as +END +BodyForceRegistry.as +K 25 +svn:wc:ra_dav:version-url +V 124 +/!svn/ver/4196/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/registry/BodyForceRegistry.as +END +BodyForceRegistration.as +K 25 +svn:wc:ra_dav:version-url +V 128 +/!svn/ver/4196/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/registry/BodyForceRegistration.as +END +ParticleForceRegistry.as +K 25 +svn:wc:ra_dav:version-url +V 128 +/!svn/ver/4196/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/registry/ParticleForceRegistry.as +END diff --git a/0.0.0.18/src/alternativa/physics/registry/.svn/entries b/0.0.0.18/src/alternativa/physics/registry/.svn/entries new file mode 100644 index 0000000..f225d87 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/registry/.svn/entries @@ -0,0 +1,76 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/registry +http://svndev.alternativaplatform.com + + + +2008-11-24T06:53:44.272574Z +4196 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +ParticleForceRegistration.as +file + + + + +2010-10-28T04:34:31.000000Z +eacf3bd4d8f93fcaf786f50cccdc2be2 +2008-11-24T06:53:44.272574Z +4196 +mike + +BodyForceRegistry.as +file + + + + +2010-10-28T04:34:31.000000Z +bc7c7f17acaa538a68392942fe5b4ddf +2008-11-24T06:53:44.272574Z +4196 +mike + +BodyForceRegistration.as +file + + + + +2010-10-28T04:34:31.000000Z +8b67908405c7eebf4c91e715ff7e26fd +2008-11-24T06:53:44.272574Z +4196 +mike + +ParticleForceRegistry.as +file + + + + +2010-10-28T04:34:31.000000Z +ed017f73d079580385616a6cf0724645 +2008-11-24T06:53:44.272574Z +4196 +mike + diff --git a/0.0.0.18/src/alternativa/physics/registry/.svn/format b/0.0.0.18/src/alternativa/physics/registry/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/registry/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.0.18/src/alternativa/physics/registry/.svn/text-base/BodyForceRegistration.as.svn-base b/0.0.0.18/src/alternativa/physics/registry/.svn/text-base/BodyForceRegistration.as.svn-base new file mode 100644 index 0000000..71ca5b4 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/registry/.svn/text-base/BodyForceRegistration.as.svn-base @@ -0,0 +1,16 @@ +package alternativa.physics.registry { + import alternativa.physics.force.IRigidBodyForceGenerator; + import alternativa.physics.rigid.RigidBody; + + public class BodyForceRegistration { + public var body:RigidBody; + public var forceGenerator:IRigidBodyForceGenerator; + + public var next:BodyForceRegistration; + + public function BodyForceRegistration(body:RigidBody, forceGenerator:IRigidBodyForceGenerator) { + this.body = body; + this.forceGenerator = forceGenerator; + } + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/registry/.svn/text-base/BodyForceRegistry.as.svn-base b/0.0.0.18/src/alternativa/physics/registry/.svn/text-base/BodyForceRegistry.as.svn-base new file mode 100644 index 0000000..e7a0292 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/registry/.svn/text-base/BodyForceRegistry.as.svn-base @@ -0,0 +1,47 @@ +package alternativa.physics.registry { + import alternativa.physics.force.IRigidBodyForceGenerator; + import alternativa.physics.rigid.RigidBody; + + public class BodyForceRegistry { + + protected var registrations:BodyForceRegistration; + + public function BodyForceRegistry() { + } + + public function add(body:RigidBody, fg:IRigidBodyForceGenerator):void { + var registration:BodyForceRegistration = new BodyForceRegistration(body, fg); + registration.next = registrations; + registrations = registration; + } + + public function remove(body:RigidBody, fg:IRigidBodyForceGenerator):void { + var current:BodyForceRegistration = registrations; + var prev:BodyForceRegistration = null; + while (current != null) { + if (current.body == body && current.forceGenerator == fg) { + if (current == registrations) { + registrations = current.next; + } else { + prev.next = current.next; + } + break; + } + prev = current; + current = current.next; + } + } + + public function clear():void { + registrations = null; + } + + public function updateForces(time:Number):void { + var current:BodyForceRegistration = registrations; + while (current != null) { + current.forceGenerator.updateForce(current.body, time); + current = current.next; + } + } + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/registry/.svn/text-base/ParticleForceRegistration.as.svn-base b/0.0.0.18/src/alternativa/physics/registry/.svn/text-base/ParticleForceRegistration.as.svn-base new file mode 100644 index 0000000..2e4ca84 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/registry/.svn/text-base/ParticleForceRegistration.as.svn-base @@ -0,0 +1,16 @@ +package alternativa.physics.registry { + import alternativa.physics.force.IParticelForceGenerator; + import alternativa.physics.particle.Particle; + + public class ParticleForceRegistration { + public var particle:Particle; + public var forceGenerator:IParticelForceGenerator; + + public var next:ParticleForceRegistration; + + public function ParticleForceRegistration(particle:Particle, forceGenerator:IParticelForceGenerator) { + this.particle = particle; + this.forceGenerator = forceGenerator; + } + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/registry/.svn/text-base/ParticleForceRegistry.as.svn-base b/0.0.0.18/src/alternativa/physics/registry/.svn/text-base/ParticleForceRegistry.as.svn-base new file mode 100644 index 0000000..79e3b81 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/registry/.svn/text-base/ParticleForceRegistry.as.svn-base @@ -0,0 +1,47 @@ +package alternativa.physics.registry { + import alternativa.physics.force.IParticelForceGenerator; + import alternativa.physics.particle.Particle; + + public class ParticleForceRegistry { + + protected var registrations:ParticleForceRegistration; + + public function ParticleForceRegistry() { + } + + public function add(particle:Particle, fg:IParticelForceGenerator):void { + var registration:ParticleForceRegistration = new ParticleForceRegistration(particle, fg); + registration.next = registrations; + registrations = registration; + } + + public function remove(particle:Particle, fg:IParticelForceGenerator):void { + var current:ParticleForceRegistration = registrations; + var prev:ParticleForceRegistration = null; + while (current != null) { + if (current.particle == particle && current.forceGenerator == fg) { + if (current == registrations) { + registrations = current.next; + } else { + prev.next = current.next; + } + break; + } + prev = current; + current = current.next; + } + } + + public function clear():void { + registrations = null; + } + + public function updateForces(time:Number):void { + var current:ParticleForceRegistration = registrations; + while (current != null) { + current.forceGenerator.updateForce(current.particle, time); + current = current.next; + } + } + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/registry/BodyForceRegistration.as b/0.0.0.18/src/alternativa/physics/registry/BodyForceRegistration.as new file mode 100644 index 0000000..71ca5b4 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/registry/BodyForceRegistration.as @@ -0,0 +1,16 @@ +package alternativa.physics.registry { + import alternativa.physics.force.IRigidBodyForceGenerator; + import alternativa.physics.rigid.RigidBody; + + public class BodyForceRegistration { + public var body:RigidBody; + public var forceGenerator:IRigidBodyForceGenerator; + + public var next:BodyForceRegistration; + + public function BodyForceRegistration(body:RigidBody, forceGenerator:IRigidBodyForceGenerator) { + this.body = body; + this.forceGenerator = forceGenerator; + } + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/registry/BodyForceRegistry.as b/0.0.0.18/src/alternativa/physics/registry/BodyForceRegistry.as new file mode 100644 index 0000000..e7a0292 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/registry/BodyForceRegistry.as @@ -0,0 +1,47 @@ +package alternativa.physics.registry { + import alternativa.physics.force.IRigidBodyForceGenerator; + import alternativa.physics.rigid.RigidBody; + + public class BodyForceRegistry { + + protected var registrations:BodyForceRegistration; + + public function BodyForceRegistry() { + } + + public function add(body:RigidBody, fg:IRigidBodyForceGenerator):void { + var registration:BodyForceRegistration = new BodyForceRegistration(body, fg); + registration.next = registrations; + registrations = registration; + } + + public function remove(body:RigidBody, fg:IRigidBodyForceGenerator):void { + var current:BodyForceRegistration = registrations; + var prev:BodyForceRegistration = null; + while (current != null) { + if (current.body == body && current.forceGenerator == fg) { + if (current == registrations) { + registrations = current.next; + } else { + prev.next = current.next; + } + break; + } + prev = current; + current = current.next; + } + } + + public function clear():void { + registrations = null; + } + + public function updateForces(time:Number):void { + var current:BodyForceRegistration = registrations; + while (current != null) { + current.forceGenerator.updateForce(current.body, time); + current = current.next; + } + } + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/registry/ParticleForceRegistration.as b/0.0.0.18/src/alternativa/physics/registry/ParticleForceRegistration.as new file mode 100644 index 0000000..2e4ca84 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/registry/ParticleForceRegistration.as @@ -0,0 +1,16 @@ +package alternativa.physics.registry { + import alternativa.physics.force.IParticelForceGenerator; + import alternativa.physics.particle.Particle; + + public class ParticleForceRegistration { + public var particle:Particle; + public var forceGenerator:IParticelForceGenerator; + + public var next:ParticleForceRegistration; + + public function ParticleForceRegistration(particle:Particle, forceGenerator:IParticelForceGenerator) { + this.particle = particle; + this.forceGenerator = forceGenerator; + } + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/registry/ParticleForceRegistry.as b/0.0.0.18/src/alternativa/physics/registry/ParticleForceRegistry.as new file mode 100644 index 0000000..79e3b81 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/registry/ParticleForceRegistry.as @@ -0,0 +1,47 @@ +package alternativa.physics.registry { + import alternativa.physics.force.IParticelForceGenerator; + import alternativa.physics.particle.Particle; + + public class ParticleForceRegistry { + + protected var registrations:ParticleForceRegistration; + + public function ParticleForceRegistry() { + } + + public function add(particle:Particle, fg:IParticelForceGenerator):void { + var registration:ParticleForceRegistration = new ParticleForceRegistration(particle, fg); + registration.next = registrations; + registrations = registration; + } + + public function remove(particle:Particle, fg:IParticelForceGenerator):void { + var current:ParticleForceRegistration = registrations; + var prev:ParticleForceRegistration = null; + while (current != null) { + if (current.particle == particle && current.forceGenerator == fg) { + if (current == registrations) { + registrations = current.next; + } else { + prev.next = current.next; + } + break; + } + prev = current; + current = current.next; + } + } + + public function clear():void { + registrations = null; + } + + public function updateForces(time:Number):void { + var current:ParticleForceRegistration = registrations; + while (current != null) { + current.forceGenerator.updateForce(current.particle, time); + current = current.next; + } + } + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/rigid/.svn/all-wcprops b/0.0.0.18/src/alternativa/physics/rigid/.svn/all-wcprops new file mode 100644 index 0000000..4d6643b --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/rigid/.svn/all-wcprops @@ -0,0 +1,47 @@ +K 25 +svn:wc:ra_dav:version-url +V 100 +/!svn/ver/5193/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/rigid +END +RigidBodyContactGenerator.as +K 25 +svn:wc:ra_dav:version-url +V 129 +/!svn/ver/4732/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/rigid/RigidBodyContactGenerator.as +END +BodyForceRegistry.as +K 25 +svn:wc:ra_dav:version-url +V 121 +/!svn/ver/4375/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/rigid/BodyForceRegistry.as +END +RigidWorld.as +K 25 +svn:wc:ra_dav:version-url +V 114 +/!svn/ver/4741/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/rigid/RigidWorld.as +END +BodyForceRegistration.as +K 25 +svn:wc:ra_dav:version-url +V 125 +/!svn/ver/4375/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/rigid/BodyForceRegistration.as +END +RigidBodyContact.as +K 25 +svn:wc:ra_dav:version-url +V 120 +/!svn/ver/4813/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/rigid/RigidBodyContact.as +END +RigidBodyContactResolver.as +K 25 +svn:wc:ra_dav:version-url +V 128 +/!svn/ver/4270/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/rigid/RigidBodyContactResolver.as +END +RigidBody.as +K 25 +svn:wc:ra_dav:version-url +V 113 +/!svn/ver/5193/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/rigid/RigidBody.as +END diff --git a/0.0.0.18/src/alternativa/physics/rigid/.svn/entries b/0.0.0.18/src/alternativa/physics/rigid/.svn/entries new file mode 100644 index 0000000..2cc5f81 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/rigid/.svn/entries @@ -0,0 +1,115 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/rigid +http://svndev.alternativaplatform.com + + + +2008-12-17T11:38:42.040147Z +5193 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +RigidBodyContactGenerator.as +file + + + + +2010-10-28T04:34:31.000000Z +577dac0a54d7d990734d27170ddf1f14 +2008-12-03T05:15:16.843707Z +4732 +mike + +BodyForceRegistry.as +file + + + + +2010-10-28T04:34:31.000000Z +e2125f3fa80a45e6e3a304c33e5cbd47 +2008-11-26T12:04:03.108808Z +4375 +mike + +RigidWorld.as +file + + + + +2010-10-28T04:34:31.000000Z +0031086a4c4b3f39f81a0f224ad8560a +2008-12-03T08:31:09.097064Z +4741 +mike + +generators +dir + +BodyForceRegistration.as +file + + + + +2010-10-28T04:34:31.000000Z +bed2d6357d27f7311216db6acd156c4e +2008-11-26T12:04:03.108808Z +4375 +mike + +RigidBodyContact.as +file + + + + +2010-10-28T04:34:31.000000Z +473e71d1b8a62f4f9bcd94c2e38d26d6 +2008-12-10T13:18:05.069708Z +4813 +mike + +RigidBodyContactResolver.as +file + + + + +2010-10-28T04:34:31.000000Z +ad30ebb588f523d3255ce0a768a39b40 +2008-11-25T04:51:53.783696Z +4270 +mike + +RigidBody.as +file + + + + +2010-10-28T04:34:31.000000Z +8f0e9d6652c8f023b9c0b7a78135030d +2008-12-17T11:38:42.040147Z +5193 +mike + diff --git a/0.0.0.18/src/alternativa/physics/rigid/.svn/format b/0.0.0.18/src/alternativa/physics/rigid/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/rigid/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.0.18/src/alternativa/physics/rigid/.svn/text-base/BodyForceRegistration.as.svn-base b/0.0.0.18/src/alternativa/physics/rigid/.svn/text-base/BodyForceRegistration.as.svn-base new file mode 100644 index 0000000..e030bbb --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/rigid/.svn/text-base/BodyForceRegistration.as.svn-base @@ -0,0 +1,16 @@ +package alternativa.physics.rigid { + import alternativa.physics.force.IRigidBodyForceGenerator; + import alternativa.physics.rigid.RigidBody; + + public class BodyForceRegistration { + public var body:RigidBody; + public var forceGenerator:IRigidBodyForceGenerator; + + public var next:BodyForceRegistration; + + public function BodyForceRegistration(body:RigidBody, forceGenerator:IRigidBodyForceGenerator) { + this.body = body; + this.forceGenerator = forceGenerator; + } + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/rigid/.svn/text-base/BodyForceRegistry.as.svn-base b/0.0.0.18/src/alternativa/physics/rigid/.svn/text-base/BodyForceRegistry.as.svn-base new file mode 100644 index 0000000..a538a20 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/rigid/.svn/text-base/BodyForceRegistry.as.svn-base @@ -0,0 +1,47 @@ +package alternativa.physics.rigid { + import alternativa.physics.force.IRigidBodyForceGenerator; + import alternativa.physics.rigid.RigidBody; + + public class BodyForceRegistry { + + protected var registrations:BodyForceRegistration; + + public function BodyForceRegistry() { + } + + public function add(body:RigidBody, fg:IRigidBodyForceGenerator):void { + var registration:BodyForceRegistration = new BodyForceRegistration(body, fg); + registration.next = registrations; + registrations = registration; + } + + public function remove(body:RigidBody, fg:IRigidBodyForceGenerator):void { + var current:BodyForceRegistration = registrations; + var prev:BodyForceRegistration = null; + while (current != null) { + if (current.body == body && current.forceGenerator == fg) { + if (current == registrations) { + registrations = current.next; + } else { + prev.next = current.next; + } + break; + } + prev = current; + current = current.next; + } + } + + public function clear():void { + registrations = null; + } + + public function updateForces(time:Number):void { + var current:BodyForceRegistration = registrations; + while (current != null) { + current.forceGenerator.updateForce(current.body, time); + current = current.next; + } + } + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/rigid/.svn/text-base/RigidBody.as.svn-base b/0.0.0.18/src/alternativa/physics/rigid/.svn/text-base/RigidBody.as.svn-base new file mode 100644 index 0000000..da46e3f --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/rigid/.svn/text-base/RigidBody.as.svn-base @@ -0,0 +1,707 @@ +package alternativa.physics.rigid { + + import alternativa.physics.altphysics; + import alternativa.types.Matrix3D; + import alternativa.types.Point3D; + import alternativa.types.Quaternion; + + use namespace altphysics; + + /** + * Класс представляет твёрдое тело. + */ + public class RigidBody { + + private var angularAcceleration:Point3D = new Point3D(); + private var motionThreshold:Number = 0.1; + private var motionThreshold2:Number = 2*motionThreshold; + private var baseBias:Number = 0.8; + private var name:String; + + public var next:RigidBody; + public var prev:RigidBody; + + altphysics var mass:Number; + altphysics var inverseMass:Number; + altphysics var inverseInertiaTensor:Matrix3D = new Matrix3D(); + + altphysics var linearDamping:Number = 0.95; + altphysics var angularDamping:Number = 0.95; + altphysics var awake:Boolean = true; + altphysics var canSleep:Boolean = true; + + altphysics var acceleration:Point3D = new Point3D(); + altphysics var angularVelocity:Point3D = new Point3D(); + + altphysics var position:Point3D = new Point3D(); + altphysics var orientation:Quaternion = new Quaternion(1, 0, 0, 0); + altphysics var velocity:Point3D = new Point3D(); + altphysics var rotation:Point3D = new Point3D(); + altphysics var lastFrameAcceleration:Point3D = new Point3D(); + + altphysics var transformMatrix:Matrix3D = new Matrix3D(); + altphysics var inverseInertiaTensorWorld:Matrix3D = new Matrix3D(); + + altphysics var forceAccum:Point3D = new Point3D(); + altphysics var torqueAccum:Point3D = new Point3D(); + altphysics var motion:Number = 10; + + /** + * Создаёт новый экзепляр твёрдого тела. + * + * @param inverseMass величина, обратная массе тела. Значение 0 указывает, что тело имеет имеет бесконечную массу и момент инерции. + * @param inverseInertiaTensor обратный тензор момента инерции тела. Значение null указывает, что тело имеет бесконечную массу и момент инерции. + */ + public function RigidBody(inverseMass:Number = 1, inverseInertiaTensor:Matrix3D = null) { + if (inverseMass == 0 || inverseInertiaTensor == null) { + setInfinteMass(); + } else { + this.inverseMass = inverseMass; + this.mass = 1/inverseMass; + this.inverseInertiaTensor.copy(inverseInertiaTensor); + } + } + + /** + * @return имя тела + */ + public function getName():String { + return name; + } + + /** + * Устанавливает имя тела. + * + * @param value имя тела + */ + public function setName(value:String):void { + name = value; + } + + /** + * Предельное количество движения, ниже которого тело усыпляеся. + */ + public function getMotionThreshold():Number { + return motionThreshold; + } + + /** + * @private + */ + public function setMotionThreshold(value:Number):void { + motionThreshold = value; + motionThreshold2 = value*2; + } + + /** + * + * @param value + */ + public function setBaseBias(value:Number):void { + baseBias = value; + } + + /** + * + * @param value + */ + public function getBaseBias():Number { + return baseBias; + } + + /** + * + */ + public function getMass():Number { + return mass; + } + + /** + * + */ + public function setMass(value:Number):void { + if (value <= 0) { + inverseMass = 0; + mass = 0; + } else { + mass = value; + inverseMass = 1/value; + } + } + + /** + * + */ + public function getInverseMass():Number { + return inverseMass; + } + + /** + * @private + */ + public function setInverseMass(value:Number):void { + if (value > 0) { + inverseMass = value; + mass = 1/value; + } else { + inverseMass = 0; + mass = 0; + } + } + + /** + * + * @param m + */ + public function setIntertiaTensor(intertiaTensor:Matrix3D):void { + inverseInertiaTensor.copy(intertiaTensor); + inverseInertiaTensor.invert(); + } + + /** + * + * @param intertiaTensor + */ + public function getIntertiaTensor(intertiaTensor:Matrix3D):void { + intertiaTensor.copy(inverseInertiaTensor); + intertiaTensor.invert(); + } + + /** + * + * @param inverseInertiaTensor + */ + public function setInverseIntertiaTensor(inverseInertiaTensor:Matrix3D):void { + this.inverseInertiaTensor.copy(inverseInertiaTensor); + } + + /** + * + * @param inverseInertiaTensor + */ + public function getInverseIntertiaTensor(inverseInertiaTensor:Matrix3D):void { + inverseInertiaTensor.copy(this.inverseInertiaTensor); + } + + /** + * + */ + public function setInfinteMass():void { + inverseMass = 0; + mass = 0; + inverseInertiaTensor.toIdentity(); + inverseInertiaTensor.a = inverseInertiaTensor.f = inverseInertiaTensor.k = 0; + } + + /** + * + * @param linearDamping + * @param angularDamping + */ + public function setDamping(linearDamping:Number, angularDamping:Number):void { + this.linearDamping = linearDamping; + this.angularDamping = angularDamping; + } + + /** + * + * @param value + */ + public function setLinearDamping(value:Number):void { + linearDamping = value; + } + + /** + * + * @return + */ + public function getLinearDamping():Number { + return linearDamping; + } + + /** + * + * @param value + */ + public function setAngularDamping(value:Number):void { + angularDamping = value; + } + + /** + * + * @return + */ + public function getAngularDamping():Number { + return angularDamping; + } + + /** + * + * @param position + */ + public function setPosition(position:Point3D):void { + this.position.x = position.x; + this.position.y = position.y; + this.position.z = position.z; + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setPositionComponents(x:Number, y:Number, z:Number):void { + position.x = x; + position.y = y; + position.z = z; + } + + /** + * + * @param position + */ + public function getPosition(position:Point3D):void { + position.x = this.position.x; + position.y = this.position.y; + position.z = this.position.z; + } + + /** + * + * @param orientation + */ + public function setOrientation(orientation:Quaternion):void { + this.orientation.copy(orientation); + this.orientation.normalize(); + } + + /** + * + * @param r + * @param i + * @param j + * @param k + */ + public function setOrientationComponents(w:Number, x:Number, y:Number, z:Number):void { + orientation.reset(w, x, y, z); + orientation.normalize(); + } + + /** + * + * @param orientation + */ + public function getOrientation(orientation:Quaternion):void { + orientation.copy(this.orientation); + } + + /** + * + * @param matrix + */ + public function getTransformMatrix(matrix:Matrix3D):void { + matrix.copy(transformMatrix); + } + + /** + * + * @param velocity + */ + public function setVelocity(velocity:Point3D):void { + this.velocity.copy(velocity); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setVelocityComponents(x:Number, y:Number, z:Number):void { + velocity.reset(x, y, z); + } + + /** + * + * @param velocity + */ + public function getVelocity(velocity:Point3D):void { + velocity.copy(this.velocity); + } + + /** + * + * @param acceleration + * + */ + public function setAcceleration(acceleration:Point3D):void { + this.acceleration.copy(acceleration); + } + + /** + * + * @param x + * @param y + * @param z + * + */ + public function setAccelerationComponents(x:Number, y:Number, z:Number):void { + acceleration.reset(x, y, z); + } + + /** + * + * @param acceleration + * + */ + public function getAcceleration(acceleration:Point3D):void { + acceleration.copy(this.acceleration); + } + + /** + * + * @param rotation + * + */ + public function setRotation(rotation:Point3D):void { + this.rotation.copy(rotation); + } + + /** + * + * @param x + * @param y + * @param z + * + */ + public function setRotationComponents(x:Number, y:Number, z:Number):void { + rotation.reset(x, y, z); + } + + /** + * + * @param rotation + * + */ + public function addRotation(deltaRotation:Point3D):void { + rotation.add(deltaRotation); + } + + /** + * + * @param rotation + * + */ + public function getRotation(rotation:Point3D):void { + rotation.copy(this.rotation); + } + + /** + * + * @param angularVelocity + */ + public function setAngularVelocity(angularVelocity:Point3D):void { + this.angularVelocity.x = angularVelocity.x; + this.angularVelocity.y = angularVelocity.y; + this.angularVelocity.z = angularVelocity.z; + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setAngularVelocityComponents(x:Number, y:Number, z:Number):void { + angularVelocity.x = x; + angularVelocity.y = y; + angularVelocity.z = z; + } + + /** + * + * @param awake + */ + public function setAwake(awake:Boolean = true):void { + this.awake = awake; + if (awake) { + motion = motionThreshold2; + } else { + velocity.x = 0; + velocity.y = 0; + velocity.z = 0; + + rotation.x = 0; + rotation.y = 0; + rotation.z = 0; + } + } + + /** + * + * @return + */ + public function isAwake():Boolean { + return awake; + } + + /** + * + * @param canSleep + */ + public function setCanSleep(canSleep:Boolean = true):void { + this.canSleep = canSleep; + if (!canSleep && !awake) { + setAwake(); + } + } + + /** + * + * @return + */ + public function isCanSleep():Boolean { + return canSleep; + } + + /** + * Добавляет глобальную силу, приложенную к центру масс тела. + * + * @param force вектор силы в глобальной системе координат + */ + public function addForce(force:Point3D):void { + forceAccum.x += force.x; + forceAccum.y += force.y; + forceAccum.z += force.z; + setAwake(); + } + + /** + * + * @param force + */ + public function addForceComponents(x:Number, y:Number, z:Number):void { + forceAccum.x += x; + forceAccum.y += y; + forceAccum.z += z; + setAwake(); + } + + /** + * Добавляет глобальную силу, приложенную к заданной локальной точке тела. + * + * @param force вектор силы в глобальной системе координат + * @param localPoint локальные координаты точки приложения силы + */ + public function addForceAtLocalPoint(force:Point3D, localPoint:Point3D):void { + forceAccum.x += force.x; + forceAccum.y += force.y; + forceAccum.z += force.z; + // Трансформация локальных координат в глобальные + var x:Number = transformMatrix.a*localPoint.x + transformMatrix.b*localPoint.y + transformMatrix.c*localPoint.z; + var y:Number = transformMatrix.e*localPoint.x + transformMatrix.f*localPoint.y + transformMatrix.g*localPoint.z; + var z:Number = transformMatrix.i*localPoint.x + transformMatrix.j*localPoint.y + transformMatrix.k*localPoint.z; + torqueAccum.x += y*force.z - z*force.y; + torqueAccum.y += z*force.x - x*force.z; + torqueAccum.z += x*force.y - y*force.x; + setAwake(); + } + + /** + * Добавляет глобальную силу, приложенную к заданной точке тела, выраженной в глобальных координатах. + * + * @param force вектор силы в глобальной системе координат + * @param point глобальные координаты точки приложения силы + */ + public function addForceAtPoint(force:Point3D, point:Point3D):void { + forceAccum.x += force.x; + forceAccum.y += force.y; + forceAccum.z += force.z; + var x:Number = point.x - position.x; + var y:Number = point.y - position.y; + var z:Number = point.z - position.z; + torqueAccum.x += y*force.z - z*force.y; + torqueAccum.y += z*force.x - x*force.z; + torqueAccum.z += x*force.y - y*force.x; + setAwake(); + } + + /** + * Добавляет локальную силу, приложенную в заданной локальной точке тела. + * + * @param localForce вектор локальной силы + * @param localPoint локальные координаты точки приложения силы + */ + public function addLocalForce(localForce:Point3D, localPoint:Point3D):void { + // Трансформация локального вектора силы в глобальный + var globalForceX:Number = transformMatrix.a*localForce.x + transformMatrix.b*localForce.y + transformMatrix.c*localForce.z; + var globalForceY:Number = transformMatrix.e*localForce.x + transformMatrix.f*localForce.y + transformMatrix.g*localForce.z; + var globalForceZ:Number = transformMatrix.i*localForce.x + transformMatrix.j*localForce.y + transformMatrix.k*localForce.z; + forceAccum.x += globalForceX; + forceAccum.y += globalForceY; + forceAccum.z += globalForceZ; + // Получение глобального вектора плеча силы + var globalPointX:Number = transformMatrix.a*localPoint.x + transformMatrix.b*localPoint.y + transformMatrix.c*localPoint.z; + var globalPointY:Number = transformMatrix.e*localPoint.x + transformMatrix.f*localPoint.y + transformMatrix.g*localPoint.z; + var globalPointZ:Number = transformMatrix.i*localPoint.x + transformMatrix.j*localPoint.y + transformMatrix.k*localPoint.z; + // Приращение момента силы + torqueAccum.x += globalPointY*globalForceZ - globalPointZ*globalForceY; + torqueAccum.y += globalPointZ*globalForceX - globalPointX*globalForceZ; + torqueAccum.z += globalPointX*globalForceY - globalPointY*globalForceX; + setAwake(); + } + + /** + * Добавляет момент силы. + * + * @param torque вектор момента силы + */ + public function addTorque(torque:Point3D):void { + torqueAccum.x += torque.x; + torqueAccum.y += torque.y; + torqueAccum.z += torque.z; + setAwake(); + } + + /** + * Добавляет момент силы, заданный покомпонентно. + */ + public function addTorqueComponents(x:Number, y:Number, z:Number):void { + torqueAccum.x += x; + torqueAccum.y += y; + torqueAccum.z += z; + setAwake(); + } + + /** + * Вычисляет векторные параметры тела в глобальной системе координат. + */ + public function calculateDerivedData():void { + // inlined position.normalize() + var d:Number = orientation.w*orientation.w + orientation.x*orientation.x + orientation.y*orientation.y + orientation.z*orientation.z; + if (d == 0) { + orientation.w = 1; + } else { + d = 1/Math.sqrt(d); + orientation.w *= d; + orientation.x *= d; + orientation.y *= d; + orientation.z *= d; + } + // Вычисление глобальной матрицы трансформации + orientation.toMatrix3D(transformMatrix); + transformMatrix.d = position.x; + transformMatrix.h = position.y; + transformMatrix.l = position.z; + // Расчёт обратного тензора инерции в мировых координатах + var a:Number = transformMatrix.a*inverseInertiaTensor.a + transformMatrix.b*inverseInertiaTensor.e + transformMatrix.c*inverseInertiaTensor.i; + var b:Number = transformMatrix.a*inverseInertiaTensor.b + transformMatrix.b*inverseInertiaTensor.f + transformMatrix.c*inverseInertiaTensor.j; + var c:Number = transformMatrix.a*inverseInertiaTensor.c + transformMatrix.b*inverseInertiaTensor.g + transformMatrix.c*inverseInertiaTensor.k; + var e:Number = transformMatrix.e*inverseInertiaTensor.a + transformMatrix.f*inverseInertiaTensor.e + transformMatrix.g*inverseInertiaTensor.i; + var f:Number = transformMatrix.e*inverseInertiaTensor.b + transformMatrix.f*inverseInertiaTensor.f + transformMatrix.g*inverseInertiaTensor.j; + var g:Number = transformMatrix.e*inverseInertiaTensor.c + transformMatrix.f*inverseInertiaTensor.g + transformMatrix.g*inverseInertiaTensor.k; + var i:Number = transformMatrix.i*inverseInertiaTensor.a + transformMatrix.j*inverseInertiaTensor.e + transformMatrix.k*inverseInertiaTensor.i; + var j:Number = transformMatrix.i*inverseInertiaTensor.b + transformMatrix.j*inverseInertiaTensor.f + transformMatrix.k*inverseInertiaTensor.j; + var k:Number = transformMatrix.i*inverseInertiaTensor.c + transformMatrix.j*inverseInertiaTensor.g + transformMatrix.k*inverseInertiaTensor.k; + // Здесь подразумевается, что матрица трансформации тела не содержит масштабирования + // Произведение [transformMatrix*inverseInertiaTensor]*transpose(transformMatrix) + inverseInertiaTensorWorld.a = a*transformMatrix.a + b*transformMatrix.b + c*transformMatrix.c; + inverseInertiaTensorWorld.b = a*transformMatrix.e + b*transformMatrix.f + c*transformMatrix.g; + inverseInertiaTensorWorld.c = a*transformMatrix.i + b*transformMatrix.j + c*transformMatrix.k; + inverseInertiaTensorWorld.e = e*transformMatrix.a + f*transformMatrix.b + g*transformMatrix.c; + inverseInertiaTensorWorld.f = e*transformMatrix.e + f*transformMatrix.f + g*transformMatrix.g; + inverseInertiaTensorWorld.g = e*transformMatrix.i + f*transformMatrix.j + g*transformMatrix.k; + inverseInertiaTensorWorld.i = i*transformMatrix.a + j*transformMatrix.b + k*transformMatrix.c; + inverseInertiaTensorWorld.j = i*transformMatrix.e + j*transformMatrix.f + k*transformMatrix.g; + inverseInertiaTensorWorld.k = i*transformMatrix.i + j*transformMatrix.j + k*transformMatrix.k; + } + + /** + * Интегрирует динамические характеристики и обновляет параметры состояния тела. + * + * @param time временной шаг интегрирования + */ + public function integrate(time:Number):void { + if (!awake) { + return; + } + + lastFrameAcceleration.x = acceleration.x + forceAccum.x*inverseMass; + lastFrameAcceleration.y = acceleration.y + forceAccum.y*inverseMass; + lastFrameAcceleration.z = acceleration.z + forceAccum.z*inverseMass; + // angularAcceleration = inverseInertiaTensorWorld*torqueAccum + angularAcceleration.x = inverseInertiaTensorWorld.a*torqueAccum.x + inverseInertiaTensorWorld.b*torqueAccum.y + inverseInertiaTensorWorld.c*torqueAccum.z; + angularAcceleration.y = inverseInertiaTensorWorld.e*torqueAccum.x + inverseInertiaTensorWorld.f*torqueAccum.y + inverseInertiaTensorWorld.g*torqueAccum.z; + angularAcceleration.z = inverseInertiaTensorWorld.i*torqueAccum.x + inverseInertiaTensorWorld.j*torqueAccum.y + inverseInertiaTensorWorld.k*torqueAccum.z; + + var d:Number = Math.pow(linearDamping, time); + var dtime:Number = d*time; + + velocity.x += lastFrameAcceleration.x*dtime; + velocity.y += lastFrameAcceleration.y*dtime; + velocity.z += lastFrameAcceleration.z*dtime; + + rotation.x += angularAcceleration.x*dtime; + rotation.y += angularAcceleration.y*dtime; + rotation.z += angularAcceleration.z*dtime; + + rotation.x += angularVelocity.x*time; + rotation.y += angularVelocity.y*time; + rotation.z += angularVelocity.z*time; + + position.x += velocity.x*time; + position.y += velocity.y*time; + position.z += velocity.z*time; + + orientation.addScaledVector(rotation, time); + + // TODO: Нужно ли двойное уменьшение скоростей? + velocity.x *= d; + velocity.y *= d; + velocity.z *= d; + + rotation.x *= d; + rotation.y *= d; + rotation.z *= d; + + calculateDerivedData(); + // inlined clearAccumulators() + forceAccum.x = 0; + forceAccum.y = 0; + forceAccum.z = 0; + torqueAccum.x = 0; + torqueAccum.y = 0; + torqueAccum.z = 0; + + // Усыпление тела при необходимости + if (canSleep) { + var currentMotion:Number = velocity.x*velocity.x + velocity.y*velocity.y + velocity.z*velocity.z + + rotation.x*rotation.x + rotation.y*rotation.y + rotation.z*rotation.z; + var bias:Number = Math.pow(baseBias, time); + motion = bias*motion + (1 - bias)*currentMotion; +// trace("[RigidBody::integrate]", name, "motion", motion, "motionEpsilon", motionEpsilon); + if (motion < motionThreshold) { + setAwake(false); + } else { + if (motion > motionThreshold2) { + motion = motionThreshold2; + } + } + } + } + + /** + * + */ + public function clearAccumulators():void { + forceAccum.x = 0; + forceAccum.y = 0; + forceAccum.z = 0; + torqueAccum.x = 0; + torqueAccum.y = 0; + torqueAccum.z = 0; + } + + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/rigid/.svn/text-base/RigidBodyContact.as.svn-base b/0.0.0.18/src/alternativa/physics/rigid/.svn/text-base/RigidBodyContact.as.svn-base new file mode 100644 index 0000000..22b3c65 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/rigid/.svn/text-base/RigidBodyContact.as.svn-base @@ -0,0 +1,550 @@ +package alternativa.physics.rigid { + + import alternativa.physics.*; + import alternativa.types.Matrix3D; + import alternativa.types.Point3D; + + use namespace altphysics; + + /** + * Класс описывает контакт между телами. Контакты могут составлять двунаправленный список. + */ + public class RigidBodyContact { + + private var tmpVelocityVector:Point3D = new Point3D(); + private var impulse:Point3D = new Point3D(); + + public static var velocityLimit:Number = 1; + + /** + * Ссылка на следующий контакт в списке. + */ + public var next:RigidBodyContact; + /** + * Ссылка на предыдущий контакт в списке. + */ + public var prev:RigidBodyContact; + /** + * Индекс контакта в списке. + */ + public var index:int; + + /** + * Первое тело, к которому относится контакт. + */ + public var body1:RigidBody; + /** + * Первое тело, к которому относится контакт (может быть null). + */ + public var body2:RigidBody; + /** + * Точка контакта. + */ + public var contactPoint:Point3D = new Point3D(); + /** + * Нормаль контакта. Нормаль направлена от второго тела к первому. В локальной системе координат контакта + * нормаль является базисным вектором, задающим направление оси X. + */ + public var contactNormal:Point3D = new Point3D(); + /** + * Коэффициент взаимного трения в точке контакта. + */ + public var friction:Number = 0; + /** + * Коэффициент упругости удара. + */ + public var restitution:Number = 0; + /** + * Глубина взаимного проникновения тел в точке контакта. + */ + public var penetration:Number; + + public var timeStamp:int; + + /** + * Матрица перехода из локальной системы координат контакта в мировую систему координат без компонентов параллельного переноса. + * Матрица задаёт базис локальной системы координат контакта. + */ + altphysics var contactToWorld:Matrix3D = new Matrix3D(); + /** + * Относительная скорость в точке контакта. Вычисляется в методе calculateInternals(). + */ + altphysics var contactVelocity:Point3D = new Point3D(); + /** + * Приращение скорости в точке контакта по направлению нормали, необходимое для прекращения сближения. Вычисляется в методе calculateInternals(). + */ + altphysics var desiredDeltaVelocity:Number; + /** + * Координаты точки контакта относительныо первого тела. Вычисляется в методе calculateInternals(). + */ + altphysics var relativeContactPosition1:Point3D = new Point3D(); + /** + * Координаты точки контакта относительныо второго тела. Вычисляется в методе calculateInternals(). + */ + altphysics var relativeContactPosition2:Point3D = new Point3D(); + + /** + * + * @param index + */ + public function RigidBodyContact(index:int) { + this.index = index; + } + + /** + * + * @param body1 + * @param body2 + * @param friction + * @param restitution + */ + public function setBodyData(body1:RigidBody, body2:RigidBody, friction:Number, restitution:Number):void { + this.body1 = body1; + this.body2 = body2; + this.friction = friction; + this.restitution = restitution; + } + + /** + * + */ + public function matchAwakeState():void { + if (body2 == null) { + return; + } + + if (body1.awake != body2.awake) { + if (body1.awake) { + body2.setAwake(); + } else { + body1.setAwake(); + } + } + } + + /** + * Вычисляет и сохраняет базис локальной системы координат контакта. + */ + public function calculateContactBasis():void { + // Нормаль является ортом X локальной системы координат + contactToWorld.a = contactNormal.x; + contactToWorld.e = contactNormal.y; + contactToWorld.i = contactNormal.z; + var nx:Number = contactNormal.x > 0 ? contactNormal.x : -contactNormal.x; + var ny:Number = contactNormal.y > 0 ? contactNormal.y : -contactNormal.y; + var s:Number; + if (nx > ny) { + // Нормаль ближе к глобальной оси X, используем ось Y для вычислений. Локальный орт Y находится как результат векторного произведения globalY x contactNormal. + s = 1/Math.sqrt(nx*nx + contactNormal.z*contactNormal.z); + contactToWorld.b = contactNormal.z*s; + contactToWorld.f = 0; + contactToWorld.j = -contactNormal.x*s; + // Локальный орт Z находится как результат векторного произведения contactNormal x localY. + contactToWorld.c = contactNormal.y*contactToWorld.j; + contactToWorld.g = contactNormal.z*contactToWorld.b - contactNormal.x*contactToWorld.j; + contactToWorld.k = -contactNormal.y*contactToWorld.b; + } else { + // Нормаль ближе к глобальной оси Y, используем ось X для вычислений. Локальный орт Y находится как результат векторного произведения globalX x contactNormal. + s = 1/Math.sqrt(ny*ny + contactNormal.z*contactNormal.z); + contactToWorld.b = 0; + contactToWorld.f = -contactNormal.z*s; + contactToWorld.j = contactNormal.y*s; + // Локальный орт Z находится как результат векторного произведения contactNormal x localY. + contactToWorld.c = contactNormal.y*contactToWorld.j - contactNormal.z*contactToWorld.f; + contactToWorld.g = -contactNormal.x*contactToWorld.j; + contactToWorld.k = contactNormal.x*contactToWorld.f; + } + } + + /** + * Вычисляет скорость точки заданного тела в точке контакта в его локальной системе координат. + * + * @param body тело, для которого выполняются вычисления + * @param relativeContactPosition положение точки контакта относительно тела + * @param time временной шаг + * @param result в эту переменную записывается результат + */ + public function calculateLocalVelocity(body:RigidBody, relativeContactPosition:Point3D, time:Number, result:Point3D):void { + // Расчёт прироста скорости за последний кадр от действующих сил + var x:Number = body.lastFrameAcceleration.x*time; + var y:Number = body.lastFrameAcceleration.y*time; + var z:Number = body.lastFrameAcceleration.z*time; + // Прирост скорости в системе координат контакта + // Убираем прирост скорости в направлении контакта + var accVelocityY:Number = contactToWorld.b*x + contactToWorld.f*y + contactToWorld.j*z; + var accVelocityZ:Number = contactToWorld.c*x + contactToWorld.g*y + contactToWorld.k*z; + + // Скорость точки контакта в мировой системе координат + // Скорость за счёт вращения тела + var rotation:Point3D = body.rotation; + x = rotation.y*relativeContactPosition.z - rotation.z*relativeContactPosition.y + body.velocity.x; + y = rotation.z*relativeContactPosition.x - rotation.x*relativeContactPosition.z + body.velocity.y; + z = rotation.x*relativeContactPosition.y - rotation.y*relativeContactPosition.x + body.velocity.z; + // Скорость точки контакта в системе координат контакта + // Касательные компоненты прироста скорости добавляются к скорости точки контакта. В дальнейшем они либо уберутся силой трения, + // либо уменьшатся ею. + result.x = contactToWorld.a*x + contactToWorld.e*y + contactToWorld.i*z; + result.y = contactToWorld.b*x + contactToWorld.f*y + contactToWorld.j*z + accVelocityY; + result.z = contactToWorld.c*x + contactToWorld.g*y + contactToWorld.k*z + accVelocityZ; + } + + /** + * Вычисляет и устанавливает внутреннее значение для требуемого изменения относительной скорости вдоль нормали контакта. + * С реакциями опоры разбираемся путём вычитания из скорости сближения прироста скорости, полученного во время последнего кадра. + * + * @param time + */ + public function calculateDesiredVelocity(time:Number):void { + // Вычисляем проекцию прироста скорости за текущий кадр на нормаль контакта. Для относительного прироста скорости, вызывающего + // сближение объектов, значение будет отрицательным. + var velocityFromAcc:Number = 0; + if (body1.awake) { + velocityFromAcc = (body1.lastFrameAcceleration.x*contactNormal.x + body1.lastFrameAcceleration.y*contactNormal.y + body1.lastFrameAcceleration.z*contactNormal.z)*time; + } + if (body2 != null && body2.awake) { + velocityFromAcc -= (body2.lastFrameAcceleration.x*contactNormal.x + body2.lastFrameAcceleration.y*contactNormal.y + body2.lastFrameAcceleration.z*contactNormal.z)*time; + } + // Если относительная скорость вдоль нормали контакта слишком мала, игнорируем её + var thisRestitution:Number = restitution; + if (contactVelocity.x > -velocityLimit && contactVelocity.x < velocityLimit) { + thisRestitution = 0; + } + // Вычисляем дельту для скорости отскока, убрав влияние прироста скорости за последний кадр + desiredDeltaVelocity = -contactVelocity.x - thisRestitution*(contactVelocity.x - velocityFromAcc); + } + + /** + * Расчитывает внутренние данные о контакта на основе данных о его состоянии. Вызывается до того, как будет запущен + * алгоритм разрешения контактов. Эта функция в нормальных условиях никогда не должна вызываться вручную. + * + * @param time + */ + public function calculateInternals(time:Number):void { + // Расчёт базиса системы координат контакта + // === inlined calculateContactBasis(); + contactToWorld.a = contactNormal.x; + contactToWorld.e = contactNormal.y; + contactToWorld.i = contactNormal.z; + var nx:Number = contactNormal.x > 0 ? contactNormal.x : -contactNormal.x; + var ny:Number = contactNormal.y > 0 ? contactNormal.y : -contactNormal.y; + var s:Number; + if (nx > ny) { + // Нормаль ближе к глобальной оси X, используем ось Y для вычислений. Локальный орт Y находится как результат векторного произведения globalY x contactNormal. + s = 1/Math.sqrt(nx*nx + contactNormal.z*contactNormal.z); + contactToWorld.b = contactNormal.z*s; + contactToWorld.f = 0; + contactToWorld.j = -contactNormal.x*s; + // Локальный орт Z находится как результат векторного произведения contactNormal x localY. + contactToWorld.c = contactNormal.y*contactToWorld.j; + contactToWorld.g = contactNormal.z*contactToWorld.b - contactNormal.x*contactToWorld.j; + contactToWorld.k = -contactNormal.y*contactToWorld.b; + } else { + // Нормаль ближе к глобальной оси Y, используем ось X для вычислений. Локальный орт Y находится как результат векторного произведения globalX x contactNormal. + s = 1/Math.sqrt(ny*ny + contactNormal.z*contactNormal.z); + contactToWorld.b = 0; + contactToWorld.f = -contactNormal.z*s; + contactToWorld.j = contactNormal.y*s; + // Локальный орт Z находится как результат векторного произведения contactNormal x localY. + contactToWorld.c = contactNormal.y*contactToWorld.j - contactNormal.z*contactToWorld.f; + contactToWorld.g = -contactNormal.x*contactToWorld.j; + contactToWorld.k = contactNormal.x*contactToWorld.f; + } + // === end inlined + + // Расчёт локальной скорости в точке контакта для первого тела + relativeContactPosition1.x = contactPoint.x - body1.position.x; + relativeContactPosition1.y = contactPoint.y - body1.position.y; + relativeContactPosition1.z = contactPoint.z - body1.position.z; + calculateLocalVelocity(body1, relativeContactPosition1, time, contactVelocity); + // Расчёт локальной скорости в точке контакта для второго тела + if (body2 != null) { + relativeContactPosition2.x = contactPoint.x - body2.position.x; + relativeContactPosition2.y = contactPoint.y - body2.position.y; + relativeContactPosition2.z = contactPoint.z - body2.position.z; + calculateLocalVelocity(body2, relativeContactPosition2, time, tmpVelocityVector); + // Расчёт относительной скорости в точке контакта + contactVelocity.x -= tmpVelocityVector.x; + contactVelocity.y -= tmpVelocityVector.y; + contactVelocity.z -= tmpVelocityVector.z; + } + // Расчёт вектора изменения скорости, требуемого для разрешения контакта + calculateDesiredVelocity(time); + } + + /** + * Выполняет разделение контакта посредством импульса, пропорционально инерции тел. + */ + public function applyVelocityChange(velocityChange1:Point3D, velocityChange2:Point3D, rotationChange1:Point3D, rotationChange2:Point3D):void { + var inverseInertiaTensor1:Matrix3D = body1.inverseInertiaTensorWorld; + var inverseInertiaTensor2:Matrix3D; + if (body2 != null) { + inverseInertiaTensor2 = body2.inverseInertiaTensorWorld; + } + + if (friction == 0) { + calculateFrictionlessImpulse(inverseInertiaTensor1, inverseInertiaTensor2, impulse); + } else { + calculateFrictionImpulse(inverseInertiaTensor1, inverseInertiaTensor2, impulse); + } + + // Переведём импульс в мировые координаты + var x:Number = impulse.x; + var y:Number = impulse.y; + var z:Number = impulse.z; + impulse.x = contactToWorld.a*x + contactToWorld.b*y + contactToWorld.c*z; + impulse.y = contactToWorld.e*x + contactToWorld.f*y + contactToWorld.g*z; + impulse.z = contactToWorld.i*x + contactToWorld.j*y + contactToWorld.k*z; + + // Разделим импульс на вращательную и линейную часть + x = relativeContactPosition1.y*impulse.z - relativeContactPosition1.z*impulse.y; + y = relativeContactPosition1.z*impulse.x - relativeContactPosition1.x*impulse.z; + z = relativeContactPosition1.x*impulse.y - relativeContactPosition1.y*impulse.x; + rotationChange1.x = inverseInertiaTensor1.a*x + inverseInertiaTensor1.b*y + inverseInertiaTensor1.c*z; + rotationChange1.y = inverseInertiaTensor1.e*x + inverseInertiaTensor1.f*y + inverseInertiaTensor1.g*z; + rotationChange1.z = inverseInertiaTensor1.i*x + inverseInertiaTensor1.j*y + inverseInertiaTensor1.k*z; + + velocityChange1.x = impulse.x*body1.inverseMass; + velocityChange1.y = impulse.y*body1.inverseMass; + velocityChange1.z = impulse.z*body1.inverseMass; + + body1.velocity.x += velocityChange1.x; + body1.velocity.y += velocityChange1.y; + body1.velocity.z += velocityChange1.z; + + body1.rotation.x += rotationChange1.x; + body1.rotation.y += rotationChange1.y; + body1.rotation.z += rotationChange1.z; + + if (body2 != null) { + x = relativeContactPosition2.z*impulse.y - relativeContactPosition2.y*impulse.z; + y = relativeContactPosition2.x*impulse.z - relativeContactPosition2.z*impulse.x; + z = relativeContactPosition2.y*impulse.x - relativeContactPosition2.x*impulse.y; + rotationChange2.x = inverseInertiaTensor2.a*x + inverseInertiaTensor2.b*y + inverseInertiaTensor2.c*z; + rotationChange2.y = inverseInertiaTensor2.e*x + inverseInertiaTensor2.f*y + inverseInertiaTensor2.g*z; + rotationChange2.z = inverseInertiaTensor2.i*x + inverseInertiaTensor2.j*y + inverseInertiaTensor2.k*z; + + velocityChange2.x = -impulse.x*body2.inverseMass; + velocityChange2.y = -impulse.y*body2.inverseMass; + velocityChange2.z = -impulse.z*body2.inverseMass; + + body2.velocity.x += velocityChange2.x; + body2.velocity.y += velocityChange2.y; + body2.velocity.z += velocityChange2.z; + + body2.rotation.x += rotationChange2.x; + body2.rotation.y += rotationChange2.y; + body2.rotation.z += rotationChange2.z; + } + } + + /** + * Вычисляет импульс, необходимый для коррекции скоростей в точке контакта в отсутствие сил трения. + */ + public function calculateFrictionlessImpulse(inverseInertiaTensor1:Matrix3D, inverseInertiaTensor2:Matrix3D, result:Point3D):void { + tmpVelocityVector.x = relativeContactPosition1.y*contactNormal.z - relativeContactPosition1.z*contactNormal.y; + tmpVelocityVector.y = relativeContactPosition1.z*contactNormal.x - relativeContactPosition1.x*contactNormal.z; + tmpVelocityVector.z = relativeContactPosition1.x*contactNormal.y - relativeContactPosition1.y*contactNormal.x; + + var x:Number = inverseInertiaTensor1.a*tmpVelocityVector.x + inverseInertiaTensor1.b*tmpVelocityVector.y + inverseInertiaTensor1.c*tmpVelocityVector.z; + var y:Number = inverseInertiaTensor1.e*tmpVelocityVector.x + inverseInertiaTensor1.f*tmpVelocityVector.y + inverseInertiaTensor1.g*tmpVelocityVector.z; + var z:Number = inverseInertiaTensor1.i*tmpVelocityVector.x + inverseInertiaTensor1.j*tmpVelocityVector.y + inverseInertiaTensor1.k*tmpVelocityVector.z; + + tmpVelocityVector.x = y*relativeContactPosition1.z - z*relativeContactPosition1.y; + tmpVelocityVector.y = z*relativeContactPosition1.x - x*relativeContactPosition1.z; + tmpVelocityVector.z = x*relativeContactPosition1.y - y*relativeContactPosition1.x; + + var deltaVelocity:Number = tmpVelocityVector.x*contactNormal.x + tmpVelocityVector.y*contactNormal.y + tmpVelocityVector.z*contactNormal.z + body1.inverseMass; + + if (body2 != null) { + tmpVelocityVector.x = relativeContactPosition2.y*contactNormal.z - relativeContactPosition2.z*contactNormal.y; + tmpVelocityVector.y = relativeContactPosition2.z*contactNormal.x - relativeContactPosition2.x*contactNormal.z; + tmpVelocityVector.z = relativeContactPosition2.x*contactNormal.y - relativeContactPosition2.y*contactNormal.x; + + x = inverseInertiaTensor2.a*tmpVelocityVector.x + inverseInertiaTensor2.b*tmpVelocityVector.y + inverseInertiaTensor2.c*tmpVelocityVector.z; + y = inverseInertiaTensor2.e*tmpVelocityVector.x + inverseInertiaTensor2.f*tmpVelocityVector.y + inverseInertiaTensor2.g*tmpVelocityVector.z; + z = inverseInertiaTensor2.i*tmpVelocityVector.x + inverseInertiaTensor2.j*tmpVelocityVector.y + inverseInertiaTensor2.k*tmpVelocityVector.z; + + tmpVelocityVector.x = y*relativeContactPosition2.z - z*relativeContactPosition2.y; + tmpVelocityVector.y = z*relativeContactPosition2.x - x*relativeContactPosition2.z; + tmpVelocityVector.z = x*relativeContactPosition2.y - y*relativeContactPosition2.x; + + deltaVelocity += tmpVelocityVector.x*contactNormal.x + tmpVelocityVector.y*contactNormal.y + tmpVelocityVector.z*contactNormal.z + body2.inverseMass; + } + + result.x = desiredDeltaVelocity/deltaVelocity; + result.y = 0; + result.z = 0; + } + + private var impulseToTorque:Matrix3D = new Matrix3D(); + private var deltaVelWorldMatrix:Matrix3D = new Matrix3D(); + private var deltaVelWorldMatrix2:Matrix3D = new Matrix3D(); + /** + * Вычисляет импульс, необходимый для коррекции скоростей в точке контакта при наличии сил трения. + */ + public function calculateFrictionImpulse(inverseInertiaTensor1:Matrix3D, inverseInertiaTensor2:Matrix3D, result:Point3D):void { + var inverseMass:Number = body1.inverseMass; + + relativeContactPosition1.createSkewSymmetricMatrix(impulseToTorque); + + deltaVelWorldMatrix.copy(impulseToTorque); + deltaVelWorldMatrix.inverseCombine(inverseInertiaTensor1); + deltaVelWorldMatrix.inverseCombine(impulseToTorque); + deltaVelWorldMatrix.multByScalar(-1); + + if (body2 != null) { + relativeContactPosition2.createSkewSymmetricMatrix(impulseToTorque); + deltaVelWorldMatrix2.copy(impulseToTorque); + deltaVelWorldMatrix2.inverseCombine(inverseInertiaTensor2); + deltaVelWorldMatrix2.inverseCombine(impulseToTorque); + deltaVelWorldMatrix2.multByScalar(-1); + + deltaVelWorldMatrix.add(deltaVelWorldMatrix2); + inverseMass += body2.inverseMass; + } + + deltaVelWorldMatrix2.copy(contactToWorld); + deltaVelWorldMatrix2.transpose(); + deltaVelWorldMatrix2.inverseCombine(deltaVelWorldMatrix); + deltaVelWorldMatrix2.inverseCombine(contactToWorld); + deltaVelWorldMatrix2.a += inverseMass; + deltaVelWorldMatrix2.f += inverseMass; + deltaVelWorldMatrix2.k += inverseMass; + + deltaVelWorldMatrix.copy(deltaVelWorldMatrix2); + deltaVelWorldMatrix.invert(); + + result.reset(desiredDeltaVelocity, -contactVelocity.y, -contactVelocity.z); + result.transform(deltaVelWorldMatrix); + + var planarImpulse:Number = Math.sqrt(result.y*result.y + result.z*result.z); + if (planarImpulse > result.x*friction) { + result.y /= planarImpulse; + result.z /= planarImpulse; + result.x = desiredDeltaVelocity/(deltaVelWorldMatrix2.a + deltaVelWorldMatrix2.b*friction*result.y + deltaVelWorldMatrix2.c*friction*result.z); + result.y *= friction*result.x; + result.z *= friction*result.x; + } + } + + private var angularInertiaWorld:Point3D = new Point3D(); + /** + * Выполняет разделение контакта с учётом инертности тел. + */ + public function applyPositionChange(linearChange1:Point3D, linearChange2:Point3D, angularChange1:Point3D, angularChange2:Point3D, penetration:Number):void { + var totalInertia:Number = 0; + var angularInertia1:Number; + var angularInertia2:Number; + var linearInertia1:Number; + var linearInertia2:Number; + + // body1 + angularInertiaWorld.x = relativeContactPosition1.y*contactNormal.z - relativeContactPosition1.z*contactNormal.y; + angularInertiaWorld.y = relativeContactPosition1.z*contactNormal.x - relativeContactPosition1.x*contactNormal.z; + angularInertiaWorld.z = relativeContactPosition1.x*contactNormal.y - relativeContactPosition1.y*contactNormal.x; + var m:Matrix3D = body1.inverseInertiaTensorWorld; + var x:Number = m.a*angularInertiaWorld.x + m.b*angularInertiaWorld.y + m.c*angularInertiaWorld.z; + var y:Number = m.e*angularInertiaWorld.x + m.f*angularInertiaWorld.y + m.g*angularInertiaWorld.z; + var z:Number = m.i*angularInertiaWorld.x + m.j*angularInertiaWorld.y + m.k*angularInertiaWorld.z; + angularInertiaWorld.x = y*relativeContactPosition1.z - z*relativeContactPosition1.y; + angularInertiaWorld.y = z*relativeContactPosition1.x - x*relativeContactPosition1.z; + angularInertiaWorld.z = x*relativeContactPosition1.y - y*relativeContactPosition1.x; + + angularInertia1 = angularInertiaWorld.x*contactNormal.x + angularInertiaWorld.y*contactNormal.y + angularInertiaWorld.z*contactNormal.z; + linearInertia1 = body1.inverseMass; + + totalInertia = linearInertia1 + angularInertia1; + + if (body2 != null) { + angularInertiaWorld.x = relativeContactPosition2.y*contactNormal.z - relativeContactPosition2.z*contactNormal.y; + angularInertiaWorld.y = relativeContactPosition2.z*contactNormal.x - relativeContactPosition2.x*contactNormal.z; + angularInertiaWorld.z = relativeContactPosition2.x*contactNormal.y - relativeContactPosition2.y*contactNormal.x; + m = body2.inverseInertiaTensorWorld; + x = m.a*angularInertiaWorld.x + m.b*angularInertiaWorld.y + m.c*angularInertiaWorld.z; + y = m.e*angularInertiaWorld.x + m.f*angularInertiaWorld.y + m.g*angularInertiaWorld.z; + z = m.i*angularInertiaWorld.x + m.j*angularInertiaWorld.y + m.k*angularInertiaWorld.z; + angularInertiaWorld.x = y*relativeContactPosition2.z - z*relativeContactPosition2.y; + angularInertiaWorld.y = z*relativeContactPosition2.x - x*relativeContactPosition2.z; + angularInertiaWorld.z = x*relativeContactPosition2.y - y*relativeContactPosition2.x; + + angularInertia2 = angularInertiaWorld.x*contactNormal.x + angularInertiaWorld.y*contactNormal.y + angularInertiaWorld.z*contactNormal.z; + linearInertia2 = body2.inverseMass; + + totalInertia += linearInertia2 + angularInertia2; + } + + // body1 + applyPostionChangeToBody(body1, relativeContactPosition1, angularInertia1, linearInertia1, totalInertia, 1, angularChange1, linearChange1); + + if (body2 != null) { + applyPostionChangeToBody(body2, relativeContactPosition2, angularInertia2, linearInertia2, totalInertia, -1, angularChange2, linearChange2); + } + } + + private var projection:Point3D = new Point3D(); + /** + * + * @param body + * @param relativeContactPosition + * @param angularInertia + * @param linearInertia + * @param totalInertia + * @param sign + * @param angularChange + * @param linearChange + */ + private function applyPostionChangeToBody(body:RigidBody, relativeContactPosition:Point3D, angularInertia:Number, linearInertia:Number, totalInertia:Number, sign:Number, angularChange:Point3D, linearChange:Point3D):void { + var angularLimit:Number = 0.2; + var angularMove:Number = sign*penetration*angularInertia/totalInertia; + var linearMove:Number = sign*penetration*linearInertia/totalInertia; + + projection.copy(contactNormal); + projection.multiply(-relativeContactPosition.dot(contactNormal)); + projection.add(relativeContactPosition); + + var maxMagnitude:Number = angularLimit*projection.length; + + var totalMove:Number; + if (angularMove < -maxMagnitude) { + totalMove = angularMove + linearMove; + angularMove = -maxMagnitude; + linearMove = totalMove - angularMove; + } else if (angularMove > maxMagnitude) { + totalMove = angularMove + linearMove; + angularMove = maxMagnitude; + linearMove = totalMove - angularMove; + } + + if (angularMove == 0) { + angularChange.reset(); + } else { + angularChange.copy(relativeContactPosition); + angularChange.cross(contactNormal); + angularChange.transform(body.inverseInertiaTensorWorld); + angularChange.multiply(angularMove/angularInertia); + } + + linearChange.copy(contactNormal); + linearChange.multiply(linearMove); + + body.position.add(linearChange); + body.orientation.addScaledVector(angularChange, 1); + if (!body.awake) { + body.calculateDerivedData(); + } + } + + public function toString():String { + return "RigidBodyContact" + + "\n timeStamp: " + timeStamp + + "\n body1: " + body1.getName() + + "\n body2: " + (body2 == null ? "" : body2.getName()) + + "\n contactPoint: " + contactPoint + + "\n contactNormal: " + contactNormal + + "\n restitution: " + restitution + + "\n penetration: " + penetration + + "\n friction: " + friction; + } + + } +} diff --git a/0.0.0.18/src/alternativa/physics/rigid/.svn/text-base/RigidBodyContactGenerator.as.svn-base b/0.0.0.18/src/alternativa/physics/rigid/.svn/text-base/RigidBodyContactGenerator.as.svn-base new file mode 100644 index 0000000..18dfb88 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/rigid/.svn/text-base/RigidBodyContactGenerator.as.svn-base @@ -0,0 +1,22 @@ +package alternativa.physics.rigid { + + /** + * Генератор контактов для твёрдых тел. + */ + public class RigidBodyContactGenerator { + /** + * Следующий генератор в списке. + */ + public var next:RigidBodyContactGenerator; + + /** + * Метод создаёт контакты. + * + * @param contact первый свободный для записи контакт в списке контактов мира + * @return следующий свободный для записи контакт после окончания работы метода. Значение null означает, что доступные контакты исчерпаны. + */ + public function addContacts(contact:RigidBodyContact):RigidBodyContact { + return null; + } + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/rigid/.svn/text-base/RigidBodyContactResolver.as.svn-base b/0.0.0.18/src/alternativa/physics/rigid/.svn/text-base/RigidBodyContactResolver.as.svn-base new file mode 100644 index 0000000..7f6df6c --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/rigid/.svn/text-base/RigidBodyContactResolver.as.svn-base @@ -0,0 +1,237 @@ +package alternativa.physics.rigid { + + import alternativa.physics.*; + import alternativa.types.Point3D; + + use namespace altphysics; + + /** + * + */ + public class RigidBodyContactResolver { + + private var velocityIterations:int; + private var positionIterations:int; + private var velocityEpsilon:Number; + private var positionEpsilon:Number; + + private var velocityIterationsUsed:int; + private var positionIterationsUsed:int; + + private var linearChange1:Point3D = new Point3D(); + private var linearChange2:Point3D = new Point3D(); + private var angularChange1:Point3D = new Point3D(); + private var angularChange2:Point3D = new Point3D(); + + private var velocityChange1:Point3D = new Point3D(); + private var velocityChange2:Point3D = new Point3D(); + private var rotationChange1:Point3D = new Point3D(); + private var rotationChange2:Point3D = new Point3D(); + + private var deltaVector:Point3D = new Point3D(); + + /** + * + * @param velocityIterations + * @param positionIterations + * @param velocityEpsilon + * @param positionEpsilon + */ + public function RigidBodyContactResolver(velocityIterations:int, positionIterations:int, velocityEpsilon:Number = 0.001, positionEpsilon:Number = 0.001) { + this.velocityIterations = velocityIterations; + this.positionIterations = positionIterations; + this.velocityEpsilon = velocityEpsilon; + this.positionEpsilon = positionEpsilon; + } + + /** + * + * @param velocityIterations + * @param positionEpsilon + */ + public function setIterations(velocityIterations:int, positionIterations:int):void { + this.velocityIterations = velocityIterations; + this.positionIterations = positionIterations; + } + + /** + * + * @return + */ + private function isValid():Boolean { + return (positionIterations > 0) && (velocityIterations > 0) && (positionEpsilon >= 0) && (velocityEpsilon >= 0); + } + + /** + * + * @param firstContact + * @param numContacts + * @param time + */ + public function resolveContacts(firstContact:RigidBodyContact, numContacts:int, time:Number):void { + if (numContacts == 0 || !isValid()) { + return; + } + // Подготовка контактов к обработке + prepareContacts(firstContact, numContacts, time); + // Разделение взаимно проникающих контактов + adjustPositions(firstContact, numContacts, time); + // Коррекция скорости для контактов + adjustVelocities(firstContact, numContacts, time); + } + + /** + * + * @param firstContact + * @param numContacts + * @param time + */ + public function prepareContacts(firstContact:RigidBodyContact, numContacts:int, time:Number):void { + var contact:RigidBodyContact = firstContact; + while (contact.index < numContacts) { + // Вычисление внутренних данных контакта, необходимых для дальнейшей работы + contact.calculateInternals(time); + contact = contact.next; + } + } + + /** + * + * @param firstContact + * @param numContacts + * @param time + */ + public function adjustPositions(firstContact:RigidBodyContact, numContacts:int, time:Number):void { + positionIterationsUsed = 0; + // Итерационное устранение взаимного проникновения тел в точках контактов в порядке наибольшего пересечения тел + while (positionIterationsUsed < positionIterations) { + var max:Number = positionEpsilon; + var mostSevereContact:RigidBodyContact = null; + // Поиск контакта с наибольшим пересечением + var contact:RigidBodyContact = firstContact; + while (contact.index < numContacts) { + if (contact.penetration > max) { + max = contact.penetration; + mostSevereContact = contact; + } + contact = contact.next; + } + if (mostSevereContact == null) { + // Алгоритм прерывается, если не осталось пересечений + return; + } + // Будим спящего + mostSevereContact.matchAwakeState(); + + // Разделяем контакт + mostSevereContact.applyPositionChange(linearChange1, linearChange2, angularChange1, angularChange2, max); + // Обновляем величину проникновения в каждом контакте, содержащем обработанные объекты + contact = firstContact; + while (contact.index < numContacts) { + if (contact.body1 == mostSevereContact.body1) { + deltaVector.copy(angularChange1); + deltaVector.cross(contact.relativeContactPosition1); + deltaVector.add(linearChange1); + contact.penetration -= deltaVector.dot(contact.contactNormal); + } + if (contact.body1 == mostSevereContact.body2) { + deltaVector.copy(angularChange2); + deltaVector.cross(contact.relativeContactPosition1); + deltaVector.add(linearChange2); + contact.penetration -= deltaVector.dot(contact.contactNormal); + } + if (contact.body2 != null) { + if (contact.body2 == mostSevereContact.body1) { + deltaVector.copy(angularChange1); + deltaVector.cross(contact.relativeContactPosition2); + deltaVector.add(linearChange1); + contact.penetration += deltaVector.dot(contact.contactNormal); + } + if (contact.body2 == mostSevereContact.body2) { + deltaVector.copy(angularChange2); + deltaVector.cross(contact.relativeContactPosition2); + deltaVector.add(linearChange2); + contact.penetration += deltaVector.dot(contact.contactNormal); + } + } + contact = contact.next; + } + positionIterationsUsed++; + } + } + + /** + * + * @param firstContact + * @param numContacts + * @param time + */ + public function adjustVelocities(firstContact:RigidBodyContact, numContacts:int, time:Number):void { + velocityIterationsUsed = 0; + while (velocityIterationsUsed < velocityIterations) { + var max:Number = velocityEpsilon; + var mostSevereContact:RigidBodyContact = null; + var contact:RigidBodyContact = firstContact; + while (contact.index < numContacts) { + if (contact.desiredDeltaVelocity > max) { + max = contact.desiredDeltaVelocity; + mostSevereContact = contact; + } + contact = contact.next; + } + if (mostSevereContact == null) { + return; + } + // Будим спящих + mostSevereContact.matchAwakeState(); + // Корректируем скорости + mostSevereContact.applyVelocityChange(velocityChange1, velocityChange2, rotationChange1, rotationChange2); + // Обновляем скорости сближения для каждого контакта, содержащего обработанные объекты + contact = firstContact; + var needRecalculation:Boolean = false; + while (contact.index < numContacts) { + if (contact.body1 == mostSevereContact.body1) { + deltaVector.copy(rotationChange1); + deltaVector.cross(contact.relativeContactPosition1); + deltaVector.add(velocityChange1); + deltaVector.transformTranspose(contact.contactToWorld); + contact.contactVelocity.add(deltaVector); + needRecalculation = true; + } + if (contact.body1 == mostSevereContact.body2) { + deltaVector.copy(rotationChange2); + deltaVector.cross(contact.relativeContactPosition1); + deltaVector.add(velocityChange2); + deltaVector.transformTranspose(contact.contactToWorld); + contact.contactVelocity.add(deltaVector); + needRecalculation = true; + } + if (contact.body2 != null) { + if (contact.body2 == mostSevereContact.body1) { + deltaVector.copy(rotationChange1); + deltaVector.cross(contact.relativeContactPosition2); + deltaVector.add(velocityChange1); + deltaVector.transformTranspose(contact.contactToWorld); + contact.contactVelocity.subtract(deltaVector); + needRecalculation = true; + } + if (contact.body2 == mostSevereContact.body2) { + deltaVector.copy(rotationChange2); + deltaVector.cross(contact.relativeContactPosition2); + deltaVector.add(velocityChange2); + deltaVector.transformTranspose(contact.contactToWorld); + contact.contactVelocity.subtract(deltaVector); + needRecalculation = true; + } + } + if (needRecalculation) { + contact.calculateDesiredVelocity(time); + } + contact = contact.next; + } + velocityIterationsUsed++; + } + } + + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/rigid/.svn/text-base/RigidWorld.as.svn-base b/0.0.0.18/src/alternativa/physics/rigid/.svn/text-base/RigidWorld.as.svn-base new file mode 100644 index 0000000..def9635 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/rigid/.svn/text-base/RigidWorld.as.svn-base @@ -0,0 +1,136 @@ +package alternativa.physics.rigid { + + public class RigidWorld { + + private var firstBody:RigidBody; + private var lastBody:RigidBody; + private var contactGenerators:RigidBodyContactGenerator; + private var lastContactGenerator:RigidBodyContactGenerator; + + public var contacts:RigidBodyContact; + private var maxContacts:int; + private var resolver:RigidBodyContactResolver; + + public var forceRegistry:BodyForceRegistry = new BodyForceRegistry(); + public var calculateIterations:Boolean; + + public var numContacts:int; + + public function RigidWorld(maxContacts:int, iterations:int) { + this.maxContacts = maxContacts; + resolver = new RigidBodyContactResolver(iterations, iterations); + createContactList(maxContacts); + } + + private function createContactList(numContacts:int):void { + if (numContacts > 0) { + var contact:RigidBodyContact = contacts = new RigidBodyContact(0); + for (var i:int = 1; i < numContacts; i++) { + contact.next = new RigidBodyContact(i); + contact = contact.next; + } + } + } + + public function addBody(body:RigidBody):void { + if (firstBody == null) { + firstBody = body; + } else { + lastBody.next = body; + } + lastBody = body; + while (lastBody.next != null) { + lastBody = lastBody.next; + } + } + + public function removeBody(body:RigidBody):void { + if (body == firstBody) { + firstBody = firstBody.next; + body.next = null; + } else { + var current:RigidBody = firstBody; + var prev:RigidBody = null; + while (current != body && current != null) { + prev = current; + current = current.next; + } + if (current != null) { + if (lastBody == body) { + lastBody = prev; + } + prev.next = current.next; + current.next = null; + } + } + } + + public function addContactGenerator(gen:RigidBodyContactGenerator):void { + if (contactGenerators == null) { + contactGenerators = gen; + } else { + lastContactGenerator.next = gen; + } + lastContactGenerator = gen; + while (lastContactGenerator.next != null) { + lastContactGenerator = lastContactGenerator.next; + } + } + + public function removeContactGenerator(gen:RigidBodyContactGenerator):void { + if (gen == contactGenerators) { + contactGenerators = gen.next; + gen.next = null; + } else { + var prev:RigidBodyContactGenerator = contactGenerators; + var curr:RigidBodyContactGenerator = contactGenerators.next; + while (curr != gen && curr != null) { + prev = curr; + curr = curr.next; + } + if (curr == gen) { + if (lastContactGenerator == gen) { + lastContactGenerator = prev; + } + prev.next = curr.next; + curr.next = null; + } + } + } + + public function startFrame():void { + var body:RigidBody = firstBody; + while (body != null) { + body.clearAccumulators(); + body.calculateDerivedData(); + body = body.next; + } + } + + private function generateContacts():int { + var contact:RigidBodyContact = contacts; + var gen:RigidBodyContactGenerator = contactGenerators; + while (gen != null && contact != null) { + contact = gen.addContacts(contact); + gen = gen.next; + } + return contact == null ? maxContacts : contact.index; + } + + public function runPhysics(time:Number):void { + forceRegistry.updateForces(time); + var body:RigidBody = firstBody; + while (body != null) { + body.integrate(time); + body = body.next; + } + numContacts = generateContacts(); +// trace("[RigidWorld.runPhysics] numContacts", numContacts); + if (calculateIterations) { + resolver.setIterations(4*numContacts, 4*numContacts); + } + resolver.resolveContacts(contacts, numContacts, time); + } + + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/rigid/BodyForceRegistration.as b/0.0.0.18/src/alternativa/physics/rigid/BodyForceRegistration.as new file mode 100644 index 0000000..e030bbb --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/rigid/BodyForceRegistration.as @@ -0,0 +1,16 @@ +package alternativa.physics.rigid { + import alternativa.physics.force.IRigidBodyForceGenerator; + import alternativa.physics.rigid.RigidBody; + + public class BodyForceRegistration { + public var body:RigidBody; + public var forceGenerator:IRigidBodyForceGenerator; + + public var next:BodyForceRegistration; + + public function BodyForceRegistration(body:RigidBody, forceGenerator:IRigidBodyForceGenerator) { + this.body = body; + this.forceGenerator = forceGenerator; + } + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/rigid/BodyForceRegistry.as b/0.0.0.18/src/alternativa/physics/rigid/BodyForceRegistry.as new file mode 100644 index 0000000..a538a20 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/rigid/BodyForceRegistry.as @@ -0,0 +1,47 @@ +package alternativa.physics.rigid { + import alternativa.physics.force.IRigidBodyForceGenerator; + import alternativa.physics.rigid.RigidBody; + + public class BodyForceRegistry { + + protected var registrations:BodyForceRegistration; + + public function BodyForceRegistry() { + } + + public function add(body:RigidBody, fg:IRigidBodyForceGenerator):void { + var registration:BodyForceRegistration = new BodyForceRegistration(body, fg); + registration.next = registrations; + registrations = registration; + } + + public function remove(body:RigidBody, fg:IRigidBodyForceGenerator):void { + var current:BodyForceRegistration = registrations; + var prev:BodyForceRegistration = null; + while (current != null) { + if (current.body == body && current.forceGenerator == fg) { + if (current == registrations) { + registrations = current.next; + } else { + prev.next = current.next; + } + break; + } + prev = current; + current = current.next; + } + } + + public function clear():void { + registrations = null; + } + + public function updateForces(time:Number):void { + var current:BodyForceRegistration = registrations; + while (current != null) { + current.forceGenerator.updateForce(current.body, time); + current = current.next; + } + } + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/rigid/RigidBody.as b/0.0.0.18/src/alternativa/physics/rigid/RigidBody.as new file mode 100644 index 0000000..da46e3f --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/rigid/RigidBody.as @@ -0,0 +1,707 @@ +package alternativa.physics.rigid { + + import alternativa.physics.altphysics; + import alternativa.types.Matrix3D; + import alternativa.types.Point3D; + import alternativa.types.Quaternion; + + use namespace altphysics; + + /** + * Класс представляет твёрдое тело. + */ + public class RigidBody { + + private var angularAcceleration:Point3D = new Point3D(); + private var motionThreshold:Number = 0.1; + private var motionThreshold2:Number = 2*motionThreshold; + private var baseBias:Number = 0.8; + private var name:String; + + public var next:RigidBody; + public var prev:RigidBody; + + altphysics var mass:Number; + altphysics var inverseMass:Number; + altphysics var inverseInertiaTensor:Matrix3D = new Matrix3D(); + + altphysics var linearDamping:Number = 0.95; + altphysics var angularDamping:Number = 0.95; + altphysics var awake:Boolean = true; + altphysics var canSleep:Boolean = true; + + altphysics var acceleration:Point3D = new Point3D(); + altphysics var angularVelocity:Point3D = new Point3D(); + + altphysics var position:Point3D = new Point3D(); + altphysics var orientation:Quaternion = new Quaternion(1, 0, 0, 0); + altphysics var velocity:Point3D = new Point3D(); + altphysics var rotation:Point3D = new Point3D(); + altphysics var lastFrameAcceleration:Point3D = new Point3D(); + + altphysics var transformMatrix:Matrix3D = new Matrix3D(); + altphysics var inverseInertiaTensorWorld:Matrix3D = new Matrix3D(); + + altphysics var forceAccum:Point3D = new Point3D(); + altphysics var torqueAccum:Point3D = new Point3D(); + altphysics var motion:Number = 10; + + /** + * Создаёт новый экзепляр твёрдого тела. + * + * @param inverseMass величина, обратная массе тела. Значение 0 указывает, что тело имеет имеет бесконечную массу и момент инерции. + * @param inverseInertiaTensor обратный тензор момента инерции тела. Значение null указывает, что тело имеет бесконечную массу и момент инерции. + */ + public function RigidBody(inverseMass:Number = 1, inverseInertiaTensor:Matrix3D = null) { + if (inverseMass == 0 || inverseInertiaTensor == null) { + setInfinteMass(); + } else { + this.inverseMass = inverseMass; + this.mass = 1/inverseMass; + this.inverseInertiaTensor.copy(inverseInertiaTensor); + } + } + + /** + * @return имя тела + */ + public function getName():String { + return name; + } + + /** + * Устанавливает имя тела. + * + * @param value имя тела + */ + public function setName(value:String):void { + name = value; + } + + /** + * Предельное количество движения, ниже которого тело усыпляеся. + */ + public function getMotionThreshold():Number { + return motionThreshold; + } + + /** + * @private + */ + public function setMotionThreshold(value:Number):void { + motionThreshold = value; + motionThreshold2 = value*2; + } + + /** + * + * @param value + */ + public function setBaseBias(value:Number):void { + baseBias = value; + } + + /** + * + * @param value + */ + public function getBaseBias():Number { + return baseBias; + } + + /** + * + */ + public function getMass():Number { + return mass; + } + + /** + * + */ + public function setMass(value:Number):void { + if (value <= 0) { + inverseMass = 0; + mass = 0; + } else { + mass = value; + inverseMass = 1/value; + } + } + + /** + * + */ + public function getInverseMass():Number { + return inverseMass; + } + + /** + * @private + */ + public function setInverseMass(value:Number):void { + if (value > 0) { + inverseMass = value; + mass = 1/value; + } else { + inverseMass = 0; + mass = 0; + } + } + + /** + * + * @param m + */ + public function setIntertiaTensor(intertiaTensor:Matrix3D):void { + inverseInertiaTensor.copy(intertiaTensor); + inverseInertiaTensor.invert(); + } + + /** + * + * @param intertiaTensor + */ + public function getIntertiaTensor(intertiaTensor:Matrix3D):void { + intertiaTensor.copy(inverseInertiaTensor); + intertiaTensor.invert(); + } + + /** + * + * @param inverseInertiaTensor + */ + public function setInverseIntertiaTensor(inverseInertiaTensor:Matrix3D):void { + this.inverseInertiaTensor.copy(inverseInertiaTensor); + } + + /** + * + * @param inverseInertiaTensor + */ + public function getInverseIntertiaTensor(inverseInertiaTensor:Matrix3D):void { + inverseInertiaTensor.copy(this.inverseInertiaTensor); + } + + /** + * + */ + public function setInfinteMass():void { + inverseMass = 0; + mass = 0; + inverseInertiaTensor.toIdentity(); + inverseInertiaTensor.a = inverseInertiaTensor.f = inverseInertiaTensor.k = 0; + } + + /** + * + * @param linearDamping + * @param angularDamping + */ + public function setDamping(linearDamping:Number, angularDamping:Number):void { + this.linearDamping = linearDamping; + this.angularDamping = angularDamping; + } + + /** + * + * @param value + */ + public function setLinearDamping(value:Number):void { + linearDamping = value; + } + + /** + * + * @return + */ + public function getLinearDamping():Number { + return linearDamping; + } + + /** + * + * @param value + */ + public function setAngularDamping(value:Number):void { + angularDamping = value; + } + + /** + * + * @return + */ + public function getAngularDamping():Number { + return angularDamping; + } + + /** + * + * @param position + */ + public function setPosition(position:Point3D):void { + this.position.x = position.x; + this.position.y = position.y; + this.position.z = position.z; + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setPositionComponents(x:Number, y:Number, z:Number):void { + position.x = x; + position.y = y; + position.z = z; + } + + /** + * + * @param position + */ + public function getPosition(position:Point3D):void { + position.x = this.position.x; + position.y = this.position.y; + position.z = this.position.z; + } + + /** + * + * @param orientation + */ + public function setOrientation(orientation:Quaternion):void { + this.orientation.copy(orientation); + this.orientation.normalize(); + } + + /** + * + * @param r + * @param i + * @param j + * @param k + */ + public function setOrientationComponents(w:Number, x:Number, y:Number, z:Number):void { + orientation.reset(w, x, y, z); + orientation.normalize(); + } + + /** + * + * @param orientation + */ + public function getOrientation(orientation:Quaternion):void { + orientation.copy(this.orientation); + } + + /** + * + * @param matrix + */ + public function getTransformMatrix(matrix:Matrix3D):void { + matrix.copy(transformMatrix); + } + + /** + * + * @param velocity + */ + public function setVelocity(velocity:Point3D):void { + this.velocity.copy(velocity); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setVelocityComponents(x:Number, y:Number, z:Number):void { + velocity.reset(x, y, z); + } + + /** + * + * @param velocity + */ + public function getVelocity(velocity:Point3D):void { + velocity.copy(this.velocity); + } + + /** + * + * @param acceleration + * + */ + public function setAcceleration(acceleration:Point3D):void { + this.acceleration.copy(acceleration); + } + + /** + * + * @param x + * @param y + * @param z + * + */ + public function setAccelerationComponents(x:Number, y:Number, z:Number):void { + acceleration.reset(x, y, z); + } + + /** + * + * @param acceleration + * + */ + public function getAcceleration(acceleration:Point3D):void { + acceleration.copy(this.acceleration); + } + + /** + * + * @param rotation + * + */ + public function setRotation(rotation:Point3D):void { + this.rotation.copy(rotation); + } + + /** + * + * @param x + * @param y + * @param z + * + */ + public function setRotationComponents(x:Number, y:Number, z:Number):void { + rotation.reset(x, y, z); + } + + /** + * + * @param rotation + * + */ + public function addRotation(deltaRotation:Point3D):void { + rotation.add(deltaRotation); + } + + /** + * + * @param rotation + * + */ + public function getRotation(rotation:Point3D):void { + rotation.copy(this.rotation); + } + + /** + * + * @param angularVelocity + */ + public function setAngularVelocity(angularVelocity:Point3D):void { + this.angularVelocity.x = angularVelocity.x; + this.angularVelocity.y = angularVelocity.y; + this.angularVelocity.z = angularVelocity.z; + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setAngularVelocityComponents(x:Number, y:Number, z:Number):void { + angularVelocity.x = x; + angularVelocity.y = y; + angularVelocity.z = z; + } + + /** + * + * @param awake + */ + public function setAwake(awake:Boolean = true):void { + this.awake = awake; + if (awake) { + motion = motionThreshold2; + } else { + velocity.x = 0; + velocity.y = 0; + velocity.z = 0; + + rotation.x = 0; + rotation.y = 0; + rotation.z = 0; + } + } + + /** + * + * @return + */ + public function isAwake():Boolean { + return awake; + } + + /** + * + * @param canSleep + */ + public function setCanSleep(canSleep:Boolean = true):void { + this.canSleep = canSleep; + if (!canSleep && !awake) { + setAwake(); + } + } + + /** + * + * @return + */ + public function isCanSleep():Boolean { + return canSleep; + } + + /** + * Добавляет глобальную силу, приложенную к центру масс тела. + * + * @param force вектор силы в глобальной системе координат + */ + public function addForce(force:Point3D):void { + forceAccum.x += force.x; + forceAccum.y += force.y; + forceAccum.z += force.z; + setAwake(); + } + + /** + * + * @param force + */ + public function addForceComponents(x:Number, y:Number, z:Number):void { + forceAccum.x += x; + forceAccum.y += y; + forceAccum.z += z; + setAwake(); + } + + /** + * Добавляет глобальную силу, приложенную к заданной локальной точке тела. + * + * @param force вектор силы в глобальной системе координат + * @param localPoint локальные координаты точки приложения силы + */ + public function addForceAtLocalPoint(force:Point3D, localPoint:Point3D):void { + forceAccum.x += force.x; + forceAccum.y += force.y; + forceAccum.z += force.z; + // Трансформация локальных координат в глобальные + var x:Number = transformMatrix.a*localPoint.x + transformMatrix.b*localPoint.y + transformMatrix.c*localPoint.z; + var y:Number = transformMatrix.e*localPoint.x + transformMatrix.f*localPoint.y + transformMatrix.g*localPoint.z; + var z:Number = transformMatrix.i*localPoint.x + transformMatrix.j*localPoint.y + transformMatrix.k*localPoint.z; + torqueAccum.x += y*force.z - z*force.y; + torqueAccum.y += z*force.x - x*force.z; + torqueAccum.z += x*force.y - y*force.x; + setAwake(); + } + + /** + * Добавляет глобальную силу, приложенную к заданной точке тела, выраженной в глобальных координатах. + * + * @param force вектор силы в глобальной системе координат + * @param point глобальные координаты точки приложения силы + */ + public function addForceAtPoint(force:Point3D, point:Point3D):void { + forceAccum.x += force.x; + forceAccum.y += force.y; + forceAccum.z += force.z; + var x:Number = point.x - position.x; + var y:Number = point.y - position.y; + var z:Number = point.z - position.z; + torqueAccum.x += y*force.z - z*force.y; + torqueAccum.y += z*force.x - x*force.z; + torqueAccum.z += x*force.y - y*force.x; + setAwake(); + } + + /** + * Добавляет локальную силу, приложенную в заданной локальной точке тела. + * + * @param localForce вектор локальной силы + * @param localPoint локальные координаты точки приложения силы + */ + public function addLocalForce(localForce:Point3D, localPoint:Point3D):void { + // Трансформация локального вектора силы в глобальный + var globalForceX:Number = transformMatrix.a*localForce.x + transformMatrix.b*localForce.y + transformMatrix.c*localForce.z; + var globalForceY:Number = transformMatrix.e*localForce.x + transformMatrix.f*localForce.y + transformMatrix.g*localForce.z; + var globalForceZ:Number = transformMatrix.i*localForce.x + transformMatrix.j*localForce.y + transformMatrix.k*localForce.z; + forceAccum.x += globalForceX; + forceAccum.y += globalForceY; + forceAccum.z += globalForceZ; + // Получение глобального вектора плеча силы + var globalPointX:Number = transformMatrix.a*localPoint.x + transformMatrix.b*localPoint.y + transformMatrix.c*localPoint.z; + var globalPointY:Number = transformMatrix.e*localPoint.x + transformMatrix.f*localPoint.y + transformMatrix.g*localPoint.z; + var globalPointZ:Number = transformMatrix.i*localPoint.x + transformMatrix.j*localPoint.y + transformMatrix.k*localPoint.z; + // Приращение момента силы + torqueAccum.x += globalPointY*globalForceZ - globalPointZ*globalForceY; + torqueAccum.y += globalPointZ*globalForceX - globalPointX*globalForceZ; + torqueAccum.z += globalPointX*globalForceY - globalPointY*globalForceX; + setAwake(); + } + + /** + * Добавляет момент силы. + * + * @param torque вектор момента силы + */ + public function addTorque(torque:Point3D):void { + torqueAccum.x += torque.x; + torqueAccum.y += torque.y; + torqueAccum.z += torque.z; + setAwake(); + } + + /** + * Добавляет момент силы, заданный покомпонентно. + */ + public function addTorqueComponents(x:Number, y:Number, z:Number):void { + torqueAccum.x += x; + torqueAccum.y += y; + torqueAccum.z += z; + setAwake(); + } + + /** + * Вычисляет векторные параметры тела в глобальной системе координат. + */ + public function calculateDerivedData():void { + // inlined position.normalize() + var d:Number = orientation.w*orientation.w + orientation.x*orientation.x + orientation.y*orientation.y + orientation.z*orientation.z; + if (d == 0) { + orientation.w = 1; + } else { + d = 1/Math.sqrt(d); + orientation.w *= d; + orientation.x *= d; + orientation.y *= d; + orientation.z *= d; + } + // Вычисление глобальной матрицы трансформации + orientation.toMatrix3D(transformMatrix); + transformMatrix.d = position.x; + transformMatrix.h = position.y; + transformMatrix.l = position.z; + // Расчёт обратного тензора инерции в мировых координатах + var a:Number = transformMatrix.a*inverseInertiaTensor.a + transformMatrix.b*inverseInertiaTensor.e + transformMatrix.c*inverseInertiaTensor.i; + var b:Number = transformMatrix.a*inverseInertiaTensor.b + transformMatrix.b*inverseInertiaTensor.f + transformMatrix.c*inverseInertiaTensor.j; + var c:Number = transformMatrix.a*inverseInertiaTensor.c + transformMatrix.b*inverseInertiaTensor.g + transformMatrix.c*inverseInertiaTensor.k; + var e:Number = transformMatrix.e*inverseInertiaTensor.a + transformMatrix.f*inverseInertiaTensor.e + transformMatrix.g*inverseInertiaTensor.i; + var f:Number = transformMatrix.e*inverseInertiaTensor.b + transformMatrix.f*inverseInertiaTensor.f + transformMatrix.g*inverseInertiaTensor.j; + var g:Number = transformMatrix.e*inverseInertiaTensor.c + transformMatrix.f*inverseInertiaTensor.g + transformMatrix.g*inverseInertiaTensor.k; + var i:Number = transformMatrix.i*inverseInertiaTensor.a + transformMatrix.j*inverseInertiaTensor.e + transformMatrix.k*inverseInertiaTensor.i; + var j:Number = transformMatrix.i*inverseInertiaTensor.b + transformMatrix.j*inverseInertiaTensor.f + transformMatrix.k*inverseInertiaTensor.j; + var k:Number = transformMatrix.i*inverseInertiaTensor.c + transformMatrix.j*inverseInertiaTensor.g + transformMatrix.k*inverseInertiaTensor.k; + // Здесь подразумевается, что матрица трансформации тела не содержит масштабирования + // Произведение [transformMatrix*inverseInertiaTensor]*transpose(transformMatrix) + inverseInertiaTensorWorld.a = a*transformMatrix.a + b*transformMatrix.b + c*transformMatrix.c; + inverseInertiaTensorWorld.b = a*transformMatrix.e + b*transformMatrix.f + c*transformMatrix.g; + inverseInertiaTensorWorld.c = a*transformMatrix.i + b*transformMatrix.j + c*transformMatrix.k; + inverseInertiaTensorWorld.e = e*transformMatrix.a + f*transformMatrix.b + g*transformMatrix.c; + inverseInertiaTensorWorld.f = e*transformMatrix.e + f*transformMatrix.f + g*transformMatrix.g; + inverseInertiaTensorWorld.g = e*transformMatrix.i + f*transformMatrix.j + g*transformMatrix.k; + inverseInertiaTensorWorld.i = i*transformMatrix.a + j*transformMatrix.b + k*transformMatrix.c; + inverseInertiaTensorWorld.j = i*transformMatrix.e + j*transformMatrix.f + k*transformMatrix.g; + inverseInertiaTensorWorld.k = i*transformMatrix.i + j*transformMatrix.j + k*transformMatrix.k; + } + + /** + * Интегрирует динамические характеристики и обновляет параметры состояния тела. + * + * @param time временной шаг интегрирования + */ + public function integrate(time:Number):void { + if (!awake) { + return; + } + + lastFrameAcceleration.x = acceleration.x + forceAccum.x*inverseMass; + lastFrameAcceleration.y = acceleration.y + forceAccum.y*inverseMass; + lastFrameAcceleration.z = acceleration.z + forceAccum.z*inverseMass; + // angularAcceleration = inverseInertiaTensorWorld*torqueAccum + angularAcceleration.x = inverseInertiaTensorWorld.a*torqueAccum.x + inverseInertiaTensorWorld.b*torqueAccum.y + inverseInertiaTensorWorld.c*torqueAccum.z; + angularAcceleration.y = inverseInertiaTensorWorld.e*torqueAccum.x + inverseInertiaTensorWorld.f*torqueAccum.y + inverseInertiaTensorWorld.g*torqueAccum.z; + angularAcceleration.z = inverseInertiaTensorWorld.i*torqueAccum.x + inverseInertiaTensorWorld.j*torqueAccum.y + inverseInertiaTensorWorld.k*torqueAccum.z; + + var d:Number = Math.pow(linearDamping, time); + var dtime:Number = d*time; + + velocity.x += lastFrameAcceleration.x*dtime; + velocity.y += lastFrameAcceleration.y*dtime; + velocity.z += lastFrameAcceleration.z*dtime; + + rotation.x += angularAcceleration.x*dtime; + rotation.y += angularAcceleration.y*dtime; + rotation.z += angularAcceleration.z*dtime; + + rotation.x += angularVelocity.x*time; + rotation.y += angularVelocity.y*time; + rotation.z += angularVelocity.z*time; + + position.x += velocity.x*time; + position.y += velocity.y*time; + position.z += velocity.z*time; + + orientation.addScaledVector(rotation, time); + + // TODO: Нужно ли двойное уменьшение скоростей? + velocity.x *= d; + velocity.y *= d; + velocity.z *= d; + + rotation.x *= d; + rotation.y *= d; + rotation.z *= d; + + calculateDerivedData(); + // inlined clearAccumulators() + forceAccum.x = 0; + forceAccum.y = 0; + forceAccum.z = 0; + torqueAccum.x = 0; + torqueAccum.y = 0; + torqueAccum.z = 0; + + // Усыпление тела при необходимости + if (canSleep) { + var currentMotion:Number = velocity.x*velocity.x + velocity.y*velocity.y + velocity.z*velocity.z + + rotation.x*rotation.x + rotation.y*rotation.y + rotation.z*rotation.z; + var bias:Number = Math.pow(baseBias, time); + motion = bias*motion + (1 - bias)*currentMotion; +// trace("[RigidBody::integrate]", name, "motion", motion, "motionEpsilon", motionEpsilon); + if (motion < motionThreshold) { + setAwake(false); + } else { + if (motion > motionThreshold2) { + motion = motionThreshold2; + } + } + } + } + + /** + * + */ + public function clearAccumulators():void { + forceAccum.x = 0; + forceAccum.y = 0; + forceAccum.z = 0; + torqueAccum.x = 0; + torqueAccum.y = 0; + torqueAccum.z = 0; + } + + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/rigid/RigidBodyContact.as b/0.0.0.18/src/alternativa/physics/rigid/RigidBodyContact.as new file mode 100644 index 0000000..22b3c65 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/rigid/RigidBodyContact.as @@ -0,0 +1,550 @@ +package alternativa.physics.rigid { + + import alternativa.physics.*; + import alternativa.types.Matrix3D; + import alternativa.types.Point3D; + + use namespace altphysics; + + /** + * Класс описывает контакт между телами. Контакты могут составлять двунаправленный список. + */ + public class RigidBodyContact { + + private var tmpVelocityVector:Point3D = new Point3D(); + private var impulse:Point3D = new Point3D(); + + public static var velocityLimit:Number = 1; + + /** + * Ссылка на следующий контакт в списке. + */ + public var next:RigidBodyContact; + /** + * Ссылка на предыдущий контакт в списке. + */ + public var prev:RigidBodyContact; + /** + * Индекс контакта в списке. + */ + public var index:int; + + /** + * Первое тело, к которому относится контакт. + */ + public var body1:RigidBody; + /** + * Первое тело, к которому относится контакт (может быть null). + */ + public var body2:RigidBody; + /** + * Точка контакта. + */ + public var contactPoint:Point3D = new Point3D(); + /** + * Нормаль контакта. Нормаль направлена от второго тела к первому. В локальной системе координат контакта + * нормаль является базисным вектором, задающим направление оси X. + */ + public var contactNormal:Point3D = new Point3D(); + /** + * Коэффициент взаимного трения в точке контакта. + */ + public var friction:Number = 0; + /** + * Коэффициент упругости удара. + */ + public var restitution:Number = 0; + /** + * Глубина взаимного проникновения тел в точке контакта. + */ + public var penetration:Number; + + public var timeStamp:int; + + /** + * Матрица перехода из локальной системы координат контакта в мировую систему координат без компонентов параллельного переноса. + * Матрица задаёт базис локальной системы координат контакта. + */ + altphysics var contactToWorld:Matrix3D = new Matrix3D(); + /** + * Относительная скорость в точке контакта. Вычисляется в методе calculateInternals(). + */ + altphysics var contactVelocity:Point3D = new Point3D(); + /** + * Приращение скорости в точке контакта по направлению нормали, необходимое для прекращения сближения. Вычисляется в методе calculateInternals(). + */ + altphysics var desiredDeltaVelocity:Number; + /** + * Координаты точки контакта относительныо первого тела. Вычисляется в методе calculateInternals(). + */ + altphysics var relativeContactPosition1:Point3D = new Point3D(); + /** + * Координаты точки контакта относительныо второго тела. Вычисляется в методе calculateInternals(). + */ + altphysics var relativeContactPosition2:Point3D = new Point3D(); + + /** + * + * @param index + */ + public function RigidBodyContact(index:int) { + this.index = index; + } + + /** + * + * @param body1 + * @param body2 + * @param friction + * @param restitution + */ + public function setBodyData(body1:RigidBody, body2:RigidBody, friction:Number, restitution:Number):void { + this.body1 = body1; + this.body2 = body2; + this.friction = friction; + this.restitution = restitution; + } + + /** + * + */ + public function matchAwakeState():void { + if (body2 == null) { + return; + } + + if (body1.awake != body2.awake) { + if (body1.awake) { + body2.setAwake(); + } else { + body1.setAwake(); + } + } + } + + /** + * Вычисляет и сохраняет базис локальной системы координат контакта. + */ + public function calculateContactBasis():void { + // Нормаль является ортом X локальной системы координат + contactToWorld.a = contactNormal.x; + contactToWorld.e = contactNormal.y; + contactToWorld.i = contactNormal.z; + var nx:Number = contactNormal.x > 0 ? contactNormal.x : -contactNormal.x; + var ny:Number = contactNormal.y > 0 ? contactNormal.y : -contactNormal.y; + var s:Number; + if (nx > ny) { + // Нормаль ближе к глобальной оси X, используем ось Y для вычислений. Локальный орт Y находится как результат векторного произведения globalY x contactNormal. + s = 1/Math.sqrt(nx*nx + contactNormal.z*contactNormal.z); + contactToWorld.b = contactNormal.z*s; + contactToWorld.f = 0; + contactToWorld.j = -contactNormal.x*s; + // Локальный орт Z находится как результат векторного произведения contactNormal x localY. + contactToWorld.c = contactNormal.y*contactToWorld.j; + contactToWorld.g = contactNormal.z*contactToWorld.b - contactNormal.x*contactToWorld.j; + contactToWorld.k = -contactNormal.y*contactToWorld.b; + } else { + // Нормаль ближе к глобальной оси Y, используем ось X для вычислений. Локальный орт Y находится как результат векторного произведения globalX x contactNormal. + s = 1/Math.sqrt(ny*ny + contactNormal.z*contactNormal.z); + contactToWorld.b = 0; + contactToWorld.f = -contactNormal.z*s; + contactToWorld.j = contactNormal.y*s; + // Локальный орт Z находится как результат векторного произведения contactNormal x localY. + contactToWorld.c = contactNormal.y*contactToWorld.j - contactNormal.z*contactToWorld.f; + contactToWorld.g = -contactNormal.x*contactToWorld.j; + contactToWorld.k = contactNormal.x*contactToWorld.f; + } + } + + /** + * Вычисляет скорость точки заданного тела в точке контакта в его локальной системе координат. + * + * @param body тело, для которого выполняются вычисления + * @param relativeContactPosition положение точки контакта относительно тела + * @param time временной шаг + * @param result в эту переменную записывается результат + */ + public function calculateLocalVelocity(body:RigidBody, relativeContactPosition:Point3D, time:Number, result:Point3D):void { + // Расчёт прироста скорости за последний кадр от действующих сил + var x:Number = body.lastFrameAcceleration.x*time; + var y:Number = body.lastFrameAcceleration.y*time; + var z:Number = body.lastFrameAcceleration.z*time; + // Прирост скорости в системе координат контакта + // Убираем прирост скорости в направлении контакта + var accVelocityY:Number = contactToWorld.b*x + contactToWorld.f*y + contactToWorld.j*z; + var accVelocityZ:Number = contactToWorld.c*x + contactToWorld.g*y + contactToWorld.k*z; + + // Скорость точки контакта в мировой системе координат + // Скорость за счёт вращения тела + var rotation:Point3D = body.rotation; + x = rotation.y*relativeContactPosition.z - rotation.z*relativeContactPosition.y + body.velocity.x; + y = rotation.z*relativeContactPosition.x - rotation.x*relativeContactPosition.z + body.velocity.y; + z = rotation.x*relativeContactPosition.y - rotation.y*relativeContactPosition.x + body.velocity.z; + // Скорость точки контакта в системе координат контакта + // Касательные компоненты прироста скорости добавляются к скорости точки контакта. В дальнейшем они либо уберутся силой трения, + // либо уменьшатся ею. + result.x = contactToWorld.a*x + contactToWorld.e*y + contactToWorld.i*z; + result.y = contactToWorld.b*x + contactToWorld.f*y + contactToWorld.j*z + accVelocityY; + result.z = contactToWorld.c*x + contactToWorld.g*y + contactToWorld.k*z + accVelocityZ; + } + + /** + * Вычисляет и устанавливает внутреннее значение для требуемого изменения относительной скорости вдоль нормали контакта. + * С реакциями опоры разбираемся путём вычитания из скорости сближения прироста скорости, полученного во время последнего кадра. + * + * @param time + */ + public function calculateDesiredVelocity(time:Number):void { + // Вычисляем проекцию прироста скорости за текущий кадр на нормаль контакта. Для относительного прироста скорости, вызывающего + // сближение объектов, значение будет отрицательным. + var velocityFromAcc:Number = 0; + if (body1.awake) { + velocityFromAcc = (body1.lastFrameAcceleration.x*contactNormal.x + body1.lastFrameAcceleration.y*contactNormal.y + body1.lastFrameAcceleration.z*contactNormal.z)*time; + } + if (body2 != null && body2.awake) { + velocityFromAcc -= (body2.lastFrameAcceleration.x*contactNormal.x + body2.lastFrameAcceleration.y*contactNormal.y + body2.lastFrameAcceleration.z*contactNormal.z)*time; + } + // Если относительная скорость вдоль нормали контакта слишком мала, игнорируем её + var thisRestitution:Number = restitution; + if (contactVelocity.x > -velocityLimit && contactVelocity.x < velocityLimit) { + thisRestitution = 0; + } + // Вычисляем дельту для скорости отскока, убрав влияние прироста скорости за последний кадр + desiredDeltaVelocity = -contactVelocity.x - thisRestitution*(contactVelocity.x - velocityFromAcc); + } + + /** + * Расчитывает внутренние данные о контакта на основе данных о его состоянии. Вызывается до того, как будет запущен + * алгоритм разрешения контактов. Эта функция в нормальных условиях никогда не должна вызываться вручную. + * + * @param time + */ + public function calculateInternals(time:Number):void { + // Расчёт базиса системы координат контакта + // === inlined calculateContactBasis(); + contactToWorld.a = contactNormal.x; + contactToWorld.e = contactNormal.y; + contactToWorld.i = contactNormal.z; + var nx:Number = contactNormal.x > 0 ? contactNormal.x : -contactNormal.x; + var ny:Number = contactNormal.y > 0 ? contactNormal.y : -contactNormal.y; + var s:Number; + if (nx > ny) { + // Нормаль ближе к глобальной оси X, используем ось Y для вычислений. Локальный орт Y находится как результат векторного произведения globalY x contactNormal. + s = 1/Math.sqrt(nx*nx + contactNormal.z*contactNormal.z); + contactToWorld.b = contactNormal.z*s; + contactToWorld.f = 0; + contactToWorld.j = -contactNormal.x*s; + // Локальный орт Z находится как результат векторного произведения contactNormal x localY. + contactToWorld.c = contactNormal.y*contactToWorld.j; + contactToWorld.g = contactNormal.z*contactToWorld.b - contactNormal.x*contactToWorld.j; + contactToWorld.k = -contactNormal.y*contactToWorld.b; + } else { + // Нормаль ближе к глобальной оси Y, используем ось X для вычислений. Локальный орт Y находится как результат векторного произведения globalX x contactNormal. + s = 1/Math.sqrt(ny*ny + contactNormal.z*contactNormal.z); + contactToWorld.b = 0; + contactToWorld.f = -contactNormal.z*s; + contactToWorld.j = contactNormal.y*s; + // Локальный орт Z находится как результат векторного произведения contactNormal x localY. + contactToWorld.c = contactNormal.y*contactToWorld.j - contactNormal.z*contactToWorld.f; + contactToWorld.g = -contactNormal.x*contactToWorld.j; + contactToWorld.k = contactNormal.x*contactToWorld.f; + } + // === end inlined + + // Расчёт локальной скорости в точке контакта для первого тела + relativeContactPosition1.x = contactPoint.x - body1.position.x; + relativeContactPosition1.y = contactPoint.y - body1.position.y; + relativeContactPosition1.z = contactPoint.z - body1.position.z; + calculateLocalVelocity(body1, relativeContactPosition1, time, contactVelocity); + // Расчёт локальной скорости в точке контакта для второго тела + if (body2 != null) { + relativeContactPosition2.x = contactPoint.x - body2.position.x; + relativeContactPosition2.y = contactPoint.y - body2.position.y; + relativeContactPosition2.z = contactPoint.z - body2.position.z; + calculateLocalVelocity(body2, relativeContactPosition2, time, tmpVelocityVector); + // Расчёт относительной скорости в точке контакта + contactVelocity.x -= tmpVelocityVector.x; + contactVelocity.y -= tmpVelocityVector.y; + contactVelocity.z -= tmpVelocityVector.z; + } + // Расчёт вектора изменения скорости, требуемого для разрешения контакта + calculateDesiredVelocity(time); + } + + /** + * Выполняет разделение контакта посредством импульса, пропорционально инерции тел. + */ + public function applyVelocityChange(velocityChange1:Point3D, velocityChange2:Point3D, rotationChange1:Point3D, rotationChange2:Point3D):void { + var inverseInertiaTensor1:Matrix3D = body1.inverseInertiaTensorWorld; + var inverseInertiaTensor2:Matrix3D; + if (body2 != null) { + inverseInertiaTensor2 = body2.inverseInertiaTensorWorld; + } + + if (friction == 0) { + calculateFrictionlessImpulse(inverseInertiaTensor1, inverseInertiaTensor2, impulse); + } else { + calculateFrictionImpulse(inverseInertiaTensor1, inverseInertiaTensor2, impulse); + } + + // Переведём импульс в мировые координаты + var x:Number = impulse.x; + var y:Number = impulse.y; + var z:Number = impulse.z; + impulse.x = contactToWorld.a*x + contactToWorld.b*y + contactToWorld.c*z; + impulse.y = contactToWorld.e*x + contactToWorld.f*y + contactToWorld.g*z; + impulse.z = contactToWorld.i*x + contactToWorld.j*y + contactToWorld.k*z; + + // Разделим импульс на вращательную и линейную часть + x = relativeContactPosition1.y*impulse.z - relativeContactPosition1.z*impulse.y; + y = relativeContactPosition1.z*impulse.x - relativeContactPosition1.x*impulse.z; + z = relativeContactPosition1.x*impulse.y - relativeContactPosition1.y*impulse.x; + rotationChange1.x = inverseInertiaTensor1.a*x + inverseInertiaTensor1.b*y + inverseInertiaTensor1.c*z; + rotationChange1.y = inverseInertiaTensor1.e*x + inverseInertiaTensor1.f*y + inverseInertiaTensor1.g*z; + rotationChange1.z = inverseInertiaTensor1.i*x + inverseInertiaTensor1.j*y + inverseInertiaTensor1.k*z; + + velocityChange1.x = impulse.x*body1.inverseMass; + velocityChange1.y = impulse.y*body1.inverseMass; + velocityChange1.z = impulse.z*body1.inverseMass; + + body1.velocity.x += velocityChange1.x; + body1.velocity.y += velocityChange1.y; + body1.velocity.z += velocityChange1.z; + + body1.rotation.x += rotationChange1.x; + body1.rotation.y += rotationChange1.y; + body1.rotation.z += rotationChange1.z; + + if (body2 != null) { + x = relativeContactPosition2.z*impulse.y - relativeContactPosition2.y*impulse.z; + y = relativeContactPosition2.x*impulse.z - relativeContactPosition2.z*impulse.x; + z = relativeContactPosition2.y*impulse.x - relativeContactPosition2.x*impulse.y; + rotationChange2.x = inverseInertiaTensor2.a*x + inverseInertiaTensor2.b*y + inverseInertiaTensor2.c*z; + rotationChange2.y = inverseInertiaTensor2.e*x + inverseInertiaTensor2.f*y + inverseInertiaTensor2.g*z; + rotationChange2.z = inverseInertiaTensor2.i*x + inverseInertiaTensor2.j*y + inverseInertiaTensor2.k*z; + + velocityChange2.x = -impulse.x*body2.inverseMass; + velocityChange2.y = -impulse.y*body2.inverseMass; + velocityChange2.z = -impulse.z*body2.inverseMass; + + body2.velocity.x += velocityChange2.x; + body2.velocity.y += velocityChange2.y; + body2.velocity.z += velocityChange2.z; + + body2.rotation.x += rotationChange2.x; + body2.rotation.y += rotationChange2.y; + body2.rotation.z += rotationChange2.z; + } + } + + /** + * Вычисляет импульс, необходимый для коррекции скоростей в точке контакта в отсутствие сил трения. + */ + public function calculateFrictionlessImpulse(inverseInertiaTensor1:Matrix3D, inverseInertiaTensor2:Matrix3D, result:Point3D):void { + tmpVelocityVector.x = relativeContactPosition1.y*contactNormal.z - relativeContactPosition1.z*contactNormal.y; + tmpVelocityVector.y = relativeContactPosition1.z*contactNormal.x - relativeContactPosition1.x*contactNormal.z; + tmpVelocityVector.z = relativeContactPosition1.x*contactNormal.y - relativeContactPosition1.y*contactNormal.x; + + var x:Number = inverseInertiaTensor1.a*tmpVelocityVector.x + inverseInertiaTensor1.b*tmpVelocityVector.y + inverseInertiaTensor1.c*tmpVelocityVector.z; + var y:Number = inverseInertiaTensor1.e*tmpVelocityVector.x + inverseInertiaTensor1.f*tmpVelocityVector.y + inverseInertiaTensor1.g*tmpVelocityVector.z; + var z:Number = inverseInertiaTensor1.i*tmpVelocityVector.x + inverseInertiaTensor1.j*tmpVelocityVector.y + inverseInertiaTensor1.k*tmpVelocityVector.z; + + tmpVelocityVector.x = y*relativeContactPosition1.z - z*relativeContactPosition1.y; + tmpVelocityVector.y = z*relativeContactPosition1.x - x*relativeContactPosition1.z; + tmpVelocityVector.z = x*relativeContactPosition1.y - y*relativeContactPosition1.x; + + var deltaVelocity:Number = tmpVelocityVector.x*contactNormal.x + tmpVelocityVector.y*contactNormal.y + tmpVelocityVector.z*contactNormal.z + body1.inverseMass; + + if (body2 != null) { + tmpVelocityVector.x = relativeContactPosition2.y*contactNormal.z - relativeContactPosition2.z*contactNormal.y; + tmpVelocityVector.y = relativeContactPosition2.z*contactNormal.x - relativeContactPosition2.x*contactNormal.z; + tmpVelocityVector.z = relativeContactPosition2.x*contactNormal.y - relativeContactPosition2.y*contactNormal.x; + + x = inverseInertiaTensor2.a*tmpVelocityVector.x + inverseInertiaTensor2.b*tmpVelocityVector.y + inverseInertiaTensor2.c*tmpVelocityVector.z; + y = inverseInertiaTensor2.e*tmpVelocityVector.x + inverseInertiaTensor2.f*tmpVelocityVector.y + inverseInertiaTensor2.g*tmpVelocityVector.z; + z = inverseInertiaTensor2.i*tmpVelocityVector.x + inverseInertiaTensor2.j*tmpVelocityVector.y + inverseInertiaTensor2.k*tmpVelocityVector.z; + + tmpVelocityVector.x = y*relativeContactPosition2.z - z*relativeContactPosition2.y; + tmpVelocityVector.y = z*relativeContactPosition2.x - x*relativeContactPosition2.z; + tmpVelocityVector.z = x*relativeContactPosition2.y - y*relativeContactPosition2.x; + + deltaVelocity += tmpVelocityVector.x*contactNormal.x + tmpVelocityVector.y*contactNormal.y + tmpVelocityVector.z*contactNormal.z + body2.inverseMass; + } + + result.x = desiredDeltaVelocity/deltaVelocity; + result.y = 0; + result.z = 0; + } + + private var impulseToTorque:Matrix3D = new Matrix3D(); + private var deltaVelWorldMatrix:Matrix3D = new Matrix3D(); + private var deltaVelWorldMatrix2:Matrix3D = new Matrix3D(); + /** + * Вычисляет импульс, необходимый для коррекции скоростей в точке контакта при наличии сил трения. + */ + public function calculateFrictionImpulse(inverseInertiaTensor1:Matrix3D, inverseInertiaTensor2:Matrix3D, result:Point3D):void { + var inverseMass:Number = body1.inverseMass; + + relativeContactPosition1.createSkewSymmetricMatrix(impulseToTorque); + + deltaVelWorldMatrix.copy(impulseToTorque); + deltaVelWorldMatrix.inverseCombine(inverseInertiaTensor1); + deltaVelWorldMatrix.inverseCombine(impulseToTorque); + deltaVelWorldMatrix.multByScalar(-1); + + if (body2 != null) { + relativeContactPosition2.createSkewSymmetricMatrix(impulseToTorque); + deltaVelWorldMatrix2.copy(impulseToTorque); + deltaVelWorldMatrix2.inverseCombine(inverseInertiaTensor2); + deltaVelWorldMatrix2.inverseCombine(impulseToTorque); + deltaVelWorldMatrix2.multByScalar(-1); + + deltaVelWorldMatrix.add(deltaVelWorldMatrix2); + inverseMass += body2.inverseMass; + } + + deltaVelWorldMatrix2.copy(contactToWorld); + deltaVelWorldMatrix2.transpose(); + deltaVelWorldMatrix2.inverseCombine(deltaVelWorldMatrix); + deltaVelWorldMatrix2.inverseCombine(contactToWorld); + deltaVelWorldMatrix2.a += inverseMass; + deltaVelWorldMatrix2.f += inverseMass; + deltaVelWorldMatrix2.k += inverseMass; + + deltaVelWorldMatrix.copy(deltaVelWorldMatrix2); + deltaVelWorldMatrix.invert(); + + result.reset(desiredDeltaVelocity, -contactVelocity.y, -contactVelocity.z); + result.transform(deltaVelWorldMatrix); + + var planarImpulse:Number = Math.sqrt(result.y*result.y + result.z*result.z); + if (planarImpulse > result.x*friction) { + result.y /= planarImpulse; + result.z /= planarImpulse; + result.x = desiredDeltaVelocity/(deltaVelWorldMatrix2.a + deltaVelWorldMatrix2.b*friction*result.y + deltaVelWorldMatrix2.c*friction*result.z); + result.y *= friction*result.x; + result.z *= friction*result.x; + } + } + + private var angularInertiaWorld:Point3D = new Point3D(); + /** + * Выполняет разделение контакта с учётом инертности тел. + */ + public function applyPositionChange(linearChange1:Point3D, linearChange2:Point3D, angularChange1:Point3D, angularChange2:Point3D, penetration:Number):void { + var totalInertia:Number = 0; + var angularInertia1:Number; + var angularInertia2:Number; + var linearInertia1:Number; + var linearInertia2:Number; + + // body1 + angularInertiaWorld.x = relativeContactPosition1.y*contactNormal.z - relativeContactPosition1.z*contactNormal.y; + angularInertiaWorld.y = relativeContactPosition1.z*contactNormal.x - relativeContactPosition1.x*contactNormal.z; + angularInertiaWorld.z = relativeContactPosition1.x*contactNormal.y - relativeContactPosition1.y*contactNormal.x; + var m:Matrix3D = body1.inverseInertiaTensorWorld; + var x:Number = m.a*angularInertiaWorld.x + m.b*angularInertiaWorld.y + m.c*angularInertiaWorld.z; + var y:Number = m.e*angularInertiaWorld.x + m.f*angularInertiaWorld.y + m.g*angularInertiaWorld.z; + var z:Number = m.i*angularInertiaWorld.x + m.j*angularInertiaWorld.y + m.k*angularInertiaWorld.z; + angularInertiaWorld.x = y*relativeContactPosition1.z - z*relativeContactPosition1.y; + angularInertiaWorld.y = z*relativeContactPosition1.x - x*relativeContactPosition1.z; + angularInertiaWorld.z = x*relativeContactPosition1.y - y*relativeContactPosition1.x; + + angularInertia1 = angularInertiaWorld.x*contactNormal.x + angularInertiaWorld.y*contactNormal.y + angularInertiaWorld.z*contactNormal.z; + linearInertia1 = body1.inverseMass; + + totalInertia = linearInertia1 + angularInertia1; + + if (body2 != null) { + angularInertiaWorld.x = relativeContactPosition2.y*contactNormal.z - relativeContactPosition2.z*contactNormal.y; + angularInertiaWorld.y = relativeContactPosition2.z*contactNormal.x - relativeContactPosition2.x*contactNormal.z; + angularInertiaWorld.z = relativeContactPosition2.x*contactNormal.y - relativeContactPosition2.y*contactNormal.x; + m = body2.inverseInertiaTensorWorld; + x = m.a*angularInertiaWorld.x + m.b*angularInertiaWorld.y + m.c*angularInertiaWorld.z; + y = m.e*angularInertiaWorld.x + m.f*angularInertiaWorld.y + m.g*angularInertiaWorld.z; + z = m.i*angularInertiaWorld.x + m.j*angularInertiaWorld.y + m.k*angularInertiaWorld.z; + angularInertiaWorld.x = y*relativeContactPosition2.z - z*relativeContactPosition2.y; + angularInertiaWorld.y = z*relativeContactPosition2.x - x*relativeContactPosition2.z; + angularInertiaWorld.z = x*relativeContactPosition2.y - y*relativeContactPosition2.x; + + angularInertia2 = angularInertiaWorld.x*contactNormal.x + angularInertiaWorld.y*contactNormal.y + angularInertiaWorld.z*contactNormal.z; + linearInertia2 = body2.inverseMass; + + totalInertia += linearInertia2 + angularInertia2; + } + + // body1 + applyPostionChangeToBody(body1, relativeContactPosition1, angularInertia1, linearInertia1, totalInertia, 1, angularChange1, linearChange1); + + if (body2 != null) { + applyPostionChangeToBody(body2, relativeContactPosition2, angularInertia2, linearInertia2, totalInertia, -1, angularChange2, linearChange2); + } + } + + private var projection:Point3D = new Point3D(); + /** + * + * @param body + * @param relativeContactPosition + * @param angularInertia + * @param linearInertia + * @param totalInertia + * @param sign + * @param angularChange + * @param linearChange + */ + private function applyPostionChangeToBody(body:RigidBody, relativeContactPosition:Point3D, angularInertia:Number, linearInertia:Number, totalInertia:Number, sign:Number, angularChange:Point3D, linearChange:Point3D):void { + var angularLimit:Number = 0.2; + var angularMove:Number = sign*penetration*angularInertia/totalInertia; + var linearMove:Number = sign*penetration*linearInertia/totalInertia; + + projection.copy(contactNormal); + projection.multiply(-relativeContactPosition.dot(contactNormal)); + projection.add(relativeContactPosition); + + var maxMagnitude:Number = angularLimit*projection.length; + + var totalMove:Number; + if (angularMove < -maxMagnitude) { + totalMove = angularMove + linearMove; + angularMove = -maxMagnitude; + linearMove = totalMove - angularMove; + } else if (angularMove > maxMagnitude) { + totalMove = angularMove + linearMove; + angularMove = maxMagnitude; + linearMove = totalMove - angularMove; + } + + if (angularMove == 0) { + angularChange.reset(); + } else { + angularChange.copy(relativeContactPosition); + angularChange.cross(contactNormal); + angularChange.transform(body.inverseInertiaTensorWorld); + angularChange.multiply(angularMove/angularInertia); + } + + linearChange.copy(contactNormal); + linearChange.multiply(linearMove); + + body.position.add(linearChange); + body.orientation.addScaledVector(angularChange, 1); + if (!body.awake) { + body.calculateDerivedData(); + } + } + + public function toString():String { + return "RigidBodyContact" + + "\n timeStamp: " + timeStamp + + "\n body1: " + body1.getName() + + "\n body2: " + (body2 == null ? "" : body2.getName()) + + "\n contactPoint: " + contactPoint + + "\n contactNormal: " + contactNormal + + "\n restitution: " + restitution + + "\n penetration: " + penetration + + "\n friction: " + friction; + } + + } +} diff --git a/0.0.0.18/src/alternativa/physics/rigid/RigidBodyContactGenerator.as b/0.0.0.18/src/alternativa/physics/rigid/RigidBodyContactGenerator.as new file mode 100644 index 0000000..18dfb88 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/rigid/RigidBodyContactGenerator.as @@ -0,0 +1,22 @@ +package alternativa.physics.rigid { + + /** + * Генератор контактов для твёрдых тел. + */ + public class RigidBodyContactGenerator { + /** + * Следующий генератор в списке. + */ + public var next:RigidBodyContactGenerator; + + /** + * Метод создаёт контакты. + * + * @param contact первый свободный для записи контакт в списке контактов мира + * @return следующий свободный для записи контакт после окончания работы метода. Значение null означает, что доступные контакты исчерпаны. + */ + public function addContacts(contact:RigidBodyContact):RigidBodyContact { + return null; + } + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/rigid/RigidBodyContactResolver.as b/0.0.0.18/src/alternativa/physics/rigid/RigidBodyContactResolver.as new file mode 100644 index 0000000..7f6df6c --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/rigid/RigidBodyContactResolver.as @@ -0,0 +1,237 @@ +package alternativa.physics.rigid { + + import alternativa.physics.*; + import alternativa.types.Point3D; + + use namespace altphysics; + + /** + * + */ + public class RigidBodyContactResolver { + + private var velocityIterations:int; + private var positionIterations:int; + private var velocityEpsilon:Number; + private var positionEpsilon:Number; + + private var velocityIterationsUsed:int; + private var positionIterationsUsed:int; + + private var linearChange1:Point3D = new Point3D(); + private var linearChange2:Point3D = new Point3D(); + private var angularChange1:Point3D = new Point3D(); + private var angularChange2:Point3D = new Point3D(); + + private var velocityChange1:Point3D = new Point3D(); + private var velocityChange2:Point3D = new Point3D(); + private var rotationChange1:Point3D = new Point3D(); + private var rotationChange2:Point3D = new Point3D(); + + private var deltaVector:Point3D = new Point3D(); + + /** + * + * @param velocityIterations + * @param positionIterations + * @param velocityEpsilon + * @param positionEpsilon + */ + public function RigidBodyContactResolver(velocityIterations:int, positionIterations:int, velocityEpsilon:Number = 0.001, positionEpsilon:Number = 0.001) { + this.velocityIterations = velocityIterations; + this.positionIterations = positionIterations; + this.velocityEpsilon = velocityEpsilon; + this.positionEpsilon = positionEpsilon; + } + + /** + * + * @param velocityIterations + * @param positionEpsilon + */ + public function setIterations(velocityIterations:int, positionIterations:int):void { + this.velocityIterations = velocityIterations; + this.positionIterations = positionIterations; + } + + /** + * + * @return + */ + private function isValid():Boolean { + return (positionIterations > 0) && (velocityIterations > 0) && (positionEpsilon >= 0) && (velocityEpsilon >= 0); + } + + /** + * + * @param firstContact + * @param numContacts + * @param time + */ + public function resolveContacts(firstContact:RigidBodyContact, numContacts:int, time:Number):void { + if (numContacts == 0 || !isValid()) { + return; + } + // Подготовка контактов к обработке + prepareContacts(firstContact, numContacts, time); + // Разделение взаимно проникающих контактов + adjustPositions(firstContact, numContacts, time); + // Коррекция скорости для контактов + adjustVelocities(firstContact, numContacts, time); + } + + /** + * + * @param firstContact + * @param numContacts + * @param time + */ + public function prepareContacts(firstContact:RigidBodyContact, numContacts:int, time:Number):void { + var contact:RigidBodyContact = firstContact; + while (contact.index < numContacts) { + // Вычисление внутренних данных контакта, необходимых для дальнейшей работы + contact.calculateInternals(time); + contact = contact.next; + } + } + + /** + * + * @param firstContact + * @param numContacts + * @param time + */ + public function adjustPositions(firstContact:RigidBodyContact, numContacts:int, time:Number):void { + positionIterationsUsed = 0; + // Итерационное устранение взаимного проникновения тел в точках контактов в порядке наибольшего пересечения тел + while (positionIterationsUsed < positionIterations) { + var max:Number = positionEpsilon; + var mostSevereContact:RigidBodyContact = null; + // Поиск контакта с наибольшим пересечением + var contact:RigidBodyContact = firstContact; + while (contact.index < numContacts) { + if (contact.penetration > max) { + max = contact.penetration; + mostSevereContact = contact; + } + contact = contact.next; + } + if (mostSevereContact == null) { + // Алгоритм прерывается, если не осталось пересечений + return; + } + // Будим спящего + mostSevereContact.matchAwakeState(); + + // Разделяем контакт + mostSevereContact.applyPositionChange(linearChange1, linearChange2, angularChange1, angularChange2, max); + // Обновляем величину проникновения в каждом контакте, содержащем обработанные объекты + contact = firstContact; + while (contact.index < numContacts) { + if (contact.body1 == mostSevereContact.body1) { + deltaVector.copy(angularChange1); + deltaVector.cross(contact.relativeContactPosition1); + deltaVector.add(linearChange1); + contact.penetration -= deltaVector.dot(contact.contactNormal); + } + if (contact.body1 == mostSevereContact.body2) { + deltaVector.copy(angularChange2); + deltaVector.cross(contact.relativeContactPosition1); + deltaVector.add(linearChange2); + contact.penetration -= deltaVector.dot(contact.contactNormal); + } + if (contact.body2 != null) { + if (contact.body2 == mostSevereContact.body1) { + deltaVector.copy(angularChange1); + deltaVector.cross(contact.relativeContactPosition2); + deltaVector.add(linearChange1); + contact.penetration += deltaVector.dot(contact.contactNormal); + } + if (contact.body2 == mostSevereContact.body2) { + deltaVector.copy(angularChange2); + deltaVector.cross(contact.relativeContactPosition2); + deltaVector.add(linearChange2); + contact.penetration += deltaVector.dot(contact.contactNormal); + } + } + contact = contact.next; + } + positionIterationsUsed++; + } + } + + /** + * + * @param firstContact + * @param numContacts + * @param time + */ + public function adjustVelocities(firstContact:RigidBodyContact, numContacts:int, time:Number):void { + velocityIterationsUsed = 0; + while (velocityIterationsUsed < velocityIterations) { + var max:Number = velocityEpsilon; + var mostSevereContact:RigidBodyContact = null; + var contact:RigidBodyContact = firstContact; + while (contact.index < numContacts) { + if (contact.desiredDeltaVelocity > max) { + max = contact.desiredDeltaVelocity; + mostSevereContact = contact; + } + contact = contact.next; + } + if (mostSevereContact == null) { + return; + } + // Будим спящих + mostSevereContact.matchAwakeState(); + // Корректируем скорости + mostSevereContact.applyVelocityChange(velocityChange1, velocityChange2, rotationChange1, rotationChange2); + // Обновляем скорости сближения для каждого контакта, содержащего обработанные объекты + contact = firstContact; + var needRecalculation:Boolean = false; + while (contact.index < numContacts) { + if (contact.body1 == mostSevereContact.body1) { + deltaVector.copy(rotationChange1); + deltaVector.cross(contact.relativeContactPosition1); + deltaVector.add(velocityChange1); + deltaVector.transformTranspose(contact.contactToWorld); + contact.contactVelocity.add(deltaVector); + needRecalculation = true; + } + if (contact.body1 == mostSevereContact.body2) { + deltaVector.copy(rotationChange2); + deltaVector.cross(contact.relativeContactPosition1); + deltaVector.add(velocityChange2); + deltaVector.transformTranspose(contact.contactToWorld); + contact.contactVelocity.add(deltaVector); + needRecalculation = true; + } + if (contact.body2 != null) { + if (contact.body2 == mostSevereContact.body1) { + deltaVector.copy(rotationChange1); + deltaVector.cross(contact.relativeContactPosition2); + deltaVector.add(velocityChange1); + deltaVector.transformTranspose(contact.contactToWorld); + contact.contactVelocity.subtract(deltaVector); + needRecalculation = true; + } + if (contact.body2 == mostSevereContact.body2) { + deltaVector.copy(rotationChange2); + deltaVector.cross(contact.relativeContactPosition2); + deltaVector.add(velocityChange2); + deltaVector.transformTranspose(contact.contactToWorld); + contact.contactVelocity.subtract(deltaVector); + needRecalculation = true; + } + } + if (needRecalculation) { + contact.calculateDesiredVelocity(time); + } + contact = contact.next; + } + velocityIterationsUsed++; + } + } + + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/rigid/RigidWorld.as b/0.0.0.18/src/alternativa/physics/rigid/RigidWorld.as new file mode 100644 index 0000000..def9635 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/rigid/RigidWorld.as @@ -0,0 +1,136 @@ +package alternativa.physics.rigid { + + public class RigidWorld { + + private var firstBody:RigidBody; + private var lastBody:RigidBody; + private var contactGenerators:RigidBodyContactGenerator; + private var lastContactGenerator:RigidBodyContactGenerator; + + public var contacts:RigidBodyContact; + private var maxContacts:int; + private var resolver:RigidBodyContactResolver; + + public var forceRegistry:BodyForceRegistry = new BodyForceRegistry(); + public var calculateIterations:Boolean; + + public var numContacts:int; + + public function RigidWorld(maxContacts:int, iterations:int) { + this.maxContacts = maxContacts; + resolver = new RigidBodyContactResolver(iterations, iterations); + createContactList(maxContacts); + } + + private function createContactList(numContacts:int):void { + if (numContacts > 0) { + var contact:RigidBodyContact = contacts = new RigidBodyContact(0); + for (var i:int = 1; i < numContacts; i++) { + contact.next = new RigidBodyContact(i); + contact = contact.next; + } + } + } + + public function addBody(body:RigidBody):void { + if (firstBody == null) { + firstBody = body; + } else { + lastBody.next = body; + } + lastBody = body; + while (lastBody.next != null) { + lastBody = lastBody.next; + } + } + + public function removeBody(body:RigidBody):void { + if (body == firstBody) { + firstBody = firstBody.next; + body.next = null; + } else { + var current:RigidBody = firstBody; + var prev:RigidBody = null; + while (current != body && current != null) { + prev = current; + current = current.next; + } + if (current != null) { + if (lastBody == body) { + lastBody = prev; + } + prev.next = current.next; + current.next = null; + } + } + } + + public function addContactGenerator(gen:RigidBodyContactGenerator):void { + if (contactGenerators == null) { + contactGenerators = gen; + } else { + lastContactGenerator.next = gen; + } + lastContactGenerator = gen; + while (lastContactGenerator.next != null) { + lastContactGenerator = lastContactGenerator.next; + } + } + + public function removeContactGenerator(gen:RigidBodyContactGenerator):void { + if (gen == contactGenerators) { + contactGenerators = gen.next; + gen.next = null; + } else { + var prev:RigidBodyContactGenerator = contactGenerators; + var curr:RigidBodyContactGenerator = contactGenerators.next; + while (curr != gen && curr != null) { + prev = curr; + curr = curr.next; + } + if (curr == gen) { + if (lastContactGenerator == gen) { + lastContactGenerator = prev; + } + prev.next = curr.next; + curr.next = null; + } + } + } + + public function startFrame():void { + var body:RigidBody = firstBody; + while (body != null) { + body.clearAccumulators(); + body.calculateDerivedData(); + body = body.next; + } + } + + private function generateContacts():int { + var contact:RigidBodyContact = contacts; + var gen:RigidBodyContactGenerator = contactGenerators; + while (gen != null && contact != null) { + contact = gen.addContacts(contact); + gen = gen.next; + } + return contact == null ? maxContacts : contact.index; + } + + public function runPhysics(time:Number):void { + forceRegistry.updateForces(time); + var body:RigidBody = firstBody; + while (body != null) { + body.integrate(time); + body = body.next; + } + numContacts = generateContacts(); +// trace("[RigidWorld.runPhysics] numContacts", numContacts); + if (calculateIterations) { + resolver.setIterations(4*numContacts, 4*numContacts); + } + resolver.resolveContacts(contacts, numContacts, time); + } + + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/rigid/generators/.svn/all-wcprops b/0.0.0.18/src/alternativa/physics/rigid/generators/.svn/all-wcprops new file mode 100644 index 0000000..3171901 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/rigid/generators/.svn/all-wcprops @@ -0,0 +1,35 @@ +K 25 +svn:wc:ra_dav:version-url +V 111 +/!svn/ver/4732/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/rigid/generators +END +AnchoredCableContactGenerator.as +K 25 +svn:wc:ra_dav:version-url +V 144 +/!svn/ver/4732/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/rigid/generators/AnchoredCableContactGenerator.as +END +RigidBoxCollisionData.as +K 25 +svn:wc:ra_dav:version-url +V 136 +/!svn/ver/4385/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/rigid/generators/RigidBoxCollisionData.as +END +BoxWithBoxContactGenerator.as +K 25 +svn:wc:ra_dav:version-url +V 141 +/!svn/ver/4403/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/rigid/generators/BoxWithBoxContactGenerator.as +END +SphereWithPlaneContactGenerator.as +K 25 +svn:wc:ra_dav:version-url +V 146 +/!svn/ver/4385/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/rigid/generators/SphereWithPlaneContactGenerator.as +END +RigidBox.as +K 25 +svn:wc:ra_dav:version-url +V 123 +/!svn/ver/4603/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/rigid/generators/RigidBox.as +END diff --git a/0.0.0.18/src/alternativa/physics/rigid/generators/.svn/entries b/0.0.0.18/src/alternativa/physics/rigid/generators/.svn/entries new file mode 100644 index 0000000..5fc7886 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/rigid/generators/.svn/entries @@ -0,0 +1,89 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp9/libraries/AlternativaPhysics/trunk/src/alternativa/physics/rigid/generators +http://svndev.alternativaplatform.com + + + +2008-12-03T05:15:16.843707Z +4732 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +AnchoredCableContactGenerator.as +file + + + + +2010-10-28T04:34:31.000000Z +aa5b3540cfe5796ebad67e11f47a4aa9 +2008-12-03T05:15:16.843707Z +4732 +mike + +RigidBoxCollisionData.as +file + + + + +2010-10-28T04:34:31.000000Z +607a1d12afedbe3344ae967c6f599fd4 +2008-11-26T13:17:08.913613Z +4385 +mike + +BoxWithBoxContactGenerator.as +file + + + + +2010-10-28T04:34:31.000000Z +47da766c6f7ca03fb1622378c740abb4 +2008-11-26T16:00:07.127812Z +4403 +mike + +SphereWithPlaneContactGenerator.as +file + + + + +2010-10-28T04:34:31.000000Z +558aa2a28f3cc4a3932c19e5e2ab230c +2008-11-26T13:17:08.913613Z +4385 +mike +has-props + +RigidBox.as +file + + + + +2010-10-28T04:34:31.000000Z +505d9996c9461cc769d2accf7295c871 +2008-11-30T13:03:39.561308Z +4603 +mike + diff --git a/0.0.0.18/src/alternativa/physics/rigid/generators/.svn/format b/0.0.0.18/src/alternativa/physics/rigid/generators/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/rigid/generators/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.0.18/src/alternativa/physics/rigid/generators/.svn/prop-base/SphereWithPlaneContactGenerator.as.svn-base b/0.0.0.18/src/alternativa/physics/rigid/generators/.svn/prop-base/SphereWithPlaneContactGenerator.as.svn-base new file mode 100644 index 0000000..3160658 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/rigid/generators/.svn/prop-base/SphereWithPlaneContactGenerator.as.svn-base @@ -0,0 +1,5 @@ +K 13 +svn:mergeinfo +V 0 + +END diff --git a/0.0.0.18/src/alternativa/physics/rigid/generators/.svn/text-base/AnchoredCableContactGenerator.as.svn-base b/0.0.0.18/src/alternativa/physics/rigid/generators/.svn/text-base/AnchoredCableContactGenerator.as.svn-base new file mode 100644 index 0000000..32e6169 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/rigid/generators/.svn/text-base/AnchoredCableContactGenerator.as.svn-base @@ -0,0 +1,56 @@ +package alternativa.physics.rigid.generators { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.RigidBody; + import alternativa.physics.rigid.RigidBodyContact; + import alternativa.physics.rigid.RigidBodyContactGenerator; + import alternativa.types.Point3D; + + use namespace altphysics; + + public class AnchoredCableContactGenerator extends RigidBodyContactGenerator { + + private var body:RigidBody; + private var mountPoint:Point3D; + private var _anchorPoint:Point3D; + private var cableLength:Number; + private var restitution:Number; + + private var vector:Point3D = new Point3D(); + private var point:Point3D = new Point3D(); + + public function AnchoredCableContactGenerator(body:RigidBody, mountPoint:Point3D, anchorPoint:Point3D, cableLength:Number, restitution:Number) { + super(); + this.body = body; + this.mountPoint = mountPoint.clone(); + _anchorPoint = anchorPoint.clone(); + this.cableLength = cableLength; + this.restitution = restitution; + } + + public function set anchorPoint(value:Point3D):void { + _anchorPoint.copy(value); + } + + override public function addContacts(contact:RigidBodyContact):RigidBodyContact { + point.copy(mountPoint); + point.transform(body.transformMatrix); + vector.difference(_anchorPoint, point); + var len:Number = vector.length; + var pen:Number = len - cableLength; + if (pen > 0) { + vector.multiply(1/len); + contact.body1 = body; + contact.body2 = null; + contact.contactNormal.copy(vector); + contact.contactPoint.copy(point); + contact.restitution = restitution; + contact.penetration = pen; + contact.friction = 0; + return contact.next; + } + return contact; + } + + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/rigid/generators/.svn/text-base/BoxWithBoxContactGenerator.as.svn-base b/0.0.0.18/src/alternativa/physics/rigid/generators/.svn/text-base/BoxWithBoxContactGenerator.as.svn-base new file mode 100644 index 0000000..2a51fdb --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/rigid/generators/.svn/text-base/BoxWithBoxContactGenerator.as.svn-base @@ -0,0 +1,319 @@ +package alternativa.physics.rigid.generators { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionBox; + import alternativa.physics.collision.CollisionData; + import alternativa.physics.collision.CollisionDetector; + import alternativa.physics.rigid.RigidBodyContact; + import alternativa.physics.rigid.RigidBodyContactGenerator; + import alternativa.types.Matrix3D; + import alternativa.types.Point3D; + + use namespace altphysics; + + public class BoxWithBoxContactGenerator extends RigidBodyContactGenerator { + + private var boxes:RigidBox; + private var collisionDetector:CollisionDetector; + private var collisionData:RigidBoxCollisionData = RigidBoxCollisionData.create(); + private var restitution:Number; + private var friction:Number; + + private var tolerance:Number = -0.01; + + public function BoxWithBoxContactGenerator(boxes:RigidBox, collisionDetector:CollisionDetector, restitution:Number, friction:Number) { + super(); + this.boxes = boxes; + this.collisionDetector = collisionDetector; + this.restitution = restitution; + this.friction = friction; + } + + public function getBoxes():RigidBox { + return boxes; + } + + public function setBoxes(boxes:RigidBox):void { + this.boxes = boxes; + } + + public function getBoxesCount():int { + var counter:int = 0; + var box:RigidBox = boxes; + while (box != null) { + counter++; + box = box.next; + } + return counter; + } + + private function updateCillisionBoxes():void { + var box:RigidBox = boxes; + while (box != null) { + box.collisionBox.updateTransform(); + box = box.next; + } + } + + override public function addContacts(contact:RigidBodyContact):RigidBodyContact { + updateCillisionBoxes(); + + var box:RigidBox = boxes; + var otherBox:RigidBox; + var key:*; + var cachedCollisionData:RigidBoxCollisionData; + while (box != null) { + otherBox = box.next; + while (otherBox != null) { + var contactExists:Boolean = collisionDetector.boxToBoxCollision(box.collisionBox, otherBox.collisionBox, collisionData); + collisionData.otherBox = otherBox; + if (contactExists) { + var cached:Boolean = false; + // Поиск и обновление полученного контакта в кэше + for (key in box.boxCollisionCache) { + cachedCollisionData = key; + if (!cachedCollisionData.fresh && cachedCollisionData.equals(collisionData)) { + cached = true; + cachedCollisionData.collisionNormal.copy(collisionData.collisionNormal); + cachedCollisionData.collisionPoint.copy(collisionData.collisionPoint); + cachedCollisionData.penetration = collisionData.penetration; + break; + } + } + if (!cached) { + // В кэше не оказалось, добавляется новый контакт + box.boxCollisionCache[collisionData] = true; + collisionData = RigidBoxCollisionData.create(); + } + } + otherBox = otherBox.next as RigidBox; + } + + // Создание контактов из кэша + var counter:int; + for (key in box.boxCollisionCache) { + cachedCollisionData = key; + if (cachedCollisionData.fresh) { + cachedCollisionData.fresh = false; + } else { + // Проверка актуальности старых контактов + var valid:Boolean = checkCachedContact(cachedCollisionData); + if (!valid) { + delete box.boxCollisionCache[key]; + RigidBoxCollisionData.destroy(cachedCollisionData); + continue; + } + cachedCollisionData.timeStamp++; + } + // Создание контакта + contact.timeStamp = cachedCollisionData.timeStamp; + contact.body1 = box.body; + contact.body2 = cachedCollisionData.otherBox.body; + contact.contactNormal.copy(cachedCollisionData.collisionNormal); + contact.contactPoint.copy(cachedCollisionData.collisionPoint); + contact.penetration = cachedCollisionData.penetration; + contact.friction = friction; + contact.restitution = restitution; + contact = contact.next; + if (contact == null) { + return null; + } + } + + box = box.next; + } + + return contact; + } + + private var _axis1:Point3D = new Point3D(); + private var _axis2:Point3D = new Point3D(); + private var _axis3:Point3D = new Point3D(); + private var _toFirst:Point3D = new Point3D(); + private var _point1:Point3D = new Point3D(); + private var _point2:Point3D = new Point3D(); + private var _collisionPoint:Point3D = new Point3D(); + /** + * + * @param data + * @return + */ + private function checkCachedContact(data:CollisionData):Boolean { + var box1:CollisionBox = data.primitive1 as CollisionBox; + var box2:CollisionBox = data.primitive2 as CollisionBox; + var axisFlag1:int = data.pointAxisCode1 & 32; + var axisFlag2:int = data.pointAxisCode2 & 32; + var axisIndex1:int; + var axisIndex2:int; + var penetration:Number; + _toFirst.difference(box1.position, box2.position); + + if (axisFlag1 == axisFlag2) { + // Столкновение вдоль вторичной оси + axisIndex1 = (data.pointAxisCode1 >> 3) & 3; + box1.transform.getAxis(axisIndex1, _axis1); + axisIndex2 = (data.pointAxisCode2 >> 3) & 3; + box2.transform.getAxis(axisIndex2, _axis2); + _axis3.cross2(_axis1, _axis2); + _axis3.normalize(); + var overlap:Number = collisionDetector.overlapOnAxis(box1, box2, _axis3, _toFirst); + if (overlap < 0) { + return false; + } else { + penetration = getEdgesPenetrationAndCollisionPoint(box1, box2, _axis1, axisIndex1, data.pointAxisCode1 & 7, _axis2, axisIndex2, data.pointAxisCode2 & 7, _toFirst, _collisionPoint); + if (penetration < tolerance) { + return false; + } + // Обновляем контакт + if (_toFirst.dot(_axis3) < 0) { + _axis3.invert(); + } + data.collisionNormal.copy(_axis3); + data.collisionPoint.copy(_collisionPoint); + data.penetration = penetration > 0 ? penetration : 0; + return true; + } + } else { + // Столкновение вдоль основной оси + // TODO: Доделать проверку на попадание вершины внутрь бокса + if (axisFlag1 != 0) { + // Столкновение вершины второго бокса с гранью первого + _toFirst.invert(); + axisIndex1 = (data.pointAxisCode1 >> 3) & 3; + penetration = getPointBoxPenetrationAndCollisionPoint(box2, box1, data.pointAxisCode2 & 7, axisIndex1, _toFirst, _collisionPoint, _axis1); + _axis1.invert(); + } else { + // Столкновение вершины первого бокса с гранью второго + axisIndex2 = (data.pointAxisCode2 >> 3) & 3; + penetration = getPointBoxPenetrationAndCollisionPoint(box1, box2, data.pointAxisCode1 & 7, axisIndex2, _toFirst, _collisionPoint, _axis1); + } + if (penetration > tolerance) { + data.collisionNormal.copy(_axis1); + data.collisionPoint.copy(_collisionPoint); + data.penetration = penetration > 0 ? penetration : 0; + } else { + return false; + } + } + return true; + } + + /** + * + */ + private function getEdgesPenetrationAndCollisionPoint(box1:CollisionBox, box2:CollisionBox, axis1:Point3D, axisIndex1:int, pointIndex1:int, axis2:Point3D, axisIndex2:int, pointIndex2:int, toFirst:Point3D, collisionPoint:Point3D):Number { + _point1.x = (pointIndex1 & 1) == 0 ? box1.halfSize.x : -box1.halfSize.x; + _point1.y = (pointIndex1 & 2) == 0 ? box1.halfSize.y : -box1.halfSize.y; + _point1.z = (pointIndex1 & 4) == 0 ? box1.halfSize.z : -box1.halfSize.z; + _point1.transform(box1.transform); + + _point2.x = (pointIndex2 & 1) == 0 ? box2.halfSize.x : -box2.halfSize.x; + _point2.y = (pointIndex2 & 2) == 0 ? box2.halfSize.y : -box2.halfSize.y; + _point2.z = (pointIndex2 & 4) == 0 ? box2.halfSize.z : -box2.halfSize.z; + _point2.transform(box2.transform); + + var k:Number = axis1.dot(axis2); + var det:Number = k*k - 1; + if (det > -0.0001 && det < 0.0001) { + // Число по модулю должно быть быть больше, чем tolerance + return -100; + } else { + collisionPoint.difference(_point2, _point1); + var c1:Number = axis1.dot(collisionPoint); + var c2:Number = axis2.dot(collisionPoint); + var t1:Number = (c2*k - c1)/det; + var size:Number = 2*(axisIndex1 == 0 ? box1.halfSize.x : (axisIndex1 == 1 ? box1.halfSize.y : box1.halfSize.z)); + if (t1 < 0 || t1 > size) { + return -100; + } else { + var t2:Number = (c2 - c1*k)/det; + size = 2*(axisIndex2 == 0 ? box2.halfSize.x : (axisIndex2 == 1 ? box2.halfSize.y : box2.halfSize.z)); + if (t2 < 0 || t2 > size) { + return -100; + } else { + _point1.x += _axis1.x*t1; + _point1.y += _axis1.y*t1; + _point1.z += _axis1.z*t1; + _point2.x += _axis2.x*t2; + _point2.y += _axis2.y*t2; + _point2.z += _axis2.z*t2; + collisionPoint.x = 0.5*(_point1.x + _point2.x); + collisionPoint.y = 0.5*(_point1.y + _point2.y); + collisionPoint.z = 0.5*(_point1.z + _point2.z); + _point1.subtract(_point2); + return _point1.dot(toFirst) > 0 ? -_point1.length : _point1.length; + } + } + } + + return -100; + } + + /** + * + * @param box1 бокс, вершина которого столкнулась с гранью второго бокса + * @param box2 бокс, грань которого столкнулась с вершиной второго бокса + * @param axisIndex1 + * @param pointIndex2 + * @param toCenter + * @param collisionPoint + * @return + */ + private function getPointBoxPenetrationAndCollisionPoint(box1:CollisionBox, box2:CollisionBox, pointIndex1:int, axisIndex2:int, toFirst:Point3D, collisionPoint:Point3D, collisionNormal:Point3D):Number { + // Локальные кординаты вершины + _point1.x = (pointIndex1 & 1) == 0 ? box1.halfSize.x : -box1.halfSize.x; + _point1.y = (pointIndex1 & 2) == 0 ? box1.halfSize.y : -box1.halfSize.y; + _point1.z = (pointIndex1 & 4) == 0 ? box1.halfSize.z : -box1.halfSize.z; + + // размер второго бокса вдоль оси столкновения + var size:Number; + if (axisIndex2 == 0) { + size = box2.halfSize.x; + } else if (axisIndex2 == 1) { + size = box2.halfSize.y; + } else { + size = box2.halfSize.z; + } + box2.transform.getAxis(axisIndex2, collisionNormal); + var distance:Number = collisionNormal.dot(toFirst); + if (distance < 0) { + distance = -distance; + collisionNormal.invert(); + } + var m:Matrix3D = box1.transform; + collisionPoint.x = m.a*_point1.x + m.b*_point1.y + m.c*_point1.z; + collisionPoint.y = m.e*_point1.x + m.f*_point1.y + m.g*_point1.z; + collisionPoint.z = m.i*_point1.x + m.j*_point1.y + m.k*_point1.z; + var projection:Number = collisionNormal.dot(collisionPoint); + var overlap:Number = size - projection - distance; + if (overlap < tolerance) { + return -100; + } + + // Глобальные координаты вершины + collisionPoint.x += m.d; + collisionPoint.y += m.h; + collisionPoint.z += m.l; + + // Координтаы вершины в системе второго бокса + _point2.copy(collisionPoint); + _point2.inverseTransform(box2.transform); + // Проверка на принадлежность вершины боксу + if (_point2.x < 0) { + _point2.x = -_point2.x; + } + if (_point2.y < 0) { + _point2.y = -_point2.y; + } + if (_point2.y < 0) { + _point2.y = -_point2.y; + } + if ((_point2.x + tolerance > box2.halfSize.x) || (_point2.y + tolerance > box2.halfSize.y) || (_point2.z + tolerance > box2.halfSize.z)) { + return -100; + } + + return overlap; + } + + } +} diff --git a/0.0.0.18/src/alternativa/physics/rigid/generators/.svn/text-base/RigidBox.as.svn-base b/0.0.0.18/src/alternativa/physics/rigid/generators/.svn/text-base/RigidBox.as.svn-base new file mode 100644 index 0000000..d83b18a --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/rigid/generators/.svn/text-base/RigidBox.as.svn-base @@ -0,0 +1,55 @@ +package alternativa.physics.rigid.generators { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionBox; + import alternativa.physics.rigid.RigidBody; + import alternativa.types.Matrix3D; + import alternativa.types.Point3D; + + import flash.utils.Dictionary; + + use namespace altphysics; + + public class RigidBox { + + public var body:RigidBody; + public var collisionBox:CollisionBox; + public var radius:Number; + + public var boxCollisionCache:Dictionary = new Dictionary(); + public var next:RigidBox; + + private var point:Point3D = new Point3D(); + + public function RigidBox(width:Number, length:Number, height:Number, mass:Number) { + body = new RigidBody(); + collisionBox = new CollisionBox(body, new Point3D(0.5*width, 0.5*length, 0.5*height)); + setParams(width, length, height, mass); + } + + public function setParams(width:Number, length:Number, height:Number, mass:Number):void { + radius = Math.sqrt(width*width + length*length + height*height)*0.5; + collisionBox.halfSize.reset(0.5*width, 0.5*length, 0.5*height); + if (mass == 0) { + body.setInfinteMass(); + } else { + var coeff:Number = mass/12; + var xx:Number = width*width; + var yy:Number = length*length; + var zz:Number = height*height; + var inertiaTensor:Matrix3D = new Matrix3D(); + inertiaTensor.a = coeff*(yy + zz); + inertiaTensor.f = coeff*(xx + zz); + inertiaTensor.k = coeff*(xx + yy); + body.setIntertiaTensor(inertiaTensor); + body.setMass(mass); + } + } + + public function setNext(box:RigidBox):RigidBox { + next = box; + body.next = box.body; + return box; + } + + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/rigid/generators/.svn/text-base/RigidBoxCollisionData.as.svn-base b/0.0.0.18/src/alternativa/physics/rigid/generators/.svn/text-base/RigidBoxCollisionData.as.svn-base new file mode 100644 index 0000000..c0b3910 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/rigid/generators/.svn/text-base/RigidBoxCollisionData.as.svn-base @@ -0,0 +1,46 @@ +package alternativa.physics.rigid.generators { + import alternativa.physics.collision.CollisionData; + + public class RigidBoxCollisionData extends CollisionData { + + private static var pool:Array = new Array(); + + public static function create():RigidBoxCollisionData { + var data:RigidBoxCollisionData = pool.pop(); + if (data == null) { + data = new RigidBoxCollisionData(); + } + data.fresh = true; + data.timeStamp = 0; + return data; + } + + public static function destroy(data:RigidBoxCollisionData):void { + data.primitive1 = null; + data.primitive2 = null; + data.otherBox = null; + pool.push(data); + } + + public var fresh:Boolean; + public var otherBox:RigidBox; + + public function RigidBoxCollisionData() { + super(); + } + + public function equals(data:RigidBoxCollisionData):Boolean { + var result:Boolean = otherBox == data.otherBox && pointAxisCode1 == data.pointAxisCode1 && pointAxisCode2 == data.pointAxisCode2; + return result; + } + + public function toString():String { + return "RigidBoxCollisionData" + + "\n collisionPoint " + collisionPoint + + "\n collisionNormal " + collisionNormal + + "\n pointAxisCode1 " + pointAxisCode1.toString(2) + + "\n pointAxisCode2 " + pointAxisCode2.toString(2); + } + + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/rigid/generators/.svn/text-base/SphereWithPlaneContactGenerator.as.svn-base b/0.0.0.18/src/alternativa/physics/rigid/generators/.svn/text-base/SphereWithPlaneContactGenerator.as.svn-base new file mode 100644 index 0000000..241eaf4 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/rigid/generators/.svn/text-base/SphereWithPlaneContactGenerator.as.svn-base @@ -0,0 +1,47 @@ +package alternativa.physics.rigid.generators { + import alternativa.physics.altphysics; + import alternativa.physics.rigid.RigidBody; + import alternativa.physics.rigid.RigidBodyContact; + import alternativa.physics.rigid.RigidBodyContactGenerator; + import alternativa.types.Point3D; + + use namespace altphysics; + + public class SphereWithPlaneContactGenerator extends RigidBodyContactGenerator { + + private var body:RigidBody; + private var planeNormal:Point3D = new Point3D(); + private var planeOffset:Number; + private var radius:Number; + private var restitution:Number; + private var friction:Number; + + public function SphereWithPlaneContactGenerator(body:RigidBody, radius:Number, planeNormal:Point3D, planeOffset:Number, restitution:Number, friction:Number) { + super(); + this.body = body; + this.radius = radius; + this.planeNormal.copy(planeNormal); + this.planeOffset = planeOffset; + this.restitution = restitution; + this.friction = friction; + } + + override public function addContacts(firstContact:RigidBodyContact):RigidBodyContact { + var offset:Number = planeNormal.dot(body.position) - planeOffset; + if (offset < radius) { + firstContact.body1 = body; + firstContact.body2 = null; + firstContact.penetration = radius - offset; + firstContact.contactNormal.copy(planeNormal); + firstContact.contactPoint.copy(planeNormal); + firstContact.contactPoint.multiply(-radius); + firstContact.contactPoint.add(body.position); + firstContact.restitution = restitution; + firstContact.friction = friction; + return firstContact.next; + } + return firstContact; + } + + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/rigid/generators/AnchoredCableContactGenerator.as b/0.0.0.18/src/alternativa/physics/rigid/generators/AnchoredCableContactGenerator.as new file mode 100644 index 0000000..32e6169 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/rigid/generators/AnchoredCableContactGenerator.as @@ -0,0 +1,56 @@ +package alternativa.physics.rigid.generators { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.RigidBody; + import alternativa.physics.rigid.RigidBodyContact; + import alternativa.physics.rigid.RigidBodyContactGenerator; + import alternativa.types.Point3D; + + use namespace altphysics; + + public class AnchoredCableContactGenerator extends RigidBodyContactGenerator { + + private var body:RigidBody; + private var mountPoint:Point3D; + private var _anchorPoint:Point3D; + private var cableLength:Number; + private var restitution:Number; + + private var vector:Point3D = new Point3D(); + private var point:Point3D = new Point3D(); + + public function AnchoredCableContactGenerator(body:RigidBody, mountPoint:Point3D, anchorPoint:Point3D, cableLength:Number, restitution:Number) { + super(); + this.body = body; + this.mountPoint = mountPoint.clone(); + _anchorPoint = anchorPoint.clone(); + this.cableLength = cableLength; + this.restitution = restitution; + } + + public function set anchorPoint(value:Point3D):void { + _anchorPoint.copy(value); + } + + override public function addContacts(contact:RigidBodyContact):RigidBodyContact { + point.copy(mountPoint); + point.transform(body.transformMatrix); + vector.difference(_anchorPoint, point); + var len:Number = vector.length; + var pen:Number = len - cableLength; + if (pen > 0) { + vector.multiply(1/len); + contact.body1 = body; + contact.body2 = null; + contact.contactNormal.copy(vector); + contact.contactPoint.copy(point); + contact.restitution = restitution; + contact.penetration = pen; + contact.friction = 0; + return contact.next; + } + return contact; + } + + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/rigid/generators/BoxWithBoxContactGenerator.as b/0.0.0.18/src/alternativa/physics/rigid/generators/BoxWithBoxContactGenerator.as new file mode 100644 index 0000000..2a51fdb --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/rigid/generators/BoxWithBoxContactGenerator.as @@ -0,0 +1,319 @@ +package alternativa.physics.rigid.generators { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionBox; + import alternativa.physics.collision.CollisionData; + import alternativa.physics.collision.CollisionDetector; + import alternativa.physics.rigid.RigidBodyContact; + import alternativa.physics.rigid.RigidBodyContactGenerator; + import alternativa.types.Matrix3D; + import alternativa.types.Point3D; + + use namespace altphysics; + + public class BoxWithBoxContactGenerator extends RigidBodyContactGenerator { + + private var boxes:RigidBox; + private var collisionDetector:CollisionDetector; + private var collisionData:RigidBoxCollisionData = RigidBoxCollisionData.create(); + private var restitution:Number; + private var friction:Number; + + private var tolerance:Number = -0.01; + + public function BoxWithBoxContactGenerator(boxes:RigidBox, collisionDetector:CollisionDetector, restitution:Number, friction:Number) { + super(); + this.boxes = boxes; + this.collisionDetector = collisionDetector; + this.restitution = restitution; + this.friction = friction; + } + + public function getBoxes():RigidBox { + return boxes; + } + + public function setBoxes(boxes:RigidBox):void { + this.boxes = boxes; + } + + public function getBoxesCount():int { + var counter:int = 0; + var box:RigidBox = boxes; + while (box != null) { + counter++; + box = box.next; + } + return counter; + } + + private function updateCillisionBoxes():void { + var box:RigidBox = boxes; + while (box != null) { + box.collisionBox.updateTransform(); + box = box.next; + } + } + + override public function addContacts(contact:RigidBodyContact):RigidBodyContact { + updateCillisionBoxes(); + + var box:RigidBox = boxes; + var otherBox:RigidBox; + var key:*; + var cachedCollisionData:RigidBoxCollisionData; + while (box != null) { + otherBox = box.next; + while (otherBox != null) { + var contactExists:Boolean = collisionDetector.boxToBoxCollision(box.collisionBox, otherBox.collisionBox, collisionData); + collisionData.otherBox = otherBox; + if (contactExists) { + var cached:Boolean = false; + // Поиск и обновление полученного контакта в кэше + for (key in box.boxCollisionCache) { + cachedCollisionData = key; + if (!cachedCollisionData.fresh && cachedCollisionData.equals(collisionData)) { + cached = true; + cachedCollisionData.collisionNormal.copy(collisionData.collisionNormal); + cachedCollisionData.collisionPoint.copy(collisionData.collisionPoint); + cachedCollisionData.penetration = collisionData.penetration; + break; + } + } + if (!cached) { + // В кэше не оказалось, добавляется новый контакт + box.boxCollisionCache[collisionData] = true; + collisionData = RigidBoxCollisionData.create(); + } + } + otherBox = otherBox.next as RigidBox; + } + + // Создание контактов из кэша + var counter:int; + for (key in box.boxCollisionCache) { + cachedCollisionData = key; + if (cachedCollisionData.fresh) { + cachedCollisionData.fresh = false; + } else { + // Проверка актуальности старых контактов + var valid:Boolean = checkCachedContact(cachedCollisionData); + if (!valid) { + delete box.boxCollisionCache[key]; + RigidBoxCollisionData.destroy(cachedCollisionData); + continue; + } + cachedCollisionData.timeStamp++; + } + // Создание контакта + contact.timeStamp = cachedCollisionData.timeStamp; + contact.body1 = box.body; + contact.body2 = cachedCollisionData.otherBox.body; + contact.contactNormal.copy(cachedCollisionData.collisionNormal); + contact.contactPoint.copy(cachedCollisionData.collisionPoint); + contact.penetration = cachedCollisionData.penetration; + contact.friction = friction; + contact.restitution = restitution; + contact = contact.next; + if (contact == null) { + return null; + } + } + + box = box.next; + } + + return contact; + } + + private var _axis1:Point3D = new Point3D(); + private var _axis2:Point3D = new Point3D(); + private var _axis3:Point3D = new Point3D(); + private var _toFirst:Point3D = new Point3D(); + private var _point1:Point3D = new Point3D(); + private var _point2:Point3D = new Point3D(); + private var _collisionPoint:Point3D = new Point3D(); + /** + * + * @param data + * @return + */ + private function checkCachedContact(data:CollisionData):Boolean { + var box1:CollisionBox = data.primitive1 as CollisionBox; + var box2:CollisionBox = data.primitive2 as CollisionBox; + var axisFlag1:int = data.pointAxisCode1 & 32; + var axisFlag2:int = data.pointAxisCode2 & 32; + var axisIndex1:int; + var axisIndex2:int; + var penetration:Number; + _toFirst.difference(box1.position, box2.position); + + if (axisFlag1 == axisFlag2) { + // Столкновение вдоль вторичной оси + axisIndex1 = (data.pointAxisCode1 >> 3) & 3; + box1.transform.getAxis(axisIndex1, _axis1); + axisIndex2 = (data.pointAxisCode2 >> 3) & 3; + box2.transform.getAxis(axisIndex2, _axis2); + _axis3.cross2(_axis1, _axis2); + _axis3.normalize(); + var overlap:Number = collisionDetector.overlapOnAxis(box1, box2, _axis3, _toFirst); + if (overlap < 0) { + return false; + } else { + penetration = getEdgesPenetrationAndCollisionPoint(box1, box2, _axis1, axisIndex1, data.pointAxisCode1 & 7, _axis2, axisIndex2, data.pointAxisCode2 & 7, _toFirst, _collisionPoint); + if (penetration < tolerance) { + return false; + } + // Обновляем контакт + if (_toFirst.dot(_axis3) < 0) { + _axis3.invert(); + } + data.collisionNormal.copy(_axis3); + data.collisionPoint.copy(_collisionPoint); + data.penetration = penetration > 0 ? penetration : 0; + return true; + } + } else { + // Столкновение вдоль основной оси + // TODO: Доделать проверку на попадание вершины внутрь бокса + if (axisFlag1 != 0) { + // Столкновение вершины второго бокса с гранью первого + _toFirst.invert(); + axisIndex1 = (data.pointAxisCode1 >> 3) & 3; + penetration = getPointBoxPenetrationAndCollisionPoint(box2, box1, data.pointAxisCode2 & 7, axisIndex1, _toFirst, _collisionPoint, _axis1); + _axis1.invert(); + } else { + // Столкновение вершины первого бокса с гранью второго + axisIndex2 = (data.pointAxisCode2 >> 3) & 3; + penetration = getPointBoxPenetrationAndCollisionPoint(box1, box2, data.pointAxisCode1 & 7, axisIndex2, _toFirst, _collisionPoint, _axis1); + } + if (penetration > tolerance) { + data.collisionNormal.copy(_axis1); + data.collisionPoint.copy(_collisionPoint); + data.penetration = penetration > 0 ? penetration : 0; + } else { + return false; + } + } + return true; + } + + /** + * + */ + private function getEdgesPenetrationAndCollisionPoint(box1:CollisionBox, box2:CollisionBox, axis1:Point3D, axisIndex1:int, pointIndex1:int, axis2:Point3D, axisIndex2:int, pointIndex2:int, toFirst:Point3D, collisionPoint:Point3D):Number { + _point1.x = (pointIndex1 & 1) == 0 ? box1.halfSize.x : -box1.halfSize.x; + _point1.y = (pointIndex1 & 2) == 0 ? box1.halfSize.y : -box1.halfSize.y; + _point1.z = (pointIndex1 & 4) == 0 ? box1.halfSize.z : -box1.halfSize.z; + _point1.transform(box1.transform); + + _point2.x = (pointIndex2 & 1) == 0 ? box2.halfSize.x : -box2.halfSize.x; + _point2.y = (pointIndex2 & 2) == 0 ? box2.halfSize.y : -box2.halfSize.y; + _point2.z = (pointIndex2 & 4) == 0 ? box2.halfSize.z : -box2.halfSize.z; + _point2.transform(box2.transform); + + var k:Number = axis1.dot(axis2); + var det:Number = k*k - 1; + if (det > -0.0001 && det < 0.0001) { + // Число по модулю должно быть быть больше, чем tolerance + return -100; + } else { + collisionPoint.difference(_point2, _point1); + var c1:Number = axis1.dot(collisionPoint); + var c2:Number = axis2.dot(collisionPoint); + var t1:Number = (c2*k - c1)/det; + var size:Number = 2*(axisIndex1 == 0 ? box1.halfSize.x : (axisIndex1 == 1 ? box1.halfSize.y : box1.halfSize.z)); + if (t1 < 0 || t1 > size) { + return -100; + } else { + var t2:Number = (c2 - c1*k)/det; + size = 2*(axisIndex2 == 0 ? box2.halfSize.x : (axisIndex2 == 1 ? box2.halfSize.y : box2.halfSize.z)); + if (t2 < 0 || t2 > size) { + return -100; + } else { + _point1.x += _axis1.x*t1; + _point1.y += _axis1.y*t1; + _point1.z += _axis1.z*t1; + _point2.x += _axis2.x*t2; + _point2.y += _axis2.y*t2; + _point2.z += _axis2.z*t2; + collisionPoint.x = 0.5*(_point1.x + _point2.x); + collisionPoint.y = 0.5*(_point1.y + _point2.y); + collisionPoint.z = 0.5*(_point1.z + _point2.z); + _point1.subtract(_point2); + return _point1.dot(toFirst) > 0 ? -_point1.length : _point1.length; + } + } + } + + return -100; + } + + /** + * + * @param box1 бокс, вершина которого столкнулась с гранью второго бокса + * @param box2 бокс, грань которого столкнулась с вершиной второго бокса + * @param axisIndex1 + * @param pointIndex2 + * @param toCenter + * @param collisionPoint + * @return + */ + private function getPointBoxPenetrationAndCollisionPoint(box1:CollisionBox, box2:CollisionBox, pointIndex1:int, axisIndex2:int, toFirst:Point3D, collisionPoint:Point3D, collisionNormal:Point3D):Number { + // Локальные кординаты вершины + _point1.x = (pointIndex1 & 1) == 0 ? box1.halfSize.x : -box1.halfSize.x; + _point1.y = (pointIndex1 & 2) == 0 ? box1.halfSize.y : -box1.halfSize.y; + _point1.z = (pointIndex1 & 4) == 0 ? box1.halfSize.z : -box1.halfSize.z; + + // размер второго бокса вдоль оси столкновения + var size:Number; + if (axisIndex2 == 0) { + size = box2.halfSize.x; + } else if (axisIndex2 == 1) { + size = box2.halfSize.y; + } else { + size = box2.halfSize.z; + } + box2.transform.getAxis(axisIndex2, collisionNormal); + var distance:Number = collisionNormal.dot(toFirst); + if (distance < 0) { + distance = -distance; + collisionNormal.invert(); + } + var m:Matrix3D = box1.transform; + collisionPoint.x = m.a*_point1.x + m.b*_point1.y + m.c*_point1.z; + collisionPoint.y = m.e*_point1.x + m.f*_point1.y + m.g*_point1.z; + collisionPoint.z = m.i*_point1.x + m.j*_point1.y + m.k*_point1.z; + var projection:Number = collisionNormal.dot(collisionPoint); + var overlap:Number = size - projection - distance; + if (overlap < tolerance) { + return -100; + } + + // Глобальные координаты вершины + collisionPoint.x += m.d; + collisionPoint.y += m.h; + collisionPoint.z += m.l; + + // Координтаы вершины в системе второго бокса + _point2.copy(collisionPoint); + _point2.inverseTransform(box2.transform); + // Проверка на принадлежность вершины боксу + if (_point2.x < 0) { + _point2.x = -_point2.x; + } + if (_point2.y < 0) { + _point2.y = -_point2.y; + } + if (_point2.y < 0) { + _point2.y = -_point2.y; + } + if ((_point2.x + tolerance > box2.halfSize.x) || (_point2.y + tolerance > box2.halfSize.y) || (_point2.z + tolerance > box2.halfSize.z)) { + return -100; + } + + return overlap; + } + + } +} diff --git a/0.0.0.18/src/alternativa/physics/rigid/generators/RigidBox.as b/0.0.0.18/src/alternativa/physics/rigid/generators/RigidBox.as new file mode 100644 index 0000000..d83b18a --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/rigid/generators/RigidBox.as @@ -0,0 +1,55 @@ +package alternativa.physics.rigid.generators { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionBox; + import alternativa.physics.rigid.RigidBody; + import alternativa.types.Matrix3D; + import alternativa.types.Point3D; + + import flash.utils.Dictionary; + + use namespace altphysics; + + public class RigidBox { + + public var body:RigidBody; + public var collisionBox:CollisionBox; + public var radius:Number; + + public var boxCollisionCache:Dictionary = new Dictionary(); + public var next:RigidBox; + + private var point:Point3D = new Point3D(); + + public function RigidBox(width:Number, length:Number, height:Number, mass:Number) { + body = new RigidBody(); + collisionBox = new CollisionBox(body, new Point3D(0.5*width, 0.5*length, 0.5*height)); + setParams(width, length, height, mass); + } + + public function setParams(width:Number, length:Number, height:Number, mass:Number):void { + radius = Math.sqrt(width*width + length*length + height*height)*0.5; + collisionBox.halfSize.reset(0.5*width, 0.5*length, 0.5*height); + if (mass == 0) { + body.setInfinteMass(); + } else { + var coeff:Number = mass/12; + var xx:Number = width*width; + var yy:Number = length*length; + var zz:Number = height*height; + var inertiaTensor:Matrix3D = new Matrix3D(); + inertiaTensor.a = coeff*(yy + zz); + inertiaTensor.f = coeff*(xx + zz); + inertiaTensor.k = coeff*(xx + yy); + body.setIntertiaTensor(inertiaTensor); + body.setMass(mass); + } + } + + public function setNext(box:RigidBox):RigidBox { + next = box; + body.next = box.body; + return box; + } + + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/rigid/generators/RigidBoxCollisionData.as b/0.0.0.18/src/alternativa/physics/rigid/generators/RigidBoxCollisionData.as new file mode 100644 index 0000000..c0b3910 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/rigid/generators/RigidBoxCollisionData.as @@ -0,0 +1,46 @@ +package alternativa.physics.rigid.generators { + import alternativa.physics.collision.CollisionData; + + public class RigidBoxCollisionData extends CollisionData { + + private static var pool:Array = new Array(); + + public static function create():RigidBoxCollisionData { + var data:RigidBoxCollisionData = pool.pop(); + if (data == null) { + data = new RigidBoxCollisionData(); + } + data.fresh = true; + data.timeStamp = 0; + return data; + } + + public static function destroy(data:RigidBoxCollisionData):void { + data.primitive1 = null; + data.primitive2 = null; + data.otherBox = null; + pool.push(data); + } + + public var fresh:Boolean; + public var otherBox:RigidBox; + + public function RigidBoxCollisionData() { + super(); + } + + public function equals(data:RigidBoxCollisionData):Boolean { + var result:Boolean = otherBox == data.otherBox && pointAxisCode1 == data.pointAxisCode1 && pointAxisCode2 == data.pointAxisCode2; + return result; + } + + public function toString():String { + return "RigidBoxCollisionData" + + "\n collisionPoint " + collisionPoint + + "\n collisionNormal " + collisionNormal + + "\n pointAxisCode1 " + pointAxisCode1.toString(2) + + "\n pointAxisCode2 " + pointAxisCode2.toString(2); + } + + } +} \ No newline at end of file diff --git a/0.0.0.18/src/alternativa/physics/rigid/generators/SphereWithPlaneContactGenerator.as b/0.0.0.18/src/alternativa/physics/rigid/generators/SphereWithPlaneContactGenerator.as new file mode 100644 index 0000000..241eaf4 --- /dev/null +++ b/0.0.0.18/src/alternativa/physics/rigid/generators/SphereWithPlaneContactGenerator.as @@ -0,0 +1,47 @@ +package alternativa.physics.rigid.generators { + import alternativa.physics.altphysics; + import alternativa.physics.rigid.RigidBody; + import alternativa.physics.rigid.RigidBodyContact; + import alternativa.physics.rigid.RigidBodyContactGenerator; + import alternativa.types.Point3D; + + use namespace altphysics; + + public class SphereWithPlaneContactGenerator extends RigidBodyContactGenerator { + + private var body:RigidBody; + private var planeNormal:Point3D = new Point3D(); + private var planeOffset:Number; + private var radius:Number; + private var restitution:Number; + private var friction:Number; + + public function SphereWithPlaneContactGenerator(body:RigidBody, radius:Number, planeNormal:Point3D, planeOffset:Number, restitution:Number, friction:Number) { + super(); + this.body = body; + this.radius = radius; + this.planeNormal.copy(planeNormal); + this.planeOffset = planeOffset; + this.restitution = restitution; + this.friction = friction; + } + + override public function addContacts(firstContact:RigidBodyContact):RigidBodyContact { + var offset:Number = planeNormal.dot(body.position) - planeOffset; + if (offset < radius) { + firstContact.body1 = body; + firstContact.body2 = null; + firstContact.penetration = radius - offset; + firstContact.contactNormal.copy(planeNormal); + firstContact.contactPoint.copy(planeNormal); + firstContact.contactPoint.multiply(-radius); + firstContact.contactPoint.add(body.position); + firstContact.restitution = restitution; + firstContact.friction = friction; + return firstContact.next; + } + return firstContact; + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/.actionScriptProperties b/0.0.1.0/.actionScriptProperties new file mode 100644 index 0000000..e5b1b89 --- /dev/null +++ b/0.0.1.0/.actionScriptProperties @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/0.0.1.0/.flexLibProperties b/0.0.1.0/.flexLibProperties new file mode 100644 index 0000000..2083d0b --- /dev/null +++ b/0.0.1.0/.flexLibProperties @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/0.0.1.0/.project b/0.0.1.0/.project new file mode 100644 index 0000000..1f1425f --- /dev/null +++ b/0.0.1.0/.project @@ -0,0 +1,19 @@ + + + AlternativaPhysics + + + + + + com.adobe.flexbuilder.project.flexbuilder + + + + + + org.maven.ide.eclipse.maven2Nature + com.adobe.flexbuilder.project.flexlibnature + com.adobe.flexbuilder.project.actionscriptnature + + diff --git a/0.0.1.0/.settings/.svn/all-wcprops b/0.0.1.0/.settings/.svn/all-wcprops new file mode 100644 index 0000000..cd91b7e --- /dev/null +++ b/0.0.1.0/.settings/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 89 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/.settings +END +org.eclipse.core.resources.prefs +K 25 +svn:wc:ra_dav:version-url +V 122 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/.settings/org.eclipse.core.resources.prefs +END diff --git a/0.0.1.0/.settings/.svn/entries b/0.0.1.0/.settings/.svn/entries new file mode 100644 index 0000000..866dd9a --- /dev/null +++ b/0.0.1.0/.settings/.svn/entries @@ -0,0 +1,40 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/.settings +http://svndev.alternativaplatform.com + + + +2009-04-01T12:25:29.638016Z +10301 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +org.eclipse.core.resources.prefs +file + + + + +2010-10-28T04:32:39.000000Z +4f70b476f3e5075e399505021df2f89b +2009-04-01T12:25:29.638016Z +10301 +mike + diff --git a/0.0.1.0/.settings/.svn/format b/0.0.1.0/.settings/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.1.0/.settings/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.1.0/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base b/0.0.1.0/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base new file mode 100644 index 0000000..b3d0d49 --- /dev/null +++ b/0.0.1.0/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base @@ -0,0 +1,3 @@ +#Fri Mar 20 00:19:33 YEKT 2009 +eclipse.preferences.version=1 +encoding/=UTF-8 diff --git a/0.0.1.0/.settings/org.eclipse.core.resources.prefs b/0.0.1.0/.settings/org.eclipse.core.resources.prefs new file mode 100644 index 0000000..b3d0d49 --- /dev/null +++ b/0.0.1.0/.settings/org.eclipse.core.resources.prefs @@ -0,0 +1,3 @@ +#Fri Mar 20 00:19:33 YEKT 2009 +eclipse.preferences.version=1 +encoding/=UTF-8 diff --git a/0.0.1.0/.svn/all-wcprops b/0.0.1.0/.svn/all-wcprops new file mode 100644 index 0000000..118966f --- /dev/null +++ b/0.0.1.0/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 79 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0 +END +.flexLibProperties +K 25 +svn:wc:ra_dav:version-url +V 98 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/.flexLibProperties +END +.project +K 25 +svn:wc:ra_dav:version-url +V 88 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/.project +END +pom.xml +K 25 +svn:wc:ra_dav:version-url +V 87 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/pom.xml +END +.actionScriptProperties +K 25 +svn:wc:ra_dav:version-url +V 103 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/.actionScriptProperties +END diff --git a/0.0.1.0/.svn/dir-prop-base b/0.0.1.0/.svn/dir-prop-base new file mode 100644 index 0000000..8eda4b3 --- /dev/null +++ b/0.0.1.0/.svn/dir-prop-base @@ -0,0 +1,5 @@ +K 13 +svn:mergeinfo +V 85 +/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D:13892-14098 +END diff --git a/0.0.1.0/.svn/entries b/0.0.1.0/.svn/entries new file mode 100644 index 0000000..f68e93b --- /dev/null +++ b/0.0.1.0/.svn/entries @@ -0,0 +1,85 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0 +http://svndev.alternativaplatform.com + + + +2009-06-23T08:51:29.377118Z +14887 +mike +has-props + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +META-INF +dir + +.flexLibProperties +file + + + + +2010-10-28T04:32:39.000000Z +fcef7ca7fe61a3459fb00858c28adec1 +2009-05-26T08:41:33.471001Z +13389 +mike + +.project +file + + + + +2010-10-28T04:32:39.000000Z +1e90cc68b52b93173b2b5de88eb5c3a3 +2009-04-20T17:15:55.260110Z +11541 +mike + +src +dir + +pom.xml +file + + + + +2010-10-28T04:32:39.000000Z +66fe2d1158b7a4fe76ade73e32168db1 +2009-06-23T08:50:57.759518Z +14886 +mike + +.actionScriptProperties +file + + + + +2010-10-28T04:32:39.000000Z +918335cb945c3f8866aeb8e6cfd1b110 +2009-06-23T08:49:19.922749Z +14884 +mike + +.settings +dir + diff --git a/0.0.1.0/.svn/format b/0.0.1.0/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.1.0/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.1.0/.svn/text-base/.actionScriptProperties.svn-base b/0.0.1.0/.svn/text-base/.actionScriptProperties.svn-base new file mode 100644 index 0000000..e5b1b89 --- /dev/null +++ b/0.0.1.0/.svn/text-base/.actionScriptProperties.svn-base @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/0.0.1.0/.svn/text-base/.flexLibProperties.svn-base b/0.0.1.0/.svn/text-base/.flexLibProperties.svn-base new file mode 100644 index 0000000..2083d0b --- /dev/null +++ b/0.0.1.0/.svn/text-base/.flexLibProperties.svn-base @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/0.0.1.0/.svn/text-base/.project.svn-base b/0.0.1.0/.svn/text-base/.project.svn-base new file mode 100644 index 0000000..1f1425f --- /dev/null +++ b/0.0.1.0/.svn/text-base/.project.svn-base @@ -0,0 +1,19 @@ + + + AlternativaPhysics + + + + + + com.adobe.flexbuilder.project.flexbuilder + + + + + + org.maven.ide.eclipse.maven2Nature + com.adobe.flexbuilder.project.flexlibnature + com.adobe.flexbuilder.project.actionscriptnature + + diff --git a/0.0.1.0/.svn/text-base/pom.xml.svn-base b/0.0.1.0/.svn/text-base/pom.xml.svn-base new file mode 100644 index 0000000..6e943fb --- /dev/null +++ b/0.0.1.0/.svn/text-base/pom.xml.svn-base @@ -0,0 +1,17 @@ + + 4.0.0 + platform.clients.fp10.libraries + AlternativaPhysics + swc + 0.0.1.0 + + platform.tools.maven + FlashBasePom + 1.0 + + + scm:svn:http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0 + + + + \ No newline at end of file diff --git a/0.0.1.0/META-INF/.svn/all-wcprops b/0.0.1.0/META-INF/.svn/all-wcprops new file mode 100644 index 0000000..3a6c795 --- /dev/null +++ b/0.0.1.0/META-INF/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 88 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/META-INF +END +MANIFEST.MF +K 25 +svn:wc:ra_dav:version-url +V 100 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/META-INF/MANIFEST.MF +END diff --git a/0.0.1.0/META-INF/.svn/entries b/0.0.1.0/META-INF/.svn/entries new file mode 100644 index 0000000..8d18702 --- /dev/null +++ b/0.0.1.0/META-INF/.svn/entries @@ -0,0 +1,40 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/META-INF +http://svndev.alternativaplatform.com + + + +2009-04-20T17:15:55.260110Z +11541 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +MANIFEST.MF +file + + + + +2010-10-28T04:32:39.000000Z +7b64dceb50a6905850ff00a0bfbe77eb +2009-04-20T17:15:55.260110Z +11541 +mike + diff --git a/0.0.1.0/META-INF/.svn/format b/0.0.1.0/META-INF/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.1.0/META-INF/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.1.0/META-INF/.svn/text-base/MANIFEST.MF.svn-base b/0.0.1.0/META-INF/.svn/text-base/MANIFEST.MF.svn-base new file mode 100644 index 0000000..5066a55 --- /dev/null +++ b/0.0.1.0/META-INF/.svn/text-base/MANIFEST.MF.svn-base @@ -0,0 +1 @@ +Bundle-Name: platform.clients.fp10.libraries.AlternativaPhysics diff --git a/0.0.1.0/META-INF/MANIFEST.MF b/0.0.1.0/META-INF/MANIFEST.MF new file mode 100644 index 0000000..5066a55 --- /dev/null +++ b/0.0.1.0/META-INF/MANIFEST.MF @@ -0,0 +1 @@ +Bundle-Name: platform.clients.fp10.libraries.AlternativaPhysics diff --git a/0.0.1.0/pom.xml b/0.0.1.0/pom.xml new file mode 100644 index 0000000..6e943fb --- /dev/null +++ b/0.0.1.0/pom.xml @@ -0,0 +1,17 @@ + + 4.0.0 + platform.clients.fp10.libraries + AlternativaPhysics + swc + 0.0.1.0 + + platform.tools.maven + FlashBasePom + 1.0 + + + scm:svn:http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0 + + + + \ No newline at end of file diff --git a/0.0.1.0/src/.svn/all-wcprops b/0.0.1.0/src/.svn/all-wcprops new file mode 100644 index 0000000..7ceed03 --- /dev/null +++ b/0.0.1.0/src/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 83 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src +END diff --git a/0.0.1.0/src/.svn/entries b/0.0.1.0/src/.svn/entries new file mode 100644 index 0000000..a9986cd --- /dev/null +++ b/0.0.1.0/src/.svn/entries @@ -0,0 +1,31 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src +http://svndev.alternativaplatform.com + + + +2009-06-17T08:27:51.730249Z +14488 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +alternativa +dir + diff --git a/0.0.1.0/src/.svn/format b/0.0.1.0/src/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.1.0/src/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.1.0/src/alternativa/.svn/all-wcprops b/0.0.1.0/src/alternativa/.svn/all-wcprops new file mode 100644 index 0000000..c537b77 --- /dev/null +++ b/0.0.1.0/src/alternativa/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 95 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa +END diff --git a/0.0.1.0/src/alternativa/.svn/entries b/0.0.1.0/src/alternativa/.svn/entries new file mode 100644 index 0000000..9f66f5e --- /dev/null +++ b/0.0.1.0/src/alternativa/.svn/entries @@ -0,0 +1,31 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa +http://svndev.alternativaplatform.com + + + +2009-06-17T08:27:51.730249Z +14488 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +physics +dir + diff --git a/0.0.1.0/src/alternativa/.svn/format b/0.0.1.0/src/alternativa/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.1.0/src/alternativa/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.1.0/src/alternativa/physics/.svn/all-wcprops b/0.0.1.0/src/alternativa/physics/.svn/all-wcprops new file mode 100644 index 0000000..6c1321c --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 103 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics +END +altphysics.as +K 25 +svn:wc:ra_dav:version-url +V 117 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/altphysics.as +END diff --git a/0.0.1.0/src/alternativa/physics/.svn/entries b/0.0.1.0/src/alternativa/physics/.svn/entries new file mode 100644 index 0000000..4b34409 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/.svn/entries @@ -0,0 +1,49 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics +http://svndev.alternativaplatform.com + + + +2009-06-17T08:27:51.730249Z +14488 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +types +dir + +altphysics.as +file + + + + +2010-10-28T04:32:39.000000Z +04fbe40a27502dbbd0dba02a76b2df50 +2009-04-20T20:09:14.715403Z +11579 +mike + +collision +dir + +rigid +dir + diff --git a/0.0.1.0/src/alternativa/physics/.svn/format b/0.0.1.0/src/alternativa/physics/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.1.0/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base b/0.0.1.0/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base new file mode 100644 index 0000000..2715307 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base @@ -0,0 +1,3 @@ +package alternativa.physics { + public namespace altphysics = "http://alternativaplatform.com/en/altphysics"; +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/altphysics.as b/0.0.1.0/src/alternativa/physics/altphysics.as new file mode 100644 index 0000000..2715307 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/altphysics.as @@ -0,0 +1,3 @@ +package alternativa.physics { + public namespace altphysics = "http://alternativaplatform.com/en/altphysics"; +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/collision/.svn/all-wcprops b/0.0.1.0/src/alternativa/physics/collision/.svn/all-wcprops new file mode 100644 index 0000000..a1a7401 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/.svn/all-wcprops @@ -0,0 +1,95 @@ +K 25 +svn:wc:ra_dav:version-url +V 113 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/collision +END +BoxSphereCollider.as +K 25 +svn:wc:ra_dav:version-url +V 134 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/collision/BoxSphereCollider.as +END +BoxBoxCollider.as +K 25 +svn:wc:ra_dav:version-url +V 131 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/collision/BoxBoxCollider.as +END +CollisionKdNode.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/collision/CollisionKdNode.as +END +SpherePlaneCollider.as +K 25 +svn:wc:ra_dav:version-url +V 136 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/collision/SpherePlaneCollider.as +END +KdTreeCollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/collision/KdTreeCollisionDetector.as +END +BoxRectCollider.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/collision/BoxRectCollider.as +END +ICollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 136 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/collision/ICollisionPredicate.as +END +BoxPlaneCollider.as +K 25 +svn:wc:ra_dav:version-url +V 133 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/collision/BoxPlaneCollider.as +END +CollisionKdTree.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/collision/CollisionKdTree.as +END +BoxTriangleCollider.as +K 25 +svn:wc:ra_dav:version-url +V 136 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/collision/BoxTriangleCollider.as +END +ICollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 135 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/collision/ICollisionDetector.as +END +IRayCollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 139 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/collision/IRayCollisionPredicate.as +END +ICollider.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/collision/ICollider.as +END +BruteForceCollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 144 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/collision/BruteForceCollisionDetector.as +END +SphereSphereCollider.as +K 25 +svn:wc:ra_dav:version-url +V 137 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/collision/SphereSphereCollider.as +END diff --git a/0.0.1.0/src/alternativa/physics/collision/.svn/entries b/0.0.1.0/src/alternativa/physics/collision/.svn/entries new file mode 100644 index 0000000..540362c --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/.svn/entries @@ -0,0 +1,214 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/collision +http://svndev.alternativaplatform.com + + + +2009-06-17T08:27:51.730249Z +14488 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +BoxSphereCollider.as +file + + + + +2010-10-28T04:32:39.000000Z +ad1dc1849019ac159d22cd93b1ba38d3 +2009-06-08T06:46:18.310732Z +14099 +mike + +BoxBoxCollider.as +file + + + + +2010-10-28T04:32:39.000000Z +e4eb46370e3c7098fa35d0da4e346d09 +2009-06-08T06:46:18.310732Z +14099 +mike + +CollisionKdNode.as +file + + + + +2010-10-28T04:32:39.000000Z +7952384e785be1fca93657d94121b246 +2009-04-17T17:39:21.367569Z +11341 +mike + +BoxRectCollider.as +file + + + + +2010-10-28T04:32:39.000000Z +8554254b88a8ecc31bf005fdee676a67 +2009-06-08T06:46:18.310732Z +14099 +mike + +KdTreeCollisionDetector.as +file + + + + +2010-10-28T04:32:39.000000Z +7ba4808350962bc29e80317568ad589c +2009-06-17T08:27:51.730249Z +14488 +mike + +SpherePlaneCollider.as +file + + + + +2010-10-28T04:32:39.000000Z +7fbbf70eef5caa9a09ce679819429874 +2009-04-13T03:07:43.422558Z +11019 +mike + +ICollisionPredicate.as +file + + + + +2010-10-28T04:32:39.000000Z +866471ea582e95e46e94eb7ae437c827 +2009-04-26T15:39:06.673206Z +11995 +mike + +BoxPlaneCollider.as +file + + + + +2010-10-28T04:32:39.000000Z +4344df47c2b9c1873d2dc711815b4737 +2009-04-13T03:07:43.422558Z +11019 +mike + +primitives +dir + +CollisionKdTree.as +file + + + + +2010-10-28T04:32:39.000000Z +92511d7533028a942e299ca3a4251da8 +2009-05-18T16:37:02.465501Z +13087 +mike + +BoxTriangleCollider.as +file + + + + +2010-10-28T04:32:39.000000Z +f95da4166903174a2c92c1ab262e719c +2009-05-26T08:41:33.471001Z +13389 +mike + +ICollisionDetector.as +file + + + + +2010-10-28T04:32:39.000000Z +c98a3e33a4a54e2e85da149e25b583fe +2009-06-08T11:22:42.165256Z +14112 +mike + +IRayCollisionPredicate.as +file + + + + +2010-10-28T04:32:39.000000Z +f7443fb728cf4e6157dcfb4f39665889 +2009-04-26T15:39:06.673206Z +11995 +mike + +types +dir + +BruteForceCollisionDetector.as +file + + + + +2010-10-28T04:32:39.000000Z +c166c05be4909e221c2f129c9e7cac32 +2009-06-08T11:22:42.165256Z +14112 +mike + +ICollider.as +file + + + + +2010-10-28T04:32:39.000000Z +bc3785279b9b7810a0c84cb4fe6084fc +2009-04-13T03:07:43.422558Z +11019 +mike + +SphereSphereCollider.as +file + + + + +2010-10-28T04:32:39.000000Z +fd6e8cf39dcf007125384bc91b347a00 +2009-06-08T06:46:18.310732Z +14099 +mike + diff --git a/0.0.1.0/src/alternativa/physics/collision/.svn/format b/0.0.1.0/src/alternativa/physics/collision/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/BoxBoxCollider.as.svn-base b/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/BoxBoxCollider.as.svn-base new file mode 100644 index 0000000..2893b49 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/BoxBoxCollider.as.svn-base @@ -0,0 +1,791 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Расчитывает точки контакта двух боксов. Нормаль контакта направляется в сторону бокса с меньшим ID. + */ + public class BoxBoxCollider implements ICollider { + + private var tolerance:Number = 0.001; + + private var pos1:Vector3 = new Vector3(); + private var pos2:Vector3 = new Vector3(); + private var vectorToBox1:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var colAxis:Vector3 = new Vector3(); + private var tmpAxis:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var tmpPoints:Vector. = new Vector.(8, true); + private var pcount:int; + + /** + * + */ + public function BoxBoxCollider() { + for (var i:int = 0; i < 8; i++) { + tmpPoints[i] = new ContactPoint(); + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + /** + * + * @param body1 + * @param body2 + * @param contactInfo + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + minOverlap = 1e10; + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + // Вектор из центра второго бокса в центр первого + pos1.x = transform1.d; pos1.y = transform1.h; pos1.z = transform1.l; + pos2.x = transform2.d; pos2.y = transform2.h; pos2.z = transform2.l; + vectorToBox1.x = pos1.x - pos2.x; + vectorToBox1.y = pos1.y - pos2.y; + vectorToBox1.z = pos1.z - pos2.z; + + // Проверка пересечения по основным осям +// box1.transform.getAxis(0, axis10); + axis10.x = transform1.a; axis10.y = transform1.e; axis10.z = transform1.i; + if (!testAxis(box1, box2, axis10, 0, vectorToBox1)) return false; +// box1.transform.getAxis(1, axis11); + axis11.x = transform1.b; axis11.y = transform1.f; axis11.z = transform1.j; + if (!testAxis(box1, box2, axis11, 1, vectorToBox1)) return false; +// box1.transform.getAxis(2, axis12); + axis12.x = transform1.c; axis12.y = transform1.g; axis12.z = transform1.k; + if (!testAxis(box1, box2, axis12, 2, vectorToBox1)) return false; + +// box2.transform.getAxis(0, axis20); + axis20.x = transform2.a; axis20.y = transform2.e; axis20.z = transform2.i; + if (!testAxis(box1, box2, axis20, 3, vectorToBox1)) return false; +// box2.transform.getAxis(1, axis21); + axis21.x = transform2.b; axis21.y = transform2.f; axis21.z = transform2.j; + if (!testAxis(box1, box2, axis21, 4, vectorToBox1)) return false; +// box2.transform.getAxis(2, axis22); + axis22.x = transform2.c; axis22.y = transform2.g; axis22.z = transform2.k; + if (!testAxis(box1, box2, axis22, 5, vectorToBox1)) return false; + + // Проверка производных осей + if (!testAxis(box1, box2, axis.vCross2(axis10, axis20), 6, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis10, axis21), 7, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis10, axis22), 8, vectorToBox1)) return false; + + if (!testAxis(box1, box2, axis.vCross2(axis11, axis20), 9, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis11, axis21), 10, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis11, axis22), 11, vectorToBox1)) return false; + + if (!testAxis(box1, box2, axis.vCross2(axis12, axis20), 12, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis12, axis21), 13, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis12, axis22), 14, vectorToBox1)) return false; + + if (bestAxisIndex < 6) { + // Контакт грань-(грань|ребро|вершина) + findFaceContactPoints(box1, box2, vectorToBox1, bestAxisIndex, contact); + } else { + // Контакт ребро-ребро + bestAxisIndex -= 6; + findEdgesIntersection(box1, box2, vectorToBox1, int(bestAxisIndex/3), bestAxisIndex%3, contact); + } + contact.body1 = box1.body; + contact.body2 = box2.body; + + return true; + } + + /** + * Выполняет поиск точек контакта грани одного бокса с гранью/ребром/вершиной другого. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param faceAxisIdx индекс оси первого бокса, перпендикулярной грани, с которой произошло столкновение + * @param contactInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, faceAxisIdx:int, contactInfo:Contact):void { + var swapNormal:Boolean = false; + if (faceAxisIdx > 2) { + // Столкновение с гранью второго бокса. Для дальнейших расчётов боксы меняются местами, + // но нормаль контакта всё равно должна быть направлена в сторону первоначального box1 + var tmpBox:CollisionBox = box1; + box1 = box2; + box2 = tmpBox; + vectorToBox1.vReverse(); + faceAxisIdx -= 3; + swapNormal = true; + } + box1.transform.getAxis(faceAxisIdx, colAxis); + var faceReversed:Boolean = colAxis.vDot(vectorToBox1) > 0; + if (!faceReversed) colAxis.vReverse(); + // Ищем ось второго бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + box2.transform.getAxis(axisIdx, axis); + var dot:Number = axis.vDot(colAxis); + if (dot < 0) dot = -dot; + if (dot > maxDot) { + maxDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Получаем список вершин грани второго бокса, переводим их в систему координат первого бокса и выполняем обрезку + // по грани первого бокса. Таким образом получается список потенциальных точек контакта. + box2.transform.getAxis(incFaceAxisIdx, axis); + getFaceVertsByAxis(box2, incFaceAxisIdx, axis.vDot(colAxis) < 0, verts1); + box2.transform.transformVectors(verts1, verts2); + box1.transform.transformVectorsInverse(verts2, verts1); + var pnum:int = clip(box1.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + pcount = 0; + for (var i:int = 0; i < pnum; i++) { + if ((pen = getPointBoxPenetration(box1.hs, verts1[i], faceAxisIdx, faceReversed)) > -tolerance) { + var cp:ContactPoint = tmpPoints[pcount++]; + box1.transform.transformVector(verts1[i], cp.pos); + cp.r1.vDiff(cp.pos, pos1); + cp.r2.vDiff(cp.pos, pos2); + cp.penetration = pen; + } + } + contactInfo.normal.vCopy(colAxis); + if (swapNormal) contactInfo.normal.vReverse(); + + if (pcount > 4) reducePoints(); + for (i = 0; i < pcount; i++) (contactInfo.points[i] as ContactPoint).copyFrom(tmpPoints[i]); + contactInfo.pcount = pcount; + } + + /** + * + * @param contactInfo + */ + private function reducePoints():void { + var i:int; + var minIdx:int; + var cp1:ContactPoint; + var cp2:ContactPoint; + while (pcount > 4) { + var minLen:Number = 1e10; + var p1:Vector3 = (tmpPoints[pcount - 1] as ContactPoint).pos; + var p2:Vector3; + for (i = 0; i < pcount; i++) { + p2 = (tmpPoints[i] as ContactPoint).pos; + var dx:Number = p2.x - p1.x; + var dy:Number = p2.y - p1.y; + var dz:Number = p2.z - p1.z; + var len:Number = dx*dx + dy*dy + dz*dz; + if (len < minLen) { + minLen = len; + minIdx = i; + } + p1 = p2; + } + cp1 = tmpPoints[minIdx == 0 ? (pcount - 1) : (minIdx - 1)]; + cp2 = tmpPoints[minIdx]; + p1 = cp1.pos; + p2 = cp2.pos; + p2.x = 0.5*(p1.x + p2.x); + p2.y = 0.5*(p1.y + p2.y); + p2.z = 0.5*(p1.z + p2.z); + cp2.penetration = 0.5*(cp1.penetration + cp2.penetration); + if (minIdx > 0) { + for (i = minIdx; i < pcount; i++) tmpPoints[i - 1] = tmpPoints[i]; + tmpPoints[pcount - 1] = cp1; + } + pcount--; + } + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + private function getFaceVertsByAxis(box:CollisionBox, axisIdx:int, reverse:Boolean, result:Vector.):void { + var hs:Vector3 = box.hs; + switch (axisIdx) { + case 0: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, -hs.y, hs.z); + } + break; + case 1: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, hs.y, hs.z); + } + break; + case 2: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + } else { + (result[0] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } + break; + } + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clip(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + return clipHighX(hs.x, pnum, verts2, verts1); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + } + return 0; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param contactInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):void { + box1.transform.getAxis(axisIdx1, axis10); + box2.transform.getAxis(axisIdx2, axis20); + colAxis.vCross2(axis10, axis20).vNormalize(); + // Разворот оси в сторону первого бокса + if (colAxis.vDot(vectorToBox1) < 0) colAxis.vReverse(); + + /* Кодирование рёбер + бит 0: 1 (тип контакта ребро-ребро) + биты 1-2: индекс направляющей оси ребра + бит 3: + бит 4: + бит 5: + */ + var edgeCode1:int = 1; + var edgeCode2:int = 1; + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + point1.vCopy(box1.hs); + point2.vCopy(box2.hs); + // x + if (axisIdx1 == 0) { + point1.x = 0; + halfLen1 = box1.hs.x; + } else { + box1.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.x = -point1.x; + edgeCode1 |= 8; // 1 << 3 + } + } + if (axisIdx2 == 0) { + point2.x = 0; + halfLen2 = box2.hs.x; + } else { + box2.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.x = -point2.x; + edgeCode2 |= 8; // 1 << 3 + } + } + // y + if (axisIdx1 == 1) { + point1.y = 0; + halfLen1 = box1.hs.y; + edgeCode1 |= 2; // 1 << 1 + } else { + box1.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.y = -point1.y; + edgeCode1 |= 16; // 1 << 4 + } + } + if (axisIdx2 == 1) { + point2.y = 0; + halfLen2 = box2.hs.y; + edgeCode2 |= 2; // 1 << 1 + } else { + box2.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.y = -point2.y; + edgeCode2 |= 16; // 1 << 4 + } + } + // z + if (axisIdx1 == 2) { + point1.z = 0; + halfLen1 = box1.hs.z; + edgeCode1 |= 4; // 2 << 1 + } else { + box1.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.z = -point1.z; + edgeCode1 |= 32; // 1 << 5 + } + } + if (axisIdx2 == 2) { + point2.z = 0; + halfLen2 = box2.hs.z; + edgeCode2 |= 4; // 2 << 1 + } else { + box2.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.z = -point2.z; + edgeCode2 |= 32; // 1 << 5 + } + } + // Получаем глобальные координаты средних точек рёбер + point1.vTransformBy4(box1.transform); + point2.vTransformBy4(box2.transform); + // Находим точку пересечения рёбер, решая систему уравнений + var k:Number = axis10.vDot(axis20); + var det:Number = k*k - 1; + vector.vDiff(point2, point1); + var c1:Number = axis10.vDot(vector); + var c2:Number = axis20.vDot(vector); + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.normal.vCopy(colAxis); + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(point1.x + axis10.x*t1 + point2.x + axis20.x*t2); + cp.pos.y = 0.5*(point1.y + axis10.y*t1 + point2.y + axis20.y*t2); + cp.pos.z = 0.5*(point1.z + axis10.z*t1 + point2.z + axis20.z*t2); + cp.r1.vDiff(cp.pos, pos1); + cp.r2.vDiff(cp.pos, pos2); + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение боксов вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box1 + * @param box2 + * @param axis + * @param axisIndex + * @param vectorToBox1 + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, axisIndex:int, vectorToBox1:Vector3):Boolean { + if (axis.vLengthSqr() < 0.0001) { + return true; + } + axis.vNormalize(); + + var overlap:Number = overlapOnAxis(box1, box2, axis, vectorToBox1); + if (overlap < -tolerance) return false; + if (overlap + tolerance < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия двух боксов вдоль заданной оси. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param axis ось + * @param vectorToBox1 вектор, соединяющий центр второго бокса с центром первого + * @return величина перекрытия боксов вдоль оси + */ + public function overlapOnAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, vectorToBox1:Vector3):Number { + var m:Matrix4 = box1.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box1.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box1.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box1.hs.z; + if (d < 0) d = -d; + projection += d; + + m = box2.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box2.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box2.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box2.hs.z; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox1.x*axis.x + vectorToBox1.y*axis.y + vectorToBox1.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} + import alternativa.physics.types.Vector3; + + class CollisionPointTmp { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + } diff --git a/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/BoxPlaneCollider.as.svn-base b/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/BoxPlaneCollider.as.svn-base new file mode 100644 index 0000000..8568dc7 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/BoxPlaneCollider.as.svn-base @@ -0,0 +1,78 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + public class BoxPlaneCollider implements ICollider { + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var normal:Vector3 = new Vector3(); + + public function BoxPlaneCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var box:RigidBox = body1 as RigidBox; +// var plane:RigidPlane; +// if (box == null) { +// box = body2 as RigidBox; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные координаты вершин бокса +// var sx:Number = box.halfSize.x; +// var sy:Number = box.halfSize.y; +// var sz:Number = box.halfSize.z; +// (verts1[0] as Vector3).reset(-sx, -sy, -sz); +// (verts1[1] as Vector3).reset(sx, -sy, -sz); +// (verts1[2] as Vector3).reset(sx, sy, -sz); +// (verts1[3] as Vector3).reset(-sx, sy, -sz); +// (verts1[4] as Vector3).reset(-sx, -sy, sz); +// (verts1[5] as Vector3).reset(sx, -sy, sz); +// (verts1[6] as Vector3).reset(sx, sy, sz); +// (verts1[7] as Vector3).reset(-sx, sy, sz); +// +// box.transform.transformVectors(verts1, verts2); +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// // Проверяем наличие столкновений с каждой вершиной +// collisionInfo.pcount = 0; +// for (var i:int = 0; i < 8; i++) { +// // Вершина добавляется в список точек столкновения, если лежит под плоскостью +// var dist:Number = (verts2[i] as Vector3).dot(normal); +// if (dist < offset) { +// var cp:ContactPoint; +// if (collisionInfo.pcount == collisionInfo.points.length) { +// cp = new ContactPoint(); +// collisionInfo.points[collisionInfo.pcount] = cp; +// } else { +// cp = collisionInfo.points[collisionInfo.pcount]; +// } +// cp.pos.copy(verts2[i]); +// cp.r1.diff(cp.pos, box.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); +// cp.penetration = offset - dist; +// collisionInfo.pcount++; +// } +// } +// if (collisionInfo.pcount > 0) { +// collisionInfo.body1 = box; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// return true; +// } + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/BoxRectCollider.as.svn-base b/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/BoxRectCollider.as.svn-base new file mode 100644 index 0000000..b5287f3 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/BoxRectCollider.as.svn-base @@ -0,0 +1,734 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + public class BoxRectCollider implements ICollider { + + private var tolerance:Number = 0.001; + + private var bPos:Vector3 = new Vector3(); + private var rPos:Vector3 = new Vector3(); + + private var vectorToBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var colAxis:Vector3 = new Vector3(); + private var tmpAxis:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxRectCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + /** + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + minOverlap = 1e10; + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + + // Вектор из центра прямоугольника в центр бокса + box.transform.getAxis(3, bPos); + rect.transform.getAxis(3, rPos); + vectorToBox.vDiff(bPos, rPos); + + // Проверка пересечения по нормали прямоугольника + rect.transform.getAxis(2, axis22); + if (!testAxis(box, rect, axis22, 0, vectorToBox)) return false; + + // Проверка пересечения по основным осям бокса + box.transform.getAxis(0, axis10); + if (!testAxis(box, rect, axis10, 1, vectorToBox)) return false; + box.transform.getAxis(1, axis11); + if (!testAxis(box, rect, axis11, 2, vectorToBox)) return false; + box.transform.getAxis(2, axis12); + if (!testAxis(box, rect, axis12, 3, vectorToBox)) return false; + + // Получаем направляющие рёбер прямоугольника + rect.transform.getAxis(0, axis20); + rect.transform.getAxis(1, axis21); + + // Проверка производных осей + if (!testAxis(box, rect, axis.vCross2(axis10, axis20), 4, vectorToBox)) return false; + if (!testAxis(box, rect, axis.vCross2(axis10, axis21), 5, vectorToBox)) return false; + + if (!testAxis(box, rect, axis.vCross2(axis11, axis20), 6, vectorToBox)) return false; + if (!testAxis(box, rect, axis.vCross2(axis11, axis21), 7, vectorToBox)) return false; + + if (!testAxis(box, rect, axis.vCross2(axis12, axis20), 8, vectorToBox)) return false; + if (!testAxis(box, rect, axis.vCross2(axis12, axis21), 9, vectorToBox)) return false; + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, rect, vectorToBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + findEdgesIntersection(box, rect, vectorToBox, int(bestAxisIndex/2), bestAxisIndex%2, contact); + } + contact.body1 = box.body; + contact.body2 = rect.body; + + return true; + } + + /** + * Выполняет поиск точек контакта бокса с прямоугольником. + * + * @param box бокс + * @param rect прямоугольник + * @param vectorToBox вектор, направленный из центра прямоугольника в центр бокса + * @param faceAxisIdx индекс оси, идентифицирующей полскость столкновения (грань бокса или полскость прямоугольника) + * @param colInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, faceAxisIdx:int, colInfo:Contact):Boolean { + var pnum:int, i:int, v:Vector3, cp:ContactPoint; + if (faceAxisIdx == 0) { + // Столкновение с плоскостью прямоугольника + + // Проверим положение бокса относительно плоскости прямоугольника + rect.transform.getAxis(2, colAxis); + var offset:Number = colAxis.vDot(rPos); + if (bPos.vDot(colAxis) < offset) return false; + + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + box.transform.getAxis(axisIdx, axis); + var dot:Number = axis.vDot(colAxis); + if (dot < 0) dot = -dot; + if (dot > maxDot) { + maxDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Получаем список вершин грани бокса, переводим их в систему координат прямоугольника и выполняем обрезку + // по прямоугольнику. Таким образом получается список потенциальных точек контакта. + box.transform.getAxis(incFaceAxisIdx, axis); + getFaceVertsByAxis(box.hs, incFaceAxisIdx, axis.vDot(colAxis) > 0, verts1); + box.transform.transformVectors(verts1, verts2); + rect.transform.transformVectorsInverse(verts2, verts1); + pnum = clipByRect(rect.hs); + // Проверяем каждую потенциальную точку на принадлежность нижней полуплоскости прямоугольника и добавляем такие точки в список контактов + colInfo.pcount = 0; + for (i = 0; i < pnum; i++) { + v = verts1[i]; + if (v.z < tolerance) { + cp = colInfo.points[colInfo.pcount++]; + rect.transform.transformVector(v, cp.pos); + cp.r1.vDiff(cp.pos, bPos); + cp.r2.vDiff(cp.pos, rPos); + cp.penetration = -v.z; + } + } + colInfo.normal.vCopy(colAxis); + } else { + // Столкновение с гранью бокса + faceAxisIdx--; + box.transform.getAxis(faceAxisIdx, colAxis); + var faceReversed:Boolean = colAxis.vDot(vectorToBox) > 0; + if (!faceReversed) colAxis.vReverse(); + + rect.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) return false; + + getFaceVertsByAxis(rect.hs, 2, false, verts1); + rect.transform.transformVectors(verts1, verts2); + box.transform.transformVectorsInverse(verts2, verts1); + pnum = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + colInfo.pcount = 0; + for (i = 0; i < pnum; i++) { + if ((pen = getPointBoxPenetration(box.hs, verts1[i], faceAxisIdx, faceReversed)) > -tolerance) { + cp = colInfo.points[colInfo.pcount++]; + box.transform.transformVector(verts1[i], cp.pos); + cp.r1.vDiff(cp.pos, bPos); + cp.r2.vDiff(cp.pos, rPos); + cp.penetration = pen; + } + } + colInfo.normal.vCopy(colAxis); + } + return true; + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + private function getFaceVertsByAxis(hs:Vector3, axisIdx:int, reverse:Boolean, result:Vector.):void { + switch (axisIdx) { + case 0: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, -hs.y, hs.z); + } + break; + case 1: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, hs.y, hs.z); + } + break; + case 2: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + } else { + (result[0] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } + break; + } + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + return clipHighX(hs.x, pnum, verts2, verts1); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + } + return 0; + } + + /** + * + * @param hs + * @return + */ + private function clipByRect(hs:Vector3):int { + var pnum:int = 4; + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box первый бокс + * @param rect второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param colInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, axisIdx1:int, axisIdx2:int, colInfo:Contact):void { + box.transform.getAxis(axisIdx1, axis10); + rect.transform.getAxis(axisIdx2, axis20); + colAxis.vCross2(axis10, axis20).vNormalize(); + // Разворот оси в сторону бокса + if (colAxis.vDot(vectorToBox) < 0) colAxis.vReverse(); + + /* Кодирование рёбер + бит 0: 1 (тип контакта ребро-ребро) + биты 1-2: индекс направляющей оси ребра + бит 3: + бит 4: + бит 5: + */ + var edgeCode1:int = 1; + var edgeCode2:int = 1; + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + point1.vCopy(box.hs); + point2.vCopy(rect.hs); + // x + if (axisIdx1 == 0) { + point1.x = 0; + halfLen1 = box.hs.x; + } else { + box.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.x = -point1.x; + edgeCode1 |= 8; // 1 << 3 + } + } + if (axisIdx2 == 0) { + point2.x = 0; + halfLen2 = rect.hs.x; + } else { + rect.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.x = -point2.x; + edgeCode2 |= 8; // 1 << 3 + } + } + // y + if (axisIdx1 == 1) { + point1.y = 0; + halfLen1 = box.hs.y; + edgeCode1 |= 2; // 1 << 1 + } else { + box.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.y = -point1.y; + edgeCode1 |= 16; // 1 << 4 + } + } + if (axisIdx2 == 1) { + point2.y = 0; + halfLen2 = rect.hs.y; + edgeCode2 |= 2; // 1 << 1 + } else { + rect.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.y = -point2.y; + edgeCode2 |= 16; // 1 << 4 + } + } + // z + if (axisIdx1 == 2) { + point1.z = 0; + halfLen1 = box.hs.z; + edgeCode1 |= 4; // 2 << 1 + } else { + box.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.z = -point1.z; + edgeCode1 |= 32; // 1 << 5 + } + } + // Получаем глобальные координаты средних точек рёбер + point1.vTransformBy4(box.transform); + point2.vTransformBy4(rect.transform); + // Находим точку пересечения рёбер, решая систему уравнений + var k:Number = axis10.vDot(axis20); + var det:Number = k*k - 1; + vector.vDiff(point2, point1); + var c1:Number = axis10.vDot(vector); + var c2:Number = axis20.vDot(vector); + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + colInfo.normal.vCopy(colAxis); + colInfo.pcount = 1; + var cp:ContactPoint = colInfo.points[0]; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(point1.x + axis10.x*t1 + point2.x + axis20.x*t2); + cp.pos.y = 0.5*(point1.y + axis10.y*t1 + point2.y + axis20.y*t2); + cp.pos.z = 0.5*(point1.z + axis10.z*t1 + point2.z + axis20.z*t2); + cp.r1.vDiff(cp.pos, bPos); + cp.r2.vDiff(cp.pos, rPos); + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box + * @param rect + * @param axis + * @param axisIndex + * @param vectorToBox + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + if (axis.vLengthSqr() < 0.0001) return true; + axis.vNormalize(); + + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -tolerance) return false; + if (overlap + tolerance < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия вдоль заданной оси. + * + * @param box бокс + * @param rect прямоугольник + * @param axis ось + * @param vectorToBox вектор, соединяющий центр прямоугольника с центром бокса + * @return величина перекрытия вдоль оси + */ + public function overlapOnAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, vectorToBox:Vector3):Number { + var m:Matrix4 = box.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box.hs.z; + if (d < 0) d = -d; + projection += d; + + m = rect.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*rect.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*rect.hs.y; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox.x*axis.x + vectorToBox.y*axis.y + vectorToBox.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/BoxSphereCollider.as.svn-base b/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/BoxSphereCollider.as.svn-base new file mode 100644 index 0000000..2b166f3 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/BoxSphereCollider.as.svn-base @@ -0,0 +1,104 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + use namespace altphysics; + /** + * + */ + public class BoxSphereCollider implements ICollider { + + private var center:Vector3 = new Vector3(); + private var closestPt:Vector3 = new Vector3(); + + private var bPos:Vector3 = new Vector3(); + private var sPos:Vector3 = new Vector3(); + + /** + * + */ + public function BoxSphereCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + // Зафиксированно столкновение + contact.body1 = sphere.body; + contact.body2 = box.body; + contact.normal.vCopy(closestPt).vTransformBy4(box.transform).vSubtract(sPos).vNormalize().vReverse(); + contact.pcount = 1; + + var cp:ContactPoint = contact.points[0]; + cp.penetration = sphere.r - Math.sqrt(distSqr); + cp.pos.vCopy(contact.normal).vScale(-sphere.r).vAdd(sPos); + cp.r1.vDiff(cp.pos, sPos); + cp.r2.vDiff(cp.pos, bPos); + + return true; + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/BoxTriangleCollider.as.svn-base b/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/BoxTriangleCollider.as.svn-base new file mode 100644 index 0000000..63c6efd --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/BoxTriangleCollider.as.svn-base @@ -0,0 +1,14 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Contact; + import alternativa.physics.collision.primitives.CollisionPrimitive; + + public class BoxTriangleCollider implements ICollider { + public function BoxTriangleCollider() { + } + + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/BruteForceCollisionDetector.as.svn-base b/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/BruteForceCollisionDetector.as.svn-base new file mode 100644 index 0000000..d2c2e2a --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/BruteForceCollisionDetector.as.svn-base @@ -0,0 +1,37 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + public class BruteForceCollisionDetector implements ICollisionDetector { + public function BruteForceCollisionDetector() { + } + + public function addPrimitive(primitive:CollisionPrimitive, isStatic:Boolean=true):Boolean { + return false; + } + + public function removePrimitive(primitive:CollisionPrimitive, isStatic:Boolean=true):Boolean { + return false; + } + + public function init():void { + } + + public function getAllCollisions(contacts:Vector.):int { + return 0; + } + + public function intersectRay(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, intersection:RayIntersection):Boolean { + return false; + } + + public function intersectRayWithStatic(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base b/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base new file mode 100644 index 0000000..e1a1657 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base @@ -0,0 +1,16 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.types.BoundBox; + + public class CollisionKdNode { + public var objects:Vector.; + public var boundBox:BoundBox; + public var parent:CollisionKdNode; + + public var axis:int = -1; // 0 - x, 1 - y, 2 - z + public var coord:Number; + public var positiveNode:CollisionKdNode; + public var negativeNode:CollisionKdNode; + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base b/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base new file mode 100644 index 0000000..6c62b79 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base @@ -0,0 +1,259 @@ +package alternativa.physics.collision { + + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + + /** + * @author mike + */ + public class CollisionKdTree { + + public var threshold:Number = 0.01; + public var minPrimitivesPerNode:int = 1; + + public var rootNode:CollisionKdNode; + public var staticChildren:Vector. = new Vector.(); + public var numStaticChildren:int; + private var staticBoundBoxes:Vector. = new Vector.(); + + /** + * @param primitive + */ + public function addStaticPrimitive(primitive:CollisionPrimitive):void { + staticChildren[numStaticChildren++] = primitive; + } + + /** + * @param primitive + * @return + */ + public function removeStaticPrimitive(primitive:CollisionPrimitive):Boolean { + var idx:int = staticChildren.indexOf(primitive); + if (idx < 0) return false; + staticChildren.splice(idx, 1); + numStaticChildren--; + return true; + } + + /** + * @param boundBox + */ + public function createTree(boundBox:BoundBox = null):void { + if (numStaticChildren == 0) return; + // Создаём корневую ноду + rootNode = new CollisionKdNode(); + rootNode.objects = new Vector.(); + // Расчитываем баунды объектов и рутовой ноды + var rootNodeBoundBox:BoundBox = rootNode.boundBox = (boundBox != null) ? boundBox : new BoundBox(); + for (var i:int = 0; i < numStaticChildren; i++) { + var child:CollisionPrimitive = staticChildren[i]; + var childBoundBox:BoundBox = staticBoundBoxes[i] = child.calculateAABB(); + rootNodeBoundBox.addBoundBox(childBoundBox); + rootNode.objects[i] = i; + } + staticBoundBoxes.length = numStaticChildren; + // Разделяем рутовую ноду + splitNode(rootNode); + } + + private var splitAxis:int; + private var splitCoord:Number; + private var splitCost:Number; + static private const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + static private const splitCoordsX:Vector. = new Vector.(); + static private const splitCoordsY:Vector. = new Vector.(); + static private const splitCoordsZ:Vector. = new Vector.(); + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + if (node.objects.length <= minPrimitivesPerNode) return; + + var objects:Vector. = node.objects; + var i:int, j:int, k:int, length:int = objects.length, c1:Number, c2:Number, ct1:Number, ct2:Number, area:Number, areaNegative:Number, areaPositive:Number, numNegative:int, numPositive:int, conflict:Boolean, cost:Number; + + var nodeBoundBox:BoundBox = node.boundBox; + + // Подготовка баунда с погрешностями + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + // Собираем опорные координаты + var numSplitCoordsX:int = 0, numSplitCoordsY:int = 0, numSplitCoordsZ:int = 0; + for (i = 0; i < length; i++) { + var boundBox:BoundBox = staticBoundBoxes[objects[i]]; + if (boundBox.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = boundBox.minX; + if (boundBox.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = boundBox.maxX; + + if (boundBox.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = boundBox.minY; + if (boundBox.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = boundBox.maxY; + + if (boundBox.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.minZ; + if (boundBox.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.maxZ; + } + + // Убираем дубликаты координат, ищем наилучший сплит + splitAxis = -1; splitCost = 1e308; + i = 0; area = (nodeBoundBox.maxY - nodeBoundBox.minY)*(nodeBoundBox.maxZ - nodeBoundBox.minZ); + while (i < numSplitCoordsX) { + if (isNaN(c1 = splitCoordsX[i++])) continue; + ct1 = c1 - threshold; + ct2 = c1 + threshold; + areaNegative = area*(c1 - nodeBoundBox.minX); + areaPositive = area*(nodeBoundBox.maxX - c1); + numNegative = numPositive = 0; + conflict = false; + // Проверяем объекты + for (j = 0; j < length; j++) { + boundBox = staticBoundBoxes[objects[j]]; + if (boundBox.maxX <= ct2) { + if (boundBox.minX < ct1) numNegative++; + } else { + if (boundBox.minX >= ct1) numPositive++; else {conflict = true; break;} + } + } + // Если хороший сплит, сохраняем + if (!conflict && (cost = areaNegative*numNegative + areaPositive*numPositive) < splitCost) { + splitCost = cost; + splitAxis = 0; + splitCoord = c1; + } + j = i; + while (++j < numSplitCoordsX) if ((c2 = splitCoordsX[j]) >= c1 - threshold && c2 <= c1 + threshold) splitCoordsX[j] = NaN; + } + i = 0; area = (nodeBoundBox.maxX - nodeBoundBox.minX)*(nodeBoundBox.maxZ - nodeBoundBox.minZ); + while (i < numSplitCoordsY) { + if (isNaN(c1 = splitCoordsY[i++])) continue; + ct1 = c1 - threshold; + ct2 = c1 + threshold; + areaNegative = area*(c1 - nodeBoundBox.minY); + areaPositive = area*(nodeBoundBox.maxY - c1); + numNegative = numPositive = 0; + conflict = false; + // Проверяем объекты + for (j = 0; j < length; j++) { + boundBox = staticBoundBoxes[objects[j]]; + if (boundBox.maxY <= ct2) { + if (boundBox.minY < ct1) numNegative++; + } else { + if (boundBox.minY >= ct1) numPositive++; else {conflict = true; break;} + } + } + // Если хороший сплит, сохраняем + if (!conflict && (cost = areaNegative*numNegative + areaPositive*numPositive) < splitCost) { + splitCost = cost; + splitAxis = 1; + splitCoord = c1; + } + j = i; + while (++j < numSplitCoordsY) if ((c2 = splitCoordsY[j]) >= c1 - threshold && c2 <= c1 + threshold) splitCoordsY[j] = NaN; + } + i = 0; area = (nodeBoundBox.maxX - nodeBoundBox.minX)*(nodeBoundBox.maxY - nodeBoundBox.minY); + while (i < numSplitCoordsZ) { + if (isNaN(c1 = splitCoordsZ[i++])) continue; + ct1 = c1 - threshold; + ct2 = c1 + threshold; + areaNegative = area*(c1 - nodeBoundBox.minZ); + areaPositive = area*(nodeBoundBox.maxZ - c1); + numNegative = numPositive = 0; + conflict = false; + // Проверяем объекты + for (j = 0; j < length; j++) { + boundBox = staticBoundBoxes[objects[j]]; + if (boundBox.maxZ <= ct2) { + if (boundBox.minZ < ct1) numNegative++; + } else { + if (boundBox.minZ >= ct1) numPositive++; else {conflict = true; break;} + } + } + // Если хороший сплит, сохраняем + if (!conflict && (cost = areaNegative*numNegative + areaPositive*numPositive) < splitCost) { + splitCost = cost; + splitAxis = 2; + splitCoord = c1; + } + j = i; + while (++j < numSplitCoordsZ) if ((c2 = splitCoordsZ[j]) >= c1 - threshold && c2 <= c1 + threshold) splitCoordsZ[j] = NaN; + } + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Разделяем ноду + var axisX:Boolean = splitAxis == 0, axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.positiveNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.positiveNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + ct1 = splitCoord - threshold; ct2 = splitCoord + threshold; + for (i = 0; i < length; i++) { + boundBox = staticBoundBoxes[objects[i]]; + var min:Number = axisX ? boundBox.minX : (axisY ? boundBox.minY : boundBox.minZ); + var max:Number = axisX ? boundBox.maxX : (axisY ? boundBox.maxY : boundBox.maxZ); + if (max <= ct2) { + // Объект в негативной стороне + if (node.negativeNode.objects == null) node.negativeNode.objects = new Vector.(); + node.negativeNode.objects.push(objects[i]); + objects[i] = -1; + } else { + if (min >= ct1) { + // Объект в положительной стороне + if (node.positiveNode.objects == null) node.positiveNode.objects = new Vector.(); + node.positiveNode.objects.push(objects[i]); + objects[i] = -1; + } else { + // Распилился + } + } + } + + // Очистка списка объектов + j = 0; + for (i = 0; i < length; i++) { + if (objects[i] >= 0) objects[j++] = objects[i]; + } + if (j > 0) objects.length = j; else node.objects = null; + + // Разделение дочерних нод + if (node.negativeNode.objects != null) splitNode(node.negativeNode); + if (node.positiveNode.objects != null) splitNode(node.positiveNode); + } + + /** + * + */ + public function traceTree():void { + traceNode("", rootNode); + } + + /** + * @param str + * @param node + */ + private function traceNode(str:String, node:CollisionKdNode):void { + if (node == null) return; + trace(str, node.axis == -1 ? "end" : ((node.axis == 0) ? "X" : ((node.axis == 1) ? "Y" : "Z")), "splitCoord=" + splitCoord, "bound", node.boundBox, "objs:", node.objects); + traceNode(str + "-", node.negativeNode); + traceNode(str + "+", node.positiveNode); + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base b/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base new file mode 100644 index 0000000..733231b --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base @@ -0,0 +1,12 @@ +package alternativa.physics.collision { + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + + /** + * Интерфейс определителя столкновений между двумя примитивами. + */ + public interface ICollider { + function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base b/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base new file mode 100644 index 0000000..83e94d7 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base @@ -0,0 +1,73 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + /** + * Интерфейс детектора столкновений. + */ + public interface ICollisionDetector { + + /** + * Добавляет физический примитив в коллайдер. + * + * @param primitive добавляемый примитив + * @param isStatic указывает тип примитива: статический или динамический + * @return true если примитив был успешно добавлен, иначе false + */ + function addPrimitive(primitive:CollisionPrimitive, isStatic:Boolean = true):Boolean; + + /** + * Удаляет физический примитив из коллайдера. + * + * @param primitive удаляемый примитив + * @param isStatic указывает тип примитива: статический или динамический + * @return true если примитив был успшено удалён, иначе false + */ + function removePrimitive(primitive:CollisionPrimitive, isStatic:Boolean = true):Boolean; + + /** + * Выполняет инициализацию детектора после обновления списка примитивов. + */ + function init():void; + + /** + * Получает все столкновения в текущей конфигурации физической геометрии. + * + * @param contacts список контактов, в кторые будет записана информация о столкновении + * @return количество найденных столкновений + */ + function getAllCollisions(contacts:Vector.):int; + + /** + * Тестирует луч на пересечение с физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param dir направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRay(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * Тестирует луч на пересечение со статической физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param dir направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRayWithStatic(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base b/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base new file mode 100644 index 0000000..bb1b756 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base @@ -0,0 +1,9 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + + public interface ICollisionPredicate { + + function considerCollision(primitive:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base b/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base new file mode 100644 index 0000000..0f3d929 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Body; + + public interface IRayCollisionPredicate { + function considerBody(body:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base b/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base new file mode 100644 index 0000000..46435f9 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base @@ -0,0 +1,423 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Детектор, хранящий статическую геометрию в kD-дереве и использующий дерево для ускорения тестов на пересечения. + */ + public class KdTreeCollisionDetector implements ICollisionDetector { + + altphysics var tree:CollisionKdTree; + altphysics var dynamicPrimitives:Vector.; + 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.(); + + addCollider(CollisionPrimitive.BOX, CollisionPrimitive.BOX, new BoxBoxCollider()); +// addCollider(CollisionPrimitive.BOX, CollisionPrimitive.PLANE, new BoxPlaneCollider()); + addCollider(CollisionPrimitive.BOX, CollisionPrimitive.SPHERE, new BoxSphereCollider()); + addCollider(CollisionPrimitive.BOX, CollisionPrimitive.RECT, new BoxRectCollider()); + +// 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():void { + tree.createTree(); +// tree.traceTree(); + } + + /** + * + * @param contacts + * @return + */ + public function getAllCollisions(contacts:Vector.):int { + var colNum:int = 0; + for (var i:int = 0; i < dynamicPrimitivesNum; i++) { + var primitive:CollisionPrimitive = dynamicPrimitives[i]; + primitive.calculateAABB(); + colNum += getPrimitiveNodeCollisions(tree.rootNode, primitive, contacts, colNum); + + // Столкновения динамических примитивов между собой + // TODO: Попробовать различные оптимизации (сортировка по баундам, встраивание в дерево) + for (var j:int = i + 1; j < dynamicPrimitivesNum; j++) { + if (collide(primitive, dynamicPrimitives[j], contacts[colNum])) colNum++; + } + } + + return colNum; + } + + /** + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + public function collide(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.collide(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 = (contact.points[0] as ContactPoint).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 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 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:Vector., startIndex:int):int { + var colNum:int = 0; + if (node.objects != null) { + // Поиск столкновений со статическими примитивами узла + var primitives:Vector. = tree.staticChildren; + var indices:Vector. = node.objects; + for (var i:int = indices.length - 1; i >= 0; i--) + if (collide(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; + } + + /** + * Тест пересечения луча с динамическими примитивами. + * + * @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 minTime:Number = maxTime + 1; + for (var i:int = 0; i < dynamicPrimitivesNum; i++) { + var primitive:CollisionPrimitive = dynamicPrimitives[i]; + if ((primitive.collisionGroup & collisionGroup) == 0) continue; + if (predicate != null && primitive.body != null && !predicate.considerBody(primitive.body)) continue; + var t:Number = primitive.getSegmentIntersection(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.objects != null && getRayNodeIntersection(origin, dir, collisionGroup, tree.staticChildren, node.objects, 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., indices:Vector., 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.getSegmentIntersection(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; + } \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/SpherePlaneCollider.as.svn-base b/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/SpherePlaneCollider.as.svn-base new file mode 100644 index 0000000..9b527d8 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/SpherePlaneCollider.as.svn-base @@ -0,0 +1,58 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + /** + * + */ + public class SpherePlaneCollider implements ICollider { + + private var normal:Vector3 = new Vector3(); + + /** + * + */ + public function SpherePlaneCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var sphere:RigidSphere = body1 as RigidSphere; +// var plane:RigidPlane; +// if (sphere == null) { +// sphere = body2 as RigidSphere; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// +// var dist:Number = sphere.state.pos.dot(normal) - offset; +// if (dist > sphere.r) return false; +// +// collisionInfo.body1 = sphere; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// collisionInfo.pcount = 1; +// +// var cp:ContactPoint = collisionInfo.points[0]; +// cp.penetration = sphere.r - dist; +// cp.pos.copy(normal).reverse().scale(sphere.r).add(sphere.state.pos); +// cp.r1.diff(cp.pos, sphere.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); + + return true; + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/SphereSphereCollider.as.svn-base b/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/SphereSphereCollider.as.svn-base new file mode 100644 index 0000000..9111a84 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/.svn/text-base/SphereSphereCollider.as.svn-base @@ -0,0 +1,60 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + public class SphereSphereCollider implements ICollider { + + private var p1:Vector3 = new Vector3(); + private var p2:Vector3 = new Vector3(); + + public function SphereSphereCollider() { + } + + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var s1:CollisionSphere; + var s2:CollisionSphere; + if (prim1.body != null) { + s1 = prim1 as CollisionSphere; + s2 = prim2 as CollisionSphere; + } else { + s1 = prim2 as CollisionSphere; + s2 = prim1 as CollisionSphere; + } + + s1.transform.getAxis(3, p1); + s2.transform.getAxis(3, p2); + var dx:Number = p1.x - p2.x; + var dy:Number = p1.y - p2.y; + var dz:Number = p1.z - p2.z; + var len:Number = dx*dx + dy*dy + dz*dz; + var sum:Number = s1.r + s2.r; + if (len > sum*sum) return false; + len = Math.sqrt(len); + dx /= len; + dy /= len; + dz /= len; + + contact.body1 = s1.body; + contact.body2 = s2.body; + contact.normal.x = dx; + contact.normal.y = dy; + contact.normal.z = dz; + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = sum - len; + cp.pos.x = p1.x - dx*s1.r; + cp.pos.y = p1.y - dy*s1.r; + cp.pos.z = p1.z - dz*s1.r; + cp.r1.vDiff(cp.pos, p1); + cp.r2.vDiff(cp.pos, p2); + + return true; + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/collision/BoxBoxCollider.as b/0.0.1.0/src/alternativa/physics/collision/BoxBoxCollider.as new file mode 100644 index 0000000..2893b49 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/BoxBoxCollider.as @@ -0,0 +1,791 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Расчитывает точки контакта двух боксов. Нормаль контакта направляется в сторону бокса с меньшим ID. + */ + public class BoxBoxCollider implements ICollider { + + private var tolerance:Number = 0.001; + + private var pos1:Vector3 = new Vector3(); + private var pos2:Vector3 = new Vector3(); + private var vectorToBox1:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var colAxis:Vector3 = new Vector3(); + private var tmpAxis:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var tmpPoints:Vector. = new Vector.(8, true); + private var pcount:int; + + /** + * + */ + public function BoxBoxCollider() { + for (var i:int = 0; i < 8; i++) { + tmpPoints[i] = new ContactPoint(); + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + /** + * + * @param body1 + * @param body2 + * @param contactInfo + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + minOverlap = 1e10; + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + // Вектор из центра второго бокса в центр первого + pos1.x = transform1.d; pos1.y = transform1.h; pos1.z = transform1.l; + pos2.x = transform2.d; pos2.y = transform2.h; pos2.z = transform2.l; + vectorToBox1.x = pos1.x - pos2.x; + vectorToBox1.y = pos1.y - pos2.y; + vectorToBox1.z = pos1.z - pos2.z; + + // Проверка пересечения по основным осям +// box1.transform.getAxis(0, axis10); + axis10.x = transform1.a; axis10.y = transform1.e; axis10.z = transform1.i; + if (!testAxis(box1, box2, axis10, 0, vectorToBox1)) return false; +// box1.transform.getAxis(1, axis11); + axis11.x = transform1.b; axis11.y = transform1.f; axis11.z = transform1.j; + if (!testAxis(box1, box2, axis11, 1, vectorToBox1)) return false; +// box1.transform.getAxis(2, axis12); + axis12.x = transform1.c; axis12.y = transform1.g; axis12.z = transform1.k; + if (!testAxis(box1, box2, axis12, 2, vectorToBox1)) return false; + +// box2.transform.getAxis(0, axis20); + axis20.x = transform2.a; axis20.y = transform2.e; axis20.z = transform2.i; + if (!testAxis(box1, box2, axis20, 3, vectorToBox1)) return false; +// box2.transform.getAxis(1, axis21); + axis21.x = transform2.b; axis21.y = transform2.f; axis21.z = transform2.j; + if (!testAxis(box1, box2, axis21, 4, vectorToBox1)) return false; +// box2.transform.getAxis(2, axis22); + axis22.x = transform2.c; axis22.y = transform2.g; axis22.z = transform2.k; + if (!testAxis(box1, box2, axis22, 5, vectorToBox1)) return false; + + // Проверка производных осей + if (!testAxis(box1, box2, axis.vCross2(axis10, axis20), 6, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis10, axis21), 7, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis10, axis22), 8, vectorToBox1)) return false; + + if (!testAxis(box1, box2, axis.vCross2(axis11, axis20), 9, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis11, axis21), 10, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis11, axis22), 11, vectorToBox1)) return false; + + if (!testAxis(box1, box2, axis.vCross2(axis12, axis20), 12, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis12, axis21), 13, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis12, axis22), 14, vectorToBox1)) return false; + + if (bestAxisIndex < 6) { + // Контакт грань-(грань|ребро|вершина) + findFaceContactPoints(box1, box2, vectorToBox1, bestAxisIndex, contact); + } else { + // Контакт ребро-ребро + bestAxisIndex -= 6; + findEdgesIntersection(box1, box2, vectorToBox1, int(bestAxisIndex/3), bestAxisIndex%3, contact); + } + contact.body1 = box1.body; + contact.body2 = box2.body; + + return true; + } + + /** + * Выполняет поиск точек контакта грани одного бокса с гранью/ребром/вершиной другого. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param faceAxisIdx индекс оси первого бокса, перпендикулярной грани, с которой произошло столкновение + * @param contactInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, faceAxisIdx:int, contactInfo:Contact):void { + var swapNormal:Boolean = false; + if (faceAxisIdx > 2) { + // Столкновение с гранью второго бокса. Для дальнейших расчётов боксы меняются местами, + // но нормаль контакта всё равно должна быть направлена в сторону первоначального box1 + var tmpBox:CollisionBox = box1; + box1 = box2; + box2 = tmpBox; + vectorToBox1.vReverse(); + faceAxisIdx -= 3; + swapNormal = true; + } + box1.transform.getAxis(faceAxisIdx, colAxis); + var faceReversed:Boolean = colAxis.vDot(vectorToBox1) > 0; + if (!faceReversed) colAxis.vReverse(); + // Ищем ось второго бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + box2.transform.getAxis(axisIdx, axis); + var dot:Number = axis.vDot(colAxis); + if (dot < 0) dot = -dot; + if (dot > maxDot) { + maxDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Получаем список вершин грани второго бокса, переводим их в систему координат первого бокса и выполняем обрезку + // по грани первого бокса. Таким образом получается список потенциальных точек контакта. + box2.transform.getAxis(incFaceAxisIdx, axis); + getFaceVertsByAxis(box2, incFaceAxisIdx, axis.vDot(colAxis) < 0, verts1); + box2.transform.transformVectors(verts1, verts2); + box1.transform.transformVectorsInverse(verts2, verts1); + var pnum:int = clip(box1.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + pcount = 0; + for (var i:int = 0; i < pnum; i++) { + if ((pen = getPointBoxPenetration(box1.hs, verts1[i], faceAxisIdx, faceReversed)) > -tolerance) { + var cp:ContactPoint = tmpPoints[pcount++]; + box1.transform.transformVector(verts1[i], cp.pos); + cp.r1.vDiff(cp.pos, pos1); + cp.r2.vDiff(cp.pos, pos2); + cp.penetration = pen; + } + } + contactInfo.normal.vCopy(colAxis); + if (swapNormal) contactInfo.normal.vReverse(); + + if (pcount > 4) reducePoints(); + for (i = 0; i < pcount; i++) (contactInfo.points[i] as ContactPoint).copyFrom(tmpPoints[i]); + contactInfo.pcount = pcount; + } + + /** + * + * @param contactInfo + */ + private function reducePoints():void { + var i:int; + var minIdx:int; + var cp1:ContactPoint; + var cp2:ContactPoint; + while (pcount > 4) { + var minLen:Number = 1e10; + var p1:Vector3 = (tmpPoints[pcount - 1] as ContactPoint).pos; + var p2:Vector3; + for (i = 0; i < pcount; i++) { + p2 = (tmpPoints[i] as ContactPoint).pos; + var dx:Number = p2.x - p1.x; + var dy:Number = p2.y - p1.y; + var dz:Number = p2.z - p1.z; + var len:Number = dx*dx + dy*dy + dz*dz; + if (len < minLen) { + minLen = len; + minIdx = i; + } + p1 = p2; + } + cp1 = tmpPoints[minIdx == 0 ? (pcount - 1) : (minIdx - 1)]; + cp2 = tmpPoints[minIdx]; + p1 = cp1.pos; + p2 = cp2.pos; + p2.x = 0.5*(p1.x + p2.x); + p2.y = 0.5*(p1.y + p2.y); + p2.z = 0.5*(p1.z + p2.z); + cp2.penetration = 0.5*(cp1.penetration + cp2.penetration); + if (minIdx > 0) { + for (i = minIdx; i < pcount; i++) tmpPoints[i - 1] = tmpPoints[i]; + tmpPoints[pcount - 1] = cp1; + } + pcount--; + } + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + private function getFaceVertsByAxis(box:CollisionBox, axisIdx:int, reverse:Boolean, result:Vector.):void { + var hs:Vector3 = box.hs; + switch (axisIdx) { + case 0: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, -hs.y, hs.z); + } + break; + case 1: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, hs.y, hs.z); + } + break; + case 2: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + } else { + (result[0] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } + break; + } + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clip(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + return clipHighX(hs.x, pnum, verts2, verts1); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + } + return 0; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param contactInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):void { + box1.transform.getAxis(axisIdx1, axis10); + box2.transform.getAxis(axisIdx2, axis20); + colAxis.vCross2(axis10, axis20).vNormalize(); + // Разворот оси в сторону первого бокса + if (colAxis.vDot(vectorToBox1) < 0) colAxis.vReverse(); + + /* Кодирование рёбер + бит 0: 1 (тип контакта ребро-ребро) + биты 1-2: индекс направляющей оси ребра + бит 3: + бит 4: + бит 5: + */ + var edgeCode1:int = 1; + var edgeCode2:int = 1; + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + point1.vCopy(box1.hs); + point2.vCopy(box2.hs); + // x + if (axisIdx1 == 0) { + point1.x = 0; + halfLen1 = box1.hs.x; + } else { + box1.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.x = -point1.x; + edgeCode1 |= 8; // 1 << 3 + } + } + if (axisIdx2 == 0) { + point2.x = 0; + halfLen2 = box2.hs.x; + } else { + box2.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.x = -point2.x; + edgeCode2 |= 8; // 1 << 3 + } + } + // y + if (axisIdx1 == 1) { + point1.y = 0; + halfLen1 = box1.hs.y; + edgeCode1 |= 2; // 1 << 1 + } else { + box1.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.y = -point1.y; + edgeCode1 |= 16; // 1 << 4 + } + } + if (axisIdx2 == 1) { + point2.y = 0; + halfLen2 = box2.hs.y; + edgeCode2 |= 2; // 1 << 1 + } else { + box2.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.y = -point2.y; + edgeCode2 |= 16; // 1 << 4 + } + } + // z + if (axisIdx1 == 2) { + point1.z = 0; + halfLen1 = box1.hs.z; + edgeCode1 |= 4; // 2 << 1 + } else { + box1.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.z = -point1.z; + edgeCode1 |= 32; // 1 << 5 + } + } + if (axisIdx2 == 2) { + point2.z = 0; + halfLen2 = box2.hs.z; + edgeCode2 |= 4; // 2 << 1 + } else { + box2.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.z = -point2.z; + edgeCode2 |= 32; // 1 << 5 + } + } + // Получаем глобальные координаты средних точек рёбер + point1.vTransformBy4(box1.transform); + point2.vTransformBy4(box2.transform); + // Находим точку пересечения рёбер, решая систему уравнений + var k:Number = axis10.vDot(axis20); + var det:Number = k*k - 1; + vector.vDiff(point2, point1); + var c1:Number = axis10.vDot(vector); + var c2:Number = axis20.vDot(vector); + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.normal.vCopy(colAxis); + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(point1.x + axis10.x*t1 + point2.x + axis20.x*t2); + cp.pos.y = 0.5*(point1.y + axis10.y*t1 + point2.y + axis20.y*t2); + cp.pos.z = 0.5*(point1.z + axis10.z*t1 + point2.z + axis20.z*t2); + cp.r1.vDiff(cp.pos, pos1); + cp.r2.vDiff(cp.pos, pos2); + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение боксов вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box1 + * @param box2 + * @param axis + * @param axisIndex + * @param vectorToBox1 + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, axisIndex:int, vectorToBox1:Vector3):Boolean { + if (axis.vLengthSqr() < 0.0001) { + return true; + } + axis.vNormalize(); + + var overlap:Number = overlapOnAxis(box1, box2, axis, vectorToBox1); + if (overlap < -tolerance) return false; + if (overlap + tolerance < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия двух боксов вдоль заданной оси. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param axis ось + * @param vectorToBox1 вектор, соединяющий центр второго бокса с центром первого + * @return величина перекрытия боксов вдоль оси + */ + public function overlapOnAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, vectorToBox1:Vector3):Number { + var m:Matrix4 = box1.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box1.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box1.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box1.hs.z; + if (d < 0) d = -d; + projection += d; + + m = box2.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box2.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box2.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box2.hs.z; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox1.x*axis.x + vectorToBox1.y*axis.y + vectorToBox1.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} + import alternativa.physics.types.Vector3; + + class CollisionPointTmp { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + } diff --git a/0.0.1.0/src/alternativa/physics/collision/BoxPlaneCollider.as b/0.0.1.0/src/alternativa/physics/collision/BoxPlaneCollider.as new file mode 100644 index 0000000..8568dc7 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/BoxPlaneCollider.as @@ -0,0 +1,78 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + public class BoxPlaneCollider implements ICollider { + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var normal:Vector3 = new Vector3(); + + public function BoxPlaneCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var box:RigidBox = body1 as RigidBox; +// var plane:RigidPlane; +// if (box == null) { +// box = body2 as RigidBox; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные координаты вершин бокса +// var sx:Number = box.halfSize.x; +// var sy:Number = box.halfSize.y; +// var sz:Number = box.halfSize.z; +// (verts1[0] as Vector3).reset(-sx, -sy, -sz); +// (verts1[1] as Vector3).reset(sx, -sy, -sz); +// (verts1[2] as Vector3).reset(sx, sy, -sz); +// (verts1[3] as Vector3).reset(-sx, sy, -sz); +// (verts1[4] as Vector3).reset(-sx, -sy, sz); +// (verts1[5] as Vector3).reset(sx, -sy, sz); +// (verts1[6] as Vector3).reset(sx, sy, sz); +// (verts1[7] as Vector3).reset(-sx, sy, sz); +// +// box.transform.transformVectors(verts1, verts2); +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// // Проверяем наличие столкновений с каждой вершиной +// collisionInfo.pcount = 0; +// for (var i:int = 0; i < 8; i++) { +// // Вершина добавляется в список точек столкновения, если лежит под плоскостью +// var dist:Number = (verts2[i] as Vector3).dot(normal); +// if (dist < offset) { +// var cp:ContactPoint; +// if (collisionInfo.pcount == collisionInfo.points.length) { +// cp = new ContactPoint(); +// collisionInfo.points[collisionInfo.pcount] = cp; +// } else { +// cp = collisionInfo.points[collisionInfo.pcount]; +// } +// cp.pos.copy(verts2[i]); +// cp.r1.diff(cp.pos, box.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); +// cp.penetration = offset - dist; +// collisionInfo.pcount++; +// } +// } +// if (collisionInfo.pcount > 0) { +// collisionInfo.body1 = box; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// return true; +// } + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/collision/BoxRectCollider.as b/0.0.1.0/src/alternativa/physics/collision/BoxRectCollider.as new file mode 100644 index 0000000..b5287f3 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/BoxRectCollider.as @@ -0,0 +1,734 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + public class BoxRectCollider implements ICollider { + + private var tolerance:Number = 0.001; + + private var bPos:Vector3 = new Vector3(); + private var rPos:Vector3 = new Vector3(); + + private var vectorToBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var colAxis:Vector3 = new Vector3(); + private var tmpAxis:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxRectCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + /** + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + minOverlap = 1e10; + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + + // Вектор из центра прямоугольника в центр бокса + box.transform.getAxis(3, bPos); + rect.transform.getAxis(3, rPos); + vectorToBox.vDiff(bPos, rPos); + + // Проверка пересечения по нормали прямоугольника + rect.transform.getAxis(2, axis22); + if (!testAxis(box, rect, axis22, 0, vectorToBox)) return false; + + // Проверка пересечения по основным осям бокса + box.transform.getAxis(0, axis10); + if (!testAxis(box, rect, axis10, 1, vectorToBox)) return false; + box.transform.getAxis(1, axis11); + if (!testAxis(box, rect, axis11, 2, vectorToBox)) return false; + box.transform.getAxis(2, axis12); + if (!testAxis(box, rect, axis12, 3, vectorToBox)) return false; + + // Получаем направляющие рёбер прямоугольника + rect.transform.getAxis(0, axis20); + rect.transform.getAxis(1, axis21); + + // Проверка производных осей + if (!testAxis(box, rect, axis.vCross2(axis10, axis20), 4, vectorToBox)) return false; + if (!testAxis(box, rect, axis.vCross2(axis10, axis21), 5, vectorToBox)) return false; + + if (!testAxis(box, rect, axis.vCross2(axis11, axis20), 6, vectorToBox)) return false; + if (!testAxis(box, rect, axis.vCross2(axis11, axis21), 7, vectorToBox)) return false; + + if (!testAxis(box, rect, axis.vCross2(axis12, axis20), 8, vectorToBox)) return false; + if (!testAxis(box, rect, axis.vCross2(axis12, axis21), 9, vectorToBox)) return false; + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, rect, vectorToBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + findEdgesIntersection(box, rect, vectorToBox, int(bestAxisIndex/2), bestAxisIndex%2, contact); + } + contact.body1 = box.body; + contact.body2 = rect.body; + + return true; + } + + /** + * Выполняет поиск точек контакта бокса с прямоугольником. + * + * @param box бокс + * @param rect прямоугольник + * @param vectorToBox вектор, направленный из центра прямоугольника в центр бокса + * @param faceAxisIdx индекс оси, идентифицирующей полскость столкновения (грань бокса или полскость прямоугольника) + * @param colInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, faceAxisIdx:int, colInfo:Contact):Boolean { + var pnum:int, i:int, v:Vector3, cp:ContactPoint; + if (faceAxisIdx == 0) { + // Столкновение с плоскостью прямоугольника + + // Проверим положение бокса относительно плоскости прямоугольника + rect.transform.getAxis(2, colAxis); + var offset:Number = colAxis.vDot(rPos); + if (bPos.vDot(colAxis) < offset) return false; + + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + box.transform.getAxis(axisIdx, axis); + var dot:Number = axis.vDot(colAxis); + if (dot < 0) dot = -dot; + if (dot > maxDot) { + maxDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Получаем список вершин грани бокса, переводим их в систему координат прямоугольника и выполняем обрезку + // по прямоугольнику. Таким образом получается список потенциальных точек контакта. + box.transform.getAxis(incFaceAxisIdx, axis); + getFaceVertsByAxis(box.hs, incFaceAxisIdx, axis.vDot(colAxis) > 0, verts1); + box.transform.transformVectors(verts1, verts2); + rect.transform.transformVectorsInverse(verts2, verts1); + pnum = clipByRect(rect.hs); + // Проверяем каждую потенциальную точку на принадлежность нижней полуплоскости прямоугольника и добавляем такие точки в список контактов + colInfo.pcount = 0; + for (i = 0; i < pnum; i++) { + v = verts1[i]; + if (v.z < tolerance) { + cp = colInfo.points[colInfo.pcount++]; + rect.transform.transformVector(v, cp.pos); + cp.r1.vDiff(cp.pos, bPos); + cp.r2.vDiff(cp.pos, rPos); + cp.penetration = -v.z; + } + } + colInfo.normal.vCopy(colAxis); + } else { + // Столкновение с гранью бокса + faceAxisIdx--; + box.transform.getAxis(faceAxisIdx, colAxis); + var faceReversed:Boolean = colAxis.vDot(vectorToBox) > 0; + if (!faceReversed) colAxis.vReverse(); + + rect.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) return false; + + getFaceVertsByAxis(rect.hs, 2, false, verts1); + rect.transform.transformVectors(verts1, verts2); + box.transform.transformVectorsInverse(verts2, verts1); + pnum = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + colInfo.pcount = 0; + for (i = 0; i < pnum; i++) { + if ((pen = getPointBoxPenetration(box.hs, verts1[i], faceAxisIdx, faceReversed)) > -tolerance) { + cp = colInfo.points[colInfo.pcount++]; + box.transform.transformVector(verts1[i], cp.pos); + cp.r1.vDiff(cp.pos, bPos); + cp.r2.vDiff(cp.pos, rPos); + cp.penetration = pen; + } + } + colInfo.normal.vCopy(colAxis); + } + return true; + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + private function getFaceVertsByAxis(hs:Vector3, axisIdx:int, reverse:Boolean, result:Vector.):void { + switch (axisIdx) { + case 0: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, -hs.y, hs.z); + } + break; + case 1: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, hs.y, hs.z); + } + break; + case 2: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + } else { + (result[0] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } + break; + } + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + return clipHighX(hs.x, pnum, verts2, verts1); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + } + return 0; + } + + /** + * + * @param hs + * @return + */ + private function clipByRect(hs:Vector3):int { + var pnum:int = 4; + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box первый бокс + * @param rect второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param colInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, axisIdx1:int, axisIdx2:int, colInfo:Contact):void { + box.transform.getAxis(axisIdx1, axis10); + rect.transform.getAxis(axisIdx2, axis20); + colAxis.vCross2(axis10, axis20).vNormalize(); + // Разворот оси в сторону бокса + if (colAxis.vDot(vectorToBox) < 0) colAxis.vReverse(); + + /* Кодирование рёбер + бит 0: 1 (тип контакта ребро-ребро) + биты 1-2: индекс направляющей оси ребра + бит 3: + бит 4: + бит 5: + */ + var edgeCode1:int = 1; + var edgeCode2:int = 1; + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + point1.vCopy(box.hs); + point2.vCopy(rect.hs); + // x + if (axisIdx1 == 0) { + point1.x = 0; + halfLen1 = box.hs.x; + } else { + box.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.x = -point1.x; + edgeCode1 |= 8; // 1 << 3 + } + } + if (axisIdx2 == 0) { + point2.x = 0; + halfLen2 = rect.hs.x; + } else { + rect.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.x = -point2.x; + edgeCode2 |= 8; // 1 << 3 + } + } + // y + if (axisIdx1 == 1) { + point1.y = 0; + halfLen1 = box.hs.y; + edgeCode1 |= 2; // 1 << 1 + } else { + box.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.y = -point1.y; + edgeCode1 |= 16; // 1 << 4 + } + } + if (axisIdx2 == 1) { + point2.y = 0; + halfLen2 = rect.hs.y; + edgeCode2 |= 2; // 1 << 1 + } else { + rect.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.y = -point2.y; + edgeCode2 |= 16; // 1 << 4 + } + } + // z + if (axisIdx1 == 2) { + point1.z = 0; + halfLen1 = box.hs.z; + edgeCode1 |= 4; // 2 << 1 + } else { + box.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.z = -point1.z; + edgeCode1 |= 32; // 1 << 5 + } + } + // Получаем глобальные координаты средних точек рёбер + point1.vTransformBy4(box.transform); + point2.vTransformBy4(rect.transform); + // Находим точку пересечения рёбер, решая систему уравнений + var k:Number = axis10.vDot(axis20); + var det:Number = k*k - 1; + vector.vDiff(point2, point1); + var c1:Number = axis10.vDot(vector); + var c2:Number = axis20.vDot(vector); + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + colInfo.normal.vCopy(colAxis); + colInfo.pcount = 1; + var cp:ContactPoint = colInfo.points[0]; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(point1.x + axis10.x*t1 + point2.x + axis20.x*t2); + cp.pos.y = 0.5*(point1.y + axis10.y*t1 + point2.y + axis20.y*t2); + cp.pos.z = 0.5*(point1.z + axis10.z*t1 + point2.z + axis20.z*t2); + cp.r1.vDiff(cp.pos, bPos); + cp.r2.vDiff(cp.pos, rPos); + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box + * @param rect + * @param axis + * @param axisIndex + * @param vectorToBox + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + if (axis.vLengthSqr() < 0.0001) return true; + axis.vNormalize(); + + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -tolerance) return false; + if (overlap + tolerance < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия вдоль заданной оси. + * + * @param box бокс + * @param rect прямоугольник + * @param axis ось + * @param vectorToBox вектор, соединяющий центр прямоугольника с центром бокса + * @return величина перекрытия вдоль оси + */ + public function overlapOnAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, vectorToBox:Vector3):Number { + var m:Matrix4 = box.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box.hs.z; + if (d < 0) d = -d; + projection += d; + + m = rect.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*rect.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*rect.hs.y; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox.x*axis.x + vectorToBox.y*axis.y + vectorToBox.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/collision/BoxSphereCollider.as b/0.0.1.0/src/alternativa/physics/collision/BoxSphereCollider.as new file mode 100644 index 0000000..2b166f3 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/BoxSphereCollider.as @@ -0,0 +1,104 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + use namespace altphysics; + /** + * + */ + public class BoxSphereCollider implements ICollider { + + private var center:Vector3 = new Vector3(); + private var closestPt:Vector3 = new Vector3(); + + private var bPos:Vector3 = new Vector3(); + private var sPos:Vector3 = new Vector3(); + + /** + * + */ + public function BoxSphereCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + // Зафиксированно столкновение + contact.body1 = sphere.body; + contact.body2 = box.body; + contact.normal.vCopy(closestPt).vTransformBy4(box.transform).vSubtract(sPos).vNormalize().vReverse(); + contact.pcount = 1; + + var cp:ContactPoint = contact.points[0]; + cp.penetration = sphere.r - Math.sqrt(distSqr); + cp.pos.vCopy(contact.normal).vScale(-sphere.r).vAdd(sPos); + cp.r1.vDiff(cp.pos, sPos); + cp.r2.vDiff(cp.pos, bPos); + + return true; + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/collision/BoxTriangleCollider.as b/0.0.1.0/src/alternativa/physics/collision/BoxTriangleCollider.as new file mode 100644 index 0000000..63c6efd --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/BoxTriangleCollider.as @@ -0,0 +1,14 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Contact; + import alternativa.physics.collision.primitives.CollisionPrimitive; + + public class BoxTriangleCollider implements ICollider { + public function BoxTriangleCollider() { + } + + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/collision/BruteForceCollisionDetector.as b/0.0.1.0/src/alternativa/physics/collision/BruteForceCollisionDetector.as new file mode 100644 index 0000000..d2c2e2a --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/BruteForceCollisionDetector.as @@ -0,0 +1,37 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + public class BruteForceCollisionDetector implements ICollisionDetector { + public function BruteForceCollisionDetector() { + } + + public function addPrimitive(primitive:CollisionPrimitive, isStatic:Boolean=true):Boolean { + return false; + } + + public function removePrimitive(primitive:CollisionPrimitive, isStatic:Boolean=true):Boolean { + return false; + } + + public function init():void { + } + + public function getAllCollisions(contacts:Vector.):int { + return 0; + } + + public function intersectRay(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, intersection:RayIntersection):Boolean { + return false; + } + + public function intersectRayWithStatic(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/collision/CollisionKdNode.as b/0.0.1.0/src/alternativa/physics/collision/CollisionKdNode.as new file mode 100644 index 0000000..e1a1657 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/CollisionKdNode.as @@ -0,0 +1,16 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.types.BoundBox; + + public class CollisionKdNode { + public var objects:Vector.; + public var boundBox:BoundBox; + public var parent:CollisionKdNode; + + public var axis:int = -1; // 0 - x, 1 - y, 2 - z + public var coord:Number; + public var positiveNode:CollisionKdNode; + public var negativeNode:CollisionKdNode; + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/collision/CollisionKdTree.as b/0.0.1.0/src/alternativa/physics/collision/CollisionKdTree.as new file mode 100644 index 0000000..6c62b79 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/CollisionKdTree.as @@ -0,0 +1,259 @@ +package alternativa.physics.collision { + + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + + /** + * @author mike + */ + public class CollisionKdTree { + + public var threshold:Number = 0.01; + public var minPrimitivesPerNode:int = 1; + + public var rootNode:CollisionKdNode; + public var staticChildren:Vector. = new Vector.(); + public var numStaticChildren:int; + private var staticBoundBoxes:Vector. = new Vector.(); + + /** + * @param primitive + */ + public function addStaticPrimitive(primitive:CollisionPrimitive):void { + staticChildren[numStaticChildren++] = primitive; + } + + /** + * @param primitive + * @return + */ + public function removeStaticPrimitive(primitive:CollisionPrimitive):Boolean { + var idx:int = staticChildren.indexOf(primitive); + if (idx < 0) return false; + staticChildren.splice(idx, 1); + numStaticChildren--; + return true; + } + + /** + * @param boundBox + */ + public function createTree(boundBox:BoundBox = null):void { + if (numStaticChildren == 0) return; + // Создаём корневую ноду + rootNode = new CollisionKdNode(); + rootNode.objects = new Vector.(); + // Расчитываем баунды объектов и рутовой ноды + var rootNodeBoundBox:BoundBox = rootNode.boundBox = (boundBox != null) ? boundBox : new BoundBox(); + for (var i:int = 0; i < numStaticChildren; i++) { + var child:CollisionPrimitive = staticChildren[i]; + var childBoundBox:BoundBox = staticBoundBoxes[i] = child.calculateAABB(); + rootNodeBoundBox.addBoundBox(childBoundBox); + rootNode.objects[i] = i; + } + staticBoundBoxes.length = numStaticChildren; + // Разделяем рутовую ноду + splitNode(rootNode); + } + + private var splitAxis:int; + private var splitCoord:Number; + private var splitCost:Number; + static private const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + static private const splitCoordsX:Vector. = new Vector.(); + static private const splitCoordsY:Vector. = new Vector.(); + static private const splitCoordsZ:Vector. = new Vector.(); + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + if (node.objects.length <= minPrimitivesPerNode) return; + + var objects:Vector. = node.objects; + var i:int, j:int, k:int, length:int = objects.length, c1:Number, c2:Number, ct1:Number, ct2:Number, area:Number, areaNegative:Number, areaPositive:Number, numNegative:int, numPositive:int, conflict:Boolean, cost:Number; + + var nodeBoundBox:BoundBox = node.boundBox; + + // Подготовка баунда с погрешностями + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + // Собираем опорные координаты + var numSplitCoordsX:int = 0, numSplitCoordsY:int = 0, numSplitCoordsZ:int = 0; + for (i = 0; i < length; i++) { + var boundBox:BoundBox = staticBoundBoxes[objects[i]]; + if (boundBox.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = boundBox.minX; + if (boundBox.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = boundBox.maxX; + + if (boundBox.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = boundBox.minY; + if (boundBox.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = boundBox.maxY; + + if (boundBox.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.minZ; + if (boundBox.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.maxZ; + } + + // Убираем дубликаты координат, ищем наилучший сплит + splitAxis = -1; splitCost = 1e308; + i = 0; area = (nodeBoundBox.maxY - nodeBoundBox.minY)*(nodeBoundBox.maxZ - nodeBoundBox.minZ); + while (i < numSplitCoordsX) { + if (isNaN(c1 = splitCoordsX[i++])) continue; + ct1 = c1 - threshold; + ct2 = c1 + threshold; + areaNegative = area*(c1 - nodeBoundBox.minX); + areaPositive = area*(nodeBoundBox.maxX - c1); + numNegative = numPositive = 0; + conflict = false; + // Проверяем объекты + for (j = 0; j < length; j++) { + boundBox = staticBoundBoxes[objects[j]]; + if (boundBox.maxX <= ct2) { + if (boundBox.minX < ct1) numNegative++; + } else { + if (boundBox.minX >= ct1) numPositive++; else {conflict = true; break;} + } + } + // Если хороший сплит, сохраняем + if (!conflict && (cost = areaNegative*numNegative + areaPositive*numPositive) < splitCost) { + splitCost = cost; + splitAxis = 0; + splitCoord = c1; + } + j = i; + while (++j < numSplitCoordsX) if ((c2 = splitCoordsX[j]) >= c1 - threshold && c2 <= c1 + threshold) splitCoordsX[j] = NaN; + } + i = 0; area = (nodeBoundBox.maxX - nodeBoundBox.minX)*(nodeBoundBox.maxZ - nodeBoundBox.minZ); + while (i < numSplitCoordsY) { + if (isNaN(c1 = splitCoordsY[i++])) continue; + ct1 = c1 - threshold; + ct2 = c1 + threshold; + areaNegative = area*(c1 - nodeBoundBox.minY); + areaPositive = area*(nodeBoundBox.maxY - c1); + numNegative = numPositive = 0; + conflict = false; + // Проверяем объекты + for (j = 0; j < length; j++) { + boundBox = staticBoundBoxes[objects[j]]; + if (boundBox.maxY <= ct2) { + if (boundBox.minY < ct1) numNegative++; + } else { + if (boundBox.minY >= ct1) numPositive++; else {conflict = true; break;} + } + } + // Если хороший сплит, сохраняем + if (!conflict && (cost = areaNegative*numNegative + areaPositive*numPositive) < splitCost) { + splitCost = cost; + splitAxis = 1; + splitCoord = c1; + } + j = i; + while (++j < numSplitCoordsY) if ((c2 = splitCoordsY[j]) >= c1 - threshold && c2 <= c1 + threshold) splitCoordsY[j] = NaN; + } + i = 0; area = (nodeBoundBox.maxX - nodeBoundBox.minX)*(nodeBoundBox.maxY - nodeBoundBox.minY); + while (i < numSplitCoordsZ) { + if (isNaN(c1 = splitCoordsZ[i++])) continue; + ct1 = c1 - threshold; + ct2 = c1 + threshold; + areaNegative = area*(c1 - nodeBoundBox.minZ); + areaPositive = area*(nodeBoundBox.maxZ - c1); + numNegative = numPositive = 0; + conflict = false; + // Проверяем объекты + for (j = 0; j < length; j++) { + boundBox = staticBoundBoxes[objects[j]]; + if (boundBox.maxZ <= ct2) { + if (boundBox.minZ < ct1) numNegative++; + } else { + if (boundBox.minZ >= ct1) numPositive++; else {conflict = true; break;} + } + } + // Если хороший сплит, сохраняем + if (!conflict && (cost = areaNegative*numNegative + areaPositive*numPositive) < splitCost) { + splitCost = cost; + splitAxis = 2; + splitCoord = c1; + } + j = i; + while (++j < numSplitCoordsZ) if ((c2 = splitCoordsZ[j]) >= c1 - threshold && c2 <= c1 + threshold) splitCoordsZ[j] = NaN; + } + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Разделяем ноду + var axisX:Boolean = splitAxis == 0, axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.positiveNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.positiveNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + ct1 = splitCoord - threshold; ct2 = splitCoord + threshold; + for (i = 0; i < length; i++) { + boundBox = staticBoundBoxes[objects[i]]; + var min:Number = axisX ? boundBox.minX : (axisY ? boundBox.minY : boundBox.minZ); + var max:Number = axisX ? boundBox.maxX : (axisY ? boundBox.maxY : boundBox.maxZ); + if (max <= ct2) { + // Объект в негативной стороне + if (node.negativeNode.objects == null) node.negativeNode.objects = new Vector.(); + node.negativeNode.objects.push(objects[i]); + objects[i] = -1; + } else { + if (min >= ct1) { + // Объект в положительной стороне + if (node.positiveNode.objects == null) node.positiveNode.objects = new Vector.(); + node.positiveNode.objects.push(objects[i]); + objects[i] = -1; + } else { + // Распилился + } + } + } + + // Очистка списка объектов + j = 0; + for (i = 0; i < length; i++) { + if (objects[i] >= 0) objects[j++] = objects[i]; + } + if (j > 0) objects.length = j; else node.objects = null; + + // Разделение дочерних нод + if (node.negativeNode.objects != null) splitNode(node.negativeNode); + if (node.positiveNode.objects != null) splitNode(node.positiveNode); + } + + /** + * + */ + public function traceTree():void { + traceNode("", rootNode); + } + + /** + * @param str + * @param node + */ + private function traceNode(str:String, node:CollisionKdNode):void { + if (node == null) return; + trace(str, node.axis == -1 ? "end" : ((node.axis == 0) ? "X" : ((node.axis == 1) ? "Y" : "Z")), "splitCoord=" + splitCoord, "bound", node.boundBox, "objs:", node.objects); + traceNode(str + "-", node.negativeNode); + traceNode(str + "+", node.positiveNode); + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/collision/ICollider.as b/0.0.1.0/src/alternativa/physics/collision/ICollider.as new file mode 100644 index 0000000..733231b --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/ICollider.as @@ -0,0 +1,12 @@ +package alternativa.physics.collision { + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + + /** + * Интерфейс определителя столкновений между двумя примитивами. + */ + public interface ICollider { + function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/collision/ICollisionDetector.as b/0.0.1.0/src/alternativa/physics/collision/ICollisionDetector.as new file mode 100644 index 0000000..83e94d7 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/ICollisionDetector.as @@ -0,0 +1,73 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + /** + * Интерфейс детектора столкновений. + */ + public interface ICollisionDetector { + + /** + * Добавляет физический примитив в коллайдер. + * + * @param primitive добавляемый примитив + * @param isStatic указывает тип примитива: статический или динамический + * @return true если примитив был успешно добавлен, иначе false + */ + function addPrimitive(primitive:CollisionPrimitive, isStatic:Boolean = true):Boolean; + + /** + * Удаляет физический примитив из коллайдера. + * + * @param primitive удаляемый примитив + * @param isStatic указывает тип примитива: статический или динамический + * @return true если примитив был успшено удалён, иначе false + */ + function removePrimitive(primitive:CollisionPrimitive, isStatic:Boolean = true):Boolean; + + /** + * Выполняет инициализацию детектора после обновления списка примитивов. + */ + function init():void; + + /** + * Получает все столкновения в текущей конфигурации физической геометрии. + * + * @param contacts список контактов, в кторые будет записана информация о столкновении + * @return количество найденных столкновений + */ + function getAllCollisions(contacts:Vector.):int; + + /** + * Тестирует луч на пересечение с физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param dir направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRay(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * Тестирует луч на пересечение со статической физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param dir направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRayWithStatic(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/collision/ICollisionPredicate.as b/0.0.1.0/src/alternativa/physics/collision/ICollisionPredicate.as new file mode 100644 index 0000000..bb1b756 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/ICollisionPredicate.as @@ -0,0 +1,9 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + + public interface ICollisionPredicate { + + function considerCollision(primitive:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/collision/IRayCollisionPredicate.as b/0.0.1.0/src/alternativa/physics/collision/IRayCollisionPredicate.as new file mode 100644 index 0000000..0f3d929 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/IRayCollisionPredicate.as @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Body; + + public interface IRayCollisionPredicate { + function considerBody(body:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/collision/KdTreeCollisionDetector.as b/0.0.1.0/src/alternativa/physics/collision/KdTreeCollisionDetector.as new file mode 100644 index 0000000..46435f9 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/KdTreeCollisionDetector.as @@ -0,0 +1,423 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Детектор, хранящий статическую геометрию в kD-дереве и использующий дерево для ускорения тестов на пересечения. + */ + public class KdTreeCollisionDetector implements ICollisionDetector { + + altphysics var tree:CollisionKdTree; + altphysics var dynamicPrimitives:Vector.; + 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.(); + + addCollider(CollisionPrimitive.BOX, CollisionPrimitive.BOX, new BoxBoxCollider()); +// addCollider(CollisionPrimitive.BOX, CollisionPrimitive.PLANE, new BoxPlaneCollider()); + addCollider(CollisionPrimitive.BOX, CollisionPrimitive.SPHERE, new BoxSphereCollider()); + addCollider(CollisionPrimitive.BOX, CollisionPrimitive.RECT, new BoxRectCollider()); + +// 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():void { + tree.createTree(); +// tree.traceTree(); + } + + /** + * + * @param contacts + * @return + */ + public function getAllCollisions(contacts:Vector.):int { + var colNum:int = 0; + for (var i:int = 0; i < dynamicPrimitivesNum; i++) { + var primitive:CollisionPrimitive = dynamicPrimitives[i]; + primitive.calculateAABB(); + colNum += getPrimitiveNodeCollisions(tree.rootNode, primitive, contacts, colNum); + + // Столкновения динамических примитивов между собой + // TODO: Попробовать различные оптимизации (сортировка по баундам, встраивание в дерево) + for (var j:int = i + 1; j < dynamicPrimitivesNum; j++) { + if (collide(primitive, dynamicPrimitives[j], contacts[colNum])) colNum++; + } + } + + return colNum; + } + + /** + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + public function collide(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.collide(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 = (contact.points[0] as ContactPoint).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 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 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:Vector., startIndex:int):int { + var colNum:int = 0; + if (node.objects != null) { + // Поиск столкновений со статическими примитивами узла + var primitives:Vector. = tree.staticChildren; + var indices:Vector. = node.objects; + for (var i:int = indices.length - 1; i >= 0; i--) + if (collide(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; + } + + /** + * Тест пересечения луча с динамическими примитивами. + * + * @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 minTime:Number = maxTime + 1; + for (var i:int = 0; i < dynamicPrimitivesNum; i++) { + var primitive:CollisionPrimitive = dynamicPrimitives[i]; + if ((primitive.collisionGroup & collisionGroup) == 0) continue; + if (predicate != null && primitive.body != null && !predicate.considerBody(primitive.body)) continue; + var t:Number = primitive.getSegmentIntersection(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.objects != null && getRayNodeIntersection(origin, dir, collisionGroup, tree.staticChildren, node.objects, 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., indices:Vector., 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.getSegmentIntersection(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; + } \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/collision/SpherePlaneCollider.as b/0.0.1.0/src/alternativa/physics/collision/SpherePlaneCollider.as new file mode 100644 index 0000000..9b527d8 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/SpherePlaneCollider.as @@ -0,0 +1,58 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + /** + * + */ + public class SpherePlaneCollider implements ICollider { + + private var normal:Vector3 = new Vector3(); + + /** + * + */ + public function SpherePlaneCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var sphere:RigidSphere = body1 as RigidSphere; +// var plane:RigidPlane; +// if (sphere == null) { +// sphere = body2 as RigidSphere; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// +// var dist:Number = sphere.state.pos.dot(normal) - offset; +// if (dist > sphere.r) return false; +// +// collisionInfo.body1 = sphere; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// collisionInfo.pcount = 1; +// +// var cp:ContactPoint = collisionInfo.points[0]; +// cp.penetration = sphere.r - dist; +// cp.pos.copy(normal).reverse().scale(sphere.r).add(sphere.state.pos); +// cp.r1.diff(cp.pos, sphere.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); + + return true; + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/collision/SphereSphereCollider.as b/0.0.1.0/src/alternativa/physics/collision/SphereSphereCollider.as new file mode 100644 index 0000000..9111a84 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/SphereSphereCollider.as @@ -0,0 +1,60 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + public class SphereSphereCollider implements ICollider { + + private var p1:Vector3 = new Vector3(); + private var p2:Vector3 = new Vector3(); + + public function SphereSphereCollider() { + } + + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var s1:CollisionSphere; + var s2:CollisionSphere; + if (prim1.body != null) { + s1 = prim1 as CollisionSphere; + s2 = prim2 as CollisionSphere; + } else { + s1 = prim2 as CollisionSphere; + s2 = prim1 as CollisionSphere; + } + + s1.transform.getAxis(3, p1); + s2.transform.getAxis(3, p2); + var dx:Number = p1.x - p2.x; + var dy:Number = p1.y - p2.y; + var dz:Number = p1.z - p2.z; + var len:Number = dx*dx + dy*dy + dz*dz; + var sum:Number = s1.r + s2.r; + if (len > sum*sum) return false; + len = Math.sqrt(len); + dx /= len; + dy /= len; + dz /= len; + + contact.body1 = s1.body; + contact.body2 = s2.body; + contact.normal.x = dx; + contact.normal.y = dy; + contact.normal.z = dz; + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = sum - len; + cp.pos.x = p1.x - dx*s1.r; + cp.pos.y = p1.y - dy*s1.r; + cp.pos.z = p1.z - dz*s1.r; + cp.r1.vDiff(cp.pos, p1); + cp.r2.vDiff(cp.pos, p2); + + return true; + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/collision/primitives/.svn/all-wcprops b/0.0.1.0/src/alternativa/physics/collision/primitives/.svn/all-wcprops new file mode 100644 index 0000000..8d67246 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/primitives/.svn/all-wcprops @@ -0,0 +1,35 @@ +K 25 +svn:wc:ra_dav:version-url +V 124 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/collision/primitives +END +CollisionTriangle.as +K 25 +svn:wc:ra_dav:version-url +V 145 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/collision/primitives/CollisionTriangle.as +END +CollisionSphere.as +K 25 +svn:wc:ra_dav:version-url +V 143 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/collision/primitives/CollisionSphere.as +END +CollisionBox.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/collision/primitives/CollisionBox.as +END +CollisionPrimitive.as +K 25 +svn:wc:ra_dav:version-url +V 146 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/collision/primitives/CollisionPrimitive.as +END +CollisionRect.as +K 25 +svn:wc:ra_dav:version-url +V 141 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/collision/primitives/CollisionRect.as +END diff --git a/0.0.1.0/src/alternativa/physics/collision/primitives/.svn/entries b/0.0.1.0/src/alternativa/physics/collision/primitives/.svn/entries new file mode 100644 index 0000000..76d14b6 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/primitives/.svn/entries @@ -0,0 +1,88 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/collision/primitives +http://svndev.alternativaplatform.com + + + +2009-06-08T07:53:17.170410Z +14100 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +CollisionTriangle.as +file + + + + +2010-10-28T04:32:39.000000Z +a1ccd11eb25d172d2739fce7fef21ee2 +2009-06-08T06:46:18.310732Z +14099 +mike + +CollisionSphere.as +file + + + + +2010-10-28T04:32:39.000000Z +1ca67429cc8bf27435b6ec46f8d6f80a +2009-05-26T08:41:33.471001Z +13389 +mike + +CollisionBox.as +file + + + + +2010-10-28T04:32:39.000000Z +e6a441a4317416b0d13d365f5b26ace4 +2009-06-08T07:53:17.170410Z +14100 +mike + +CollisionPrimitive.as +file + + + + +2010-10-28T04:32:39.000000Z +09336a8257b1c0e46da8f1d11975b5b1 +2009-06-08T07:53:17.170410Z +14100 +mike + +CollisionRect.as +file + + + + +2010-10-28T04:32:39.000000Z +c76d5cb0626964578f2df221c8072142 +2009-06-08T07:53:17.170410Z +14100 +mike + diff --git a/0.0.1.0/src/alternativa/physics/collision/primitives/.svn/format b/0.0.1.0/src/alternativa/physics/collision/primitives/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/primitives/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.1.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base b/0.0.1.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base new file mode 100644 index 0000000..e3d90cc --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base @@ -0,0 +1,178 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + /** + * Ориентированный бокс. + */ + public class CollisionBox extends CollisionPrimitive { + + // Половинные размеры вдоль каждой из осей + public var hs:Vector3 = new Vector3(); + + // Используются в определении пересечения с лучом + private static var _o:Vector3 = new Vector3(); + private static var _v:Vector3 = new Vector3(); + + /** + * @param hs + * @param collisionGroup + */ + public function CollisionBox(hs:Vector3, collisionGroup:int) { + super(BOX, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + var t:Matrix4 = transform; + aabb.maxX = hs.x*(t.a < 0 ? -t.a : t.a) + hs.y*(t.b < 0 ? -t.b : t.b) + hs.z*(t.c < 0 ? -t.c : t.c); + aabb.minX = -aabb.maxX; + + aabb.maxY = hs.x*(t.e < 0 ? -t.e : t.e) + hs.y*(t.f < 0 ? -t.f : t.f) + hs.z*(t.g < 0 ? -t.g : t.g); + aabb.minY = -aabb.maxY; + + aabb.maxZ = hs.x*(t.i < 0 ? -t.i : t.i) + hs.y*(t.j < 0 ? -t.j : t.j) + hs.z*(t.k < 0 ? -t.k : t.k); + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var box:CollisionBox = source as CollisionBox; + if (box == null) return this; + super.copyFrom(box); + hs.vCopy(box.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionBox(hs, collisionGroup); + } + + /** + * Вычисляет параметры пересечения заданного сегмента с примитивом. + * + * @param origin начальная точка сегмента в мировых координатах + * @param vector вектор сегмента в мировых координатах + * @param threshold погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную + * погрешность, считается равной нулю. + * @param normal возвращаемое значение. Нормаль к примитиву в точке пересечения с сегментом. + * @return в случае наличия пересечения возвращается время точки пересечения, в противном случае возвращается -1. + */ + override public function getSegmentIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var tMin:Number = -1, tMax:Number = 1e308, t1:Number, t2:Number; + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var xx:Number = origin.x - transform.d; + var yy:Number = origin.y - transform.h; + var zz:Number = origin.z - transform.l; + _o.x = transform.a*xx + transform.e*yy + transform.i*zz; + _o.y = transform.b*xx + transform.f*yy + transform.j*zz; + _o.z = transform.c*xx + transform.g*yy + transform.k*zz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + _v.x = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + _v.y = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + _v.z = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + // X + if (_v.x < threshold && _v.x > -threshold) { + if (_o.x < -hs.x || _o.x > hs.x) return -1; + } else { + t1 = (-hs.x - _o.x)/_v.x; + t2 = (hs.x - _o.x)/_v.x; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.x = -1; + normal.y = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.x = 1; + normal.y = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Y + if (_v.y < threshold && _v.y > -threshold) { + if (_o.y < -hs.y || _o.y > hs.y) return -1; + } else { + t1 = (-hs.y - _o.y)/_v.y; + t2 = (hs.y - _o.y)/_v.y; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.y = -1; + normal.x = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.y = 1; + normal.x = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Z + if (_v.z < threshold && _v.z > -threshold) { + if (_o.z < -hs.z || _o.z > hs.z) return -1; + } else { + t1 = (-hs.z - _o.z)/_v.z; + t2 = (hs.z - _o.z)/_v.z; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.z = -1; + normal.x = normal.y = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.z = 1; + normal.x = normal.y = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Перевод нормали в мировую систему координат + // Inlined normal.vDeltaTransformBy4(transform); + xx = normal.x, yy = normal.y, zz = normal.z; + normal.x = transform.a*xx + transform.b*yy + transform.c*zz; + normal.y = transform.e*xx + transform.f*yy + transform.g*zz; + normal.z = transform.i*xx + transform.j*yy + transform.k*zz; + + return tMin; + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionPrimitive.as.svn-base b/0.0.1.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionPrimitive.as.svn-base new file mode 100644 index 0000000..3f92dc2 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionPrimitive.as.svn-base @@ -0,0 +1,128 @@ +package alternativa.physics.collision.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.ICollisionPredicate; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Базовый класс для примитивов, использующихся детектором столкновений. + */ + public class CollisionPrimitive { + // Константы типов примитива + public static const BOX:int = 1; + public static const PLANE:int = 2; + public static const SPHERE:int = 3; + public static const RECT:int = 4; + public static const TRIANGLE:int = 5; + + // Тип примитива + public var type:int; + // Группы примитива. Каждая группа определяется установленным битом. Столкновения проверяются только для примитивов, + // имеющих хотя бы одну общую группу. + public var collisionGroup:int; + // Предикат примитива, вызывающийся детектором при нахождении столкновения. В зависимости от возвращённого + // предикатом результата столкновение либо регистрируется, либо игнорируется. Nullable. + public var postCollisionPredicate:ICollisionPredicate; + + // Тело, владеющее примитивом. Может быть null. + altphysics var body:Body; + // Трансформация примитива в системе координат тела, если оно указано. Не допускается масштабирование матрицы. + altphysics var localTransform:Matrix4 = new Matrix4(); + // Полная трансформация примитива. Не допускается масштабирование матрицы. + altphysics var transform:Matrix4 = new Matrix4(); + // AABB в мировой системе координат. Расчитывается системой вызовом функции calculateBoundBox(). + altphysics var aabb:BoundBox = new BoundBox(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param type тип примитива + * @param collisionGroup группа примитива + */ + public function CollisionPrimitive(type:int, collisionGroup:int) { + this.type = type; + this.collisionGroup = collisionGroup; + } + + /** + * Устанавливает тело, владеющее примитивом. + * + * @param body тело, к которому относится примитив. Если указано значение null, матрица локальной трансформации удаляется. + * @param localTransform трансформация примитива в системе координат тела. Указание значения null равносильно + * заданию единичной матрицы. + */ + public function setBody(body:Body, localTransform:Matrix4 = null):void { + if (this.body == body) return; + this.body = body; + if (body == null) this.localTransform = null; + else { + if (this.localTransform == null) this.localTransform = new Matrix4(); + if (localTransform != null) this.localTransform.copy(localTransform); + } + } + + /** + * Наследники должны переопределять этот метод, реализуя в нём корректный расчёт ограничивающего бокса, выравненного + * по осям мировой системы координат. + * + * @return ссылка на свой баунд бокс + */ + public function calculateAABB():BoundBox { + return aabb; + } + + /** + * Клонирует примитив. Переопределять не рекомендуется. Вместо этого переопределяются методы createPrimitive() и copyFrom(). + * + * @return клон примитива + */ + public function clone():CollisionPrimitive { + var p:CollisionPrimitive = createPrimitive(); + return p.copyFrom(this); + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + type = source.type; + transform.copy(source.transform); + collisionGroup = source.collisionGroup; + setBody(source.body, source.localTransform); + aabb.copyFrom(source.aabb); + return this; + } + + /** + * Создаёт новый экземпляр примитива соотвествующего типа. + * + * @return новый экземпляр примитива + */ + protected function createPrimitive():CollisionPrimitive { + return new CollisionPrimitive(type, collisionGroup); + } + + /** + * Метод должен вычислять параметры пересечения заданного сегмента с примитивом. + * + * @param origin начальная точка сегмента в мировых координатах + * @param vector вектор сегмента в мировых координатах + * @param threshold погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную + * погрешность, считается равной нулю. + * @param normal возвращаемое значение. Нормаль к примитиву в точке пересечения с сегментом. + * @return в случае наличия пересечения возвращается время точки пересечения, в противном случае возвращается -1. + */ + public function getSegmentIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + return -1; + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base b/0.0.1.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base new file mode 100644 index 0000000..7d6aa47 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base @@ -0,0 +1,126 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + /** + * Ориентированный прямоугольник. Задаётся половинами размеров вдоль осей X (ширина) и Y (длина) локальной системы + * координат примитива. Таким образом, прямоугольник лежит в плоскости XY, его стороны параллельны осям этой + * плоскости, а нормаль направлена вдоль локальной оси Z. + * + * Прямоугольник может быть одно- или двусторонним. В случае одностороннего прямоугольника, + * столкновения с внутренней стороны не регистрируются. + */ + public class CollisionRect extends CollisionPrimitive { + // Половинные размеры прямоугольника вдоль осей X и Y. Нормаль направлена вдоль оси Z. + public var hs:Vector3 = new Vector3(); + // Флаг указывает, является примитив одно- или двусторонним + public var twoSided:Boolean = true; + + // Малое значение. Используется для указания фиктивной высоты примитива и в функции определения пересечения с сегментом. + private static const EPSILON:Number = 0.01; + // Переменные используются в функции поиска пересечения с лучом + private static var _o:Vector3 = new Vector3(); + private static var _v:Vector3 = new Vector3(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param hs половинные размерв прямоугольника вдоль осей X и Y. Значение z игнорируется. + * @param collisionGroup группа примитива + */ + public function CollisionRect(hs:Vector3, collisionGroup:int) { + super(RECT, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * Расчитывает ограничивающий бокс прямоугольника. Для избежания проблем высота примитива принимается равной + * не нулю, а малому значению. + * + * @return + */ + override public function calculateAABB():BoundBox { + // Баунд бокс прямоугольника имеет минимальную высоту, отличную от нуля во избежание проблем с построением kd-дерева + var t:Matrix4 = transform; + aabb.maxX = hs.x*(t.a < 0 ? -t.a : t.a) + hs.y*(t.b < 0 ? -t.b : t.b) + EPSILON*(t.c < 0 ? -t.c : t.c); + aabb.minX = -aabb.maxX; + + aabb.maxY = hs.x*(t.e < 0 ? -t.e : t.e) + hs.y*(t.f < 0 ? -t.f : t.f) + EPSILON*(t.g < 0 ? -t.g : t.g); + aabb.minY = -aabb.maxY; + + aabb.maxZ = hs.x*(t.i < 0 ? -t.i : t.i) + hs.y*(t.j < 0 ? -t.j : t.j) + EPSILON*(t.k < 0 ? -t.k : t.k); + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var rect:CollisionRect = source as CollisionRect; + if (rect == null) return this; + super.copyFrom(rect); + hs.vCopy(rect.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionRect(hs, collisionGroup); + } + + /** + * @param origin + * @param vector + * @param collisionGroup + * @param threshold + * @param normal + * @return + */ + override public function getSegmentIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var xx:Number = origin.x - transform.d; + var yy:Number = origin.y - transform.h; + var zz:Number = origin.z - transform.l; + _o.x = transform.a*xx + transform.e*yy + transform.i*zz; + _o.y = transform.b*xx + transform.f*yy + transform.j*zz; + _o.z = transform.c*xx + transform.g*yy + transform.k*zz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + _v.x = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + _v.y = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + _v.z = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + + // Проверка параллельности сегмента и плоскости примитива + if (_v.z > -threshold && _v.z < threshold) return -1; + var t:Number = -_o.z/_v.z; + if (t < 0) return -1; + // Проверка вхождения точки пересечения в прямоугольник + _o.x += _v.x*t; + _o.y += _v.y*t; + _o.z = 0; + if (_o.x < (-hs.x - EPSILON) || _o.x > (hs.x + EPSILON) || _o.y < (-hs.y - EPSILON) || _o.y > (hs.y + EPSILON)) return -1; + + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + return t; + } + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base b/0.0.1.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base new file mode 100644 index 0000000..1a9e6d1 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base @@ -0,0 +1,80 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + + /** + * Сфера. + */ + public class CollisionSphere extends CollisionPrimitive { + + // Радиус сферы + public var r:Number = 0; + + /** + * + * @param r + * @param collisionGroup + */ + public function CollisionSphere(r:Number, collisionGroup:int) { + super(SPHERE, collisionGroup); + this.r = r; + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + aabb.maxX = transform.d + r; + aabb.minX = transform.d - r; + + aabb.maxY = transform.h + r; + aabb.minY = transform.h - r; + + aabb.maxZ = transform.l + r; + aabb.minZ = transform.l - r; + + return aabb; + } + + /** + * @param origin + * @param vector + * @param threshold + * @param normal + * @return + */ + override public function getSegmentIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var px:Number = origin.x - transform.d; + var py:Number = origin.y - transform.h; + var pz:Number = origin.z - transform.l; + var k:Number = vector.x*px + vector.y*py + vector.z*pz; + if (k > 0) return -1; + var a:Number = vector.x*vector.x + vector.y*vector.y + vector.z*vector.z; + var D:Number = k*k - a*(px*px + py*py + pz*pz - r*r); + if (D < 0) return -1; + return -(k + Math.sqrt(D))/a; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var sphere:CollisionSphere = source as CollisionSphere; + if (sphere == null) return this; + super.copyFrom(sphere); + r = sphere.r; + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionSphere(r, collisionGroup); + } + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base b/0.0.1.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base new file mode 100644 index 0000000..a48803a --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base @@ -0,0 +1,39 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + public class CollisionTriangle extends CollisionPrimitive { + + public var v0:Vector3 = new Vector3(); + public var v1:Vector3 = new Vector3(); + public var v2:Vector3 = new Vector3(); + public var e0:Vector3 = new Vector3(); + public var e1:Vector3 = new Vector3(); + public var e2:Vector3 = new Vector3(); + public var len0:Number; + public var len1:Number; + public var len2:Number; + + public function CollisionTriangle(v0:Vector3, v1:Vector3, v2:Vector3, collisionGroup:int) { + super(TRIANGLE, collisionGroup); + + this.v0.vCopy(v0); + this.v1.vCopy(v1); + this.v2.vCopy(v2); + + e0.vDiff(v1, v0); + len0 = e0.vLength(); + e0.vNormalize(); + + e1.vDiff(v2, v1); + len1 = e1.vLength(); + e1.vNormalize(); + + e2.vDiff(v0, v2); + len2 = e2.vLength(); + e2.vNormalize(); + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/collision/primitives/CollisionBox.as b/0.0.1.0/src/alternativa/physics/collision/primitives/CollisionBox.as new file mode 100644 index 0000000..e3d90cc --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/primitives/CollisionBox.as @@ -0,0 +1,178 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + /** + * Ориентированный бокс. + */ + public class CollisionBox extends CollisionPrimitive { + + // Половинные размеры вдоль каждой из осей + public var hs:Vector3 = new Vector3(); + + // Используются в определении пересечения с лучом + private static var _o:Vector3 = new Vector3(); + private static var _v:Vector3 = new Vector3(); + + /** + * @param hs + * @param collisionGroup + */ + public function CollisionBox(hs:Vector3, collisionGroup:int) { + super(BOX, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + var t:Matrix4 = transform; + aabb.maxX = hs.x*(t.a < 0 ? -t.a : t.a) + hs.y*(t.b < 0 ? -t.b : t.b) + hs.z*(t.c < 0 ? -t.c : t.c); + aabb.minX = -aabb.maxX; + + aabb.maxY = hs.x*(t.e < 0 ? -t.e : t.e) + hs.y*(t.f < 0 ? -t.f : t.f) + hs.z*(t.g < 0 ? -t.g : t.g); + aabb.minY = -aabb.maxY; + + aabb.maxZ = hs.x*(t.i < 0 ? -t.i : t.i) + hs.y*(t.j < 0 ? -t.j : t.j) + hs.z*(t.k < 0 ? -t.k : t.k); + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var box:CollisionBox = source as CollisionBox; + if (box == null) return this; + super.copyFrom(box); + hs.vCopy(box.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionBox(hs, collisionGroup); + } + + /** + * Вычисляет параметры пересечения заданного сегмента с примитивом. + * + * @param origin начальная точка сегмента в мировых координатах + * @param vector вектор сегмента в мировых координатах + * @param threshold погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную + * погрешность, считается равной нулю. + * @param normal возвращаемое значение. Нормаль к примитиву в точке пересечения с сегментом. + * @return в случае наличия пересечения возвращается время точки пересечения, в противном случае возвращается -1. + */ + override public function getSegmentIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var tMin:Number = -1, tMax:Number = 1e308, t1:Number, t2:Number; + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var xx:Number = origin.x - transform.d; + var yy:Number = origin.y - transform.h; + var zz:Number = origin.z - transform.l; + _o.x = transform.a*xx + transform.e*yy + transform.i*zz; + _o.y = transform.b*xx + transform.f*yy + transform.j*zz; + _o.z = transform.c*xx + transform.g*yy + transform.k*zz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + _v.x = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + _v.y = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + _v.z = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + // X + if (_v.x < threshold && _v.x > -threshold) { + if (_o.x < -hs.x || _o.x > hs.x) return -1; + } else { + t1 = (-hs.x - _o.x)/_v.x; + t2 = (hs.x - _o.x)/_v.x; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.x = -1; + normal.y = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.x = 1; + normal.y = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Y + if (_v.y < threshold && _v.y > -threshold) { + if (_o.y < -hs.y || _o.y > hs.y) return -1; + } else { + t1 = (-hs.y - _o.y)/_v.y; + t2 = (hs.y - _o.y)/_v.y; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.y = -1; + normal.x = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.y = 1; + normal.x = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Z + if (_v.z < threshold && _v.z > -threshold) { + if (_o.z < -hs.z || _o.z > hs.z) return -1; + } else { + t1 = (-hs.z - _o.z)/_v.z; + t2 = (hs.z - _o.z)/_v.z; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.z = -1; + normal.x = normal.y = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.z = 1; + normal.x = normal.y = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Перевод нормали в мировую систему координат + // Inlined normal.vDeltaTransformBy4(transform); + xx = normal.x, yy = normal.y, zz = normal.z; + normal.x = transform.a*xx + transform.b*yy + transform.c*zz; + normal.y = transform.e*xx + transform.f*yy + transform.g*zz; + normal.z = transform.i*xx + transform.j*yy + transform.k*zz; + + return tMin; + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/collision/primitives/CollisionPrimitive.as b/0.0.1.0/src/alternativa/physics/collision/primitives/CollisionPrimitive.as new file mode 100644 index 0000000..3f92dc2 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/primitives/CollisionPrimitive.as @@ -0,0 +1,128 @@ +package alternativa.physics.collision.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.ICollisionPredicate; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Базовый класс для примитивов, использующихся детектором столкновений. + */ + public class CollisionPrimitive { + // Константы типов примитива + public static const BOX:int = 1; + public static const PLANE:int = 2; + public static const SPHERE:int = 3; + public static const RECT:int = 4; + public static const TRIANGLE:int = 5; + + // Тип примитива + public var type:int; + // Группы примитива. Каждая группа определяется установленным битом. Столкновения проверяются только для примитивов, + // имеющих хотя бы одну общую группу. + public var collisionGroup:int; + // Предикат примитива, вызывающийся детектором при нахождении столкновения. В зависимости от возвращённого + // предикатом результата столкновение либо регистрируется, либо игнорируется. Nullable. + public var postCollisionPredicate:ICollisionPredicate; + + // Тело, владеющее примитивом. Может быть null. + altphysics var body:Body; + // Трансформация примитива в системе координат тела, если оно указано. Не допускается масштабирование матрицы. + altphysics var localTransform:Matrix4 = new Matrix4(); + // Полная трансформация примитива. Не допускается масштабирование матрицы. + altphysics var transform:Matrix4 = new Matrix4(); + // AABB в мировой системе координат. Расчитывается системой вызовом функции calculateBoundBox(). + altphysics var aabb:BoundBox = new BoundBox(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param type тип примитива + * @param collisionGroup группа примитива + */ + public function CollisionPrimitive(type:int, collisionGroup:int) { + this.type = type; + this.collisionGroup = collisionGroup; + } + + /** + * Устанавливает тело, владеющее примитивом. + * + * @param body тело, к которому относится примитив. Если указано значение null, матрица локальной трансформации удаляется. + * @param localTransform трансформация примитива в системе координат тела. Указание значения null равносильно + * заданию единичной матрицы. + */ + public function setBody(body:Body, localTransform:Matrix4 = null):void { + if (this.body == body) return; + this.body = body; + if (body == null) this.localTransform = null; + else { + if (this.localTransform == null) this.localTransform = new Matrix4(); + if (localTransform != null) this.localTransform.copy(localTransform); + } + } + + /** + * Наследники должны переопределять этот метод, реализуя в нём корректный расчёт ограничивающего бокса, выравненного + * по осям мировой системы координат. + * + * @return ссылка на свой баунд бокс + */ + public function calculateAABB():BoundBox { + return aabb; + } + + /** + * Клонирует примитив. Переопределять не рекомендуется. Вместо этого переопределяются методы createPrimitive() и copyFrom(). + * + * @return клон примитива + */ + public function clone():CollisionPrimitive { + var p:CollisionPrimitive = createPrimitive(); + return p.copyFrom(this); + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + type = source.type; + transform.copy(source.transform); + collisionGroup = source.collisionGroup; + setBody(source.body, source.localTransform); + aabb.copyFrom(source.aabb); + return this; + } + + /** + * Создаёт новый экземпляр примитива соотвествующего типа. + * + * @return новый экземпляр примитива + */ + protected function createPrimitive():CollisionPrimitive { + return new CollisionPrimitive(type, collisionGroup); + } + + /** + * Метод должен вычислять параметры пересечения заданного сегмента с примитивом. + * + * @param origin начальная точка сегмента в мировых координатах + * @param vector вектор сегмента в мировых координатах + * @param threshold погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную + * погрешность, считается равной нулю. + * @param normal возвращаемое значение. Нормаль к примитиву в точке пересечения с сегментом. + * @return в случае наличия пересечения возвращается время точки пересечения, в противном случае возвращается -1. + */ + public function getSegmentIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + return -1; + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/collision/primitives/CollisionRect.as b/0.0.1.0/src/alternativa/physics/collision/primitives/CollisionRect.as new file mode 100644 index 0000000..7d6aa47 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/primitives/CollisionRect.as @@ -0,0 +1,126 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + /** + * Ориентированный прямоугольник. Задаётся половинами размеров вдоль осей X (ширина) и Y (длина) локальной системы + * координат примитива. Таким образом, прямоугольник лежит в плоскости XY, его стороны параллельны осям этой + * плоскости, а нормаль направлена вдоль локальной оси Z. + * + * Прямоугольник может быть одно- или двусторонним. В случае одностороннего прямоугольника, + * столкновения с внутренней стороны не регистрируются. + */ + public class CollisionRect extends CollisionPrimitive { + // Половинные размеры прямоугольника вдоль осей X и Y. Нормаль направлена вдоль оси Z. + public var hs:Vector3 = new Vector3(); + // Флаг указывает, является примитив одно- или двусторонним + public var twoSided:Boolean = true; + + // Малое значение. Используется для указания фиктивной высоты примитива и в функции определения пересечения с сегментом. + private static const EPSILON:Number = 0.01; + // Переменные используются в функции поиска пересечения с лучом + private static var _o:Vector3 = new Vector3(); + private static var _v:Vector3 = new Vector3(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param hs половинные размерв прямоугольника вдоль осей X и Y. Значение z игнорируется. + * @param collisionGroup группа примитива + */ + public function CollisionRect(hs:Vector3, collisionGroup:int) { + super(RECT, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * Расчитывает ограничивающий бокс прямоугольника. Для избежания проблем высота примитива принимается равной + * не нулю, а малому значению. + * + * @return + */ + override public function calculateAABB():BoundBox { + // Баунд бокс прямоугольника имеет минимальную высоту, отличную от нуля во избежание проблем с построением kd-дерева + var t:Matrix4 = transform; + aabb.maxX = hs.x*(t.a < 0 ? -t.a : t.a) + hs.y*(t.b < 0 ? -t.b : t.b) + EPSILON*(t.c < 0 ? -t.c : t.c); + aabb.minX = -aabb.maxX; + + aabb.maxY = hs.x*(t.e < 0 ? -t.e : t.e) + hs.y*(t.f < 0 ? -t.f : t.f) + EPSILON*(t.g < 0 ? -t.g : t.g); + aabb.minY = -aabb.maxY; + + aabb.maxZ = hs.x*(t.i < 0 ? -t.i : t.i) + hs.y*(t.j < 0 ? -t.j : t.j) + EPSILON*(t.k < 0 ? -t.k : t.k); + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var rect:CollisionRect = source as CollisionRect; + if (rect == null) return this; + super.copyFrom(rect); + hs.vCopy(rect.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionRect(hs, collisionGroup); + } + + /** + * @param origin + * @param vector + * @param collisionGroup + * @param threshold + * @param normal + * @return + */ + override public function getSegmentIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var xx:Number = origin.x - transform.d; + var yy:Number = origin.y - transform.h; + var zz:Number = origin.z - transform.l; + _o.x = transform.a*xx + transform.e*yy + transform.i*zz; + _o.y = transform.b*xx + transform.f*yy + transform.j*zz; + _o.z = transform.c*xx + transform.g*yy + transform.k*zz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + _v.x = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + _v.y = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + _v.z = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + + // Проверка параллельности сегмента и плоскости примитива + if (_v.z > -threshold && _v.z < threshold) return -1; + var t:Number = -_o.z/_v.z; + if (t < 0) return -1; + // Проверка вхождения точки пересечения в прямоугольник + _o.x += _v.x*t; + _o.y += _v.y*t; + _o.z = 0; + if (_o.x < (-hs.x - EPSILON) || _o.x > (hs.x + EPSILON) || _o.y < (-hs.y - EPSILON) || _o.y > (hs.y + EPSILON)) return -1; + + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + return t; + } + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/collision/primitives/CollisionSphere.as b/0.0.1.0/src/alternativa/physics/collision/primitives/CollisionSphere.as new file mode 100644 index 0000000..1a9e6d1 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/primitives/CollisionSphere.as @@ -0,0 +1,80 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + + /** + * Сфера. + */ + public class CollisionSphere extends CollisionPrimitive { + + // Радиус сферы + public var r:Number = 0; + + /** + * + * @param r + * @param collisionGroup + */ + public function CollisionSphere(r:Number, collisionGroup:int) { + super(SPHERE, collisionGroup); + this.r = r; + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + aabb.maxX = transform.d + r; + aabb.minX = transform.d - r; + + aabb.maxY = transform.h + r; + aabb.minY = transform.h - r; + + aabb.maxZ = transform.l + r; + aabb.minZ = transform.l - r; + + return aabb; + } + + /** + * @param origin + * @param vector + * @param threshold + * @param normal + * @return + */ + override public function getSegmentIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var px:Number = origin.x - transform.d; + var py:Number = origin.y - transform.h; + var pz:Number = origin.z - transform.l; + var k:Number = vector.x*px + vector.y*py + vector.z*pz; + if (k > 0) return -1; + var a:Number = vector.x*vector.x + vector.y*vector.y + vector.z*vector.z; + var D:Number = k*k - a*(px*px + py*py + pz*pz - r*r); + if (D < 0) return -1; + return -(k + Math.sqrt(D))/a; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var sphere:CollisionSphere = source as CollisionSphere; + if (sphere == null) return this; + super.copyFrom(sphere); + r = sphere.r; + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionSphere(r, collisionGroup); + } + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/collision/primitives/CollisionTriangle.as b/0.0.1.0/src/alternativa/physics/collision/primitives/CollisionTriangle.as new file mode 100644 index 0000000..a48803a --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/primitives/CollisionTriangle.as @@ -0,0 +1,39 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + public class CollisionTriangle extends CollisionPrimitive { + + public var v0:Vector3 = new Vector3(); + public var v1:Vector3 = new Vector3(); + public var v2:Vector3 = new Vector3(); + public var e0:Vector3 = new Vector3(); + public var e1:Vector3 = new Vector3(); + public var e2:Vector3 = new Vector3(); + public var len0:Number; + public var len1:Number; + public var len2:Number; + + public function CollisionTriangle(v0:Vector3, v1:Vector3, v2:Vector3, collisionGroup:int) { + super(TRIANGLE, collisionGroup); + + this.v0.vCopy(v0); + this.v1.vCopy(v1); + this.v2.vCopy(v2); + + e0.vDiff(v1, v0); + len0 = e0.vLength(); + e0.vNormalize(); + + e1.vDiff(v2, v1); + len1 = e1.vLength(); + e1.vNormalize(); + + e2.vDiff(v0, v2); + len2 = e2.vLength(); + e2.vNormalize(); + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/collision/types/.svn/all-wcprops b/0.0.1.0/src/alternativa/physics/collision/types/.svn/all-wcprops new file mode 100644 index 0000000..4bdd933 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/types/.svn/all-wcprops @@ -0,0 +1,23 @@ +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/collision/types +END +BoundBox.as +K 25 +svn:wc:ra_dav:version-url +V 131 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/collision/types/BoundBox.as +END +RayIntersection.as +K 25 +svn:wc:ra_dav:version-url +V 138 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/collision/types/RayIntersection.as +END +Ray.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/collision/types/Ray.as +END diff --git a/0.0.1.0/src/alternativa/physics/collision/types/.svn/entries b/0.0.1.0/src/alternativa/physics/collision/types/.svn/entries new file mode 100644 index 0000000..f7993cf --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/types/.svn/entries @@ -0,0 +1,64 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/collision/types +http://svndev.alternativaplatform.com + + + +2009-06-08T06:46:18.310732Z +14099 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +BoundBox.as +file + + + + +2010-10-28T04:32:39.000000Z +13aaaf1b1923de8a2579543651734ab7 +2009-05-18T16:37:02.465501Z +13087 +mike + +RayIntersection.as +file + + + + +2010-10-28T04:32:39.000000Z +a0a4cf3dd94468fdf44cd06734927c94 +2009-06-08T06:46:18.310732Z +14099 +mike + +Ray.as +file + + + + +2010-10-28T04:32:39.000000Z +55eeb1cf79a416bb863bee95f089dbcd +2009-04-17T17:39:21.367569Z +11341 +mike + diff --git a/0.0.1.0/src/alternativa/physics/collision/types/.svn/format b/0.0.1.0/src/alternativa/physics/collision/types/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/types/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.1.0/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base b/0.0.1.0/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base new file mode 100644 index 0000000..6efc097 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base @@ -0,0 +1,72 @@ +package alternativa.physics.collision.types { + + public class BoundBox { + + public var minX:Number = 1e308; + public var minY:Number = 1e308; + public var minZ:Number = 1e308; + public var maxX:Number = -1e308; + public var maxY:Number = -1e308; + public var maxZ:Number = -1e308; + + public function setSize(minX:Number, minY:Number, minZ:Number, maxX:Number, maxY:Number, maxZ:Number):void { + this.minX = minX; + this.minY = minY; + this.minZ = minZ; + this.maxX = maxX; + this.maxY = maxY; + this.maxZ = maxZ; + } + + public function addBoundBox(boundBox:BoundBox):void { + minX = (boundBox.minX < minX) ? boundBox.minX : minX; + minY = (boundBox.minY < minY) ? boundBox.minY : minY; + minZ = (boundBox.minZ < minZ) ? boundBox.minZ : minZ; + maxX = (boundBox.maxX > maxX) ? boundBox.maxX : maxX; + maxY = (boundBox.maxY > maxY) ? boundBox.maxY : maxY; + maxZ = (boundBox.maxZ > maxZ) ? boundBox.maxZ : maxZ; + } + + public function addPoint(x:Number, y:Number, z:Number):void { + if (x < minX) minX = x; + if (x > maxX) maxX = x; + if (y < minY) minY = y; + if (y > maxY) maxY = y; + if (z < minZ) minZ = z; + if (z > maxZ) maxZ = z; + } + + public function infinity():void { + minX = 1e308; + minY = 1e308; + minZ = 1e308; + maxX = -1e308; + maxY = -1e308; + maxZ = -1e308; + } + + public function intersects(bb:BoundBox, tolerance:Number):Boolean { + return !(minX > bb.maxX + tolerance || maxX < bb.minX - tolerance || minY > bb.maxY + tolerance || maxY < bb.minY - tolerance || minZ > bb.maxZ + tolerance || maxZ < bb.minZ - tolerance); + } + + public function copyFrom(boundBox:BoundBox):void { + minX = boundBox.minX; + minY = boundBox.minY; + minZ = boundBox.minZ; + maxX = boundBox.maxX; + maxY = boundBox.maxY; + maxZ = boundBox.maxZ; + } + + public function clone():BoundBox { + var clone:BoundBox = new BoundBox(); + clone.copyFrom(this); + return clone; + } + + public function toString():String { + return "BoundBox [" + minX + ", " + minY + ", " + minZ + " : " + maxX + ", " + maxY + ", " + maxZ + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base b/0.0.1.0/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base new file mode 100644 index 0000000..f80620a --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base @@ -0,0 +1,13 @@ +package alternativa.physics.collision.types { + import alternativa.physics.types.Vector3; + + /** + * Структура, описывающая луч и сегмент прямой в пространстве. + */ + public class Ray { + // Начало луча. Начальная точка сегмента на прямой луча. + public var origin:Vector3 = new Vector3(); + // Направляющий вектор луча. Длина вектора задаёт длину сегмента на луче. Конечная точка сегмента вычисляется как origin + dir. + public var dir:Vector3 = new Vector3(); + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base b/0.0.1.0/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base new file mode 100644 index 0000000..8a89f75 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base @@ -0,0 +1,25 @@ +package alternativa.physics.collision.types { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.types.Vector3; + + /** + * Структура описывает точку пересечения луча с физической геометрией. + */ + public class RayIntersection { + // Физический примитив, с которым пересекается луч + public var primitive:CollisionPrimitive; + // Положенеи точки пересечения + public var pos:Vector3 = new Vector3(); + // Нормаль поверхности физического примитива в точке пересечения + public var normal:Vector3 = new Vector3(); + // Время пересечения (|pos - ray.origin|/|ray.dir|) + public var t:Number = 0; + + public function copy(source:RayIntersection):void { + primitive = source.primitive; + pos.vCopy(source.pos); + normal.vCopy(source.normal); + t = source.t; + } + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/collision/types/BoundBox.as b/0.0.1.0/src/alternativa/physics/collision/types/BoundBox.as new file mode 100644 index 0000000..6efc097 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/types/BoundBox.as @@ -0,0 +1,72 @@ +package alternativa.physics.collision.types { + + public class BoundBox { + + public var minX:Number = 1e308; + public var minY:Number = 1e308; + public var minZ:Number = 1e308; + public var maxX:Number = -1e308; + public var maxY:Number = -1e308; + public var maxZ:Number = -1e308; + + public function setSize(minX:Number, minY:Number, minZ:Number, maxX:Number, maxY:Number, maxZ:Number):void { + this.minX = minX; + this.minY = minY; + this.minZ = minZ; + this.maxX = maxX; + this.maxY = maxY; + this.maxZ = maxZ; + } + + public function addBoundBox(boundBox:BoundBox):void { + minX = (boundBox.minX < minX) ? boundBox.minX : minX; + minY = (boundBox.minY < minY) ? boundBox.minY : minY; + minZ = (boundBox.minZ < minZ) ? boundBox.minZ : minZ; + maxX = (boundBox.maxX > maxX) ? boundBox.maxX : maxX; + maxY = (boundBox.maxY > maxY) ? boundBox.maxY : maxY; + maxZ = (boundBox.maxZ > maxZ) ? boundBox.maxZ : maxZ; + } + + public function addPoint(x:Number, y:Number, z:Number):void { + if (x < minX) minX = x; + if (x > maxX) maxX = x; + if (y < minY) minY = y; + if (y > maxY) maxY = y; + if (z < minZ) minZ = z; + if (z > maxZ) maxZ = z; + } + + public function infinity():void { + minX = 1e308; + minY = 1e308; + minZ = 1e308; + maxX = -1e308; + maxY = -1e308; + maxZ = -1e308; + } + + public function intersects(bb:BoundBox, tolerance:Number):Boolean { + return !(minX > bb.maxX + tolerance || maxX < bb.minX - tolerance || minY > bb.maxY + tolerance || maxY < bb.minY - tolerance || minZ > bb.maxZ + tolerance || maxZ < bb.minZ - tolerance); + } + + public function copyFrom(boundBox:BoundBox):void { + minX = boundBox.minX; + minY = boundBox.minY; + minZ = boundBox.minZ; + maxX = boundBox.maxX; + maxY = boundBox.maxY; + maxZ = boundBox.maxZ; + } + + public function clone():BoundBox { + var clone:BoundBox = new BoundBox(); + clone.copyFrom(this); + return clone; + } + + public function toString():String { + return "BoundBox [" + minX + ", " + minY + ", " + minZ + " : " + maxX + ", " + maxY + ", " + maxZ + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/collision/types/Ray.as b/0.0.1.0/src/alternativa/physics/collision/types/Ray.as new file mode 100644 index 0000000..f80620a --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/types/Ray.as @@ -0,0 +1,13 @@ +package alternativa.physics.collision.types { + import alternativa.physics.types.Vector3; + + /** + * Структура, описывающая луч и сегмент прямой в пространстве. + */ + public class Ray { + // Начало луча. Начальная точка сегмента на прямой луча. + public var origin:Vector3 = new Vector3(); + // Направляющий вектор луча. Длина вектора задаёт длину сегмента на луче. Конечная точка сегмента вычисляется как origin + dir. + public var dir:Vector3 = new Vector3(); + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/collision/types/RayIntersection.as b/0.0.1.0/src/alternativa/physics/collision/types/RayIntersection.as new file mode 100644 index 0000000..8a89f75 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/collision/types/RayIntersection.as @@ -0,0 +1,25 @@ +package alternativa.physics.collision.types { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.types.Vector3; + + /** + * Структура описывает точку пересечения луча с физической геометрией. + */ + public class RayIntersection { + // Физический примитив, с которым пересекается луч + public var primitive:CollisionPrimitive; + // Положенеи точки пересечения + public var pos:Vector3 = new Vector3(); + // Нормаль поверхности физического примитива в точке пересечения + public var normal:Vector3 = new Vector3(); + // Время пересечения (|pos - ray.origin|/|ray.dir|) + public var t:Number = 0; + + public function copy(source:RayIntersection):void { + primitive = source.primitive; + pos.vCopy(source.pos); + normal.vCopy(source.normal); + t = source.t; + } + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/rigid/.svn/all-wcprops b/0.0.1.0/src/alternativa/physics/rigid/.svn/all-wcprops new file mode 100644 index 0000000..ef4450f --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/rigid/.svn/all-wcprops @@ -0,0 +1,47 @@ +K 25 +svn:wc:ra_dav:version-url +V 109 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/rigid +END +Body.as +K 25 +svn:wc:ra_dav:version-url +V 117 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/rigid/Body.as +END +BodyState.as +K 25 +svn:wc:ra_dav:version-url +V 122 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/rigid/BodyState.as +END +PhysicsUtils.as +K 25 +svn:wc:ra_dav:version-url +V 125 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/rigid/PhysicsUtils.as +END +ContactPoint.as +K 25 +svn:wc:ra_dav:version-url +V 125 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/rigid/ContactPoint.as +END +RigidWorld.as +K 25 +svn:wc:ra_dav:version-url +V 123 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/rigid/RigidWorld.as +END +Contact.as +K 25 +svn:wc:ra_dav:version-url +V 120 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/rigid/Contact.as +END +BodyMaterial.as +K 25 +svn:wc:ra_dav:version-url +V 125 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/rigid/BodyMaterial.as +END diff --git a/0.0.1.0/src/alternativa/physics/rigid/.svn/entries b/0.0.1.0/src/alternativa/physics/rigid/.svn/entries new file mode 100644 index 0000000..0504c05 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/rigid/.svn/entries @@ -0,0 +1,118 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/rigid +http://svndev.alternativaplatform.com + + + +2009-06-08T06:46:18.310732Z +14099 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Body.as +file + + + + +2010-10-28T04:32:39.000000Z +43a884d83ae7d8a1d25eb9f1d26eb755 +2009-06-08T06:46:18.310732Z +14099 +mike + +BodyState.as +file + + + + +2010-10-28T04:32:39.000000Z +8a089205beccac13a05eccfb9cfc6387 +2009-06-08T06:46:18.310732Z +14099 +mike + +PhysicsUtils.as +file + + + + +2010-10-28T04:32:39.000000Z +6d88004ddab2ae19e0d15f5b1b7c016a +2009-04-26T21:40:17.813142Z +12023 +mike + +constraints +dir + +ContactPoint.as +file + + + + +2010-10-28T04:32:39.000000Z +a94e73e3bfc85be2a79d6cc939512d91 +2009-06-08T06:46:18.310732Z +14099 +mike + +RigidWorld.as +file + + + + +2010-10-28T04:32:39.000000Z +ee25e8fe15553bfa5789bf0e99c1524b +2009-06-08T06:46:18.310732Z +14099 +mike + +primitives +dir + +Contact.as +file + + + + +2010-10-28T04:32:39.000000Z +d65ad3d488d4a06addcbfa4aaf958f10 +2009-04-13T03:04:08.898183Z +11018 +mike + +BodyMaterial.as +file + + + + +2010-10-28T04:32:39.000000Z +49c7a55b5b26fdb03a60f24bac460f53 +2009-04-07T08:34:06.342994Z +10587 +mike + diff --git a/0.0.1.0/src/alternativa/physics/rigid/.svn/format b/0.0.1.0/src/alternativa/physics/rigid/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/rigid/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.1.0/src/alternativa/physics/rigid/.svn/text-base/Body.as.svn-base b/0.0.1.0/src/alternativa/physics/rigid/.svn/text-base/Body.as.svn-base new file mode 100644 index 0000000..4e83bfe --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/rigid/.svn/text-base/Body.as.svn-base @@ -0,0 +1,329 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Quaternion; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class Body { + + public static var linDamping:Number = 0.997; + public static var rotDamping:Number = 0.997; + + public var name:String; + public var movable:Boolean = true; + + altphysics var id:int; + // Мир, в котором находится тело + altphysics var world:RigidWorld; + // Текущее и предыдущее состояние тела. Промежуточное состояние вычисляется линейной интерполяцией. + altphysics var state:BodyState = new BodyState(); + altphysics var prevState:BodyState = new BodyState(); + // Линейное и угловое ускорение тела на текущем шаге симуляции + altphysics var accel:Vector3 = new Vector3(); + altphysics var angleAccel:Vector3 = new Vector3(); + // Материал тела + altphysics var material:BodyMaterial = new BodyMaterial(); + + altphysics var invMass:Number = 1; + altphysics var invInertia:Matrix3 = new Matrix3(); + altphysics var invInertiaWorld:Matrix3 = new Matrix3(); + altphysics var baseMatrix:Matrix3 = new Matrix3(); + + altphysics const MAX_CONTACTS:int = 20; + altphysics var contacts:Vector. = new Vector.(MAX_CONTACTS); + altphysics var contactsNum:int; + + altphysics var collisionPrimitives:Vector.; + altphysics var collisionPrimitivesNum:int; + + // Аккумулятор сил + altphysics var forceAccum:Vector3 = new Vector3(); + // Аккумулятор моментов + altphysics var torqueAccum:Vector3 = new Vector3(); + + // Внутренние переменные для избежания создания экземпляров + private static var _r:Vector3 = new Vector3(); + private static var _f:Vector3 = new Vector3(); + + /** + * + * @param invMass + * @param invInertia + */ + public function Body(invMass:Number, invInertia:Matrix3) { + this.invMass = invMass; + this.invInertia.copy(invInertia); + } + + /** + * @param prim + */ + public function addCollisionPrimitive(primitive:CollisionPrimitive, localTransform:Matrix4 = null):void { + if (primitive == null) throw new ArgumentError("Primitive cannot be null"); + if (collisionPrimitives == null) collisionPrimitives = new Vector.(); + collisionPrimitives[collisionPrimitivesNum++] = primitive; + primitive.setBody(this, localTransform); + } + + /** + * @param t + * @param result + */ + public function interpolate(t:Number, pos:Vector3, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.w = prevState.orientation.w*t1 + state.orientation.w*t; + orientation.x = prevState.orientation.x*t1 + state.orientation.x*t; + orientation.y = prevState.orientation.y*t1 + state.orientation.y*t; + orientation.z = prevState.orientation.z*t1 + state.orientation.z*t; + } + + /** + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + state.pos.vCopy(pos); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setPositionXYZ(x:Number, y:Number, z:Number):void { + state.pos.vReset(x, y, z); + } + + /** + * + * @param vel + */ + public function setVelocity(vel:Vector3):void { + state.velocity.vCopy(vel); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setVelocityXYZ(x:Number, y:Number, z:Number):void { + state.velocity.vReset(x, y, z); + } + + /** + * + * @param rot + */ + public function setRotation(rot:Vector3):void { + state.rotation.vCopy(rot); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setRotationXYZ(x:Number, y:Number, z:Number):void { + state.rotation.vReset(x, y, z); + } + + /** + * + * @param q + */ + public function setOrientation(q:Quaternion):void { + state.orientation.copy(q); + } + + /** + * @param r + * @param dir + * @param magnitude + */ + public function applyRelPosWorldImpulse(r:Vector3, dir:Vector3, magnitude:Number):void { + var d:Number = magnitude*invMass; + // Линейная часть + state.velocity.x += d*dir.x; + state.velocity.y += d*dir.y; + state.velocity.z += d*dir.z; + + // Вращательная часть + var rx:Number = r.x; + var ry:Number = r.y; + var rz:Number = r.z; + _r.x = (ry*dir.z - rz*dir.y)*magnitude; + _r.y = (rz*dir.x - rx*dir.z)*magnitude; + _r.z = (rx*dir.y - ry*dir.x)*magnitude; + _r.vTransformBy3(invInertiaWorld); + state.rotation.x += _r.x; + state.rotation.y += _r.y; + state.rotation.z += _r.z; + } + + /** + * @param f + */ + public function addForce(f:Vector3):void { + forceAccum.vAdd(f); + } + + /** + * @param pos + * @param f + */ + public function addWorldForce(pos:Vector3, force:Vector3):void { + forceAccum.vAdd(force); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(force)); + } + + /** + * @param pos + * @param f + */ + public function addWorldForceScaled(pos:Vector3, force:Vector3, scale:Number):void { + _f.x = scale*force.x, _f.y = scale*force.y, _f.z = scale*force.z; + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(_f)); + } + + /** + * @param pos + * @param f + */ + public function addLocalForce(pos:Vector3, force:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(pos, _r); + // Трансформируем вектор силы в мировую систему + baseMatrix.transformVector(force, _f); + // Добавляем силу и момент + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vCross(_f)); + } + + /** + * + * @param localPos + * @param worldForce + */ + public function addWorldForceAtLocalPoint(localPos:Vector3, worldForce:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(localPos, _r); + // Добавляем силу и момент + forceAccum.vAdd(worldForce); + torqueAccum.vAdd(_r.vCross(worldForce)); + } + + /** + * @param dt + */ + public function addExternalForces(dt:Number):void { + } + + /** + * @param t + */ + public function addTorque(t:Vector3):void { + torqueAccum.vAdd(t); + } + + /** + * + */ + altphysics function clearAccumulators():void { + forceAccum.vReset(); + torqueAccum.vReset(); + } + + /** + * + */ + altphysics function calcAccelerations():void { + accel.x = forceAccum.x*invMass; + accel.y = forceAccum.y*invMass; + accel.z = forceAccum.z*invMass; + invInertiaWorld.transformVector(torqueAccum, angleAccel); + } + + /** + * Вычисляет производные данные. + */ + altphysics function calcDerivedData():void { + // Вычисление базисной матрицы и обратного тензора инерции в мировых координатах + state.orientation.toMatrix3(baseMatrix); + invInertiaWorld.copy(invInertia).append(baseMatrix).prepend(baseMatrix.transpose()); + baseMatrix.transpose(); + if (collisionPrimitives != null) { + for (var i:int = 0; i < collisionPrimitivesNum; i++) { + var primitive:CollisionPrimitive = collisionPrimitives[i]; + primitive.transform.setFromMatrix3(baseMatrix, state.pos); + if (primitive.localTransform != null) primitive.transform.prepend(primitive.localTransform); + } + } + } + + /** + * + */ + altphysics function saveState():void { + prevState.copy(state); + } + + /** + * + */ + altphysics function restoreState():void { + state.copy(prevState); + } + + /** + * @param dt + */ + altphysics function integrateVelocity(dt:Number):void { + // v = v + a*t + state.velocity.x += accel.x*dt; + state.velocity.y += accel.y*dt; + state.velocity.z += accel.z*dt; + // rot = rot + eps*t + state.rotation.x += angleAccel.x*dt; + state.rotation.y += angleAccel.y*dt; + state.rotation.z += angleAccel.z*dt; + + state.velocity.x *= linDamping; + state.velocity.y *= linDamping; + state.velocity.z *= linDamping; + + state.rotation.x *= rotDamping; + state.rotation.y *= rotDamping; + state.rotation.z *= rotDamping; + } + + /** + * + */ + altphysics function integratePosition(dt:Number):void { + // pos = pos + v*t + state.pos.x += state.velocity.x*dt; + state.pos.y += state.velocity.y*dt; + state.pos.z += state.velocity.z*dt; + // q = q + 0.5*rot*q + state.orientation.addScaledVector(state.rotation, dt); + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/rigid/.svn/text-base/BodyMaterial.as.svn-base b/0.0.1.0/src/alternativa/physics/rigid/.svn/text-base/BodyMaterial.as.svn-base new file mode 100644 index 0000000..61d4382 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/rigid/.svn/text-base/BodyMaterial.as.svn-base @@ -0,0 +1,11 @@ +package alternativa.physics.rigid { + + public class BodyMaterial { + + public var restitution:Number = 0; + public var friction:Number = 0.3; +// public var dynamicFriction:Number = 0.2; +// public var dynamicFriction:Number = 0.2; + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/rigid/.svn/text-base/BodyState.as.svn-base b/0.0.1.0/src/alternativa/physics/rigid/.svn/text-base/BodyState.as.svn-base new file mode 100644 index 0000000..408b0ea --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/rigid/.svn/text-base/BodyState.as.svn-base @@ -0,0 +1,39 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Quaternion; + import alternativa.physics.types.Vector3; + + /** + * Класс описывает состояние твёрдого тела. + */ + public class BodyState { + /** + * Положение тела. + */ + public var pos:Vector3 = new Vector3(); + /** + * Ориентация тела. + */ + public var orientation:Quaternion = new Quaternion(); + /** + * Скорость тела. + */ + public var velocity:Vector3 = new Vector3(); + /** + * Угловая скорость тела. + */ + public var rotation:Vector3 = new Vector3(); + + /** + * Копирует значение указанного объекта. + * + * @param state + */ + public function copy(state:BodyState):void { + pos.vCopy(state.pos); + orientation.copy(state.orientation); + velocity.vCopy(state.velocity); + rotation.vCopy(state.rotation); + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/rigid/.svn/text-base/Contact.as.svn-base b/0.0.1.0/src/alternativa/physics/rigid/.svn/text-base/Contact.as.svn-base new file mode 100644 index 0000000..b73fc55 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/rigid/.svn/text-base/Contact.as.svn-base @@ -0,0 +1,53 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.types.Vector3; + + /** + * Информация о контакте между двумя телами. Содержит нормаль, множество точек контакта и прочие величины. + */ + public class Contact { + +// private static var pool:Vector. = new Vector.(); +// +// public static function create():ContactInfo { +// if (pool.length > 0) { +// return pool.pop(); +// } +// return new ContactInfo(); +// } +// +// public static function destroy(collInfo:ContactInfo):void { +// collInfo.body1 = collInfo.body2 = null; +// pool.push(collInfo); +// } + + // Максимальное количество точек контакта + private const N:int = 8; + + public var body1:Body; + public var body2:Body; + // Взаимный коэффициент отскока + public var restitution:Number; + // Взаимный коэффициент трения + public var friction:Number; + // Нормаль контакта. Направлена от второго тела к первому. + public var normal:Vector3 = new Vector3(); + // Список точек контакта + public var points:Vector. = new Vector.(N, true); + // Количество точек контакта + public var pcount:int; + // Максимальная глубина пересечения тел + public var maxPenetration:Number = 0; + + public var satisfied:Boolean; + + /** + * + */ + public function Contact() { + for (var i:int = 0; i < N; i++) points[i] = new ContactPoint(); + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/rigid/.svn/text-base/ContactPoint.as.svn-base b/0.0.1.0/src/alternativa/physics/rigid/.svn/text-base/ContactPoint.as.svn-base new file mode 100644 index 0000000..c36f958 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/rigid/.svn/text-base/ContactPoint.as.svn-base @@ -0,0 +1,52 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Vector3; + + public class ContactPoint { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + + // Величины, расчитываемые перед началом фазы решения контактов + + // Требуемая проекция конечной скорости на нормаль для упругого контакта + public var normalVel:Number; + // Минимальная скорость разделения неупругого контакта + public var minSepVel:Number; + // Изменение проекции скорости на единицу нормального импульса + public var velByUnitImpulseN:Number; + + public var angularInertia1:Number; + public var angularInertia2:Number; + + // Радиус-вектор точки контакта относительно центра первого тела + public var r1:Vector3 = new Vector3(); + // Радиус-вектор точки контакта относительно центра второго тела + public var r2:Vector3 = new Vector3(); + + // Величины, накапливаемые во время фазы решения контактов + + // Накопленный импульс, применяемый для получения требуемой относительной скорости в точке контакта + public var accumImpulseN:Number; + // Накопленный импульс, применяемый для разделения тел в точке контакта. Не создаёт момента. +// public var accumSepImpulse:Number; + + public var satisfied:Boolean; + + /** + * + * @param cp + */ + public function copyFrom(cp:ContactPoint):void { + pos.vCopy(cp.pos); + penetration = cp.penetration; + feature1 = cp.feature1; + feature2 = cp.feature2; + r1.vCopy(cp.r1); + r2.vCopy(cp.r2); + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/rigid/.svn/text-base/PhysicsUtils.as.svn-base b/0.0.1.0/src/alternativa/physics/rigid/.svn/text-base/PhysicsUtils.as.svn-base new file mode 100644 index 0000000..a9aaa99 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/rigid/.svn/text-base/PhysicsUtils.as.svn-base @@ -0,0 +1,49 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + public class PhysicsUtils { + + /** + * @param mass + * @param halfSize + * @param result + */ + public static function getBoxInvInertia(mass:Number, halfSize:Vector3, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + result.a = 3/(mass*(yy + zz)); + result.f = 3/(mass*(zz + xx)); + result.k = 3/(mass*(xx + yy)); + } + + /** + * @param mass + * @param r + * @param h + * @param result + */ + public static function getCylinderInvInertia(mass:Number, r:Number, h:Number, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + + result.a = result.f = 1/(mass*(h*h/12 + r*r/4)); + result.k = 2/(mass*r*r); + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/rigid/.svn/text-base/RigidWorld.as.svn-base b/0.0.1.0/src/alternativa/physics/rigid/.svn/text-base/RigidWorld.as.svn-base new file mode 100644 index 0000000..fb3d9c7 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/rigid/.svn/text-base/RigidWorld.as.svn-base @@ -0,0 +1,603 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.ICollisionDetector; + import alternativa.physics.collision.KdTreeCollisionDetector; + import alternativa.physics.rigid.constraints.Constraint; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Класс реализует физическую симуляцию поведения твёрдых тел. + */ + public class RigidWorld { + + private static var lastBodyId:int; + + // Максимальное количество контактов + altphysics const MAX_CONTACTS:int = 1000; + + ////////////////////////////////////////////////////////////////// + // Настроечные параметры симуляции. Могут быть изменены в любой + // момент времени без нарушения корректной работы симуляции. + ////////////////////////////////////////////////////////////////// + // Количество шагов, за которое пересекающиеся тела должны разделиться + public var penResolutionSteps:int = 10; + // Величина допустимой глубины пересечения + public var allowedPenetration:Number = 0.1; + // Максимальная скорость, добавляемая с целью разделения тел + public var maxPenResolutionSpeed:Number = 0.5; + // Количество итераций для обработки упругих контактов + public var collisionIterations:int = 5; + // Количество итераций для обработки неупругих контактов + public var contactIterations:int = 5; + // Флаг использования предсказания состояний + public var usePrediction:Boolean = false; + + // Переменные для процедуры разделения тел путём непосредственного их перемещенеия. + // !!!!!!!!!!!!!!!!!!!!!!!! + // !!! Экспериментально !!! + // !!!!!!!!!!!!!!!!!!!!!!!! + public var staticSeparationIterations:int = 10; + public var staticSeparationSteps:int = 10; + public var maxAngleMove:Number = 10; + public var useStaticSeparation:Boolean = false; + + // Вектор гравитации + altphysics var _gravity:Vector3 = new Vector3(0, 0, -9.8); + // Использующийся детектор столкновений + altphysics var collisionDetector:ICollisionDetector; + // Список тел, участвующих в симуляции + altphysics var bodies:Vector. = new Vector.(); + // Количество тел, участвующих в симуляции + altphysics var bodiesNum:int; + // Список контактов на текущем шаге симуляции + altphysics var contacts:Vector. = new Vector.(MAX_CONTACTS, true); + // Количество контактов на текущем шаге симуляции + altphysics var contactsNum:int; + // Список ограничений + altphysics var constraints:Vector. = new Vector.(); + // Количество ограничений + altphysics var constraintsNum:int; + // Временная метка. Число прошедших шагов с начала симуляции. + altphysics var timeStamp:uint; + + // Временные переменные для избежания создания экземпляров + private var _r:Vector3 = new Vector3(); + private var _t:Vector3 = new Vector3(); + private var _v:Vector3 = new Vector3(); + private var _v1:Vector3 = new Vector3(); + private var _v2:Vector3 = new Vector3(); + + /** + * + */ + public function RigidWorld() { + for (var i:int = 0; i < MAX_CONTACTS; i++) contacts[i] = new Contact(); + collisionDetector = new KdTreeCollisionDetector(); + } + + /** + * Вектор гравитации. + */ + public function get gravity():Vector3 { + return _gravity.vClone(); + } + + /** + * @private + */ + public function set gravity(value:Vector3):void { + _gravity.vCopy(value); + } + + /** + * Добавляет тело в симуляцию. + * @param body + * @return + */ + public function addBody(body:Body, isStatic:Boolean):Boolean { + var i:int = bodies.indexOf(body); + if (i > -1) return false; + bodies[bodiesNum++] = body; + body.world = this; + body.id = lastBodyId++; + if (body.collisionPrimitives != null) { + for (i = 0; i < body.collisionPrimitivesNum; i++) collisionDetector.addPrimitive(body.collisionPrimitives[i], isStatic); + } + return true; + } + + /** + * Удаляет тело из симуляции. + * @param body + * @return + */ + public function removeBody(body:Body, isStatic:Boolean):Boolean { + var i:int = bodies.indexOf(body); + if (i == -1) return false; + bodies.splice(i, 1); + body.world = null; + if (body.collisionPrimitives != null) { + for (i = 0; i < body.collisionPrimitivesNum; i++) collisionDetector.removePrimitive(body.collisionPrimitives[i], isStatic); + } + bodiesNum--; + return true; + } + + /** + * Добавляет ограничение. + * @param c + */ + public function addConstraint(c:Constraint):void { + constraints[constraintsNum++] = c; + c.world = this; + } + + /** + * Удаляет ограничение. + * @param c + */ + public function removeConstraint(c:Constraint):Boolean { + var idx:int = constraints.indexOf(c); + if (idx < 0) return false; + constraints.splice(idx, 1) + constraintsNum--; + c.world = null; + return true; + } + + /** + * Применяет к телам действующте на них силы. Аккумуляторы сил и моментов тел очищаются после завершения шага + * симуляции, поэтому на момент вызова метода могут уже содержать некоторые значения. + * + * @param dt промежуток времени, в течении которого действуют силы + */ + private function applyForces(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i]; + body.addExternalForces(dt); + body.calcAccelerations(); + // Ускорение свободного падения применяется только к подвижным телам во избежание некорректного изменения + // фиктивной скорости неподвижных тел. + if (body.movable) body.accel.vAdd(_gravity); + } + } + + /** + * Определяет все столкновения на текущем шаге симуляции и заполняет список получившихся контактов. + * + * @param dt длительность шага симуляции + */ + private function detectCollisions(dt:Number):void { + var i:int; + var j:int; + var body:Body; + + for (i = 0; i < bodiesNum; i++) { + body = bodies[i]; + body.contactsNum = 0; + body.saveState(); + // При включённом режиме предсказания состояние тел интегрируется на один шаг вперёд + if (usePrediction) { + body.integrateVelocity(dt); + body.integratePosition(dt); + } + body.calcDerivedData(); + } + + contactsNum = collisionDetector.getAllCollisions(contacts); + + // Расчёт относительных векторов точки контакта вынесен сюда из-за необходимости учитывать + // положение тел в предсказанном состоянии + for (i = 0; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + var b1:Body = contact.body1; + var b2:Body = contact.body2; + for (j = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.r1.x = cp.pos.x - b1.state.pos.x; + cp.r1.y = cp.pos.y - b1.state.pos.y; + cp.r1.z = cp.pos.z - b1.state.pos.z; + if (b2 != null) { + cp.r2.x = cp.pos.x - b2.state.pos.x; + cp.r2.y = cp.pos.y - b2.state.pos.y; + cp.r2.z = cp.pos.z - b2.state.pos.z; + } + } + } + + // Восстановление состояния тел + if (usePrediction) { + for (i = 0; i < bodiesNum; i++) { + body = bodies[i]; + body.restoreState(); + body.calcDerivedData(); + } + } + } + + /** + * Подготваливает полученные из детектора столкновений контакты, расчитывая значения, не меняющиеся + * в ходе шага симуляции. + */ + private function preProcessContacts(dt:Number):void { + var i:int; + for (i = 0; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + var b1:Body = contact.body1; + var b2:Body = contact.body2; + contact.restitution = b1.material.restitution; + if (b2 != null && b2.material.restitution < contact.restitution) contact.restitution = b2.material.restitution; + contact.friction = b1.material.friction; + if (b2 != null && b2.material.friction < contact.friction) contact.friction = b2.material.friction; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.accumImpulseN = 0; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + cp.velByUnitImpulseN = 0; + if (b1.movable) { + cp.angularInertia1 = _v.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1).vDot(contact.normal); + cp.velByUnitImpulseN += b1.invMass + cp.angularInertia1; + } + if (b2 != null && b2.movable) { + cp.angularInertia2 = _v.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2).vDot(contact.normal); + cp.velByUnitImpulseN += b2.invMass + cp.angularInertia2; + } + // Расчёт требуемой конечной скорости для упругого контакта + calcSepVelocity(b1, b2, cp, _v); + cp.normalVel = _v.vDot(contact.normal); + if (cp.normalVel < 0) cp.normalVel = - contact.restitution*cp.normalVel; + // Скорость разделения неупругого контакта + cp.minSepVel = cp.penetration > allowedPenetration ? (cp.penetration - allowedPenetration)/(penResolutionSteps*dt) : 0; + if (cp.minSepVel > maxPenResolutionSpeed) cp.minSepVel = maxPenResolutionSpeed; + } + } + for (i = 0; i < constraintsNum; i++) (constraints[i] as Constraint).preProcess(dt); + } + + /** + * + * @param dt + * @param forceInelastic + */ + private function processContacts(dt:Number, forceInelastic:Boolean):void { + var iterNum:int = forceInelastic ? contactIterations : collisionIterations; + var i:int; + var forwardLoop:Boolean = false; + for (var iter:int = 0; iter < iterNum; iter++) { + forwardLoop = !forwardLoop; + for (i = 0; i < contactsNum; i++) resolveContact(contacts[i], forceInelastic, forwardLoop); + // Ограничения + for (i = 0; i < constraintsNum; i++) (constraints[i] as Constraint).apply(dt); + } + + // Разделение контактов путём непосредственного изменения координат и ориентации. Экспериментально. + if (forceInelastic && useStaticSeparation) performStaticSeparation(); + } + + /** + * + */ + private function resolveContact(contactInfo:Contact, forceInelastic:Boolean, forwardLoop:Boolean):void { + var b1:Body = contactInfo.body1; + var b2:Body = contactInfo.body2; + var normal:Vector3 = contactInfo.normal; + var i:int; + if (forwardLoop) { + for (i = 0; i < contactInfo.pcount; i++) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } else { + for (i = contactInfo.pcount - 1; i >= 0; i--) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } + } + + /** + * + * @param idx + * @param b1 + * @param b2 + * @param colInfo + * @param normal + * @param forceInelastic + */ + private function resolveContactPoint(idx:int, b1:Body, b2:Body, contact:Contact, normal:Vector3, forceInelastic:Boolean):void { + var cp:ContactPoint = contact.points[idx]; + if (!forceInelastic) cp.satisfied = true; + + var newVel:Number = 0; + calcSepVelocity(b1, b2, cp, _v); + var sepVel:Number = _v.vDot(contact.normal); + if (forceInelastic) { + var minSpeVel:Number = useStaticSeparation ? 0 : cp.minSepVel; + if (sepVel < minSpeVel) cp.satisfied = false; + else if (cp.satisfied) return; + newVel = minSpeVel; + } else { + newVel = cp.normalVel; + } + var deltaVel:Number = newVel - sepVel; + var impulse:Number = deltaVel/cp.velByUnitImpulseN; + var accumImpulse:Number = cp.accumImpulseN + impulse; + if (accumImpulse < 0) accumImpulse = 0; + var deltaImpulse:Number = accumImpulse - cp.accumImpulseN; + cp.accumImpulseN = accumImpulse; + // Применяем импульс к телам + if (b1.movable) b1.applyRelPosWorldImpulse(cp.r1, normal, deltaImpulse); + if (b2 != null && b2.movable) b2.applyRelPosWorldImpulse(cp.r2, normal, -deltaImpulse); + + // Учёт силы трения + calcSepVelocity(b1, b2, cp, _v); + // Расчитываем изменение касательной скорости на единицу касательного импульса + var tanSpeedByUnitImpulse:Number = 0; + _v.vAddScaled(-_v.vDot(contact.normal), contact.normal); + var tanSpeed:Number = _v.vLength(); + if (tanSpeed < 0.001) return; + _t.vCopy(_v).vNormalize().vReverse(); + // dV = b.invMass + ((invI * (r % t)) % r) * t + if (b1.movable) { + _v.vCross2(cp.r1, _t).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1); + tanSpeedByUnitImpulse += b1.invMass + _v.vDot(_t); + } + if (b2 != null && b2.movable) { + _v.vCross2(cp.r2, _t).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2); + tanSpeedByUnitImpulse += b2.invMass + _v.vDot(_t); + } + + var tanImpulse:Number = tanSpeed/tanSpeedByUnitImpulse; + var max:Number = contact.friction*cp.accumImpulseN; + if (max < 0) { + if (tanImpulse < max) tanImpulse = max; + } else { + if (tanImpulse > max) tanImpulse = max; + } + + // Применяем импульс к телам + if (b1.movable) b1.applyRelPosWorldImpulse(cp.r1, _t, tanImpulse); + if (b2 != null && b2.movable) b2.applyRelPosWorldImpulse(cp.r2, _t, -tanImpulse); + } + + /** + * + * @param cp + * @param normal + * @return + */ + private function calcSepVelocity(body1:Body, body2:Body, cp:ContactPoint, result:Vector3):void { + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 + result.vCopy(body1.state.velocity).vAdd(_v1.vCross2(body1.state.rotation, cp.r1)); + // V2 = V2_c + w2%r2 + if (body2 != null) result.vSubtract(body2.state.velocity).vSubtract(_v2.vCross2(body2.state.rotation, cp.r2)); + } + + /** + * + * @param dt + */ + private function intergateVelocities(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + (bodies[i] as Body).integrateVelocity(dt); + } + } + + /** + * + * @param dt + */ + private function integratePositions(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i] as Body; + if (body.movable) body.integratePosition(dt); + } + } + + /** + * + */ + private function performStaticSeparation():void { + var iterNum:int = staticSeparationIterations; +// iterNum = 100; + + // 1. В начале каждой итерации для всех контактов сбрасывается флаг satisfied с одновременным поиском наихудшего контакта, т.е. имеющего наибольшую величину пересечения тел. + // 2. Если найденный контакт имеет величину пересечения меньше предельно допустимой процедура прерывается, т.к. разделение контактов не требуется. + // 3. Если процедура продолжается, то выполняется разделение найденного контакта и установка его флага satisfied в true, после чего запускается внутренний цикл, состоящий + // из contactsNum - 1 итераций. На каждой итерации ищется наихудший контакт среди оставшихся (satisfied == false). Если такой контакт найден, то выполняется его разделение + // и переход к следующей итерации внутреннего цикла, иначе внутренний цикл прерывается и выполняется переход к следующей итерации внешнего цикла (шаг 1.). + for (var iter:int = 0; iter < iterNum; iter++) { + // Ищем контакт с максимальной величиной пересечения тел, одновременно сбрасывая флаги satisfied в false + var worstContact:Contact = contacts[0]; + var i:int; + for (i = 1; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + contact.satisfied = false; + if (contact.maxPenetration > worstContact.maxPenetration) worstContact = contact; + } + if (worstContact.maxPenetration <= allowedPenetration) return; + + resolveInterpenetration(worstContact); + // Внутренний цикл по оставшимся контактам + for (i = 1; i < contactsNum; i++) { + worstContact = getWorstContact(); + if (worstContact == null) break; + resolveInterpenetration(worstContact); + } + } + } + + /** + * + */ + private function getWorstContact():Contact { + var maxPen:Number = 0; + var worst:Contact = null; + for (var i:int = 0; i < contactsNum; i++) { + var c:Contact = contacts[i]; + if (!c.satisfied && c.maxPenetration > maxPen) { + worst = c; + maxPen = c.maxPenetration; + } + } + return maxPen > allowedPenetration ? worst : null; + } + + /** + * Разделяет указанный контакт, выполняя staticSeparationSteps итераций по списку точек контакта. + * 1. В начале каждой итерации ищется точка с наибольшим пересечением, одновременно сбрасываются флаги satisfied у точек. + * 2. Для найденной точки выполняется процедура разделения. + * 3. После выполняется pcount - 1 итерация по списку точек, каждый раз ищется наихудшая среди имеющих satisfied == false, для которой выполняется процедура разделения. + * Если наихудшая точка не найдена, то выполнется переход к следующей итерации внешнего цикла (шаг 1). + * + * В конце процедуры разделения обновляются значения пересечений для остальных точек контакта, а также для всех прочих контактов, относящихся к телам текущего. + * + * @param contact контакт для разделения + */ + private function resolveInterpenetration(contact:Contact):void { + contact.satisfied = true; + + for (var step:int = 0; step < staticSeparationSteps; step++) { + var worstCp:ContactPoint = contact.points[0]; + var cp:ContactPoint; + var i:int; + for (i = 1; i < contact.pcount; i++) { + cp = contact.points[i]; + cp.satisfied = false; + if (cp.penetration > worstCp.penetration) worstCp = cp; + } + if (worstCp.penetration <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + // Разделяем оставшиеся точки + var maxPen:Number = 0; + for (i = 1; i < contact.pcount; i++) { + // Поиск наихудшей точки + for (var j:int = 0; j < contact.pcount; j++) { + cp = contact.points[j]; + if (cp.satisfied) continue; + if (cp.penetration > maxPen) { + maxPen = cp.penetration; + worstCp = cp; + } + } + if (maxPen <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + } + } + } + + /** + * + * @param cp + * @param contact + */ + private function separateContactPoint(cp:ContactPoint, contact:Contact):void { + cp.satisfied = true; + + var b1:Body = contact.body1; + var b2:Body = contact.body2; + var totalMove:Number = cp.penetration - allowedPenetration; + var moveCoeff:Number = totalMove/cp.velByUnitImpulseN; + var linMove1:Number; + var angleMove1:Number; + if (b1.movable) { + linMove1 = b1.invMass*moveCoeff; + angleMove1 = cp.angularInertia1*moveCoeff; + if (angleMove1 > maxAngleMove) { + linMove1 += angleMove1 - maxAngleMove; + angleMove1 -= maxAngleMove; + } + b1.state.pos.vAddScaled(linMove1, contact.normal); + _v1.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vScale(angleMove1); + b1.state.orientation.addScaledVector(_v1, 1); + } + var linMove2:Number; + var angleMove2:Number; + if (b2 != null && b2.movable) { + linMove2 = b2.invMass*moveCoeff; + angleMove2 = cp.angularInertia2*moveCoeff; + if (angleMove2 > maxAngleMove) { + linMove2 += angleMove2 - maxAngleMove; + angleMove2 -= maxAngleMove; + } + b2.state.pos.vAddScaled(-linMove2, contact.normal); + _v2.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vScale(angleMove2); + _v2.vReverse(); + b2.state.orientation.addScaledVector(_v2, 1); + } + cp.penetration = allowedPenetration; + // Обновляем пересечения в других точках + var i:int; + for (i = 0; i < contact.pcount; i++) { + var cp1:ContactPoint = contact.points[i]; + if (cp1 == cp) continue; + var angularMove:Number; + if (b1.movable) { + angularMove = _v.vCross2(_v1, cp1.r1).vDot(contact.normal); + cp1.penetration -= linMove1 + angularMove; + } + if (b2 != null && b2.movable) { + angularMove = _v.vCross2(_v2, cp1.r2).vDot(contact.normal); + cp1.penetration -= linMove2 - angularMove; + } + // Обновление максимального значения для контакта + if (cp1.penetration > contact.maxPenetration) contact.maxPenetration = cp1.penetration; + } + // Обновляем пересечения для других контактов + var c:Contact; + var j:int; + if (b1.movable) { + for (i = 0; i < b1.contactsNum; i++) { + c = b1.contacts[i]; + if (c == contact) continue; + for (j = 0; j < c.pcount; j++) { + cp1 = c.points[j]; + if (b1 == c.body1) cp1.penetration -= linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r1).vDot(c.normal); + else cp1.penetration += linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r2).vDot(c.normal); + if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; + } + } + } + if (b2 != null && b2.movable) { + for (i = 0; i < b2.contactsNum; i++) { + c = b2.contacts[i]; + if (c == contact) continue; + for (j = 0; j < c.pcount; j++) { + cp1 = c.points[j]; + if (b2 == c.body1) cp1.penetration -= linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r1).vDot(c.normal); + else cp1.penetration += linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r2).vDot(c.normal); + if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; + } + } + } + } + + /** + * + */ + private function postPhysics():void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i]; + body.clearAccumulators(); + body.calcDerivedData(); + } + } + + /** + * + * @param dt + */ + public function runPhysics(dt:Number):void { + timeStamp++; + applyForces(dt); + detectCollisions(dt); + preProcessContacts(dt); + processContacts(dt, false); + intergateVelocities(dt); + processContacts(dt, true); + integratePositions(dt); + postPhysics(); + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/rigid/Body.as b/0.0.1.0/src/alternativa/physics/rigid/Body.as new file mode 100644 index 0000000..4e83bfe --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/rigid/Body.as @@ -0,0 +1,329 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Quaternion; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class Body { + + public static var linDamping:Number = 0.997; + public static var rotDamping:Number = 0.997; + + public var name:String; + public var movable:Boolean = true; + + altphysics var id:int; + // Мир, в котором находится тело + altphysics var world:RigidWorld; + // Текущее и предыдущее состояние тела. Промежуточное состояние вычисляется линейной интерполяцией. + altphysics var state:BodyState = new BodyState(); + altphysics var prevState:BodyState = new BodyState(); + // Линейное и угловое ускорение тела на текущем шаге симуляции + altphysics var accel:Vector3 = new Vector3(); + altphysics var angleAccel:Vector3 = new Vector3(); + // Материал тела + altphysics var material:BodyMaterial = new BodyMaterial(); + + altphysics var invMass:Number = 1; + altphysics var invInertia:Matrix3 = new Matrix3(); + altphysics var invInertiaWorld:Matrix3 = new Matrix3(); + altphysics var baseMatrix:Matrix3 = new Matrix3(); + + altphysics const MAX_CONTACTS:int = 20; + altphysics var contacts:Vector. = new Vector.(MAX_CONTACTS); + altphysics var contactsNum:int; + + altphysics var collisionPrimitives:Vector.; + altphysics var collisionPrimitivesNum:int; + + // Аккумулятор сил + altphysics var forceAccum:Vector3 = new Vector3(); + // Аккумулятор моментов + altphysics var torqueAccum:Vector3 = new Vector3(); + + // Внутренние переменные для избежания создания экземпляров + private static var _r:Vector3 = new Vector3(); + private static var _f:Vector3 = new Vector3(); + + /** + * + * @param invMass + * @param invInertia + */ + public function Body(invMass:Number, invInertia:Matrix3) { + this.invMass = invMass; + this.invInertia.copy(invInertia); + } + + /** + * @param prim + */ + public function addCollisionPrimitive(primitive:CollisionPrimitive, localTransform:Matrix4 = null):void { + if (primitive == null) throw new ArgumentError("Primitive cannot be null"); + if (collisionPrimitives == null) collisionPrimitives = new Vector.(); + collisionPrimitives[collisionPrimitivesNum++] = primitive; + primitive.setBody(this, localTransform); + } + + /** + * @param t + * @param result + */ + public function interpolate(t:Number, pos:Vector3, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.w = prevState.orientation.w*t1 + state.orientation.w*t; + orientation.x = prevState.orientation.x*t1 + state.orientation.x*t; + orientation.y = prevState.orientation.y*t1 + state.orientation.y*t; + orientation.z = prevState.orientation.z*t1 + state.orientation.z*t; + } + + /** + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + state.pos.vCopy(pos); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setPositionXYZ(x:Number, y:Number, z:Number):void { + state.pos.vReset(x, y, z); + } + + /** + * + * @param vel + */ + public function setVelocity(vel:Vector3):void { + state.velocity.vCopy(vel); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setVelocityXYZ(x:Number, y:Number, z:Number):void { + state.velocity.vReset(x, y, z); + } + + /** + * + * @param rot + */ + public function setRotation(rot:Vector3):void { + state.rotation.vCopy(rot); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setRotationXYZ(x:Number, y:Number, z:Number):void { + state.rotation.vReset(x, y, z); + } + + /** + * + * @param q + */ + public function setOrientation(q:Quaternion):void { + state.orientation.copy(q); + } + + /** + * @param r + * @param dir + * @param magnitude + */ + public function applyRelPosWorldImpulse(r:Vector3, dir:Vector3, magnitude:Number):void { + var d:Number = magnitude*invMass; + // Линейная часть + state.velocity.x += d*dir.x; + state.velocity.y += d*dir.y; + state.velocity.z += d*dir.z; + + // Вращательная часть + var rx:Number = r.x; + var ry:Number = r.y; + var rz:Number = r.z; + _r.x = (ry*dir.z - rz*dir.y)*magnitude; + _r.y = (rz*dir.x - rx*dir.z)*magnitude; + _r.z = (rx*dir.y - ry*dir.x)*magnitude; + _r.vTransformBy3(invInertiaWorld); + state.rotation.x += _r.x; + state.rotation.y += _r.y; + state.rotation.z += _r.z; + } + + /** + * @param f + */ + public function addForce(f:Vector3):void { + forceAccum.vAdd(f); + } + + /** + * @param pos + * @param f + */ + public function addWorldForce(pos:Vector3, force:Vector3):void { + forceAccum.vAdd(force); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(force)); + } + + /** + * @param pos + * @param f + */ + public function addWorldForceScaled(pos:Vector3, force:Vector3, scale:Number):void { + _f.x = scale*force.x, _f.y = scale*force.y, _f.z = scale*force.z; + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(_f)); + } + + /** + * @param pos + * @param f + */ + public function addLocalForce(pos:Vector3, force:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(pos, _r); + // Трансформируем вектор силы в мировую систему + baseMatrix.transformVector(force, _f); + // Добавляем силу и момент + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vCross(_f)); + } + + /** + * + * @param localPos + * @param worldForce + */ + public function addWorldForceAtLocalPoint(localPos:Vector3, worldForce:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(localPos, _r); + // Добавляем силу и момент + forceAccum.vAdd(worldForce); + torqueAccum.vAdd(_r.vCross(worldForce)); + } + + /** + * @param dt + */ + public function addExternalForces(dt:Number):void { + } + + /** + * @param t + */ + public function addTorque(t:Vector3):void { + torqueAccum.vAdd(t); + } + + /** + * + */ + altphysics function clearAccumulators():void { + forceAccum.vReset(); + torqueAccum.vReset(); + } + + /** + * + */ + altphysics function calcAccelerations():void { + accel.x = forceAccum.x*invMass; + accel.y = forceAccum.y*invMass; + accel.z = forceAccum.z*invMass; + invInertiaWorld.transformVector(torqueAccum, angleAccel); + } + + /** + * Вычисляет производные данные. + */ + altphysics function calcDerivedData():void { + // Вычисление базисной матрицы и обратного тензора инерции в мировых координатах + state.orientation.toMatrix3(baseMatrix); + invInertiaWorld.copy(invInertia).append(baseMatrix).prepend(baseMatrix.transpose()); + baseMatrix.transpose(); + if (collisionPrimitives != null) { + for (var i:int = 0; i < collisionPrimitivesNum; i++) { + var primitive:CollisionPrimitive = collisionPrimitives[i]; + primitive.transform.setFromMatrix3(baseMatrix, state.pos); + if (primitive.localTransform != null) primitive.transform.prepend(primitive.localTransform); + } + } + } + + /** + * + */ + altphysics function saveState():void { + prevState.copy(state); + } + + /** + * + */ + altphysics function restoreState():void { + state.copy(prevState); + } + + /** + * @param dt + */ + altphysics function integrateVelocity(dt:Number):void { + // v = v + a*t + state.velocity.x += accel.x*dt; + state.velocity.y += accel.y*dt; + state.velocity.z += accel.z*dt; + // rot = rot + eps*t + state.rotation.x += angleAccel.x*dt; + state.rotation.y += angleAccel.y*dt; + state.rotation.z += angleAccel.z*dt; + + state.velocity.x *= linDamping; + state.velocity.y *= linDamping; + state.velocity.z *= linDamping; + + state.rotation.x *= rotDamping; + state.rotation.y *= rotDamping; + state.rotation.z *= rotDamping; + } + + /** + * + */ + altphysics function integratePosition(dt:Number):void { + // pos = pos + v*t + state.pos.x += state.velocity.x*dt; + state.pos.y += state.velocity.y*dt; + state.pos.z += state.velocity.z*dt; + // q = q + 0.5*rot*q + state.orientation.addScaledVector(state.rotation, dt); + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/rigid/BodyMaterial.as b/0.0.1.0/src/alternativa/physics/rigid/BodyMaterial.as new file mode 100644 index 0000000..61d4382 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/rigid/BodyMaterial.as @@ -0,0 +1,11 @@ +package alternativa.physics.rigid { + + public class BodyMaterial { + + public var restitution:Number = 0; + public var friction:Number = 0.3; +// public var dynamicFriction:Number = 0.2; +// public var dynamicFriction:Number = 0.2; + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/rigid/BodyState.as b/0.0.1.0/src/alternativa/physics/rigid/BodyState.as new file mode 100644 index 0000000..408b0ea --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/rigid/BodyState.as @@ -0,0 +1,39 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Quaternion; + import alternativa.physics.types.Vector3; + + /** + * Класс описывает состояние твёрдого тела. + */ + public class BodyState { + /** + * Положение тела. + */ + public var pos:Vector3 = new Vector3(); + /** + * Ориентация тела. + */ + public var orientation:Quaternion = new Quaternion(); + /** + * Скорость тела. + */ + public var velocity:Vector3 = new Vector3(); + /** + * Угловая скорость тела. + */ + public var rotation:Vector3 = new Vector3(); + + /** + * Копирует значение указанного объекта. + * + * @param state + */ + public function copy(state:BodyState):void { + pos.vCopy(state.pos); + orientation.copy(state.orientation); + velocity.vCopy(state.velocity); + rotation.vCopy(state.rotation); + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/rigid/Contact.as b/0.0.1.0/src/alternativa/physics/rigid/Contact.as new file mode 100644 index 0000000..b73fc55 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/rigid/Contact.as @@ -0,0 +1,53 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.types.Vector3; + + /** + * Информация о контакте между двумя телами. Содержит нормаль, множество точек контакта и прочие величины. + */ + public class Contact { + +// private static var pool:Vector. = new Vector.(); +// +// public static function create():ContactInfo { +// if (pool.length > 0) { +// return pool.pop(); +// } +// return new ContactInfo(); +// } +// +// public static function destroy(collInfo:ContactInfo):void { +// collInfo.body1 = collInfo.body2 = null; +// pool.push(collInfo); +// } + + // Максимальное количество точек контакта + private const N:int = 8; + + public var body1:Body; + public var body2:Body; + // Взаимный коэффициент отскока + public var restitution:Number; + // Взаимный коэффициент трения + public var friction:Number; + // Нормаль контакта. Направлена от второго тела к первому. + public var normal:Vector3 = new Vector3(); + // Список точек контакта + public var points:Vector. = new Vector.(N, true); + // Количество точек контакта + public var pcount:int; + // Максимальная глубина пересечения тел + public var maxPenetration:Number = 0; + + public var satisfied:Boolean; + + /** + * + */ + public function Contact() { + for (var i:int = 0; i < N; i++) points[i] = new ContactPoint(); + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/rigid/ContactPoint.as b/0.0.1.0/src/alternativa/physics/rigid/ContactPoint.as new file mode 100644 index 0000000..c36f958 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/rigid/ContactPoint.as @@ -0,0 +1,52 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Vector3; + + public class ContactPoint { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + + // Величины, расчитываемые перед началом фазы решения контактов + + // Требуемая проекция конечной скорости на нормаль для упругого контакта + public var normalVel:Number; + // Минимальная скорость разделения неупругого контакта + public var minSepVel:Number; + // Изменение проекции скорости на единицу нормального импульса + public var velByUnitImpulseN:Number; + + public var angularInertia1:Number; + public var angularInertia2:Number; + + // Радиус-вектор точки контакта относительно центра первого тела + public var r1:Vector3 = new Vector3(); + // Радиус-вектор точки контакта относительно центра второго тела + public var r2:Vector3 = new Vector3(); + + // Величины, накапливаемые во время фазы решения контактов + + // Накопленный импульс, применяемый для получения требуемой относительной скорости в точке контакта + public var accumImpulseN:Number; + // Накопленный импульс, применяемый для разделения тел в точке контакта. Не создаёт момента. +// public var accumSepImpulse:Number; + + public var satisfied:Boolean; + + /** + * + * @param cp + */ + public function copyFrom(cp:ContactPoint):void { + pos.vCopy(cp.pos); + penetration = cp.penetration; + feature1 = cp.feature1; + feature2 = cp.feature2; + r1.vCopy(cp.r1); + r2.vCopy(cp.r2); + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/rigid/PhysicsUtils.as b/0.0.1.0/src/alternativa/physics/rigid/PhysicsUtils.as new file mode 100644 index 0000000..a9aaa99 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/rigid/PhysicsUtils.as @@ -0,0 +1,49 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + public class PhysicsUtils { + + /** + * @param mass + * @param halfSize + * @param result + */ + public static function getBoxInvInertia(mass:Number, halfSize:Vector3, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + result.a = 3/(mass*(yy + zz)); + result.f = 3/(mass*(zz + xx)); + result.k = 3/(mass*(xx + yy)); + } + + /** + * @param mass + * @param r + * @param h + * @param result + */ + public static function getCylinderInvInertia(mass:Number, r:Number, h:Number, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + + result.a = result.f = 1/(mass*(h*h/12 + r*r/4)); + result.k = 2/(mass*r*r); + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/rigid/RigidWorld.as b/0.0.1.0/src/alternativa/physics/rigid/RigidWorld.as new file mode 100644 index 0000000..fb3d9c7 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/rigid/RigidWorld.as @@ -0,0 +1,603 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.ICollisionDetector; + import alternativa.physics.collision.KdTreeCollisionDetector; + import alternativa.physics.rigid.constraints.Constraint; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Класс реализует физическую симуляцию поведения твёрдых тел. + */ + public class RigidWorld { + + private static var lastBodyId:int; + + // Максимальное количество контактов + altphysics const MAX_CONTACTS:int = 1000; + + ////////////////////////////////////////////////////////////////// + // Настроечные параметры симуляции. Могут быть изменены в любой + // момент времени без нарушения корректной работы симуляции. + ////////////////////////////////////////////////////////////////// + // Количество шагов, за которое пересекающиеся тела должны разделиться + public var penResolutionSteps:int = 10; + // Величина допустимой глубины пересечения + public var allowedPenetration:Number = 0.1; + // Максимальная скорость, добавляемая с целью разделения тел + public var maxPenResolutionSpeed:Number = 0.5; + // Количество итераций для обработки упругих контактов + public var collisionIterations:int = 5; + // Количество итераций для обработки неупругих контактов + public var contactIterations:int = 5; + // Флаг использования предсказания состояний + public var usePrediction:Boolean = false; + + // Переменные для процедуры разделения тел путём непосредственного их перемещенеия. + // !!!!!!!!!!!!!!!!!!!!!!!! + // !!! Экспериментально !!! + // !!!!!!!!!!!!!!!!!!!!!!!! + public var staticSeparationIterations:int = 10; + public var staticSeparationSteps:int = 10; + public var maxAngleMove:Number = 10; + public var useStaticSeparation:Boolean = false; + + // Вектор гравитации + altphysics var _gravity:Vector3 = new Vector3(0, 0, -9.8); + // Использующийся детектор столкновений + altphysics var collisionDetector:ICollisionDetector; + // Список тел, участвующих в симуляции + altphysics var bodies:Vector. = new Vector.(); + // Количество тел, участвующих в симуляции + altphysics var bodiesNum:int; + // Список контактов на текущем шаге симуляции + altphysics var contacts:Vector. = new Vector.(MAX_CONTACTS, true); + // Количество контактов на текущем шаге симуляции + altphysics var contactsNum:int; + // Список ограничений + altphysics var constraints:Vector. = new Vector.(); + // Количество ограничений + altphysics var constraintsNum:int; + // Временная метка. Число прошедших шагов с начала симуляции. + altphysics var timeStamp:uint; + + // Временные переменные для избежания создания экземпляров + private var _r:Vector3 = new Vector3(); + private var _t:Vector3 = new Vector3(); + private var _v:Vector3 = new Vector3(); + private var _v1:Vector3 = new Vector3(); + private var _v2:Vector3 = new Vector3(); + + /** + * + */ + public function RigidWorld() { + for (var i:int = 0; i < MAX_CONTACTS; i++) contacts[i] = new Contact(); + collisionDetector = new KdTreeCollisionDetector(); + } + + /** + * Вектор гравитации. + */ + public function get gravity():Vector3 { + return _gravity.vClone(); + } + + /** + * @private + */ + public function set gravity(value:Vector3):void { + _gravity.vCopy(value); + } + + /** + * Добавляет тело в симуляцию. + * @param body + * @return + */ + public function addBody(body:Body, isStatic:Boolean):Boolean { + var i:int = bodies.indexOf(body); + if (i > -1) return false; + bodies[bodiesNum++] = body; + body.world = this; + body.id = lastBodyId++; + if (body.collisionPrimitives != null) { + for (i = 0; i < body.collisionPrimitivesNum; i++) collisionDetector.addPrimitive(body.collisionPrimitives[i], isStatic); + } + return true; + } + + /** + * Удаляет тело из симуляции. + * @param body + * @return + */ + public function removeBody(body:Body, isStatic:Boolean):Boolean { + var i:int = bodies.indexOf(body); + if (i == -1) return false; + bodies.splice(i, 1); + body.world = null; + if (body.collisionPrimitives != null) { + for (i = 0; i < body.collisionPrimitivesNum; i++) collisionDetector.removePrimitive(body.collisionPrimitives[i], isStatic); + } + bodiesNum--; + return true; + } + + /** + * Добавляет ограничение. + * @param c + */ + public function addConstraint(c:Constraint):void { + constraints[constraintsNum++] = c; + c.world = this; + } + + /** + * Удаляет ограничение. + * @param c + */ + public function removeConstraint(c:Constraint):Boolean { + var idx:int = constraints.indexOf(c); + if (idx < 0) return false; + constraints.splice(idx, 1) + constraintsNum--; + c.world = null; + return true; + } + + /** + * Применяет к телам действующте на них силы. Аккумуляторы сил и моментов тел очищаются после завершения шага + * симуляции, поэтому на момент вызова метода могут уже содержать некоторые значения. + * + * @param dt промежуток времени, в течении которого действуют силы + */ + private function applyForces(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i]; + body.addExternalForces(dt); + body.calcAccelerations(); + // Ускорение свободного падения применяется только к подвижным телам во избежание некорректного изменения + // фиктивной скорости неподвижных тел. + if (body.movable) body.accel.vAdd(_gravity); + } + } + + /** + * Определяет все столкновения на текущем шаге симуляции и заполняет список получившихся контактов. + * + * @param dt длительность шага симуляции + */ + private function detectCollisions(dt:Number):void { + var i:int; + var j:int; + var body:Body; + + for (i = 0; i < bodiesNum; i++) { + body = bodies[i]; + body.contactsNum = 0; + body.saveState(); + // При включённом режиме предсказания состояние тел интегрируется на один шаг вперёд + if (usePrediction) { + body.integrateVelocity(dt); + body.integratePosition(dt); + } + body.calcDerivedData(); + } + + contactsNum = collisionDetector.getAllCollisions(contacts); + + // Расчёт относительных векторов точки контакта вынесен сюда из-за необходимости учитывать + // положение тел в предсказанном состоянии + for (i = 0; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + var b1:Body = contact.body1; + var b2:Body = contact.body2; + for (j = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.r1.x = cp.pos.x - b1.state.pos.x; + cp.r1.y = cp.pos.y - b1.state.pos.y; + cp.r1.z = cp.pos.z - b1.state.pos.z; + if (b2 != null) { + cp.r2.x = cp.pos.x - b2.state.pos.x; + cp.r2.y = cp.pos.y - b2.state.pos.y; + cp.r2.z = cp.pos.z - b2.state.pos.z; + } + } + } + + // Восстановление состояния тел + if (usePrediction) { + for (i = 0; i < bodiesNum; i++) { + body = bodies[i]; + body.restoreState(); + body.calcDerivedData(); + } + } + } + + /** + * Подготваливает полученные из детектора столкновений контакты, расчитывая значения, не меняющиеся + * в ходе шага симуляции. + */ + private function preProcessContacts(dt:Number):void { + var i:int; + for (i = 0; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + var b1:Body = contact.body1; + var b2:Body = contact.body2; + contact.restitution = b1.material.restitution; + if (b2 != null && b2.material.restitution < contact.restitution) contact.restitution = b2.material.restitution; + contact.friction = b1.material.friction; + if (b2 != null && b2.material.friction < contact.friction) contact.friction = b2.material.friction; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.accumImpulseN = 0; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + cp.velByUnitImpulseN = 0; + if (b1.movable) { + cp.angularInertia1 = _v.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1).vDot(contact.normal); + cp.velByUnitImpulseN += b1.invMass + cp.angularInertia1; + } + if (b2 != null && b2.movable) { + cp.angularInertia2 = _v.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2).vDot(contact.normal); + cp.velByUnitImpulseN += b2.invMass + cp.angularInertia2; + } + // Расчёт требуемой конечной скорости для упругого контакта + calcSepVelocity(b1, b2, cp, _v); + cp.normalVel = _v.vDot(contact.normal); + if (cp.normalVel < 0) cp.normalVel = - contact.restitution*cp.normalVel; + // Скорость разделения неупругого контакта + cp.minSepVel = cp.penetration > allowedPenetration ? (cp.penetration - allowedPenetration)/(penResolutionSteps*dt) : 0; + if (cp.minSepVel > maxPenResolutionSpeed) cp.minSepVel = maxPenResolutionSpeed; + } + } + for (i = 0; i < constraintsNum; i++) (constraints[i] as Constraint).preProcess(dt); + } + + /** + * + * @param dt + * @param forceInelastic + */ + private function processContacts(dt:Number, forceInelastic:Boolean):void { + var iterNum:int = forceInelastic ? contactIterations : collisionIterations; + var i:int; + var forwardLoop:Boolean = false; + for (var iter:int = 0; iter < iterNum; iter++) { + forwardLoop = !forwardLoop; + for (i = 0; i < contactsNum; i++) resolveContact(contacts[i], forceInelastic, forwardLoop); + // Ограничения + for (i = 0; i < constraintsNum; i++) (constraints[i] as Constraint).apply(dt); + } + + // Разделение контактов путём непосредственного изменения координат и ориентации. Экспериментально. + if (forceInelastic && useStaticSeparation) performStaticSeparation(); + } + + /** + * + */ + private function resolveContact(contactInfo:Contact, forceInelastic:Boolean, forwardLoop:Boolean):void { + var b1:Body = contactInfo.body1; + var b2:Body = contactInfo.body2; + var normal:Vector3 = contactInfo.normal; + var i:int; + if (forwardLoop) { + for (i = 0; i < contactInfo.pcount; i++) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } else { + for (i = contactInfo.pcount - 1; i >= 0; i--) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } + } + + /** + * + * @param idx + * @param b1 + * @param b2 + * @param colInfo + * @param normal + * @param forceInelastic + */ + private function resolveContactPoint(idx:int, b1:Body, b2:Body, contact:Contact, normal:Vector3, forceInelastic:Boolean):void { + var cp:ContactPoint = contact.points[idx]; + if (!forceInelastic) cp.satisfied = true; + + var newVel:Number = 0; + calcSepVelocity(b1, b2, cp, _v); + var sepVel:Number = _v.vDot(contact.normal); + if (forceInelastic) { + var minSpeVel:Number = useStaticSeparation ? 0 : cp.minSepVel; + if (sepVel < minSpeVel) cp.satisfied = false; + else if (cp.satisfied) return; + newVel = minSpeVel; + } else { + newVel = cp.normalVel; + } + var deltaVel:Number = newVel - sepVel; + var impulse:Number = deltaVel/cp.velByUnitImpulseN; + var accumImpulse:Number = cp.accumImpulseN + impulse; + if (accumImpulse < 0) accumImpulse = 0; + var deltaImpulse:Number = accumImpulse - cp.accumImpulseN; + cp.accumImpulseN = accumImpulse; + // Применяем импульс к телам + if (b1.movable) b1.applyRelPosWorldImpulse(cp.r1, normal, deltaImpulse); + if (b2 != null && b2.movable) b2.applyRelPosWorldImpulse(cp.r2, normal, -deltaImpulse); + + // Учёт силы трения + calcSepVelocity(b1, b2, cp, _v); + // Расчитываем изменение касательной скорости на единицу касательного импульса + var tanSpeedByUnitImpulse:Number = 0; + _v.vAddScaled(-_v.vDot(contact.normal), contact.normal); + var tanSpeed:Number = _v.vLength(); + if (tanSpeed < 0.001) return; + _t.vCopy(_v).vNormalize().vReverse(); + // dV = b.invMass + ((invI * (r % t)) % r) * t + if (b1.movable) { + _v.vCross2(cp.r1, _t).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1); + tanSpeedByUnitImpulse += b1.invMass + _v.vDot(_t); + } + if (b2 != null && b2.movable) { + _v.vCross2(cp.r2, _t).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2); + tanSpeedByUnitImpulse += b2.invMass + _v.vDot(_t); + } + + var tanImpulse:Number = tanSpeed/tanSpeedByUnitImpulse; + var max:Number = contact.friction*cp.accumImpulseN; + if (max < 0) { + if (tanImpulse < max) tanImpulse = max; + } else { + if (tanImpulse > max) tanImpulse = max; + } + + // Применяем импульс к телам + if (b1.movable) b1.applyRelPosWorldImpulse(cp.r1, _t, tanImpulse); + if (b2 != null && b2.movable) b2.applyRelPosWorldImpulse(cp.r2, _t, -tanImpulse); + } + + /** + * + * @param cp + * @param normal + * @return + */ + private function calcSepVelocity(body1:Body, body2:Body, cp:ContactPoint, result:Vector3):void { + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 + result.vCopy(body1.state.velocity).vAdd(_v1.vCross2(body1.state.rotation, cp.r1)); + // V2 = V2_c + w2%r2 + if (body2 != null) result.vSubtract(body2.state.velocity).vSubtract(_v2.vCross2(body2.state.rotation, cp.r2)); + } + + /** + * + * @param dt + */ + private function intergateVelocities(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + (bodies[i] as Body).integrateVelocity(dt); + } + } + + /** + * + * @param dt + */ + private function integratePositions(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i] as Body; + if (body.movable) body.integratePosition(dt); + } + } + + /** + * + */ + private function performStaticSeparation():void { + var iterNum:int = staticSeparationIterations; +// iterNum = 100; + + // 1. В начале каждой итерации для всех контактов сбрасывается флаг satisfied с одновременным поиском наихудшего контакта, т.е. имеющего наибольшую величину пересечения тел. + // 2. Если найденный контакт имеет величину пересечения меньше предельно допустимой процедура прерывается, т.к. разделение контактов не требуется. + // 3. Если процедура продолжается, то выполняется разделение найденного контакта и установка его флага satisfied в true, после чего запускается внутренний цикл, состоящий + // из contactsNum - 1 итераций. На каждой итерации ищется наихудший контакт среди оставшихся (satisfied == false). Если такой контакт найден, то выполняется его разделение + // и переход к следующей итерации внутреннего цикла, иначе внутренний цикл прерывается и выполняется переход к следующей итерации внешнего цикла (шаг 1.). + for (var iter:int = 0; iter < iterNum; iter++) { + // Ищем контакт с максимальной величиной пересечения тел, одновременно сбрасывая флаги satisfied в false + var worstContact:Contact = contacts[0]; + var i:int; + for (i = 1; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + contact.satisfied = false; + if (contact.maxPenetration > worstContact.maxPenetration) worstContact = contact; + } + if (worstContact.maxPenetration <= allowedPenetration) return; + + resolveInterpenetration(worstContact); + // Внутренний цикл по оставшимся контактам + for (i = 1; i < contactsNum; i++) { + worstContact = getWorstContact(); + if (worstContact == null) break; + resolveInterpenetration(worstContact); + } + } + } + + /** + * + */ + private function getWorstContact():Contact { + var maxPen:Number = 0; + var worst:Contact = null; + for (var i:int = 0; i < contactsNum; i++) { + var c:Contact = contacts[i]; + if (!c.satisfied && c.maxPenetration > maxPen) { + worst = c; + maxPen = c.maxPenetration; + } + } + return maxPen > allowedPenetration ? worst : null; + } + + /** + * Разделяет указанный контакт, выполняя staticSeparationSteps итераций по списку точек контакта. + * 1. В начале каждой итерации ищется точка с наибольшим пересечением, одновременно сбрасываются флаги satisfied у точек. + * 2. Для найденной точки выполняется процедура разделения. + * 3. После выполняется pcount - 1 итерация по списку точек, каждый раз ищется наихудшая среди имеющих satisfied == false, для которой выполняется процедура разделения. + * Если наихудшая точка не найдена, то выполнется переход к следующей итерации внешнего цикла (шаг 1). + * + * В конце процедуры разделения обновляются значения пересечений для остальных точек контакта, а также для всех прочих контактов, относящихся к телам текущего. + * + * @param contact контакт для разделения + */ + private function resolveInterpenetration(contact:Contact):void { + contact.satisfied = true; + + for (var step:int = 0; step < staticSeparationSteps; step++) { + var worstCp:ContactPoint = contact.points[0]; + var cp:ContactPoint; + var i:int; + for (i = 1; i < contact.pcount; i++) { + cp = contact.points[i]; + cp.satisfied = false; + if (cp.penetration > worstCp.penetration) worstCp = cp; + } + if (worstCp.penetration <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + // Разделяем оставшиеся точки + var maxPen:Number = 0; + for (i = 1; i < contact.pcount; i++) { + // Поиск наихудшей точки + for (var j:int = 0; j < contact.pcount; j++) { + cp = contact.points[j]; + if (cp.satisfied) continue; + if (cp.penetration > maxPen) { + maxPen = cp.penetration; + worstCp = cp; + } + } + if (maxPen <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + } + } + } + + /** + * + * @param cp + * @param contact + */ + private function separateContactPoint(cp:ContactPoint, contact:Contact):void { + cp.satisfied = true; + + var b1:Body = contact.body1; + var b2:Body = contact.body2; + var totalMove:Number = cp.penetration - allowedPenetration; + var moveCoeff:Number = totalMove/cp.velByUnitImpulseN; + var linMove1:Number; + var angleMove1:Number; + if (b1.movable) { + linMove1 = b1.invMass*moveCoeff; + angleMove1 = cp.angularInertia1*moveCoeff; + if (angleMove1 > maxAngleMove) { + linMove1 += angleMove1 - maxAngleMove; + angleMove1 -= maxAngleMove; + } + b1.state.pos.vAddScaled(linMove1, contact.normal); + _v1.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vScale(angleMove1); + b1.state.orientation.addScaledVector(_v1, 1); + } + var linMove2:Number; + var angleMove2:Number; + if (b2 != null && b2.movable) { + linMove2 = b2.invMass*moveCoeff; + angleMove2 = cp.angularInertia2*moveCoeff; + if (angleMove2 > maxAngleMove) { + linMove2 += angleMove2 - maxAngleMove; + angleMove2 -= maxAngleMove; + } + b2.state.pos.vAddScaled(-linMove2, contact.normal); + _v2.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vScale(angleMove2); + _v2.vReverse(); + b2.state.orientation.addScaledVector(_v2, 1); + } + cp.penetration = allowedPenetration; + // Обновляем пересечения в других точках + var i:int; + for (i = 0; i < contact.pcount; i++) { + var cp1:ContactPoint = contact.points[i]; + if (cp1 == cp) continue; + var angularMove:Number; + if (b1.movable) { + angularMove = _v.vCross2(_v1, cp1.r1).vDot(contact.normal); + cp1.penetration -= linMove1 + angularMove; + } + if (b2 != null && b2.movable) { + angularMove = _v.vCross2(_v2, cp1.r2).vDot(contact.normal); + cp1.penetration -= linMove2 - angularMove; + } + // Обновление максимального значения для контакта + if (cp1.penetration > contact.maxPenetration) contact.maxPenetration = cp1.penetration; + } + // Обновляем пересечения для других контактов + var c:Contact; + var j:int; + if (b1.movable) { + for (i = 0; i < b1.contactsNum; i++) { + c = b1.contacts[i]; + if (c == contact) continue; + for (j = 0; j < c.pcount; j++) { + cp1 = c.points[j]; + if (b1 == c.body1) cp1.penetration -= linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r1).vDot(c.normal); + else cp1.penetration += linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r2).vDot(c.normal); + if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; + } + } + } + if (b2 != null && b2.movable) { + for (i = 0; i < b2.contactsNum; i++) { + c = b2.contacts[i]; + if (c == contact) continue; + for (j = 0; j < c.pcount; j++) { + cp1 = c.points[j]; + if (b2 == c.body1) cp1.penetration -= linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r1).vDot(c.normal); + else cp1.penetration += linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r2).vDot(c.normal); + if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; + } + } + } + } + + /** + * + */ + private function postPhysics():void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i]; + body.clearAccumulators(); + body.calcDerivedData(); + } + } + + /** + * + * @param dt + */ + public function runPhysics(dt:Number):void { + timeStamp++; + applyForces(dt); + detectCollisions(dt); + preProcessContacts(dt); + processContacts(dt, false); + intergateVelocities(dt); + processContacts(dt, true); + integratePositions(dt); + postPhysics(); + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/rigid/constraints/.svn/all-wcprops b/0.0.1.0/src/alternativa/physics/rigid/constraints/.svn/all-wcprops new file mode 100644 index 0000000..c1da71f --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/rigid/constraints/.svn/all-wcprops @@ -0,0 +1,17 @@ +K 25 +svn:wc:ra_dav:version-url +V 121 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/rigid/constraints +END +Constraint.as +K 25 +svn:wc:ra_dav:version-url +V 135 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/rigid/constraints/Constraint.as +END +MaxDistanceConstraint.as +K 25 +svn:wc:ra_dav:version-url +V 146 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/rigid/constraints/MaxDistanceConstraint.as +END diff --git a/0.0.1.0/src/alternativa/physics/rigid/constraints/.svn/entries b/0.0.1.0/src/alternativa/physics/rigid/constraints/.svn/entries new file mode 100644 index 0000000..5f29f78 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/rigid/constraints/.svn/entries @@ -0,0 +1,52 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/rigid/constraints +http://svndev.alternativaplatform.com + + + +2009-06-08T06:46:18.310732Z +14099 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Constraint.as +file + + + + +2010-10-28T04:32:39.000000Z +757824f9eeb3c89243d104910e0d0529 +2009-04-23T13:43:38.903278Z +11779 +mike + +MaxDistanceConstraint.as +file + + + + +2010-10-28T04:32:39.000000Z +3fdf03f90fcf4b9e1bd8046b789014b4 +2009-06-08T06:46:18.310732Z +14099 +mike + diff --git a/0.0.1.0/src/alternativa/physics/rigid/constraints/.svn/format b/0.0.1.0/src/alternativa/physics/rigid/constraints/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/rigid/constraints/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.1.0/src/alternativa/physics/rigid/constraints/.svn/text-base/Constraint.as.svn-base b/0.0.1.0/src/alternativa/physics/rigid/constraints/.svn/text-base/Constraint.as.svn-base new file mode 100644 index 0000000..c2cd912 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/rigid/constraints/.svn/text-base/Constraint.as.svn-base @@ -0,0 +1,35 @@ +package alternativa.physics.rigid.constraints { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.RigidWorld; + + use namespace altphysics; + + /** + * + */ + public class Constraint { + + altphysics var satisfied:Boolean; + altphysics var world:RigidWorld; + + /** + * + */ + public function Constraint() { + } + + /** + * @param dt + */ + public function preProcess(dt:Number):void { + } + + /** + * @param dt + */ + public function apply(dt:Number):void { + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/rigid/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base b/0.0.1.0/src/alternativa/physics/rigid/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base new file mode 100644 index 0000000..0856660 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/rigid/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base @@ -0,0 +1,118 @@ +package alternativa.physics.rigid.constraints { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.BodyState; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class MaxDistanceConstraint extends Constraint { + // Первое тело ограничения + altphysics var body1:Body; + // Второе тело ограничения. Может быть null. + altphysics var body2:Body; + // Первая точка крепления в системе координат первого тела + altphysics var r1:Vector3; + // Вторая точка привязки в системе координат второго тела. В случае отсутствия второго тела, координаты задаются в мировой системе. + altphysics var r2:Vector3; + // Максимально допустимое расстояние между точками крепления + altphysics var maxDistance:Number; + // Радиус-вектор первой точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr1:Vector3 = new Vector3(); + // Радиус-вектор второй точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr2:Vector3 = new Vector3(); + // Минимальная скорость сближения для удовлетворения ограничения + private var minClosingVel:Number; + // Изменение относительной скорости на единицу импульса + private var velByUnitImpulseN:Number; + // Направление корректирующего импульса + private var impulseDirection:Vector3 = new Vector3(); + + // Вспомогательные переменные + private static var _v:Vector3 = new Vector3(); + private static var _v1:Vector3 = new Vector3(); + private static var _v2:Vector3 = new Vector3(); + + /** + * @param body1 + * @param body2 + * @param r1 + * @param r2 + * @param maxDistance + */ + public function MaxDistanceConstraint(body1:Body, body2:Body, r1:Vector3, r2:Vector3, maxDistance:Number) { + super(); + this.body1 = body1; + this.body2 = body2; + this.r1 = r1.vClone(); + this.r2 = r2.vClone(); + this.maxDistance = maxDistance; + } + + /** + * @param dt + */ + override public function preProcess(dt:Number):void { + // Вычислим расстояние между точками + body1.baseMatrix.transformVector(r1, wr1); + if (body2 != null) body2.baseMatrix.transformVector(r2, wr2); + else wr2.vCopy(r2); + var p1:Vector3 = body1.state.pos; + impulseDirection.x = wr2.x - wr1.x - p1.x; + impulseDirection.y = wr2.y - wr1.y - p1.y; + impulseDirection.z = wr2.z - wr1.z - p1.z; + if (body2 != null) { + var p2:Vector3 = body2.state.pos; + impulseDirection.x += p2.x; + impulseDirection.y += p2.y; + impulseDirection.z += p2.z; + } + + var delta:Number = impulseDirection.vLength() - maxDistance; + if (delta > 0) { + satisfied = false; + impulseDirection.vNormalize(); + minClosingVel = delta/(world.penResolutionSteps*dt); + if (minClosingVel > world.maxPenResolutionSpeed) minClosingVel = world.maxPenResolutionSpeed; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + velByUnitImpulseN = 0; + if (body1.movable) velByUnitImpulseN += body1.invMass + _v.vCross2(wr1, impulseDirection).vTransformBy3(body1.invInertiaWorld).vCross(wr1).vDot(impulseDirection); + if (body2 != null && body2.movable) velByUnitImpulseN += body2.invMass + _v.vCross2(wr2, impulseDirection).vTransformBy3(body2.invInertiaWorld).vCross(wr2).vDot(impulseDirection); + } else satisfied = true; + } + + /** + * @param dt + */ + override public function apply(dt:Number):void { + if (satisfied) return; + // Расчитываем проекцию скорости относительной скорости + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 + var state:BodyState = body1.state; + _v1.vCross2(state.rotation, wr1); + _v.x = state.velocity.x + _v1.x; + _v.y = state.velocity.y + _v1.y; + _v.z = state.velocity.z + _v1.z; + // V2 = V2_c + w2%r2 + if (body2 != null) { + state = body2.state; + _v2.vCross2(state.rotation, wr2); + _v.x -= state.velocity.x - _v2.x; + _v.y -= state.velocity.y - _v2.y; + _v.z -= state.velocity.z - _v2.z; + } + var closingVel:Number = _v.x*impulseDirection.x + _v.y*impulseDirection.y + _v.z*impulseDirection.z; + if (closingVel > minClosingVel) return; + var impulse:Number = (minClosingVel - closingVel)/velByUnitImpulseN; + if (body1.movable) body1.applyRelPosWorldImpulse(wr1, impulseDirection, impulse); + if (body2 != null && body2.movable) body2.applyRelPosWorldImpulse(wr2, impulseDirection, -impulse); + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/rigid/constraints/Constraint.as b/0.0.1.0/src/alternativa/physics/rigid/constraints/Constraint.as new file mode 100644 index 0000000..c2cd912 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/rigid/constraints/Constraint.as @@ -0,0 +1,35 @@ +package alternativa.physics.rigid.constraints { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.RigidWorld; + + use namespace altphysics; + + /** + * + */ + public class Constraint { + + altphysics var satisfied:Boolean; + altphysics var world:RigidWorld; + + /** + * + */ + public function Constraint() { + } + + /** + * @param dt + */ + public function preProcess(dt:Number):void { + } + + /** + * @param dt + */ + public function apply(dt:Number):void { + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/rigid/constraints/MaxDistanceConstraint.as b/0.0.1.0/src/alternativa/physics/rigid/constraints/MaxDistanceConstraint.as new file mode 100644 index 0000000..0856660 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/rigid/constraints/MaxDistanceConstraint.as @@ -0,0 +1,118 @@ +package alternativa.physics.rigid.constraints { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.BodyState; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class MaxDistanceConstraint extends Constraint { + // Первое тело ограничения + altphysics var body1:Body; + // Второе тело ограничения. Может быть null. + altphysics var body2:Body; + // Первая точка крепления в системе координат первого тела + altphysics var r1:Vector3; + // Вторая точка привязки в системе координат второго тела. В случае отсутствия второго тела, координаты задаются в мировой системе. + altphysics var r2:Vector3; + // Максимально допустимое расстояние между точками крепления + altphysics var maxDistance:Number; + // Радиус-вектор первой точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr1:Vector3 = new Vector3(); + // Радиус-вектор второй точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr2:Vector3 = new Vector3(); + // Минимальная скорость сближения для удовлетворения ограничения + private var minClosingVel:Number; + // Изменение относительной скорости на единицу импульса + private var velByUnitImpulseN:Number; + // Направление корректирующего импульса + private var impulseDirection:Vector3 = new Vector3(); + + // Вспомогательные переменные + private static var _v:Vector3 = new Vector3(); + private static var _v1:Vector3 = new Vector3(); + private static var _v2:Vector3 = new Vector3(); + + /** + * @param body1 + * @param body2 + * @param r1 + * @param r2 + * @param maxDistance + */ + public function MaxDistanceConstraint(body1:Body, body2:Body, r1:Vector3, r2:Vector3, maxDistance:Number) { + super(); + this.body1 = body1; + this.body2 = body2; + this.r1 = r1.vClone(); + this.r2 = r2.vClone(); + this.maxDistance = maxDistance; + } + + /** + * @param dt + */ + override public function preProcess(dt:Number):void { + // Вычислим расстояние между точками + body1.baseMatrix.transformVector(r1, wr1); + if (body2 != null) body2.baseMatrix.transformVector(r2, wr2); + else wr2.vCopy(r2); + var p1:Vector3 = body1.state.pos; + impulseDirection.x = wr2.x - wr1.x - p1.x; + impulseDirection.y = wr2.y - wr1.y - p1.y; + impulseDirection.z = wr2.z - wr1.z - p1.z; + if (body2 != null) { + var p2:Vector3 = body2.state.pos; + impulseDirection.x += p2.x; + impulseDirection.y += p2.y; + impulseDirection.z += p2.z; + } + + var delta:Number = impulseDirection.vLength() - maxDistance; + if (delta > 0) { + satisfied = false; + impulseDirection.vNormalize(); + minClosingVel = delta/(world.penResolutionSteps*dt); + if (minClosingVel > world.maxPenResolutionSpeed) minClosingVel = world.maxPenResolutionSpeed; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + velByUnitImpulseN = 0; + if (body1.movable) velByUnitImpulseN += body1.invMass + _v.vCross2(wr1, impulseDirection).vTransformBy3(body1.invInertiaWorld).vCross(wr1).vDot(impulseDirection); + if (body2 != null && body2.movable) velByUnitImpulseN += body2.invMass + _v.vCross2(wr2, impulseDirection).vTransformBy3(body2.invInertiaWorld).vCross(wr2).vDot(impulseDirection); + } else satisfied = true; + } + + /** + * @param dt + */ + override public function apply(dt:Number):void { + if (satisfied) return; + // Расчитываем проекцию скорости относительной скорости + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 + var state:BodyState = body1.state; + _v1.vCross2(state.rotation, wr1); + _v.x = state.velocity.x + _v1.x; + _v.y = state.velocity.y + _v1.y; + _v.z = state.velocity.z + _v1.z; + // V2 = V2_c + w2%r2 + if (body2 != null) { + state = body2.state; + _v2.vCross2(state.rotation, wr2); + _v.x -= state.velocity.x - _v2.x; + _v.y -= state.velocity.y - _v2.y; + _v.z -= state.velocity.z - _v2.z; + } + var closingVel:Number = _v.x*impulseDirection.x + _v.y*impulseDirection.y + _v.z*impulseDirection.z; + if (closingVel > minClosingVel) return; + var impulse:Number = (minClosingVel - closingVel)/velByUnitImpulseN; + if (body1.movable) body1.applyRelPosWorldImpulse(wr1, impulseDirection, impulse); + if (body2 != null && body2.movable) body2.applyRelPosWorldImpulse(wr2, impulseDirection, -impulse); + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/rigid/primitives/.svn/all-wcprops b/0.0.1.0/src/alternativa/physics/rigid/primitives/.svn/all-wcprops new file mode 100644 index 0000000..65ac5e8 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/rigid/primitives/.svn/all-wcprops @@ -0,0 +1,35 @@ +K 25 +svn:wc:ra_dav:version-url +V 120 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/rigid/primitives +END +RigidPlane.as +K 25 +svn:wc:ra_dav:version-url +V 134 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/rigid/primitives/RigidPlane.as +END +RigidSphere.as +K 25 +svn:wc:ra_dav:version-url +V 135 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/rigid/primitives/RigidSphere.as +END +RigidBox.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/rigid/primitives/RigidBox.as +END +RigidCylinder.as +K 25 +svn:wc:ra_dav:version-url +V 137 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/rigid/primitives/RigidCylinder.as +END +RigidRect.as +K 25 +svn:wc:ra_dav:version-url +V 133 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/rigid/primitives/RigidRect.as +END diff --git a/0.0.1.0/src/alternativa/physics/rigid/primitives/.svn/entries b/0.0.1.0/src/alternativa/physics/rigid/primitives/.svn/entries new file mode 100644 index 0000000..46d2009 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/rigid/primitives/.svn/entries @@ -0,0 +1,88 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/rigid/primitives +http://svndev.alternativaplatform.com + + + +2009-06-08T06:46:18.310732Z +14099 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +RigidPlane.as +file + + + + +2010-10-28T04:32:39.000000Z +c4713421e357ee1967dca9ab3ffa7290 +2009-06-08T06:46:18.310732Z +14099 +mike + +RigidSphere.as +file + + + + +2010-10-28T04:32:39.000000Z +79ef38eab05657ad9627bdac4ade7271 +2009-04-23T13:43:38.903278Z +11779 +mike + +RigidBox.as +file + + + + +2010-10-28T04:32:39.000000Z +da834a5d208ff30f3a74321f75301cd7 +2009-06-08T06:46:18.310732Z +14099 +mike + +RigidCylinder.as +file + + + + +2010-10-28T04:32:39.000000Z +61512d31f9e211dcb0864164aa6f5210 +2009-04-23T13:43:38.903278Z +11779 +mike + +RigidRect.as +file + + + + +2010-10-28T04:32:39.000000Z +4cff4e313a1f41d4ccfa6480ea78173e +2009-04-23T13:43:38.903278Z +11779 +mike + diff --git a/0.0.1.0/src/alternativa/physics/rigid/primitives/.svn/format b/0.0.1.0/src/alternativa/physics/rigid/primitives/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/rigid/primitives/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.1.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidBox.as.svn-base b/0.0.1.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidBox.as.svn-base new file mode 100644 index 0000000..c01ff61 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidBox.as.svn-base @@ -0,0 +1,56 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class RigidBox extends Body { + + /** + * + * @param halfSize + * @param mass + */ + public function RigidBox(halfSize:Vector3, mass:Number) { + super(0, Matrix3.ZERO); + addCollisionPrimitive(new CollisionBox(halfSize, 1)); + setParams(halfSize, mass); + } + + /** + * @param halfSize + * @param mass + */ + public function setParams(halfSize:Vector3, mass:Number):void { + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + invInertia.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + invInertia.a = 3*invMass/(yy + zz); + invInertia.f = 3*invMass/(zz + xx); + invInertia.k = 3*invMass/(xx + yy); + } + var prim:CollisionBox = collisionPrimitives[0] as CollisionBox; + prim.hs.vCopy(halfSize); + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidCylinder.as.svn-base b/0.0.1.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidCylinder.as.svn-base new file mode 100644 index 0000000..0f9f2b3 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidCylinder.as.svn-base @@ -0,0 +1,32 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + + use namespace altphysics; + + public class RigidCylinder extends Body { + + altphysics var r:Number; + altphysics var h:Number; + + public function RigidCylinder(mass:Number, radius:Number, height:Number) { + super(0, Matrix3.ZERO); + setParams(mass, radius, height); + } + + public function setParams(mass:Number, radius:Number, height:Number):void { + r = radius; + h = height; + invInertiaWorld.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + invInertia.a = invInertia.f = mass*(h*h/12 + r*r/4); + invInertia.k = 0.5*mass*r*r; + invInertia.invert(); + } + } + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidPlane.as.svn-base b/0.0.1.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidPlane.as.svn-base new file mode 100644 index 0000000..26f74b2 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidPlane.as.svn-base @@ -0,0 +1,23 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + public class RigidPlane extends Body { + + public var normal:Vector3 = new Vector3(); + public var offset:Number; + + public function RigidPlane() { + super(0, Matrix3.ZERO); + this.normal.vCopy(Vector3.Z_AXIS); + this.offset = 0; + movable = false; + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidRect.as.svn-base b/0.0.1.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidRect.as.svn-base new file mode 100644 index 0000000..3c3913d --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidRect.as.svn-base @@ -0,0 +1,20 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + public class RigidRect extends Body { + + public function RigidRect(halfWidth:Number, halfLength:Number) { + super(0, Matrix3.ZERO); + movable = false; + addCollisionPrimitive(new CollisionRect(new Vector3(halfWidth, halfLength, 0), 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidSphere.as.svn-base b/0.0.1.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidSphere.as.svn-base new file mode 100644 index 0000000..9668716 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidSphere.as.svn-base @@ -0,0 +1,29 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + + use namespace altphysics; + + public class RigidSphere extends Body { + + public function RigidSphere(radius:Number, mass:Number) { + var m:Matrix3; + if (mass == Infinity) { + mass = 0; + m = Matrix3.ZERO; + } else { + mass = 1/mass; + var r2:Number = radius*radius; + m = new Matrix3(); + m.a = m.f = m.k = 2.5*mass/r2; + } + super(mass, m); + if (mass == 0) movable = false; + addCollisionPrimitive(new CollisionSphere(radius, 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/rigid/primitives/RigidBox.as b/0.0.1.0/src/alternativa/physics/rigid/primitives/RigidBox.as new file mode 100644 index 0000000..c01ff61 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/rigid/primitives/RigidBox.as @@ -0,0 +1,56 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class RigidBox extends Body { + + /** + * + * @param halfSize + * @param mass + */ + public function RigidBox(halfSize:Vector3, mass:Number) { + super(0, Matrix3.ZERO); + addCollisionPrimitive(new CollisionBox(halfSize, 1)); + setParams(halfSize, mass); + } + + /** + * @param halfSize + * @param mass + */ + public function setParams(halfSize:Vector3, mass:Number):void { + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + invInertia.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + invInertia.a = 3*invMass/(yy + zz); + invInertia.f = 3*invMass/(zz + xx); + invInertia.k = 3*invMass/(xx + yy); + } + var prim:CollisionBox = collisionPrimitives[0] as CollisionBox; + prim.hs.vCopy(halfSize); + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/rigid/primitives/RigidCylinder.as b/0.0.1.0/src/alternativa/physics/rigid/primitives/RigidCylinder.as new file mode 100644 index 0000000..0f9f2b3 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/rigid/primitives/RigidCylinder.as @@ -0,0 +1,32 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + + use namespace altphysics; + + public class RigidCylinder extends Body { + + altphysics var r:Number; + altphysics var h:Number; + + public function RigidCylinder(mass:Number, radius:Number, height:Number) { + super(0, Matrix3.ZERO); + setParams(mass, radius, height); + } + + public function setParams(mass:Number, radius:Number, height:Number):void { + r = radius; + h = height; + invInertiaWorld.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + invInertia.a = invInertia.f = mass*(h*h/12 + r*r/4); + invInertia.k = 0.5*mass*r*r; + invInertia.invert(); + } + } + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/rigid/primitives/RigidPlane.as b/0.0.1.0/src/alternativa/physics/rigid/primitives/RigidPlane.as new file mode 100644 index 0000000..26f74b2 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/rigid/primitives/RigidPlane.as @@ -0,0 +1,23 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + public class RigidPlane extends Body { + + public var normal:Vector3 = new Vector3(); + public var offset:Number; + + public function RigidPlane() { + super(0, Matrix3.ZERO); + this.normal.vCopy(Vector3.Z_AXIS); + this.offset = 0; + movable = false; + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/rigid/primitives/RigidRect.as b/0.0.1.0/src/alternativa/physics/rigid/primitives/RigidRect.as new file mode 100644 index 0000000..3c3913d --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/rigid/primitives/RigidRect.as @@ -0,0 +1,20 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + public class RigidRect extends Body { + + public function RigidRect(halfWidth:Number, halfLength:Number) { + super(0, Matrix3.ZERO); + movable = false; + addCollisionPrimitive(new CollisionRect(new Vector3(halfWidth, halfLength, 0), 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/rigid/primitives/RigidSphere.as b/0.0.1.0/src/alternativa/physics/rigid/primitives/RigidSphere.as new file mode 100644 index 0000000..9668716 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/rigid/primitives/RigidSphere.as @@ -0,0 +1,29 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + + use namespace altphysics; + + public class RigidSphere extends Body { + + public function RigidSphere(radius:Number, mass:Number) { + var m:Matrix3; + if (mass == Infinity) { + mass = 0; + m = Matrix3.ZERO; + } else { + mass = 1/mass; + var r2:Number = radius*radius; + m = new Matrix3(); + m.a = m.f = m.k = 2.5*mass/r2; + } + super(mass, m); + if (mass == 0) movable = false; + addCollisionPrimitive(new CollisionSphere(radius, 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/types/.svn/all-wcprops b/0.0.1.0/src/alternativa/physics/types/.svn/all-wcprops new file mode 100644 index 0000000..a1c96cb --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/types/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 109 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/types +END +Quaternion.as +K 25 +svn:wc:ra_dav:version-url +V 123 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/types/Quaternion.as +END +Vector3.as +K 25 +svn:wc:ra_dav:version-url +V 120 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/types/Vector3.as +END +Matrix3.as +K 25 +svn:wc:ra_dav:version-url +V 120 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/types/Matrix3.as +END +Matrix4.as +K 25 +svn:wc:ra_dav:version-url +V 120 +/!svn/ver/14887/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/types/Matrix4.as +END diff --git a/0.0.1.0/src/alternativa/physics/types/.svn/entries b/0.0.1.0/src/alternativa/physics/types/.svn/entries new file mode 100644 index 0000000..13dbfab --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/types/.svn/entries @@ -0,0 +1,76 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.1.0/src/alternativa/physics/types +http://svndev.alternativaplatform.com + + + +2009-06-08T06:46:18.310732Z +14099 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Quaternion.as +file + + + + +2010-10-28T04:32:39.000000Z +595f36fdfea8e82e752bb989e97e3a9a +2009-05-04T04:55:56.466789Z +12306 +mike + +Vector3.as +file + + + + +2010-10-28T04:32:39.000000Z +c324eee86307bbf468a629c56a46bce4 +2009-06-08T06:46:18.310732Z +14099 +mike + +Matrix3.as +file + + + + +2010-10-28T04:32:39.000000Z +2e95a69258123653d92296f40390c112 +2009-05-26T08:55:28.854881Z +13390 +mike + +Matrix4.as +file + + + + +2010-10-28T04:32:39.000000Z +b2ed6277a2052c2568a1b4f19c469223 +2009-05-04T04:55:56.466789Z +12306 +mike + diff --git a/0.0.1.0/src/alternativa/physics/types/.svn/format b/0.0.1.0/src/alternativa/physics/types/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/types/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.1.0/src/alternativa/physics/types/.svn/text-base/Matrix3.as.svn-base b/0.0.1.0/src/alternativa/physics/types/.svn/text-base/Matrix3.as.svn-base new file mode 100644 index 0000000..86885d7 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/types/.svn/text-base/Matrix3.as.svn-base @@ -0,0 +1,367 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + /** + * + */ + public class Matrix3 { + + public static const ZERO:Matrix3 = new Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0); + public static const IDENTITY:Matrix3 = new Matrix3(); + + public var a:Number; + public var b:Number; + public var c:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix3(a:Number = 1, b:Number = 0, c:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1) { + this.a = a; + this.b = b; + this.c = c; + + this.e = e; + this.f = f; + this.g = g; + + this.i = i; + this.j = j; + this.k = k; + } + + /** + * + * @return + */ + public function toIdentity():Matrix3 { + a = f = k = 1; + b = c = e = g = i = j = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + var det:Number = 1/(-cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk); + + a = (ff*kk - gg*jj)*det; + b = (cc*jj - bb*kk)*det; + c = (bb*gg - cc*ff)*det; + e = (gg*ii - ee*kk)*det; + f = (aa*kk - cc*ii)*det; + g = (cc*ee - aa*gg)*det; + i = (ee*jj - ff*ii)*det; + j = (bb*ii - aa*jj)*det; + k = (aa*ff - bb*ee)*det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix3):Matrix3 { + a += m.a; + b += m.b; + c += m.c; + e += m.e; + f += m.f; + g += m.g; + i += m.i; + j += m.j; + k += m.k; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix3):Matrix3 { + a -= m.a; + b -= m.b; + c -= m.c; + e -= m.e; + f -= m.f; + g -= m.g; + i -= m.i; + j -= m.j; + k -= m.k; + + return this; + } + + /** + * + * @return + */ + public function transpose():Matrix3 { + var tmp:Number = b; + b = e; + e = tmp; + tmp = c; + c = i; + i = tmp; + tmp = g; + g = j; + j = tmp; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector3To3D(vin:Vector3, vout:Vector3D):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * + * @param matrix + */ + public function createSkewSymmetric(v:Vector3):Matrix3 { + a = f = k = 0; + b = -v.z; + c = v.y; + e = v.z; + g = -v.x; + i = -v.y; + j = v.x; + return this; + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(i:int, axis:Vector3):void { + switch (i) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + } + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix3):Matrix3 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @return + */ + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):Matrix3 { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + + return this; + } + + /** + * Метод формирует матрицу поворота на заданный угол относительно заданной оси. + * + * @param axis нормализованный вектор, задающий ось, относительно которой выполняется поворот + * @param angle угол поворота в радианах + */ + public function fromAxisAngle(axis:Vector3, angle:Number):void { + var c1:Number = Math.cos(angle); + var s:Number = Math.sin(angle); + var t:Number = 1 - c1; + var x:Number = axis.x; + var y:Number = axis.y; + var z:Number = axis.z; + + a = t*x*x + c1; + b = t*x*y - z*s; + c = t*x*z + y*s; + + e = t*x*y + z*s; + f = t*y*y + c1; + g = t*y*z - x*s; + + i = t*x*z - y*s; + j = t*y*z + x*s; + k = t*z*z + c1; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix3 { + return new Matrix3(a, b, c, e, f, g, i, j, k); + } + + public function toString():String { + return "[Matrix3 (" + a + ", " + b + ", " + c + "), (" + e + ", " + f + ", " + g + "), (" + i + ", " + j + ", " + k + ")]"; + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/types/.svn/text-base/Matrix4.as.svn-base b/0.0.1.0/src/alternativa/physics/types/.svn/text-base/Matrix4.as.svn-base new file mode 100644 index 0000000..8cc23b2 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/types/.svn/text-base/Matrix4.as.svn-base @@ -0,0 +1,402 @@ +package alternativa.physics.types { + import __AS3__.vec.Vector; + + /** + * + */ + public class Matrix4 { + + public var a:Number; + public var b:Number; + public var c:Number; + public var d:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + public var h:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + public var l:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix4(a:Number = 1, b:Number = 0, c:Number = 0, d:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, h:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1, l:Number = 0) { + this.a = a; + this.b = b; + this.c = c; + this.d = h; + + this.e = e; + this.f = f; + this.g = g; + this.h = h; + + this.i = i; + this.j = j; + this.k = k; + this.l = l; + } + + /** + * + * @return + */ + public function toIdentity():Matrix4 { + a = f = k = 1; + b = c = e = g = i = j = d = h = l = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + var det:Number = -cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk; + + a = (-gg*jj + ff*kk)/det; + b = (cc*jj - bb*kk)/det; + c = (-cc*ff + bb*gg)/det; + d = (dd*gg*jj - cc*hh*jj - dd*ff*kk + bb*hh*kk + cc*ff*ll - bb*gg*ll)/det; + e = (gg*ii - ee*kk)/det; + f = (-cc*ii + aa*kk)/det; + g = (cc*ee - aa*gg)/det; + h = (cc*hh*ii - dd*gg*ii + dd*ee*kk - aa*hh*kk - cc*ee*ll + aa*gg*ll)/det; + i = (-ff*ii + ee*jj)/det; + j = (bb*ii - aa*jj)/det; + k = (-bb*ee + aa*ff)/det; + l = (dd*ff*ii - bb*hh*ii - dd*ee*jj + aa*hh*jj + bb*ee*ll - aa*ff*ll)/det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + d = m.a*dd + m.b*hh + m.c*ll + m.d; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + h = m.e*dd + m.f*hh + m.g*ll + m.h; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + l = m.i*dd + m.j*hh + m.k*ll + m.l; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + d = aa*m.d + bb*m.h + cc*m.l + dd; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + h = ee*m.d + ff*m.h + gg*m.l + hh; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + l = ii*m.d + jj*m.h + kk*m.l + ll; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix4):Matrix4 { + a += m.a; + b += m.b; + c += m.c; + d += m.d; + e += m.e; + f += m.f; + g += m.g; + h += m.h; + i += m.i; + j += m.j; + k += m.k; + l += m.l; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix4):Matrix4 { + a -= m.a; + b -= m.b; + c -= m.c; + d -= m.d; + e -= m.e; + f -= m.f; + g -= m.g; + h -= m.h; + i -= m.i; + j -= m.j; + k -= m.k; + l -= m.l; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectors(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverse(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(idx:int, axis:Vector3):void { + switch (idx) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + case 3: + axis.x = d; + axis.y = h; + axis.z = l; + return; + } + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix4):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = m.d; + e = m.e; + f = m.f; + g = m.g; + h = m.h; + i = m.i; + j = m.j; + k = m.k; + l = m.l; + + return this; + } + + public function setFromMatrix3(m:Matrix3, offset:Vector3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = offset.x; + e = m.e; + f = m.f; + g = m.g; + h = offset.y; + i = m.i; + j = m.j; + k = m.k; + l = offset.z; + + return this; + } + + /** + * @param rotations + */ + public function getEulerAngles(rotations:Vector3):void { + if (-1 < i && i < 1) { + rotations.x = Math.atan2(j, k); + rotations.y = -Math.asin(i); + rotations.z = Math.atan2(e, a); + } else { + rotations.x = 0; + rotations.y = 0.5*((i <= -1) ? Math.PI : -Math.PI); + rotations.z = Math.atan2(-b, f); + } + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix4 { + return new Matrix4(a, b, c, d, e, f, g, h, i, j, k, l); + } + + public function toString():String { + return "[Matrix4 [" + a.toFixed(3) + " " + b.toFixed(3) + " " + c.toFixed(3) + " " + d.toFixed(3) + "] [" + e.toFixed(3) + " " + f.toFixed(3) + " " + g.toFixed(3) + " " + h.toFixed(3) + "] [" + i.toFixed(3) + " " + j.toFixed(3) + " " + k.toFixed(3) + " " + l.toFixed(3) + "]]"; + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/types/.svn/text-base/Quaternion.as.svn-base b/0.0.1.0/src/alternativa/physics/types/.svn/text-base/Quaternion.as.svn-base new file mode 100644 index 0000000..7ad2af0 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/types/.svn/text-base/Quaternion.as.svn-base @@ -0,0 +1,396 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + public class Quaternion { + + public var w:Number; + public var x:Number; + public var y:Number; + public var z:Number; + + public static function multiply(q1:Quaternion, q2:Quaternion, result:Quaternion):void { + result.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z; + result.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y; + result.y = q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z; + result.z = q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x; + } + + public static function createFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngle(axis, angle); + return q; + } + + public static function createFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngleComponents(x, y, z, angle); + return q; + } + + public function Quaternion(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0) { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + } + + public function reset(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0):Quaternion { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + return this; + } + + public function normalize():Quaternion { + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * Умножает на указанный кватернион слева: this * q + * + * @param q множитель + */ + public function prepend(q:Quaternion):Quaternion { + var ww:Number = w*q.w - x*q.x - y*q.y - z*q.z; + var xx:Number = w*q.x + x*q.w + y*q.z - z*q.y; + var yy:Number = w*q.y + y*q.w + z*q.x - x*q.z; + var zz:Number = w*q.z + z*q.w + x*q.y - y*q.x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Умножает на указанный кватернион справа: q * this + * + * @param q множитель + */ + public function append(q:Quaternion):Quaternion { + var ww:Number = q.w*w - q.x*x - q.y*y - q.z*z; + var xx:Number = q.w*x + q.x*w + q.y*z - q.z*y; + var yy:Number = q.w*y + q.y*w + q.z*x - q.x*z; + var zz:Number = q.w*z + q.z*w + q.x*y - q.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * + * @param vector + */ + public function rotateByVector(v:Vector3):Quaternion { + var ww:Number = -v.x*x - v.y*y - v.z*z; + var xx:Number = v.x*w + v.y*z - v.z*y; + var yy:Number = v.y*w + v.z*x - v.x*z; + var zz:Number = v.z*w + v.x*y - v.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Добавляет вращение, приданное вектором угловой скорости за указанное время. + * + * @param v + * @param scale + */ + public function addScaledVector(v:Vector3, scale:Number):Quaternion { + var vx:Number = v.x*scale; + var vy:Number = v.y*scale; + var vz:Number = v.z*scale; + var ww:Number = -x*vx - y*vy - z*vz; + var xx:Number = vx*w + vy*z - vz*y; + var yy:Number = vy*w + vz*x - vx*z; + var zz:Number = vz*w + vx*y - vy*x; + w += 0.5*ww; + x += 0.5*xx; + y += 0.5*yy; + z += 0.5*zz; + // inlined normalize + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param m + * @return + */ + public function toMatrix3(m:Matrix3):Quaternion { + var xx2:Number = 2*x*x; + var yy2:Number = 2*y*y; + var zz2:Number = 2*z*z; + var xy2:Number = 2*x*y; + var yz2:Number = 2*y*z; + var zx2:Number = 2*z*x; + var wx2:Number = 2*w*x; + var wy2:Number = 2*w*y; + var wz2:Number = 2*w*z; + + m.a = 1 - yy2 - zz2; + m.b = xy2 - wz2; + m.c = zx2 + wy2; + + m.e = xy2 + wz2; + m.f = 1 - xx2 - zz2; + m.g = yz2 - wx2; + + m.i = zx2 - wy2; + m.j = yz2 + wx2; + m.k = 1 - xx2 - yy2; + return this; + } + + /** + * + * @return + */ + public function length():Number { + return Math.sqrt(w*w + x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function lengthSqr():Number { + return w*w + x*x + y*y + z*z; + } + + /** + * + * @param axis + * @param angle + * @return + */ + public function setFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z); + x = axis.x*k; + y = axis.y*k; + z = axis.z*k; + return this; + } + + /** + * + * @param x + * @param y + * @param z + * @param angle + */ + public function setFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(x*x + y*y + z*z); + this.x = x*k; + this.y = y*k; + this.z = z*k; + return this; + } + + /** + * + * @param vector + */ + public function toAxisVector(v:Vector3 = null):Vector3 { + if (w < -1 || w > 1) { + normalize(); + } + if (v == null) { + v = new Vector3(); + } + if (w > -1 && w < 1) { + if (w == 0) { + v.x = x; + v.y = y; + v.z = z; + } else { + var angle:Number = 2*Math.acos(w); + var coeff:Number = 1/Math.sqrt(1 - w*w); + v.x = x*coeff*angle; + v.y = y*coeff*angle; + v.z = z*coeff*angle; + } + } else { + v.x = 0; + v.y = 0; + v.z = 0; + } + return v; + } + + /** + * + * @param rotations + */ + public function getEulerAngles(angles:Vector3):Vector3 { + var qi2:Number = 2*x*x; + var qj2:Number = 2*y*y; + var qk2:Number = 2*z*z; + var qij:Number = 2*x*y; + var qjk:Number = 2*y*z; + var qki:Number = 2*z*x; + var qri:Number = 2*w*x; + var qrj:Number = 2*w*y; + var qrk:Number = 2*w*z; + + var aa:Number = 1 - qj2 - qk2; + var bb:Number = qij - qrk; + var ee:Number = qij + qrk; + var ff:Number = 1 - qi2 - qk2; + var ii:Number = qki - qrj; + var jj:Number = qjk + qri; + var kk:Number = 1 - qi2 - qj2; + + if (-1 < ii && ii < 1) { + if (angles == null) { + angles = new Vector3(Math.atan2(jj, kk), -Math.asin(ii), Math.atan2(ee, aa)); + } else { + angles.x = Math.atan2(jj, kk); + angles.y = -Math.asin(ii); + angles.z = Math.atan2(ee, aa); + } + } else { + if (angles == null) { + angles = new Vector3(0, 0.5*((ii <= -1) ? Math.PI : -Math.PI), Math.atan2(-bb, ff)); + } else { + angles.x = 0; + angles.y = 0.5*((ii <= -1) ? Math.PI : -Math.PI); + angles.z = Math.atan2(-bb, ff); + } + } + return angles; + } + + /** + * + */ + public function conjugate():void { + x = -x; + y = -y; + z = -z; + } + + /** + * Выполняет линейную интерполяцию. + * + * @param q1 начало отрезка + * @param q2 конец отрезка + * @param t время, обычно задаётся в интервале [0, 1] + */ + public function nlerp(q1:Quaternion, q2:Quaternion, t:Number):Quaternion { + var d:Number = 1 - t; + w = q1.w*d + q2.w*t; + x = q1.x*d + q2.x*t; + y = q1.y*d + q2.y*t; + z = q1.z*d + q2.z*t; + // inlined normalize + d = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param q + * @return + */ + public function subtract(q:Quaternion):Quaternion { + w -= q.w; + x -= q.x; + y -= q.y; + z -= q.z; + return this; + } + + /** + * + * @param q1 + * @param q2 + * @return + */ + public function diff(q1:Quaternion, q2:Quaternion):Quaternion { + w = q2.w - q1.w; + x = q2.x - q1.x; + y = q2.y - q1.y; + z = q2.z - q1.z; + return this; + } + + /** + * + * @param q + * @return + */ + public function copy(q:Quaternion):Quaternion { + w = q.w; + x = q.x; + y = q.y; + z = q.z; + return this; + } + + /** + * @param reslult + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + result.w = w; + return result; + } + + /** + * + * @return + */ + public function clone():Quaternion { + return new Quaternion(w, x, y, z); + } + + /** + * + * @return + */ + public function toString():String { + return "[" + w + ", " + x + ", " + y + ", " + z + "]"; + } + } +} diff --git a/0.0.1.0/src/alternativa/physics/types/.svn/text-base/Vector3.as.svn-base b/0.0.1.0/src/alternativa/physics/types/.svn/text-base/Vector3.as.svn-base new file mode 100644 index 0000000..298ddad --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/types/.svn/text-base/Vector3.as.svn-base @@ -0,0 +1,331 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + /** + * + */ + public class Vector3 extends Vector3D { + + public static const ZERO:Vector3 = new Vector3(0, 0, 0); + public static const X_AXIS:Vector3 = new Vector3(1, 0, 0); + public static const Y_AXIS:Vector3 = new Vector3(0, 1, 0); + public static const Z_AXIS:Vector3 = new Vector3(0, 0, 1); + + /** + * + * @param x + * @param y + * @param z + */ + public function Vector3(x:Number = 0, y:Number = 0, z:Number = 0) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * + * @return + */ + public function vLength():Number { + return Math.sqrt(x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function vLengthSqr():Number { + return x*x + y*y + z*z; + } + + /** + * + * @param length + * @return + */ + public function vSetLength(length:Number):Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = length; + } else { + var k:Number = length/Math.sqrt(x*x + y*y + z*z); + x *= k; + y *= k; + z *= k; + } + return this; + } + + /** + * + * @return + */ + public function vNormalize():Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = 1; + } else { + var k:Number = Math.sqrt(d); + x /= k; + y /= k; + z /= k; + } + return this; + } + + /** + * Сложение координат. + * + * @param v точка, координаты которой прибавляются к собственным + */ + public function vAdd(v:Vector3):Vector3 { + x += v.x; + y += v.y; + z += v.z; + return this; + } + + /** + * + * @param k + * @param v + * @return + */ + public function vAddScaled(k:Number, v:Vector3):Vector3 { + x += k*v.x; + y += k*v.y; + z += k*v.z; + return this; + } + + /** + * Вычитание координат. + * + * @param v точка, координаты которой вычитаются из собственных + */ + public function vSubtract(v:Vector3):Vector3 { + x -= v.x; + y -= v.y; + z -= v.z; + return this; + } + + /** + * Вычисляет сумму векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + * @return this + */ + public function vSum(a:Vector3, b:Vector3):Vector3 { + x = a.x + b.x; + y = a.y + b.y; + z = a.z + b.z; + return this; + } + + /** + * Вычисление разности векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + */ + public function vDiff(a:Vector3, b:Vector3):Vector3 { + x = a.x - b.x; + y = a.y - b.y; + z = a.z - b.z; + return this; + } + + /** + * Умножение на скаляр. + * + * @param k число, на которое умножаются координаты + */ + public function vScale(k:Number):Vector3 { + x *= k; + y *= k; + z *= k; + return this; + } + + /** + * Инвертирование вектора. + */ + public function vReverse():Vector3 { + x = -x; + y = -y; + z = -z; + return this; + } + + /** + * + * @param v + * @return + */ + public function vDot(v:Vector3):Number { + return x*v.x + y*v.y + z*v.z; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross(v:Vector3):Vector3 { + var xx:Number = y*v.z - z*v.y; + var yy:Number = z*v.x - x*v.z; + var zz:Number = x*v.y - y*v.x; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross2(a:Vector3, b:Vector3):Vector3 { + x = a.y*b.z - a.z*b.y; + y = a.z*b.x - a.x*b.z; + z = a.x*b.y - a.y*b.x; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy3(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * + * @param matrix + */ + public function vTransformBy3Tr(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz + m.d; + y = m.e*xx + m.f*yy + m.g*zz + m.h; + z = m.i*xx + m.j*yy + m.k*zz + m.l; + return this; + } + + /** + * + * @param m + * @return + */ + public function vTransformInverseBy4(m:Matrix4):Vector3 { + var xx:Number = x - m.d; + var yy:Number = y - m.h; + var zz:Number = z - m.l; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * + * @param m + */ + public function vDeltaTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * Установка координат. + */ + public function vReset(x:Number = 0, y:Number = 0, z:Number = 0):Vector3 { + this.x = x; + this.y = y; + this.z = z; + return this; + } + + /** + * Копирование координат точки. + * + * @param v точка, координаты которой копируются + */ + public function vCopy(v:Vector3):Vector3 { + x = v.x; + y = v.y; + z = v.z; + return this; + } + + /** + * Клонирование точки. + * + * @return клонированная точка + */ + public function vClone():Vector3 { + return new Vector3(x, y, z); + } + + /** + * Копирует координаты вектора в экземпляр класса Vector3D. + * + * @param result экземпляр, в который копируются координаты + * @return переданный параметр + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + return result; + } + + /** + * @param source + * @return + */ + public function copyFromVector3D(source:Vector3D):Vector3 { + x = source.x; + y = source.y; + z = source.z; + return this; + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/types/Matrix3.as b/0.0.1.0/src/alternativa/physics/types/Matrix3.as new file mode 100644 index 0000000..86885d7 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/types/Matrix3.as @@ -0,0 +1,367 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + /** + * + */ + public class Matrix3 { + + public static const ZERO:Matrix3 = new Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0); + public static const IDENTITY:Matrix3 = new Matrix3(); + + public var a:Number; + public var b:Number; + public var c:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix3(a:Number = 1, b:Number = 0, c:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1) { + this.a = a; + this.b = b; + this.c = c; + + this.e = e; + this.f = f; + this.g = g; + + this.i = i; + this.j = j; + this.k = k; + } + + /** + * + * @return + */ + public function toIdentity():Matrix3 { + a = f = k = 1; + b = c = e = g = i = j = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + var det:Number = 1/(-cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk); + + a = (ff*kk - gg*jj)*det; + b = (cc*jj - bb*kk)*det; + c = (bb*gg - cc*ff)*det; + e = (gg*ii - ee*kk)*det; + f = (aa*kk - cc*ii)*det; + g = (cc*ee - aa*gg)*det; + i = (ee*jj - ff*ii)*det; + j = (bb*ii - aa*jj)*det; + k = (aa*ff - bb*ee)*det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix3):Matrix3 { + a += m.a; + b += m.b; + c += m.c; + e += m.e; + f += m.f; + g += m.g; + i += m.i; + j += m.j; + k += m.k; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix3):Matrix3 { + a -= m.a; + b -= m.b; + c -= m.c; + e -= m.e; + f -= m.f; + g -= m.g; + i -= m.i; + j -= m.j; + k -= m.k; + + return this; + } + + /** + * + * @return + */ + public function transpose():Matrix3 { + var tmp:Number = b; + b = e; + e = tmp; + tmp = c; + c = i; + i = tmp; + tmp = g; + g = j; + j = tmp; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector3To3D(vin:Vector3, vout:Vector3D):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * + * @param matrix + */ + public function createSkewSymmetric(v:Vector3):Matrix3 { + a = f = k = 0; + b = -v.z; + c = v.y; + e = v.z; + g = -v.x; + i = -v.y; + j = v.x; + return this; + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(i:int, axis:Vector3):void { + switch (i) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + } + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix3):Matrix3 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @return + */ + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):Matrix3 { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + + return this; + } + + /** + * Метод формирует матрицу поворота на заданный угол относительно заданной оси. + * + * @param axis нормализованный вектор, задающий ось, относительно которой выполняется поворот + * @param angle угол поворота в радианах + */ + public function fromAxisAngle(axis:Vector3, angle:Number):void { + var c1:Number = Math.cos(angle); + var s:Number = Math.sin(angle); + var t:Number = 1 - c1; + var x:Number = axis.x; + var y:Number = axis.y; + var z:Number = axis.z; + + a = t*x*x + c1; + b = t*x*y - z*s; + c = t*x*z + y*s; + + e = t*x*y + z*s; + f = t*y*y + c1; + g = t*y*z - x*s; + + i = t*x*z - y*s; + j = t*y*z + x*s; + k = t*z*z + c1; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix3 { + return new Matrix3(a, b, c, e, f, g, i, j, k); + } + + public function toString():String { + return "[Matrix3 (" + a + ", " + b + ", " + c + "), (" + e + ", " + f + ", " + g + "), (" + i + ", " + j + ", " + k + ")]"; + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/types/Matrix4.as b/0.0.1.0/src/alternativa/physics/types/Matrix4.as new file mode 100644 index 0000000..8cc23b2 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/types/Matrix4.as @@ -0,0 +1,402 @@ +package alternativa.physics.types { + import __AS3__.vec.Vector; + + /** + * + */ + public class Matrix4 { + + public var a:Number; + public var b:Number; + public var c:Number; + public var d:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + public var h:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + public var l:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix4(a:Number = 1, b:Number = 0, c:Number = 0, d:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, h:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1, l:Number = 0) { + this.a = a; + this.b = b; + this.c = c; + this.d = h; + + this.e = e; + this.f = f; + this.g = g; + this.h = h; + + this.i = i; + this.j = j; + this.k = k; + this.l = l; + } + + /** + * + * @return + */ + public function toIdentity():Matrix4 { + a = f = k = 1; + b = c = e = g = i = j = d = h = l = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + var det:Number = -cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk; + + a = (-gg*jj + ff*kk)/det; + b = (cc*jj - bb*kk)/det; + c = (-cc*ff + bb*gg)/det; + d = (dd*gg*jj - cc*hh*jj - dd*ff*kk + bb*hh*kk + cc*ff*ll - bb*gg*ll)/det; + e = (gg*ii - ee*kk)/det; + f = (-cc*ii + aa*kk)/det; + g = (cc*ee - aa*gg)/det; + h = (cc*hh*ii - dd*gg*ii + dd*ee*kk - aa*hh*kk - cc*ee*ll + aa*gg*ll)/det; + i = (-ff*ii + ee*jj)/det; + j = (bb*ii - aa*jj)/det; + k = (-bb*ee + aa*ff)/det; + l = (dd*ff*ii - bb*hh*ii - dd*ee*jj + aa*hh*jj + bb*ee*ll - aa*ff*ll)/det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + d = m.a*dd + m.b*hh + m.c*ll + m.d; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + h = m.e*dd + m.f*hh + m.g*ll + m.h; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + l = m.i*dd + m.j*hh + m.k*ll + m.l; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + d = aa*m.d + bb*m.h + cc*m.l + dd; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + h = ee*m.d + ff*m.h + gg*m.l + hh; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + l = ii*m.d + jj*m.h + kk*m.l + ll; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix4):Matrix4 { + a += m.a; + b += m.b; + c += m.c; + d += m.d; + e += m.e; + f += m.f; + g += m.g; + h += m.h; + i += m.i; + j += m.j; + k += m.k; + l += m.l; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix4):Matrix4 { + a -= m.a; + b -= m.b; + c -= m.c; + d -= m.d; + e -= m.e; + f -= m.f; + g -= m.g; + h -= m.h; + i -= m.i; + j -= m.j; + k -= m.k; + l -= m.l; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectors(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverse(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(idx:int, axis:Vector3):void { + switch (idx) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + case 3: + axis.x = d; + axis.y = h; + axis.z = l; + return; + } + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix4):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = m.d; + e = m.e; + f = m.f; + g = m.g; + h = m.h; + i = m.i; + j = m.j; + k = m.k; + l = m.l; + + return this; + } + + public function setFromMatrix3(m:Matrix3, offset:Vector3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = offset.x; + e = m.e; + f = m.f; + g = m.g; + h = offset.y; + i = m.i; + j = m.j; + k = m.k; + l = offset.z; + + return this; + } + + /** + * @param rotations + */ + public function getEulerAngles(rotations:Vector3):void { + if (-1 < i && i < 1) { + rotations.x = Math.atan2(j, k); + rotations.y = -Math.asin(i); + rotations.z = Math.atan2(e, a); + } else { + rotations.x = 0; + rotations.y = 0.5*((i <= -1) ? Math.PI : -Math.PI); + rotations.z = Math.atan2(-b, f); + } + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix4 { + return new Matrix4(a, b, c, d, e, f, g, h, i, j, k, l); + } + + public function toString():String { + return "[Matrix4 [" + a.toFixed(3) + " " + b.toFixed(3) + " " + c.toFixed(3) + " " + d.toFixed(3) + "] [" + e.toFixed(3) + " " + f.toFixed(3) + " " + g.toFixed(3) + " " + h.toFixed(3) + "] [" + i.toFixed(3) + " " + j.toFixed(3) + " " + k.toFixed(3) + " " + l.toFixed(3) + "]]"; + } + + } +} \ No newline at end of file diff --git a/0.0.1.0/src/alternativa/physics/types/Quaternion.as b/0.0.1.0/src/alternativa/physics/types/Quaternion.as new file mode 100644 index 0000000..7ad2af0 --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/types/Quaternion.as @@ -0,0 +1,396 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + public class Quaternion { + + public var w:Number; + public var x:Number; + public var y:Number; + public var z:Number; + + public static function multiply(q1:Quaternion, q2:Quaternion, result:Quaternion):void { + result.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z; + result.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y; + result.y = q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z; + result.z = q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x; + } + + public static function createFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngle(axis, angle); + return q; + } + + public static function createFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngleComponents(x, y, z, angle); + return q; + } + + public function Quaternion(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0) { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + } + + public function reset(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0):Quaternion { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + return this; + } + + public function normalize():Quaternion { + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * Умножает на указанный кватернион слева: this * q + * + * @param q множитель + */ + public function prepend(q:Quaternion):Quaternion { + var ww:Number = w*q.w - x*q.x - y*q.y - z*q.z; + var xx:Number = w*q.x + x*q.w + y*q.z - z*q.y; + var yy:Number = w*q.y + y*q.w + z*q.x - x*q.z; + var zz:Number = w*q.z + z*q.w + x*q.y - y*q.x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Умножает на указанный кватернион справа: q * this + * + * @param q множитель + */ + public function append(q:Quaternion):Quaternion { + var ww:Number = q.w*w - q.x*x - q.y*y - q.z*z; + var xx:Number = q.w*x + q.x*w + q.y*z - q.z*y; + var yy:Number = q.w*y + q.y*w + q.z*x - q.x*z; + var zz:Number = q.w*z + q.z*w + q.x*y - q.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * + * @param vector + */ + public function rotateByVector(v:Vector3):Quaternion { + var ww:Number = -v.x*x - v.y*y - v.z*z; + var xx:Number = v.x*w + v.y*z - v.z*y; + var yy:Number = v.y*w + v.z*x - v.x*z; + var zz:Number = v.z*w + v.x*y - v.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Добавляет вращение, приданное вектором угловой скорости за указанное время. + * + * @param v + * @param scale + */ + public function addScaledVector(v:Vector3, scale:Number):Quaternion { + var vx:Number = v.x*scale; + var vy:Number = v.y*scale; + var vz:Number = v.z*scale; + var ww:Number = -x*vx - y*vy - z*vz; + var xx:Number = vx*w + vy*z - vz*y; + var yy:Number = vy*w + vz*x - vx*z; + var zz:Number = vz*w + vx*y - vy*x; + w += 0.5*ww; + x += 0.5*xx; + y += 0.5*yy; + z += 0.5*zz; + // inlined normalize + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param m + * @return + */ + public function toMatrix3(m:Matrix3):Quaternion { + var xx2:Number = 2*x*x; + var yy2:Number = 2*y*y; + var zz2:Number = 2*z*z; + var xy2:Number = 2*x*y; + var yz2:Number = 2*y*z; + var zx2:Number = 2*z*x; + var wx2:Number = 2*w*x; + var wy2:Number = 2*w*y; + var wz2:Number = 2*w*z; + + m.a = 1 - yy2 - zz2; + m.b = xy2 - wz2; + m.c = zx2 + wy2; + + m.e = xy2 + wz2; + m.f = 1 - xx2 - zz2; + m.g = yz2 - wx2; + + m.i = zx2 - wy2; + m.j = yz2 + wx2; + m.k = 1 - xx2 - yy2; + return this; + } + + /** + * + * @return + */ + public function length():Number { + return Math.sqrt(w*w + x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function lengthSqr():Number { + return w*w + x*x + y*y + z*z; + } + + /** + * + * @param axis + * @param angle + * @return + */ + public function setFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z); + x = axis.x*k; + y = axis.y*k; + z = axis.z*k; + return this; + } + + /** + * + * @param x + * @param y + * @param z + * @param angle + */ + public function setFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(x*x + y*y + z*z); + this.x = x*k; + this.y = y*k; + this.z = z*k; + return this; + } + + /** + * + * @param vector + */ + public function toAxisVector(v:Vector3 = null):Vector3 { + if (w < -1 || w > 1) { + normalize(); + } + if (v == null) { + v = new Vector3(); + } + if (w > -1 && w < 1) { + if (w == 0) { + v.x = x; + v.y = y; + v.z = z; + } else { + var angle:Number = 2*Math.acos(w); + var coeff:Number = 1/Math.sqrt(1 - w*w); + v.x = x*coeff*angle; + v.y = y*coeff*angle; + v.z = z*coeff*angle; + } + } else { + v.x = 0; + v.y = 0; + v.z = 0; + } + return v; + } + + /** + * + * @param rotations + */ + public function getEulerAngles(angles:Vector3):Vector3 { + var qi2:Number = 2*x*x; + var qj2:Number = 2*y*y; + var qk2:Number = 2*z*z; + var qij:Number = 2*x*y; + var qjk:Number = 2*y*z; + var qki:Number = 2*z*x; + var qri:Number = 2*w*x; + var qrj:Number = 2*w*y; + var qrk:Number = 2*w*z; + + var aa:Number = 1 - qj2 - qk2; + var bb:Number = qij - qrk; + var ee:Number = qij + qrk; + var ff:Number = 1 - qi2 - qk2; + var ii:Number = qki - qrj; + var jj:Number = qjk + qri; + var kk:Number = 1 - qi2 - qj2; + + if (-1 < ii && ii < 1) { + if (angles == null) { + angles = new Vector3(Math.atan2(jj, kk), -Math.asin(ii), Math.atan2(ee, aa)); + } else { + angles.x = Math.atan2(jj, kk); + angles.y = -Math.asin(ii); + angles.z = Math.atan2(ee, aa); + } + } else { + if (angles == null) { + angles = new Vector3(0, 0.5*((ii <= -1) ? Math.PI : -Math.PI), Math.atan2(-bb, ff)); + } else { + angles.x = 0; + angles.y = 0.5*((ii <= -1) ? Math.PI : -Math.PI); + angles.z = Math.atan2(-bb, ff); + } + } + return angles; + } + + /** + * + */ + public function conjugate():void { + x = -x; + y = -y; + z = -z; + } + + /** + * Выполняет линейную интерполяцию. + * + * @param q1 начало отрезка + * @param q2 конец отрезка + * @param t время, обычно задаётся в интервале [0, 1] + */ + public function nlerp(q1:Quaternion, q2:Quaternion, t:Number):Quaternion { + var d:Number = 1 - t; + w = q1.w*d + q2.w*t; + x = q1.x*d + q2.x*t; + y = q1.y*d + q2.y*t; + z = q1.z*d + q2.z*t; + // inlined normalize + d = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param q + * @return + */ + public function subtract(q:Quaternion):Quaternion { + w -= q.w; + x -= q.x; + y -= q.y; + z -= q.z; + return this; + } + + /** + * + * @param q1 + * @param q2 + * @return + */ + public function diff(q1:Quaternion, q2:Quaternion):Quaternion { + w = q2.w - q1.w; + x = q2.x - q1.x; + y = q2.y - q1.y; + z = q2.z - q1.z; + return this; + } + + /** + * + * @param q + * @return + */ + public function copy(q:Quaternion):Quaternion { + w = q.w; + x = q.x; + y = q.y; + z = q.z; + return this; + } + + /** + * @param reslult + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + result.w = w; + return result; + } + + /** + * + * @return + */ + public function clone():Quaternion { + return new Quaternion(w, x, y, z); + } + + /** + * + * @return + */ + public function toString():String { + return "[" + w + ", " + x + ", " + y + ", " + z + "]"; + } + } +} diff --git a/0.0.1.0/src/alternativa/physics/types/Vector3.as b/0.0.1.0/src/alternativa/physics/types/Vector3.as new file mode 100644 index 0000000..298ddad --- /dev/null +++ b/0.0.1.0/src/alternativa/physics/types/Vector3.as @@ -0,0 +1,331 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + /** + * + */ + public class Vector3 extends Vector3D { + + public static const ZERO:Vector3 = new Vector3(0, 0, 0); + public static const X_AXIS:Vector3 = new Vector3(1, 0, 0); + public static const Y_AXIS:Vector3 = new Vector3(0, 1, 0); + public static const Z_AXIS:Vector3 = new Vector3(0, 0, 1); + + /** + * + * @param x + * @param y + * @param z + */ + public function Vector3(x:Number = 0, y:Number = 0, z:Number = 0) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * + * @return + */ + public function vLength():Number { + return Math.sqrt(x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function vLengthSqr():Number { + return x*x + y*y + z*z; + } + + /** + * + * @param length + * @return + */ + public function vSetLength(length:Number):Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = length; + } else { + var k:Number = length/Math.sqrt(x*x + y*y + z*z); + x *= k; + y *= k; + z *= k; + } + return this; + } + + /** + * + * @return + */ + public function vNormalize():Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = 1; + } else { + var k:Number = Math.sqrt(d); + x /= k; + y /= k; + z /= k; + } + return this; + } + + /** + * Сложение координат. + * + * @param v точка, координаты которой прибавляются к собственным + */ + public function vAdd(v:Vector3):Vector3 { + x += v.x; + y += v.y; + z += v.z; + return this; + } + + /** + * + * @param k + * @param v + * @return + */ + public function vAddScaled(k:Number, v:Vector3):Vector3 { + x += k*v.x; + y += k*v.y; + z += k*v.z; + return this; + } + + /** + * Вычитание координат. + * + * @param v точка, координаты которой вычитаются из собственных + */ + public function vSubtract(v:Vector3):Vector3 { + x -= v.x; + y -= v.y; + z -= v.z; + return this; + } + + /** + * Вычисляет сумму векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + * @return this + */ + public function vSum(a:Vector3, b:Vector3):Vector3 { + x = a.x + b.x; + y = a.y + b.y; + z = a.z + b.z; + return this; + } + + /** + * Вычисление разности векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + */ + public function vDiff(a:Vector3, b:Vector3):Vector3 { + x = a.x - b.x; + y = a.y - b.y; + z = a.z - b.z; + return this; + } + + /** + * Умножение на скаляр. + * + * @param k число, на которое умножаются координаты + */ + public function vScale(k:Number):Vector3 { + x *= k; + y *= k; + z *= k; + return this; + } + + /** + * Инвертирование вектора. + */ + public function vReverse():Vector3 { + x = -x; + y = -y; + z = -z; + return this; + } + + /** + * + * @param v + * @return + */ + public function vDot(v:Vector3):Number { + return x*v.x + y*v.y + z*v.z; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross(v:Vector3):Vector3 { + var xx:Number = y*v.z - z*v.y; + var yy:Number = z*v.x - x*v.z; + var zz:Number = x*v.y - y*v.x; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross2(a:Vector3, b:Vector3):Vector3 { + x = a.y*b.z - a.z*b.y; + y = a.z*b.x - a.x*b.z; + z = a.x*b.y - a.y*b.x; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy3(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * + * @param matrix + */ + public function vTransformBy3Tr(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz + m.d; + y = m.e*xx + m.f*yy + m.g*zz + m.h; + z = m.i*xx + m.j*yy + m.k*zz + m.l; + return this; + } + + /** + * + * @param m + * @return + */ + public function vTransformInverseBy4(m:Matrix4):Vector3 { + var xx:Number = x - m.d; + var yy:Number = y - m.h; + var zz:Number = z - m.l; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * + * @param m + */ + public function vDeltaTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * Установка координат. + */ + public function vReset(x:Number = 0, y:Number = 0, z:Number = 0):Vector3 { + this.x = x; + this.y = y; + this.z = z; + return this; + } + + /** + * Копирование координат точки. + * + * @param v точка, координаты которой копируются + */ + public function vCopy(v:Vector3):Vector3 { + x = v.x; + y = v.y; + z = v.z; + return this; + } + + /** + * Клонирование точки. + * + * @return клонированная точка + */ + public function vClone():Vector3 { + return new Vector3(x, y, z); + } + + /** + * Копирует координаты вектора в экземпляр класса Vector3D. + * + * @param result экземпляр, в который копируются координаты + * @return переданный параметр + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + return result; + } + + /** + * @param source + * @return + */ + public function copyFromVector3D(source:Vector3D):Vector3 { + x = source.x; + y = source.y; + z = source.z; + return this; + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/.actionScriptProperties b/0.0.2.0/.actionScriptProperties new file mode 100644 index 0000000..e5b1b89 --- /dev/null +++ b/0.0.2.0/.actionScriptProperties @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/0.0.2.0/.flexLibProperties b/0.0.2.0/.flexLibProperties new file mode 100644 index 0000000..2083d0b --- /dev/null +++ b/0.0.2.0/.flexLibProperties @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/0.0.2.0/.project b/0.0.2.0/.project new file mode 100644 index 0000000..1f1425f --- /dev/null +++ b/0.0.2.0/.project @@ -0,0 +1,19 @@ + + + AlternativaPhysics + + + + + + com.adobe.flexbuilder.project.flexbuilder + + + + + + org.maven.ide.eclipse.maven2Nature + com.adobe.flexbuilder.project.flexlibnature + com.adobe.flexbuilder.project.actionscriptnature + + diff --git a/0.0.2.0/.settings/.svn/all-wcprops b/0.0.2.0/.settings/.svn/all-wcprops new file mode 100644 index 0000000..f859e6d --- /dev/null +++ b/0.0.2.0/.settings/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 89 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/.settings +END +org.eclipse.core.resources.prefs +K 25 +svn:wc:ra_dav:version-url +V 122 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/.settings/org.eclipse.core.resources.prefs +END diff --git a/0.0.2.0/.settings/.svn/entries b/0.0.2.0/.settings/.svn/entries new file mode 100644 index 0000000..62c7539 --- /dev/null +++ b/0.0.2.0/.settings/.svn/entries @@ -0,0 +1,40 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/.settings +http://svndev.alternativaplatform.com + + + +2009-04-01T12:25:29.638016Z +10301 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +org.eclipse.core.resources.prefs +file + + + + +2010-10-28T04:32:40.000000Z +4f70b476f3e5075e399505021df2f89b +2009-04-01T12:25:29.638016Z +10301 +mike + diff --git a/0.0.2.0/.settings/.svn/format b/0.0.2.0/.settings/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.2.0/.settings/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.2.0/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base b/0.0.2.0/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base new file mode 100644 index 0000000..b3d0d49 --- /dev/null +++ b/0.0.2.0/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base @@ -0,0 +1,3 @@ +#Fri Mar 20 00:19:33 YEKT 2009 +eclipse.preferences.version=1 +encoding/=UTF-8 diff --git a/0.0.2.0/.settings/org.eclipse.core.resources.prefs b/0.0.2.0/.settings/org.eclipse.core.resources.prefs new file mode 100644 index 0000000..b3d0d49 --- /dev/null +++ b/0.0.2.0/.settings/org.eclipse.core.resources.prefs @@ -0,0 +1,3 @@ +#Fri Mar 20 00:19:33 YEKT 2009 +eclipse.preferences.version=1 +encoding/=UTF-8 diff --git a/0.0.2.0/.svn/all-wcprops b/0.0.2.0/.svn/all-wcprops new file mode 100644 index 0000000..15af25a --- /dev/null +++ b/0.0.2.0/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 79 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0 +END +.flexLibProperties +K 25 +svn:wc:ra_dav:version-url +V 98 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/.flexLibProperties +END +.project +K 25 +svn:wc:ra_dav:version-url +V 88 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/.project +END +pom.xml +K 25 +svn:wc:ra_dav:version-url +V 87 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/pom.xml +END +.actionScriptProperties +K 25 +svn:wc:ra_dav:version-url +V 103 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/.actionScriptProperties +END diff --git a/0.0.2.0/.svn/dir-prop-base b/0.0.2.0/.svn/dir-prop-base new file mode 100644 index 0000000..8eda4b3 --- /dev/null +++ b/0.0.2.0/.svn/dir-prop-base @@ -0,0 +1,5 @@ +K 13 +svn:mergeinfo +V 85 +/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D:13892-14098 +END diff --git a/0.0.2.0/.svn/entries b/0.0.2.0/.svn/entries new file mode 100644 index 0000000..2f21348 --- /dev/null +++ b/0.0.2.0/.svn/entries @@ -0,0 +1,85 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0 +http://svndev.alternativaplatform.com + + + +2009-06-29T08:50:58.692043Z +15305 +mike +has-props + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +META-INF +dir + +.flexLibProperties +file + + + + +2010-10-28T04:32:40.000000Z +fcef7ca7fe61a3459fb00858c28adec1 +2009-05-26T08:41:33.471001Z +13389 +mike + +.project +file + + + + +2010-10-28T04:32:40.000000Z +1e90cc68b52b93173b2b5de88eb5c3a3 +2009-04-20T17:15:55.260110Z +11541 +mike + +src +dir + +pom.xml +file + + + + +2010-10-28T04:32:40.000000Z +6c2d68b3b0a55ff2ff9b5769c7cec04f +2009-06-29T08:50:58.692043Z +15305 +mike + +.actionScriptProperties +file + + + + +2010-10-28T04:32:40.000000Z +918335cb945c3f8866aeb8e6cfd1b110 +2009-06-23T08:49:19.922749Z +14884 +mike + +.settings +dir + diff --git a/0.0.2.0/.svn/format b/0.0.2.0/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.2.0/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.2.0/.svn/text-base/.actionScriptProperties.svn-base b/0.0.2.0/.svn/text-base/.actionScriptProperties.svn-base new file mode 100644 index 0000000..e5b1b89 --- /dev/null +++ b/0.0.2.0/.svn/text-base/.actionScriptProperties.svn-base @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/0.0.2.0/.svn/text-base/.flexLibProperties.svn-base b/0.0.2.0/.svn/text-base/.flexLibProperties.svn-base new file mode 100644 index 0000000..2083d0b --- /dev/null +++ b/0.0.2.0/.svn/text-base/.flexLibProperties.svn-base @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/0.0.2.0/.svn/text-base/.project.svn-base b/0.0.2.0/.svn/text-base/.project.svn-base new file mode 100644 index 0000000..1f1425f --- /dev/null +++ b/0.0.2.0/.svn/text-base/.project.svn-base @@ -0,0 +1,19 @@ + + + AlternativaPhysics + + + + + + com.adobe.flexbuilder.project.flexbuilder + + + + + + org.maven.ide.eclipse.maven2Nature + com.adobe.flexbuilder.project.flexlibnature + com.adobe.flexbuilder.project.actionscriptnature + + diff --git a/0.0.2.0/.svn/text-base/pom.xml.svn-base b/0.0.2.0/.svn/text-base/pom.xml.svn-base new file mode 100644 index 0000000..1b303ad --- /dev/null +++ b/0.0.2.0/.svn/text-base/pom.xml.svn-base @@ -0,0 +1,17 @@ + + 4.0.0 + platform.clients.fp10.libraries + AlternativaPhysics + swc + 0.0.2.0 + + platform.tools.maven + FlashBasePom + 1.0 + + + scm:svn:http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0 + + + + \ No newline at end of file diff --git a/0.0.2.0/META-INF/.svn/all-wcprops b/0.0.2.0/META-INF/.svn/all-wcprops new file mode 100644 index 0000000..a2cf6fb --- /dev/null +++ b/0.0.2.0/META-INF/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 88 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/META-INF +END +MANIFEST.MF +K 25 +svn:wc:ra_dav:version-url +V 100 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/META-INF/MANIFEST.MF +END diff --git a/0.0.2.0/META-INF/.svn/entries b/0.0.2.0/META-INF/.svn/entries new file mode 100644 index 0000000..dd7245d --- /dev/null +++ b/0.0.2.0/META-INF/.svn/entries @@ -0,0 +1,40 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/META-INF +http://svndev.alternativaplatform.com + + + +2009-04-20T17:15:55.260110Z +11541 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +MANIFEST.MF +file + + + + +2010-10-28T04:32:39.000000Z +7b64dceb50a6905850ff00a0bfbe77eb +2009-04-20T17:15:55.260110Z +11541 +mike + diff --git a/0.0.2.0/META-INF/.svn/format b/0.0.2.0/META-INF/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.2.0/META-INF/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.2.0/META-INF/.svn/text-base/MANIFEST.MF.svn-base b/0.0.2.0/META-INF/.svn/text-base/MANIFEST.MF.svn-base new file mode 100644 index 0000000..5066a55 --- /dev/null +++ b/0.0.2.0/META-INF/.svn/text-base/MANIFEST.MF.svn-base @@ -0,0 +1 @@ +Bundle-Name: platform.clients.fp10.libraries.AlternativaPhysics diff --git a/0.0.2.0/META-INF/MANIFEST.MF b/0.0.2.0/META-INF/MANIFEST.MF new file mode 100644 index 0000000..5066a55 --- /dev/null +++ b/0.0.2.0/META-INF/MANIFEST.MF @@ -0,0 +1 @@ +Bundle-Name: platform.clients.fp10.libraries.AlternativaPhysics diff --git a/0.0.2.0/pom.xml b/0.0.2.0/pom.xml new file mode 100644 index 0000000..1b303ad --- /dev/null +++ b/0.0.2.0/pom.xml @@ -0,0 +1,17 @@ + + 4.0.0 + platform.clients.fp10.libraries + AlternativaPhysics + swc + 0.0.2.0 + + platform.tools.maven + FlashBasePom + 1.0 + + + scm:svn:http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0 + + + + \ No newline at end of file diff --git a/0.0.2.0/src/.svn/all-wcprops b/0.0.2.0/src/.svn/all-wcprops new file mode 100644 index 0000000..8d85b0c --- /dev/null +++ b/0.0.2.0/src/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 83 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src +END diff --git a/0.0.2.0/src/.svn/entries b/0.0.2.0/src/.svn/entries new file mode 100644 index 0000000..64c71e4 --- /dev/null +++ b/0.0.2.0/src/.svn/entries @@ -0,0 +1,31 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src +http://svndev.alternativaplatform.com + + + +2009-06-29T03:58:47.301371Z +15198 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +alternativa +dir + diff --git a/0.0.2.0/src/.svn/format b/0.0.2.0/src/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.2.0/src/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.2.0/src/alternativa/.svn/all-wcprops b/0.0.2.0/src/alternativa/.svn/all-wcprops new file mode 100644 index 0000000..35058b9 --- /dev/null +++ b/0.0.2.0/src/alternativa/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 95 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa +END diff --git a/0.0.2.0/src/alternativa/.svn/entries b/0.0.2.0/src/alternativa/.svn/entries new file mode 100644 index 0000000..dccd38e --- /dev/null +++ b/0.0.2.0/src/alternativa/.svn/entries @@ -0,0 +1,31 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa +http://svndev.alternativaplatform.com + + + +2009-06-29T03:58:47.301371Z +15198 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +physics +dir + diff --git a/0.0.2.0/src/alternativa/.svn/format b/0.0.2.0/src/alternativa/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.2.0/src/alternativa/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.2.0/src/alternativa/physics/.svn/all-wcprops b/0.0.2.0/src/alternativa/physics/.svn/all-wcprops new file mode 100644 index 0000000..2790d23 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 103 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics +END +altphysics.as +K 25 +svn:wc:ra_dav:version-url +V 117 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/altphysics.as +END diff --git a/0.0.2.0/src/alternativa/physics/.svn/entries b/0.0.2.0/src/alternativa/physics/.svn/entries new file mode 100644 index 0000000..9a1c0e3 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/.svn/entries @@ -0,0 +1,49 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics +http://svndev.alternativaplatform.com + + + +2009-06-29T03:58:47.301371Z +15198 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +types +dir + +altphysics.as +file + + + + +2010-10-28T04:32:40.000000Z +04fbe40a27502dbbd0dba02a76b2df50 +2009-04-20T20:09:14.715403Z +11579 +mike + +collision +dir + +rigid +dir + diff --git a/0.0.2.0/src/alternativa/physics/.svn/format b/0.0.2.0/src/alternativa/physics/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.2.0/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base b/0.0.2.0/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base new file mode 100644 index 0000000..2715307 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base @@ -0,0 +1,3 @@ +package alternativa.physics { + public namespace altphysics = "http://alternativaplatform.com/en/altphysics"; +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/altphysics.as b/0.0.2.0/src/alternativa/physics/altphysics.as new file mode 100644 index 0000000..2715307 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/altphysics.as @@ -0,0 +1,3 @@ +package alternativa.physics { + public namespace altphysics = "http://alternativaplatform.com/en/altphysics"; +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/collision/.svn/all-wcprops b/0.0.2.0/src/alternativa/physics/collision/.svn/all-wcprops new file mode 100644 index 0000000..bb460e8 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/.svn/all-wcprops @@ -0,0 +1,95 @@ +K 25 +svn:wc:ra_dav:version-url +V 113 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/collision +END +BoxSphereCollider.as +K 25 +svn:wc:ra_dav:version-url +V 134 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/collision/BoxSphereCollider.as +END +BoxBoxCollider.as +K 25 +svn:wc:ra_dav:version-url +V 131 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/collision/BoxBoxCollider.as +END +CollisionKdNode.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/collision/CollisionKdNode.as +END +SpherePlaneCollider.as +K 25 +svn:wc:ra_dav:version-url +V 136 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/collision/SpherePlaneCollider.as +END +KdTreeCollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/collision/KdTreeCollisionDetector.as +END +BoxRectCollider.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/collision/BoxRectCollider.as +END +ICollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 136 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/collision/ICollisionPredicate.as +END +BoxPlaneCollider.as +K 25 +svn:wc:ra_dav:version-url +V 133 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/collision/BoxPlaneCollider.as +END +CollisionKdTree.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/collision/CollisionKdTree.as +END +BoxTriangleCollider.as +K 25 +svn:wc:ra_dav:version-url +V 136 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/collision/BoxTriangleCollider.as +END +ICollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 135 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/collision/ICollisionDetector.as +END +IRayCollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 139 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/collision/IRayCollisionPredicate.as +END +ICollider.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/collision/ICollider.as +END +BruteForceCollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 144 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/collision/BruteForceCollisionDetector.as +END +SphereSphereCollider.as +K 25 +svn:wc:ra_dav:version-url +V 137 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/collision/SphereSphereCollider.as +END diff --git a/0.0.2.0/src/alternativa/physics/collision/.svn/entries b/0.0.2.0/src/alternativa/physics/collision/.svn/entries new file mode 100644 index 0000000..aa331e0 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/.svn/entries @@ -0,0 +1,214 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/collision +http://svndev.alternativaplatform.com + + + +2009-06-29T03:52:30.472906Z +15194 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +BoxSphereCollider.as +file + + + + +2010-10-28T04:32:40.000000Z +ad1dc1849019ac159d22cd93b1ba38d3 +2009-06-08T06:46:18.310732Z +14099 +mike + +BoxBoxCollider.as +file + + + + +2010-10-28T04:32:40.000000Z +e4eb46370e3c7098fa35d0da4e346d09 +2009-06-08T06:46:18.310732Z +14099 +mike + +CollisionKdNode.as +file + + + + +2010-10-28T04:32:40.000000Z +7952384e785be1fca93657d94121b246 +2009-04-17T17:39:21.367569Z +11341 +mike + +BoxRectCollider.as +file + + + + +2010-10-28T04:32:40.000000Z +8554254b88a8ecc31bf005fdee676a67 +2009-06-08T06:46:18.310732Z +14099 +mike + +KdTreeCollisionDetector.as +file + + + + +2010-10-28T04:32:40.000000Z +afefefe1aacd027b490adf1eb6a6f148 +2009-06-29T03:52:30.472906Z +15194 +mike + +SpherePlaneCollider.as +file + + + + +2010-10-28T04:32:40.000000Z +7fbbf70eef5caa9a09ce679819429874 +2009-04-13T03:07:43.422558Z +11019 +mike + +ICollisionPredicate.as +file + + + + +2010-10-28T04:32:40.000000Z +866471ea582e95e46e94eb7ae437c827 +2009-04-26T15:39:06.673206Z +11995 +mike + +BoxPlaneCollider.as +file + + + + +2010-10-28T04:32:40.000000Z +4344df47c2b9c1873d2dc711815b4737 +2009-04-13T03:07:43.422558Z +11019 +mike + +primitives +dir + +CollisionKdTree.as +file + + + + +2010-10-28T04:32:40.000000Z +92511d7533028a942e299ca3a4251da8 +2009-05-18T16:37:02.465501Z +13087 +mike + +BoxTriangleCollider.as +file + + + + +2010-10-28T04:32:40.000000Z +f95da4166903174a2c92c1ab262e719c +2009-05-26T08:41:33.471001Z +13389 +mike + +ICollisionDetector.as +file + + + + +2010-10-28T04:32:40.000000Z +c98a3e33a4a54e2e85da149e25b583fe +2009-06-08T11:22:42.165256Z +14112 +mike + +IRayCollisionPredicate.as +file + + + + +2010-10-28T04:32:40.000000Z +f7443fb728cf4e6157dcfb4f39665889 +2009-04-26T15:39:06.673206Z +11995 +mike + +types +dir + +BruteForceCollisionDetector.as +file + + + + +2010-10-28T04:32:40.000000Z +c166c05be4909e221c2f129c9e7cac32 +2009-06-08T11:22:42.165256Z +14112 +mike + +ICollider.as +file + + + + +2010-10-28T04:32:40.000000Z +bc3785279b9b7810a0c84cb4fe6084fc +2009-04-13T03:07:43.422558Z +11019 +mike + +SphereSphereCollider.as +file + + + + +2010-10-28T04:32:40.000000Z +fd6e8cf39dcf007125384bc91b347a00 +2009-06-08T06:46:18.310732Z +14099 +mike + diff --git a/0.0.2.0/src/alternativa/physics/collision/.svn/format b/0.0.2.0/src/alternativa/physics/collision/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/BoxBoxCollider.as.svn-base b/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/BoxBoxCollider.as.svn-base new file mode 100644 index 0000000..2893b49 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/BoxBoxCollider.as.svn-base @@ -0,0 +1,791 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Расчитывает точки контакта двух боксов. Нормаль контакта направляется в сторону бокса с меньшим ID. + */ + public class BoxBoxCollider implements ICollider { + + private var tolerance:Number = 0.001; + + private var pos1:Vector3 = new Vector3(); + private var pos2:Vector3 = new Vector3(); + private var vectorToBox1:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var colAxis:Vector3 = new Vector3(); + private var tmpAxis:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var tmpPoints:Vector. = new Vector.(8, true); + private var pcount:int; + + /** + * + */ + public function BoxBoxCollider() { + for (var i:int = 0; i < 8; i++) { + tmpPoints[i] = new ContactPoint(); + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + /** + * + * @param body1 + * @param body2 + * @param contactInfo + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + minOverlap = 1e10; + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + // Вектор из центра второго бокса в центр первого + pos1.x = transform1.d; pos1.y = transform1.h; pos1.z = transform1.l; + pos2.x = transform2.d; pos2.y = transform2.h; pos2.z = transform2.l; + vectorToBox1.x = pos1.x - pos2.x; + vectorToBox1.y = pos1.y - pos2.y; + vectorToBox1.z = pos1.z - pos2.z; + + // Проверка пересечения по основным осям +// box1.transform.getAxis(0, axis10); + axis10.x = transform1.a; axis10.y = transform1.e; axis10.z = transform1.i; + if (!testAxis(box1, box2, axis10, 0, vectorToBox1)) return false; +// box1.transform.getAxis(1, axis11); + axis11.x = transform1.b; axis11.y = transform1.f; axis11.z = transform1.j; + if (!testAxis(box1, box2, axis11, 1, vectorToBox1)) return false; +// box1.transform.getAxis(2, axis12); + axis12.x = transform1.c; axis12.y = transform1.g; axis12.z = transform1.k; + if (!testAxis(box1, box2, axis12, 2, vectorToBox1)) return false; + +// box2.transform.getAxis(0, axis20); + axis20.x = transform2.a; axis20.y = transform2.e; axis20.z = transform2.i; + if (!testAxis(box1, box2, axis20, 3, vectorToBox1)) return false; +// box2.transform.getAxis(1, axis21); + axis21.x = transform2.b; axis21.y = transform2.f; axis21.z = transform2.j; + if (!testAxis(box1, box2, axis21, 4, vectorToBox1)) return false; +// box2.transform.getAxis(2, axis22); + axis22.x = transform2.c; axis22.y = transform2.g; axis22.z = transform2.k; + if (!testAxis(box1, box2, axis22, 5, vectorToBox1)) return false; + + // Проверка производных осей + if (!testAxis(box1, box2, axis.vCross2(axis10, axis20), 6, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis10, axis21), 7, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis10, axis22), 8, vectorToBox1)) return false; + + if (!testAxis(box1, box2, axis.vCross2(axis11, axis20), 9, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis11, axis21), 10, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis11, axis22), 11, vectorToBox1)) return false; + + if (!testAxis(box1, box2, axis.vCross2(axis12, axis20), 12, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis12, axis21), 13, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis12, axis22), 14, vectorToBox1)) return false; + + if (bestAxisIndex < 6) { + // Контакт грань-(грань|ребро|вершина) + findFaceContactPoints(box1, box2, vectorToBox1, bestAxisIndex, contact); + } else { + // Контакт ребро-ребро + bestAxisIndex -= 6; + findEdgesIntersection(box1, box2, vectorToBox1, int(bestAxisIndex/3), bestAxisIndex%3, contact); + } + contact.body1 = box1.body; + contact.body2 = box2.body; + + return true; + } + + /** + * Выполняет поиск точек контакта грани одного бокса с гранью/ребром/вершиной другого. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param faceAxisIdx индекс оси первого бокса, перпендикулярной грани, с которой произошло столкновение + * @param contactInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, faceAxisIdx:int, contactInfo:Contact):void { + var swapNormal:Boolean = false; + if (faceAxisIdx > 2) { + // Столкновение с гранью второго бокса. Для дальнейших расчётов боксы меняются местами, + // но нормаль контакта всё равно должна быть направлена в сторону первоначального box1 + var tmpBox:CollisionBox = box1; + box1 = box2; + box2 = tmpBox; + vectorToBox1.vReverse(); + faceAxisIdx -= 3; + swapNormal = true; + } + box1.transform.getAxis(faceAxisIdx, colAxis); + var faceReversed:Boolean = colAxis.vDot(vectorToBox1) > 0; + if (!faceReversed) colAxis.vReverse(); + // Ищем ось второго бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + box2.transform.getAxis(axisIdx, axis); + var dot:Number = axis.vDot(colAxis); + if (dot < 0) dot = -dot; + if (dot > maxDot) { + maxDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Получаем список вершин грани второго бокса, переводим их в систему координат первого бокса и выполняем обрезку + // по грани первого бокса. Таким образом получается список потенциальных точек контакта. + box2.transform.getAxis(incFaceAxisIdx, axis); + getFaceVertsByAxis(box2, incFaceAxisIdx, axis.vDot(colAxis) < 0, verts1); + box2.transform.transformVectors(verts1, verts2); + box1.transform.transformVectorsInverse(verts2, verts1); + var pnum:int = clip(box1.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + pcount = 0; + for (var i:int = 0; i < pnum; i++) { + if ((pen = getPointBoxPenetration(box1.hs, verts1[i], faceAxisIdx, faceReversed)) > -tolerance) { + var cp:ContactPoint = tmpPoints[pcount++]; + box1.transform.transformVector(verts1[i], cp.pos); + cp.r1.vDiff(cp.pos, pos1); + cp.r2.vDiff(cp.pos, pos2); + cp.penetration = pen; + } + } + contactInfo.normal.vCopy(colAxis); + if (swapNormal) contactInfo.normal.vReverse(); + + if (pcount > 4) reducePoints(); + for (i = 0; i < pcount; i++) (contactInfo.points[i] as ContactPoint).copyFrom(tmpPoints[i]); + contactInfo.pcount = pcount; + } + + /** + * + * @param contactInfo + */ + private function reducePoints():void { + var i:int; + var minIdx:int; + var cp1:ContactPoint; + var cp2:ContactPoint; + while (pcount > 4) { + var minLen:Number = 1e10; + var p1:Vector3 = (tmpPoints[pcount - 1] as ContactPoint).pos; + var p2:Vector3; + for (i = 0; i < pcount; i++) { + p2 = (tmpPoints[i] as ContactPoint).pos; + var dx:Number = p2.x - p1.x; + var dy:Number = p2.y - p1.y; + var dz:Number = p2.z - p1.z; + var len:Number = dx*dx + dy*dy + dz*dz; + if (len < minLen) { + minLen = len; + minIdx = i; + } + p1 = p2; + } + cp1 = tmpPoints[minIdx == 0 ? (pcount - 1) : (minIdx - 1)]; + cp2 = tmpPoints[minIdx]; + p1 = cp1.pos; + p2 = cp2.pos; + p2.x = 0.5*(p1.x + p2.x); + p2.y = 0.5*(p1.y + p2.y); + p2.z = 0.5*(p1.z + p2.z); + cp2.penetration = 0.5*(cp1.penetration + cp2.penetration); + if (minIdx > 0) { + for (i = minIdx; i < pcount; i++) tmpPoints[i - 1] = tmpPoints[i]; + tmpPoints[pcount - 1] = cp1; + } + pcount--; + } + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + private function getFaceVertsByAxis(box:CollisionBox, axisIdx:int, reverse:Boolean, result:Vector.):void { + var hs:Vector3 = box.hs; + switch (axisIdx) { + case 0: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, -hs.y, hs.z); + } + break; + case 1: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, hs.y, hs.z); + } + break; + case 2: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + } else { + (result[0] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } + break; + } + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clip(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + return clipHighX(hs.x, pnum, verts2, verts1); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + } + return 0; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param contactInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):void { + box1.transform.getAxis(axisIdx1, axis10); + box2.transform.getAxis(axisIdx2, axis20); + colAxis.vCross2(axis10, axis20).vNormalize(); + // Разворот оси в сторону первого бокса + if (colAxis.vDot(vectorToBox1) < 0) colAxis.vReverse(); + + /* Кодирование рёбер + бит 0: 1 (тип контакта ребро-ребро) + биты 1-2: индекс направляющей оси ребра + бит 3: + бит 4: + бит 5: + */ + var edgeCode1:int = 1; + var edgeCode2:int = 1; + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + point1.vCopy(box1.hs); + point2.vCopy(box2.hs); + // x + if (axisIdx1 == 0) { + point1.x = 0; + halfLen1 = box1.hs.x; + } else { + box1.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.x = -point1.x; + edgeCode1 |= 8; // 1 << 3 + } + } + if (axisIdx2 == 0) { + point2.x = 0; + halfLen2 = box2.hs.x; + } else { + box2.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.x = -point2.x; + edgeCode2 |= 8; // 1 << 3 + } + } + // y + if (axisIdx1 == 1) { + point1.y = 0; + halfLen1 = box1.hs.y; + edgeCode1 |= 2; // 1 << 1 + } else { + box1.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.y = -point1.y; + edgeCode1 |= 16; // 1 << 4 + } + } + if (axisIdx2 == 1) { + point2.y = 0; + halfLen2 = box2.hs.y; + edgeCode2 |= 2; // 1 << 1 + } else { + box2.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.y = -point2.y; + edgeCode2 |= 16; // 1 << 4 + } + } + // z + if (axisIdx1 == 2) { + point1.z = 0; + halfLen1 = box1.hs.z; + edgeCode1 |= 4; // 2 << 1 + } else { + box1.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.z = -point1.z; + edgeCode1 |= 32; // 1 << 5 + } + } + if (axisIdx2 == 2) { + point2.z = 0; + halfLen2 = box2.hs.z; + edgeCode2 |= 4; // 2 << 1 + } else { + box2.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.z = -point2.z; + edgeCode2 |= 32; // 1 << 5 + } + } + // Получаем глобальные координаты средних точек рёбер + point1.vTransformBy4(box1.transform); + point2.vTransformBy4(box2.transform); + // Находим точку пересечения рёбер, решая систему уравнений + var k:Number = axis10.vDot(axis20); + var det:Number = k*k - 1; + vector.vDiff(point2, point1); + var c1:Number = axis10.vDot(vector); + var c2:Number = axis20.vDot(vector); + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.normal.vCopy(colAxis); + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(point1.x + axis10.x*t1 + point2.x + axis20.x*t2); + cp.pos.y = 0.5*(point1.y + axis10.y*t1 + point2.y + axis20.y*t2); + cp.pos.z = 0.5*(point1.z + axis10.z*t1 + point2.z + axis20.z*t2); + cp.r1.vDiff(cp.pos, pos1); + cp.r2.vDiff(cp.pos, pos2); + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение боксов вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box1 + * @param box2 + * @param axis + * @param axisIndex + * @param vectorToBox1 + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, axisIndex:int, vectorToBox1:Vector3):Boolean { + if (axis.vLengthSqr() < 0.0001) { + return true; + } + axis.vNormalize(); + + var overlap:Number = overlapOnAxis(box1, box2, axis, vectorToBox1); + if (overlap < -tolerance) return false; + if (overlap + tolerance < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия двух боксов вдоль заданной оси. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param axis ось + * @param vectorToBox1 вектор, соединяющий центр второго бокса с центром первого + * @return величина перекрытия боксов вдоль оси + */ + public function overlapOnAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, vectorToBox1:Vector3):Number { + var m:Matrix4 = box1.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box1.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box1.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box1.hs.z; + if (d < 0) d = -d; + projection += d; + + m = box2.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box2.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box2.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box2.hs.z; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox1.x*axis.x + vectorToBox1.y*axis.y + vectorToBox1.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} + import alternativa.physics.types.Vector3; + + class CollisionPointTmp { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + } diff --git a/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/BoxPlaneCollider.as.svn-base b/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/BoxPlaneCollider.as.svn-base new file mode 100644 index 0000000..8568dc7 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/BoxPlaneCollider.as.svn-base @@ -0,0 +1,78 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + public class BoxPlaneCollider implements ICollider { + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var normal:Vector3 = new Vector3(); + + public function BoxPlaneCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var box:RigidBox = body1 as RigidBox; +// var plane:RigidPlane; +// if (box == null) { +// box = body2 as RigidBox; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные координаты вершин бокса +// var sx:Number = box.halfSize.x; +// var sy:Number = box.halfSize.y; +// var sz:Number = box.halfSize.z; +// (verts1[0] as Vector3).reset(-sx, -sy, -sz); +// (verts1[1] as Vector3).reset(sx, -sy, -sz); +// (verts1[2] as Vector3).reset(sx, sy, -sz); +// (verts1[3] as Vector3).reset(-sx, sy, -sz); +// (verts1[4] as Vector3).reset(-sx, -sy, sz); +// (verts1[5] as Vector3).reset(sx, -sy, sz); +// (verts1[6] as Vector3).reset(sx, sy, sz); +// (verts1[7] as Vector3).reset(-sx, sy, sz); +// +// box.transform.transformVectors(verts1, verts2); +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// // Проверяем наличие столкновений с каждой вершиной +// collisionInfo.pcount = 0; +// for (var i:int = 0; i < 8; i++) { +// // Вершина добавляется в список точек столкновения, если лежит под плоскостью +// var dist:Number = (verts2[i] as Vector3).dot(normal); +// if (dist < offset) { +// var cp:ContactPoint; +// if (collisionInfo.pcount == collisionInfo.points.length) { +// cp = new ContactPoint(); +// collisionInfo.points[collisionInfo.pcount] = cp; +// } else { +// cp = collisionInfo.points[collisionInfo.pcount]; +// } +// cp.pos.copy(verts2[i]); +// cp.r1.diff(cp.pos, box.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); +// cp.penetration = offset - dist; +// collisionInfo.pcount++; +// } +// } +// if (collisionInfo.pcount > 0) { +// collisionInfo.body1 = box; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// return true; +// } + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/BoxRectCollider.as.svn-base b/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/BoxRectCollider.as.svn-base new file mode 100644 index 0000000..b5287f3 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/BoxRectCollider.as.svn-base @@ -0,0 +1,734 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + public class BoxRectCollider implements ICollider { + + private var tolerance:Number = 0.001; + + private var bPos:Vector3 = new Vector3(); + private var rPos:Vector3 = new Vector3(); + + private var vectorToBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var colAxis:Vector3 = new Vector3(); + private var tmpAxis:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxRectCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + /** + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + minOverlap = 1e10; + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + + // Вектор из центра прямоугольника в центр бокса + box.transform.getAxis(3, bPos); + rect.transform.getAxis(3, rPos); + vectorToBox.vDiff(bPos, rPos); + + // Проверка пересечения по нормали прямоугольника + rect.transform.getAxis(2, axis22); + if (!testAxis(box, rect, axis22, 0, vectorToBox)) return false; + + // Проверка пересечения по основным осям бокса + box.transform.getAxis(0, axis10); + if (!testAxis(box, rect, axis10, 1, vectorToBox)) return false; + box.transform.getAxis(1, axis11); + if (!testAxis(box, rect, axis11, 2, vectorToBox)) return false; + box.transform.getAxis(2, axis12); + if (!testAxis(box, rect, axis12, 3, vectorToBox)) return false; + + // Получаем направляющие рёбер прямоугольника + rect.transform.getAxis(0, axis20); + rect.transform.getAxis(1, axis21); + + // Проверка производных осей + if (!testAxis(box, rect, axis.vCross2(axis10, axis20), 4, vectorToBox)) return false; + if (!testAxis(box, rect, axis.vCross2(axis10, axis21), 5, vectorToBox)) return false; + + if (!testAxis(box, rect, axis.vCross2(axis11, axis20), 6, vectorToBox)) return false; + if (!testAxis(box, rect, axis.vCross2(axis11, axis21), 7, vectorToBox)) return false; + + if (!testAxis(box, rect, axis.vCross2(axis12, axis20), 8, vectorToBox)) return false; + if (!testAxis(box, rect, axis.vCross2(axis12, axis21), 9, vectorToBox)) return false; + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, rect, vectorToBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + findEdgesIntersection(box, rect, vectorToBox, int(bestAxisIndex/2), bestAxisIndex%2, contact); + } + contact.body1 = box.body; + contact.body2 = rect.body; + + return true; + } + + /** + * Выполняет поиск точек контакта бокса с прямоугольником. + * + * @param box бокс + * @param rect прямоугольник + * @param vectorToBox вектор, направленный из центра прямоугольника в центр бокса + * @param faceAxisIdx индекс оси, идентифицирующей полскость столкновения (грань бокса или полскость прямоугольника) + * @param colInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, faceAxisIdx:int, colInfo:Contact):Boolean { + var pnum:int, i:int, v:Vector3, cp:ContactPoint; + if (faceAxisIdx == 0) { + // Столкновение с плоскостью прямоугольника + + // Проверим положение бокса относительно плоскости прямоугольника + rect.transform.getAxis(2, colAxis); + var offset:Number = colAxis.vDot(rPos); + if (bPos.vDot(colAxis) < offset) return false; + + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + box.transform.getAxis(axisIdx, axis); + var dot:Number = axis.vDot(colAxis); + if (dot < 0) dot = -dot; + if (dot > maxDot) { + maxDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Получаем список вершин грани бокса, переводим их в систему координат прямоугольника и выполняем обрезку + // по прямоугольнику. Таким образом получается список потенциальных точек контакта. + box.transform.getAxis(incFaceAxisIdx, axis); + getFaceVertsByAxis(box.hs, incFaceAxisIdx, axis.vDot(colAxis) > 0, verts1); + box.transform.transformVectors(verts1, verts2); + rect.transform.transformVectorsInverse(verts2, verts1); + pnum = clipByRect(rect.hs); + // Проверяем каждую потенциальную точку на принадлежность нижней полуплоскости прямоугольника и добавляем такие точки в список контактов + colInfo.pcount = 0; + for (i = 0; i < pnum; i++) { + v = verts1[i]; + if (v.z < tolerance) { + cp = colInfo.points[colInfo.pcount++]; + rect.transform.transformVector(v, cp.pos); + cp.r1.vDiff(cp.pos, bPos); + cp.r2.vDiff(cp.pos, rPos); + cp.penetration = -v.z; + } + } + colInfo.normal.vCopy(colAxis); + } else { + // Столкновение с гранью бокса + faceAxisIdx--; + box.transform.getAxis(faceAxisIdx, colAxis); + var faceReversed:Boolean = colAxis.vDot(vectorToBox) > 0; + if (!faceReversed) colAxis.vReverse(); + + rect.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) return false; + + getFaceVertsByAxis(rect.hs, 2, false, verts1); + rect.transform.transformVectors(verts1, verts2); + box.transform.transformVectorsInverse(verts2, verts1); + pnum = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + colInfo.pcount = 0; + for (i = 0; i < pnum; i++) { + if ((pen = getPointBoxPenetration(box.hs, verts1[i], faceAxisIdx, faceReversed)) > -tolerance) { + cp = colInfo.points[colInfo.pcount++]; + box.transform.transformVector(verts1[i], cp.pos); + cp.r1.vDiff(cp.pos, bPos); + cp.r2.vDiff(cp.pos, rPos); + cp.penetration = pen; + } + } + colInfo.normal.vCopy(colAxis); + } + return true; + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + private function getFaceVertsByAxis(hs:Vector3, axisIdx:int, reverse:Boolean, result:Vector.):void { + switch (axisIdx) { + case 0: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, -hs.y, hs.z); + } + break; + case 1: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, hs.y, hs.z); + } + break; + case 2: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + } else { + (result[0] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } + break; + } + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + return clipHighX(hs.x, pnum, verts2, verts1); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + } + return 0; + } + + /** + * + * @param hs + * @return + */ + private function clipByRect(hs:Vector3):int { + var pnum:int = 4; + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box первый бокс + * @param rect второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param colInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, axisIdx1:int, axisIdx2:int, colInfo:Contact):void { + box.transform.getAxis(axisIdx1, axis10); + rect.transform.getAxis(axisIdx2, axis20); + colAxis.vCross2(axis10, axis20).vNormalize(); + // Разворот оси в сторону бокса + if (colAxis.vDot(vectorToBox) < 0) colAxis.vReverse(); + + /* Кодирование рёбер + бит 0: 1 (тип контакта ребро-ребро) + биты 1-2: индекс направляющей оси ребра + бит 3: + бит 4: + бит 5: + */ + var edgeCode1:int = 1; + var edgeCode2:int = 1; + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + point1.vCopy(box.hs); + point2.vCopy(rect.hs); + // x + if (axisIdx1 == 0) { + point1.x = 0; + halfLen1 = box.hs.x; + } else { + box.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.x = -point1.x; + edgeCode1 |= 8; // 1 << 3 + } + } + if (axisIdx2 == 0) { + point2.x = 0; + halfLen2 = rect.hs.x; + } else { + rect.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.x = -point2.x; + edgeCode2 |= 8; // 1 << 3 + } + } + // y + if (axisIdx1 == 1) { + point1.y = 0; + halfLen1 = box.hs.y; + edgeCode1 |= 2; // 1 << 1 + } else { + box.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.y = -point1.y; + edgeCode1 |= 16; // 1 << 4 + } + } + if (axisIdx2 == 1) { + point2.y = 0; + halfLen2 = rect.hs.y; + edgeCode2 |= 2; // 1 << 1 + } else { + rect.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.y = -point2.y; + edgeCode2 |= 16; // 1 << 4 + } + } + // z + if (axisIdx1 == 2) { + point1.z = 0; + halfLen1 = box.hs.z; + edgeCode1 |= 4; // 2 << 1 + } else { + box.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.z = -point1.z; + edgeCode1 |= 32; // 1 << 5 + } + } + // Получаем глобальные координаты средних точек рёбер + point1.vTransformBy4(box.transform); + point2.vTransformBy4(rect.transform); + // Находим точку пересечения рёбер, решая систему уравнений + var k:Number = axis10.vDot(axis20); + var det:Number = k*k - 1; + vector.vDiff(point2, point1); + var c1:Number = axis10.vDot(vector); + var c2:Number = axis20.vDot(vector); + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + colInfo.normal.vCopy(colAxis); + colInfo.pcount = 1; + var cp:ContactPoint = colInfo.points[0]; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(point1.x + axis10.x*t1 + point2.x + axis20.x*t2); + cp.pos.y = 0.5*(point1.y + axis10.y*t1 + point2.y + axis20.y*t2); + cp.pos.z = 0.5*(point1.z + axis10.z*t1 + point2.z + axis20.z*t2); + cp.r1.vDiff(cp.pos, bPos); + cp.r2.vDiff(cp.pos, rPos); + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box + * @param rect + * @param axis + * @param axisIndex + * @param vectorToBox + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + if (axis.vLengthSqr() < 0.0001) return true; + axis.vNormalize(); + + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -tolerance) return false; + if (overlap + tolerance < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия вдоль заданной оси. + * + * @param box бокс + * @param rect прямоугольник + * @param axis ось + * @param vectorToBox вектор, соединяющий центр прямоугольника с центром бокса + * @return величина перекрытия вдоль оси + */ + public function overlapOnAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, vectorToBox:Vector3):Number { + var m:Matrix4 = box.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box.hs.z; + if (d < 0) d = -d; + projection += d; + + m = rect.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*rect.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*rect.hs.y; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox.x*axis.x + vectorToBox.y*axis.y + vectorToBox.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/BoxSphereCollider.as.svn-base b/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/BoxSphereCollider.as.svn-base new file mode 100644 index 0000000..2b166f3 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/BoxSphereCollider.as.svn-base @@ -0,0 +1,104 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + use namespace altphysics; + /** + * + */ + public class BoxSphereCollider implements ICollider { + + private var center:Vector3 = new Vector3(); + private var closestPt:Vector3 = new Vector3(); + + private var bPos:Vector3 = new Vector3(); + private var sPos:Vector3 = new Vector3(); + + /** + * + */ + public function BoxSphereCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + // Зафиксированно столкновение + contact.body1 = sphere.body; + contact.body2 = box.body; + contact.normal.vCopy(closestPt).vTransformBy4(box.transform).vSubtract(sPos).vNormalize().vReverse(); + contact.pcount = 1; + + var cp:ContactPoint = contact.points[0]; + cp.penetration = sphere.r - Math.sqrt(distSqr); + cp.pos.vCopy(contact.normal).vScale(-sphere.r).vAdd(sPos); + cp.r1.vDiff(cp.pos, sPos); + cp.r2.vDiff(cp.pos, bPos); + + return true; + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/BoxTriangleCollider.as.svn-base b/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/BoxTriangleCollider.as.svn-base new file mode 100644 index 0000000..63c6efd --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/BoxTriangleCollider.as.svn-base @@ -0,0 +1,14 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Contact; + import alternativa.physics.collision.primitives.CollisionPrimitive; + + public class BoxTriangleCollider implements ICollider { + public function BoxTriangleCollider() { + } + + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/BruteForceCollisionDetector.as.svn-base b/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/BruteForceCollisionDetector.as.svn-base new file mode 100644 index 0000000..d2c2e2a --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/BruteForceCollisionDetector.as.svn-base @@ -0,0 +1,37 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + public class BruteForceCollisionDetector implements ICollisionDetector { + public function BruteForceCollisionDetector() { + } + + public function addPrimitive(primitive:CollisionPrimitive, isStatic:Boolean=true):Boolean { + return false; + } + + public function removePrimitive(primitive:CollisionPrimitive, isStatic:Boolean=true):Boolean { + return false; + } + + public function init():void { + } + + public function getAllCollisions(contacts:Vector.):int { + return 0; + } + + public function intersectRay(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, intersection:RayIntersection):Boolean { + return false; + } + + public function intersectRayWithStatic(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base b/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base new file mode 100644 index 0000000..e1a1657 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base @@ -0,0 +1,16 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.types.BoundBox; + + public class CollisionKdNode { + public var objects:Vector.; + public var boundBox:BoundBox; + public var parent:CollisionKdNode; + + public var axis:int = -1; // 0 - x, 1 - y, 2 - z + public var coord:Number; + public var positiveNode:CollisionKdNode; + public var negativeNode:CollisionKdNode; + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base b/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base new file mode 100644 index 0000000..6c62b79 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base @@ -0,0 +1,259 @@ +package alternativa.physics.collision { + + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + + /** + * @author mike + */ + public class CollisionKdTree { + + public var threshold:Number = 0.01; + public var minPrimitivesPerNode:int = 1; + + public var rootNode:CollisionKdNode; + public var staticChildren:Vector. = new Vector.(); + public var numStaticChildren:int; + private var staticBoundBoxes:Vector. = new Vector.(); + + /** + * @param primitive + */ + public function addStaticPrimitive(primitive:CollisionPrimitive):void { + staticChildren[numStaticChildren++] = primitive; + } + + /** + * @param primitive + * @return + */ + public function removeStaticPrimitive(primitive:CollisionPrimitive):Boolean { + var idx:int = staticChildren.indexOf(primitive); + if (idx < 0) return false; + staticChildren.splice(idx, 1); + numStaticChildren--; + return true; + } + + /** + * @param boundBox + */ + public function createTree(boundBox:BoundBox = null):void { + if (numStaticChildren == 0) return; + // Создаём корневую ноду + rootNode = new CollisionKdNode(); + rootNode.objects = new Vector.(); + // Расчитываем баунды объектов и рутовой ноды + var rootNodeBoundBox:BoundBox = rootNode.boundBox = (boundBox != null) ? boundBox : new BoundBox(); + for (var i:int = 0; i < numStaticChildren; i++) { + var child:CollisionPrimitive = staticChildren[i]; + var childBoundBox:BoundBox = staticBoundBoxes[i] = child.calculateAABB(); + rootNodeBoundBox.addBoundBox(childBoundBox); + rootNode.objects[i] = i; + } + staticBoundBoxes.length = numStaticChildren; + // Разделяем рутовую ноду + splitNode(rootNode); + } + + private var splitAxis:int; + private var splitCoord:Number; + private var splitCost:Number; + static private const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + static private const splitCoordsX:Vector. = new Vector.(); + static private const splitCoordsY:Vector. = new Vector.(); + static private const splitCoordsZ:Vector. = new Vector.(); + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + if (node.objects.length <= minPrimitivesPerNode) return; + + var objects:Vector. = node.objects; + var i:int, j:int, k:int, length:int = objects.length, c1:Number, c2:Number, ct1:Number, ct2:Number, area:Number, areaNegative:Number, areaPositive:Number, numNegative:int, numPositive:int, conflict:Boolean, cost:Number; + + var nodeBoundBox:BoundBox = node.boundBox; + + // Подготовка баунда с погрешностями + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + // Собираем опорные координаты + var numSplitCoordsX:int = 0, numSplitCoordsY:int = 0, numSplitCoordsZ:int = 0; + for (i = 0; i < length; i++) { + var boundBox:BoundBox = staticBoundBoxes[objects[i]]; + if (boundBox.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = boundBox.minX; + if (boundBox.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = boundBox.maxX; + + if (boundBox.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = boundBox.minY; + if (boundBox.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = boundBox.maxY; + + if (boundBox.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.minZ; + if (boundBox.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.maxZ; + } + + // Убираем дубликаты координат, ищем наилучший сплит + splitAxis = -1; splitCost = 1e308; + i = 0; area = (nodeBoundBox.maxY - nodeBoundBox.minY)*(nodeBoundBox.maxZ - nodeBoundBox.minZ); + while (i < numSplitCoordsX) { + if (isNaN(c1 = splitCoordsX[i++])) continue; + ct1 = c1 - threshold; + ct2 = c1 + threshold; + areaNegative = area*(c1 - nodeBoundBox.minX); + areaPositive = area*(nodeBoundBox.maxX - c1); + numNegative = numPositive = 0; + conflict = false; + // Проверяем объекты + for (j = 0; j < length; j++) { + boundBox = staticBoundBoxes[objects[j]]; + if (boundBox.maxX <= ct2) { + if (boundBox.minX < ct1) numNegative++; + } else { + if (boundBox.minX >= ct1) numPositive++; else {conflict = true; break;} + } + } + // Если хороший сплит, сохраняем + if (!conflict && (cost = areaNegative*numNegative + areaPositive*numPositive) < splitCost) { + splitCost = cost; + splitAxis = 0; + splitCoord = c1; + } + j = i; + while (++j < numSplitCoordsX) if ((c2 = splitCoordsX[j]) >= c1 - threshold && c2 <= c1 + threshold) splitCoordsX[j] = NaN; + } + i = 0; area = (nodeBoundBox.maxX - nodeBoundBox.minX)*(nodeBoundBox.maxZ - nodeBoundBox.minZ); + while (i < numSplitCoordsY) { + if (isNaN(c1 = splitCoordsY[i++])) continue; + ct1 = c1 - threshold; + ct2 = c1 + threshold; + areaNegative = area*(c1 - nodeBoundBox.minY); + areaPositive = area*(nodeBoundBox.maxY - c1); + numNegative = numPositive = 0; + conflict = false; + // Проверяем объекты + for (j = 0; j < length; j++) { + boundBox = staticBoundBoxes[objects[j]]; + if (boundBox.maxY <= ct2) { + if (boundBox.minY < ct1) numNegative++; + } else { + if (boundBox.minY >= ct1) numPositive++; else {conflict = true; break;} + } + } + // Если хороший сплит, сохраняем + if (!conflict && (cost = areaNegative*numNegative + areaPositive*numPositive) < splitCost) { + splitCost = cost; + splitAxis = 1; + splitCoord = c1; + } + j = i; + while (++j < numSplitCoordsY) if ((c2 = splitCoordsY[j]) >= c1 - threshold && c2 <= c1 + threshold) splitCoordsY[j] = NaN; + } + i = 0; area = (nodeBoundBox.maxX - nodeBoundBox.minX)*(nodeBoundBox.maxY - nodeBoundBox.minY); + while (i < numSplitCoordsZ) { + if (isNaN(c1 = splitCoordsZ[i++])) continue; + ct1 = c1 - threshold; + ct2 = c1 + threshold; + areaNegative = area*(c1 - nodeBoundBox.minZ); + areaPositive = area*(nodeBoundBox.maxZ - c1); + numNegative = numPositive = 0; + conflict = false; + // Проверяем объекты + for (j = 0; j < length; j++) { + boundBox = staticBoundBoxes[objects[j]]; + if (boundBox.maxZ <= ct2) { + if (boundBox.minZ < ct1) numNegative++; + } else { + if (boundBox.minZ >= ct1) numPositive++; else {conflict = true; break;} + } + } + // Если хороший сплит, сохраняем + if (!conflict && (cost = areaNegative*numNegative + areaPositive*numPositive) < splitCost) { + splitCost = cost; + splitAxis = 2; + splitCoord = c1; + } + j = i; + while (++j < numSplitCoordsZ) if ((c2 = splitCoordsZ[j]) >= c1 - threshold && c2 <= c1 + threshold) splitCoordsZ[j] = NaN; + } + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Разделяем ноду + var axisX:Boolean = splitAxis == 0, axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.positiveNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.positiveNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + ct1 = splitCoord - threshold; ct2 = splitCoord + threshold; + for (i = 0; i < length; i++) { + boundBox = staticBoundBoxes[objects[i]]; + var min:Number = axisX ? boundBox.minX : (axisY ? boundBox.minY : boundBox.minZ); + var max:Number = axisX ? boundBox.maxX : (axisY ? boundBox.maxY : boundBox.maxZ); + if (max <= ct2) { + // Объект в негативной стороне + if (node.negativeNode.objects == null) node.negativeNode.objects = new Vector.(); + node.negativeNode.objects.push(objects[i]); + objects[i] = -1; + } else { + if (min >= ct1) { + // Объект в положительной стороне + if (node.positiveNode.objects == null) node.positiveNode.objects = new Vector.(); + node.positiveNode.objects.push(objects[i]); + objects[i] = -1; + } else { + // Распилился + } + } + } + + // Очистка списка объектов + j = 0; + for (i = 0; i < length; i++) { + if (objects[i] >= 0) objects[j++] = objects[i]; + } + if (j > 0) objects.length = j; else node.objects = null; + + // Разделение дочерних нод + if (node.negativeNode.objects != null) splitNode(node.negativeNode); + if (node.positiveNode.objects != null) splitNode(node.positiveNode); + } + + /** + * + */ + public function traceTree():void { + traceNode("", rootNode); + } + + /** + * @param str + * @param node + */ + private function traceNode(str:String, node:CollisionKdNode):void { + if (node == null) return; + trace(str, node.axis == -1 ? "end" : ((node.axis == 0) ? "X" : ((node.axis == 1) ? "Y" : "Z")), "splitCoord=" + splitCoord, "bound", node.boundBox, "objs:", node.objects); + traceNode(str + "-", node.negativeNode); + traceNode(str + "+", node.positiveNode); + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base b/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base new file mode 100644 index 0000000..733231b --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base @@ -0,0 +1,12 @@ +package alternativa.physics.collision { + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + + /** + * Интерфейс определителя столкновений между двумя примитивами. + */ + public interface ICollider { + function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base b/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base new file mode 100644 index 0000000..83e94d7 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base @@ -0,0 +1,73 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + /** + * Интерфейс детектора столкновений. + */ + public interface ICollisionDetector { + + /** + * Добавляет физический примитив в коллайдер. + * + * @param primitive добавляемый примитив + * @param isStatic указывает тип примитива: статический или динамический + * @return true если примитив был успешно добавлен, иначе false + */ + function addPrimitive(primitive:CollisionPrimitive, isStatic:Boolean = true):Boolean; + + /** + * Удаляет физический примитив из коллайдера. + * + * @param primitive удаляемый примитив + * @param isStatic указывает тип примитива: статический или динамический + * @return true если примитив был успшено удалён, иначе false + */ + function removePrimitive(primitive:CollisionPrimitive, isStatic:Boolean = true):Boolean; + + /** + * Выполняет инициализацию детектора после обновления списка примитивов. + */ + function init():void; + + /** + * Получает все столкновения в текущей конфигурации физической геометрии. + * + * @param contacts список контактов, в кторые будет записана информация о столкновении + * @return количество найденных столкновений + */ + function getAllCollisions(contacts:Vector.):int; + + /** + * Тестирует луч на пересечение с физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param dir направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRay(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * Тестирует луч на пересечение со статической физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param dir направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRayWithStatic(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base b/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base new file mode 100644 index 0000000..bb1b756 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base @@ -0,0 +1,9 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + + public interface ICollisionPredicate { + + function considerCollision(primitive:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base b/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base new file mode 100644 index 0000000..0f3d929 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Body; + + public interface IRayCollisionPredicate { + function considerBody(body:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base b/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base new file mode 100644 index 0000000..b12218e --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base @@ -0,0 +1,452 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Детектор, хранящий статическую геометрию в kD-дереве и использующий дерево для ускорения тестов на пересечения. + */ + public class KdTreeCollisionDetector implements ICollisionDetector { + + altphysics var tree:CollisionKdTree; + altphysics var dynamicPrimitives:Vector.; + 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.(); + + addCollider(CollisionPrimitive.BOX, CollisionPrimitive.BOX, new BoxBoxCollider()); +// addCollider(CollisionPrimitive.BOX, CollisionPrimitive.PLANE, new BoxPlaneCollider()); + addCollider(CollisionPrimitive.BOX, CollisionPrimitive.SPHERE, new BoxSphereCollider()); + addCollider(CollisionPrimitive.BOX, CollisionPrimitive.RECT, new BoxRectCollider()); + +// 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():void { + tree.createTree(); +// tree.traceTree(); + } + + /** + * + * @param contacts + * @return + */ + public function getAllCollisions(contacts:Vector.):int { + var colNum:int = 0; + for (var i:int = 0; i < dynamicPrimitivesNum; i++) { + var primitive:CollisionPrimitive = dynamicPrimitives[i]; + primitive.calculateAABB(); + if (primitive.body != null && primitive.body.frozen) continue; + colNum += getPrimitiveNodeCollisions(tree.rootNode, primitive, contacts, colNum); + + // Столкновения динамических примитивов между собой + // TODO: Попробовать различные оптимизации (сортировка по баундам, встраивание в дерево) + for (var j:int = i + 1; j < dynamicPrimitivesNum; j++) { + if (collide(primitive, dynamicPrimitives[j], contacts[colNum])) colNum++; + } + } + + return colNum; + } + + /** + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + public function collide(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.collide(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 = (contact.points[0] as ContactPoint).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 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 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:Vector., startIndex:int):int { + var colNum:int = 0; + if (node.objects != null) { + // Поиск столкновений со статическими примитивами узла + var primitives:Vector. = tree.staticChildren; + var indices:Vector. = node.objects; + for (var i:int = indices.length - 1; i >= 0; i--) + if (collide(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; + } + + 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.getSegmentIntersection(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.objects != null && getRayNodeIntersection(origin, dir, collisionGroup, tree.staticChildren, node.objects, 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., indices:Vector., 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.getSegmentIntersection(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; + } \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/SpherePlaneCollider.as.svn-base b/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/SpherePlaneCollider.as.svn-base new file mode 100644 index 0000000..9b527d8 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/SpherePlaneCollider.as.svn-base @@ -0,0 +1,58 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + /** + * + */ + public class SpherePlaneCollider implements ICollider { + + private var normal:Vector3 = new Vector3(); + + /** + * + */ + public function SpherePlaneCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var sphere:RigidSphere = body1 as RigidSphere; +// var plane:RigidPlane; +// if (sphere == null) { +// sphere = body2 as RigidSphere; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// +// var dist:Number = sphere.state.pos.dot(normal) - offset; +// if (dist > sphere.r) return false; +// +// collisionInfo.body1 = sphere; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// collisionInfo.pcount = 1; +// +// var cp:ContactPoint = collisionInfo.points[0]; +// cp.penetration = sphere.r - dist; +// cp.pos.copy(normal).reverse().scale(sphere.r).add(sphere.state.pos); +// cp.r1.diff(cp.pos, sphere.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); + + return true; + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/SphereSphereCollider.as.svn-base b/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/SphereSphereCollider.as.svn-base new file mode 100644 index 0000000..9111a84 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/.svn/text-base/SphereSphereCollider.as.svn-base @@ -0,0 +1,60 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + public class SphereSphereCollider implements ICollider { + + private var p1:Vector3 = new Vector3(); + private var p2:Vector3 = new Vector3(); + + public function SphereSphereCollider() { + } + + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var s1:CollisionSphere; + var s2:CollisionSphere; + if (prim1.body != null) { + s1 = prim1 as CollisionSphere; + s2 = prim2 as CollisionSphere; + } else { + s1 = prim2 as CollisionSphere; + s2 = prim1 as CollisionSphere; + } + + s1.transform.getAxis(3, p1); + s2.transform.getAxis(3, p2); + var dx:Number = p1.x - p2.x; + var dy:Number = p1.y - p2.y; + var dz:Number = p1.z - p2.z; + var len:Number = dx*dx + dy*dy + dz*dz; + var sum:Number = s1.r + s2.r; + if (len > sum*sum) return false; + len = Math.sqrt(len); + dx /= len; + dy /= len; + dz /= len; + + contact.body1 = s1.body; + contact.body2 = s2.body; + contact.normal.x = dx; + contact.normal.y = dy; + contact.normal.z = dz; + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = sum - len; + cp.pos.x = p1.x - dx*s1.r; + cp.pos.y = p1.y - dy*s1.r; + cp.pos.z = p1.z - dz*s1.r; + cp.r1.vDiff(cp.pos, p1); + cp.r2.vDiff(cp.pos, p2); + + return true; + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/collision/BoxBoxCollider.as b/0.0.2.0/src/alternativa/physics/collision/BoxBoxCollider.as new file mode 100644 index 0000000..2893b49 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/BoxBoxCollider.as @@ -0,0 +1,791 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Расчитывает точки контакта двух боксов. Нормаль контакта направляется в сторону бокса с меньшим ID. + */ + public class BoxBoxCollider implements ICollider { + + private var tolerance:Number = 0.001; + + private var pos1:Vector3 = new Vector3(); + private var pos2:Vector3 = new Vector3(); + private var vectorToBox1:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var colAxis:Vector3 = new Vector3(); + private var tmpAxis:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var tmpPoints:Vector. = new Vector.(8, true); + private var pcount:int; + + /** + * + */ + public function BoxBoxCollider() { + for (var i:int = 0; i < 8; i++) { + tmpPoints[i] = new ContactPoint(); + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + /** + * + * @param body1 + * @param body2 + * @param contactInfo + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + minOverlap = 1e10; + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + // Вектор из центра второго бокса в центр первого + pos1.x = transform1.d; pos1.y = transform1.h; pos1.z = transform1.l; + pos2.x = transform2.d; pos2.y = transform2.h; pos2.z = transform2.l; + vectorToBox1.x = pos1.x - pos2.x; + vectorToBox1.y = pos1.y - pos2.y; + vectorToBox1.z = pos1.z - pos2.z; + + // Проверка пересечения по основным осям +// box1.transform.getAxis(0, axis10); + axis10.x = transform1.a; axis10.y = transform1.e; axis10.z = transform1.i; + if (!testAxis(box1, box2, axis10, 0, vectorToBox1)) return false; +// box1.transform.getAxis(1, axis11); + axis11.x = transform1.b; axis11.y = transform1.f; axis11.z = transform1.j; + if (!testAxis(box1, box2, axis11, 1, vectorToBox1)) return false; +// box1.transform.getAxis(2, axis12); + axis12.x = transform1.c; axis12.y = transform1.g; axis12.z = transform1.k; + if (!testAxis(box1, box2, axis12, 2, vectorToBox1)) return false; + +// box2.transform.getAxis(0, axis20); + axis20.x = transform2.a; axis20.y = transform2.e; axis20.z = transform2.i; + if (!testAxis(box1, box2, axis20, 3, vectorToBox1)) return false; +// box2.transform.getAxis(1, axis21); + axis21.x = transform2.b; axis21.y = transform2.f; axis21.z = transform2.j; + if (!testAxis(box1, box2, axis21, 4, vectorToBox1)) return false; +// box2.transform.getAxis(2, axis22); + axis22.x = transform2.c; axis22.y = transform2.g; axis22.z = transform2.k; + if (!testAxis(box1, box2, axis22, 5, vectorToBox1)) return false; + + // Проверка производных осей + if (!testAxis(box1, box2, axis.vCross2(axis10, axis20), 6, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis10, axis21), 7, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis10, axis22), 8, vectorToBox1)) return false; + + if (!testAxis(box1, box2, axis.vCross2(axis11, axis20), 9, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis11, axis21), 10, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis11, axis22), 11, vectorToBox1)) return false; + + if (!testAxis(box1, box2, axis.vCross2(axis12, axis20), 12, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis12, axis21), 13, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis12, axis22), 14, vectorToBox1)) return false; + + if (bestAxisIndex < 6) { + // Контакт грань-(грань|ребро|вершина) + findFaceContactPoints(box1, box2, vectorToBox1, bestAxisIndex, contact); + } else { + // Контакт ребро-ребро + bestAxisIndex -= 6; + findEdgesIntersection(box1, box2, vectorToBox1, int(bestAxisIndex/3), bestAxisIndex%3, contact); + } + contact.body1 = box1.body; + contact.body2 = box2.body; + + return true; + } + + /** + * Выполняет поиск точек контакта грани одного бокса с гранью/ребром/вершиной другого. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param faceAxisIdx индекс оси первого бокса, перпендикулярной грани, с которой произошло столкновение + * @param contactInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, faceAxisIdx:int, contactInfo:Contact):void { + var swapNormal:Boolean = false; + if (faceAxisIdx > 2) { + // Столкновение с гранью второго бокса. Для дальнейших расчётов боксы меняются местами, + // но нормаль контакта всё равно должна быть направлена в сторону первоначального box1 + var tmpBox:CollisionBox = box1; + box1 = box2; + box2 = tmpBox; + vectorToBox1.vReverse(); + faceAxisIdx -= 3; + swapNormal = true; + } + box1.transform.getAxis(faceAxisIdx, colAxis); + var faceReversed:Boolean = colAxis.vDot(vectorToBox1) > 0; + if (!faceReversed) colAxis.vReverse(); + // Ищем ось второго бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + box2.transform.getAxis(axisIdx, axis); + var dot:Number = axis.vDot(colAxis); + if (dot < 0) dot = -dot; + if (dot > maxDot) { + maxDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Получаем список вершин грани второго бокса, переводим их в систему координат первого бокса и выполняем обрезку + // по грани первого бокса. Таким образом получается список потенциальных точек контакта. + box2.transform.getAxis(incFaceAxisIdx, axis); + getFaceVertsByAxis(box2, incFaceAxisIdx, axis.vDot(colAxis) < 0, verts1); + box2.transform.transformVectors(verts1, verts2); + box1.transform.transformVectorsInverse(verts2, verts1); + var pnum:int = clip(box1.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + pcount = 0; + for (var i:int = 0; i < pnum; i++) { + if ((pen = getPointBoxPenetration(box1.hs, verts1[i], faceAxisIdx, faceReversed)) > -tolerance) { + var cp:ContactPoint = tmpPoints[pcount++]; + box1.transform.transformVector(verts1[i], cp.pos); + cp.r1.vDiff(cp.pos, pos1); + cp.r2.vDiff(cp.pos, pos2); + cp.penetration = pen; + } + } + contactInfo.normal.vCopy(colAxis); + if (swapNormal) contactInfo.normal.vReverse(); + + if (pcount > 4) reducePoints(); + for (i = 0; i < pcount; i++) (contactInfo.points[i] as ContactPoint).copyFrom(tmpPoints[i]); + contactInfo.pcount = pcount; + } + + /** + * + * @param contactInfo + */ + private function reducePoints():void { + var i:int; + var minIdx:int; + var cp1:ContactPoint; + var cp2:ContactPoint; + while (pcount > 4) { + var minLen:Number = 1e10; + var p1:Vector3 = (tmpPoints[pcount - 1] as ContactPoint).pos; + var p2:Vector3; + for (i = 0; i < pcount; i++) { + p2 = (tmpPoints[i] as ContactPoint).pos; + var dx:Number = p2.x - p1.x; + var dy:Number = p2.y - p1.y; + var dz:Number = p2.z - p1.z; + var len:Number = dx*dx + dy*dy + dz*dz; + if (len < minLen) { + minLen = len; + minIdx = i; + } + p1 = p2; + } + cp1 = tmpPoints[minIdx == 0 ? (pcount - 1) : (minIdx - 1)]; + cp2 = tmpPoints[minIdx]; + p1 = cp1.pos; + p2 = cp2.pos; + p2.x = 0.5*(p1.x + p2.x); + p2.y = 0.5*(p1.y + p2.y); + p2.z = 0.5*(p1.z + p2.z); + cp2.penetration = 0.5*(cp1.penetration + cp2.penetration); + if (minIdx > 0) { + for (i = minIdx; i < pcount; i++) tmpPoints[i - 1] = tmpPoints[i]; + tmpPoints[pcount - 1] = cp1; + } + pcount--; + } + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + private function getFaceVertsByAxis(box:CollisionBox, axisIdx:int, reverse:Boolean, result:Vector.):void { + var hs:Vector3 = box.hs; + switch (axisIdx) { + case 0: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, -hs.y, hs.z); + } + break; + case 1: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, hs.y, hs.z); + } + break; + case 2: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + } else { + (result[0] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } + break; + } + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clip(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + return clipHighX(hs.x, pnum, verts2, verts1); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + } + return 0; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param contactInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):void { + box1.transform.getAxis(axisIdx1, axis10); + box2.transform.getAxis(axisIdx2, axis20); + colAxis.vCross2(axis10, axis20).vNormalize(); + // Разворот оси в сторону первого бокса + if (colAxis.vDot(vectorToBox1) < 0) colAxis.vReverse(); + + /* Кодирование рёбер + бит 0: 1 (тип контакта ребро-ребро) + биты 1-2: индекс направляющей оси ребра + бит 3: + бит 4: + бит 5: + */ + var edgeCode1:int = 1; + var edgeCode2:int = 1; + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + point1.vCopy(box1.hs); + point2.vCopy(box2.hs); + // x + if (axisIdx1 == 0) { + point1.x = 0; + halfLen1 = box1.hs.x; + } else { + box1.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.x = -point1.x; + edgeCode1 |= 8; // 1 << 3 + } + } + if (axisIdx2 == 0) { + point2.x = 0; + halfLen2 = box2.hs.x; + } else { + box2.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.x = -point2.x; + edgeCode2 |= 8; // 1 << 3 + } + } + // y + if (axisIdx1 == 1) { + point1.y = 0; + halfLen1 = box1.hs.y; + edgeCode1 |= 2; // 1 << 1 + } else { + box1.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.y = -point1.y; + edgeCode1 |= 16; // 1 << 4 + } + } + if (axisIdx2 == 1) { + point2.y = 0; + halfLen2 = box2.hs.y; + edgeCode2 |= 2; // 1 << 1 + } else { + box2.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.y = -point2.y; + edgeCode2 |= 16; // 1 << 4 + } + } + // z + if (axisIdx1 == 2) { + point1.z = 0; + halfLen1 = box1.hs.z; + edgeCode1 |= 4; // 2 << 1 + } else { + box1.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.z = -point1.z; + edgeCode1 |= 32; // 1 << 5 + } + } + if (axisIdx2 == 2) { + point2.z = 0; + halfLen2 = box2.hs.z; + edgeCode2 |= 4; // 2 << 1 + } else { + box2.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.z = -point2.z; + edgeCode2 |= 32; // 1 << 5 + } + } + // Получаем глобальные координаты средних точек рёбер + point1.vTransformBy4(box1.transform); + point2.vTransformBy4(box2.transform); + // Находим точку пересечения рёбер, решая систему уравнений + var k:Number = axis10.vDot(axis20); + var det:Number = k*k - 1; + vector.vDiff(point2, point1); + var c1:Number = axis10.vDot(vector); + var c2:Number = axis20.vDot(vector); + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.normal.vCopy(colAxis); + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(point1.x + axis10.x*t1 + point2.x + axis20.x*t2); + cp.pos.y = 0.5*(point1.y + axis10.y*t1 + point2.y + axis20.y*t2); + cp.pos.z = 0.5*(point1.z + axis10.z*t1 + point2.z + axis20.z*t2); + cp.r1.vDiff(cp.pos, pos1); + cp.r2.vDiff(cp.pos, pos2); + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение боксов вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box1 + * @param box2 + * @param axis + * @param axisIndex + * @param vectorToBox1 + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, axisIndex:int, vectorToBox1:Vector3):Boolean { + if (axis.vLengthSqr() < 0.0001) { + return true; + } + axis.vNormalize(); + + var overlap:Number = overlapOnAxis(box1, box2, axis, vectorToBox1); + if (overlap < -tolerance) return false; + if (overlap + tolerance < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия двух боксов вдоль заданной оси. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param axis ось + * @param vectorToBox1 вектор, соединяющий центр второго бокса с центром первого + * @return величина перекрытия боксов вдоль оси + */ + public function overlapOnAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, vectorToBox1:Vector3):Number { + var m:Matrix4 = box1.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box1.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box1.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box1.hs.z; + if (d < 0) d = -d; + projection += d; + + m = box2.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box2.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box2.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box2.hs.z; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox1.x*axis.x + vectorToBox1.y*axis.y + vectorToBox1.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} + import alternativa.physics.types.Vector3; + + class CollisionPointTmp { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + } diff --git a/0.0.2.0/src/alternativa/physics/collision/BoxPlaneCollider.as b/0.0.2.0/src/alternativa/physics/collision/BoxPlaneCollider.as new file mode 100644 index 0000000..8568dc7 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/BoxPlaneCollider.as @@ -0,0 +1,78 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + public class BoxPlaneCollider implements ICollider { + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var normal:Vector3 = new Vector3(); + + public function BoxPlaneCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var box:RigidBox = body1 as RigidBox; +// var plane:RigidPlane; +// if (box == null) { +// box = body2 as RigidBox; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные координаты вершин бокса +// var sx:Number = box.halfSize.x; +// var sy:Number = box.halfSize.y; +// var sz:Number = box.halfSize.z; +// (verts1[0] as Vector3).reset(-sx, -sy, -sz); +// (verts1[1] as Vector3).reset(sx, -sy, -sz); +// (verts1[2] as Vector3).reset(sx, sy, -sz); +// (verts1[3] as Vector3).reset(-sx, sy, -sz); +// (verts1[4] as Vector3).reset(-sx, -sy, sz); +// (verts1[5] as Vector3).reset(sx, -sy, sz); +// (verts1[6] as Vector3).reset(sx, sy, sz); +// (verts1[7] as Vector3).reset(-sx, sy, sz); +// +// box.transform.transformVectors(verts1, verts2); +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// // Проверяем наличие столкновений с каждой вершиной +// collisionInfo.pcount = 0; +// for (var i:int = 0; i < 8; i++) { +// // Вершина добавляется в список точек столкновения, если лежит под плоскостью +// var dist:Number = (verts2[i] as Vector3).dot(normal); +// if (dist < offset) { +// var cp:ContactPoint; +// if (collisionInfo.pcount == collisionInfo.points.length) { +// cp = new ContactPoint(); +// collisionInfo.points[collisionInfo.pcount] = cp; +// } else { +// cp = collisionInfo.points[collisionInfo.pcount]; +// } +// cp.pos.copy(verts2[i]); +// cp.r1.diff(cp.pos, box.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); +// cp.penetration = offset - dist; +// collisionInfo.pcount++; +// } +// } +// if (collisionInfo.pcount > 0) { +// collisionInfo.body1 = box; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// return true; +// } + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/collision/BoxRectCollider.as b/0.0.2.0/src/alternativa/physics/collision/BoxRectCollider.as new file mode 100644 index 0000000..b5287f3 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/BoxRectCollider.as @@ -0,0 +1,734 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + public class BoxRectCollider implements ICollider { + + private var tolerance:Number = 0.001; + + private var bPos:Vector3 = new Vector3(); + private var rPos:Vector3 = new Vector3(); + + private var vectorToBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var colAxis:Vector3 = new Vector3(); + private var tmpAxis:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxRectCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + /** + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + minOverlap = 1e10; + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + + // Вектор из центра прямоугольника в центр бокса + box.transform.getAxis(3, bPos); + rect.transform.getAxis(3, rPos); + vectorToBox.vDiff(bPos, rPos); + + // Проверка пересечения по нормали прямоугольника + rect.transform.getAxis(2, axis22); + if (!testAxis(box, rect, axis22, 0, vectorToBox)) return false; + + // Проверка пересечения по основным осям бокса + box.transform.getAxis(0, axis10); + if (!testAxis(box, rect, axis10, 1, vectorToBox)) return false; + box.transform.getAxis(1, axis11); + if (!testAxis(box, rect, axis11, 2, vectorToBox)) return false; + box.transform.getAxis(2, axis12); + if (!testAxis(box, rect, axis12, 3, vectorToBox)) return false; + + // Получаем направляющие рёбер прямоугольника + rect.transform.getAxis(0, axis20); + rect.transform.getAxis(1, axis21); + + // Проверка производных осей + if (!testAxis(box, rect, axis.vCross2(axis10, axis20), 4, vectorToBox)) return false; + if (!testAxis(box, rect, axis.vCross2(axis10, axis21), 5, vectorToBox)) return false; + + if (!testAxis(box, rect, axis.vCross2(axis11, axis20), 6, vectorToBox)) return false; + if (!testAxis(box, rect, axis.vCross2(axis11, axis21), 7, vectorToBox)) return false; + + if (!testAxis(box, rect, axis.vCross2(axis12, axis20), 8, vectorToBox)) return false; + if (!testAxis(box, rect, axis.vCross2(axis12, axis21), 9, vectorToBox)) return false; + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, rect, vectorToBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + findEdgesIntersection(box, rect, vectorToBox, int(bestAxisIndex/2), bestAxisIndex%2, contact); + } + contact.body1 = box.body; + contact.body2 = rect.body; + + return true; + } + + /** + * Выполняет поиск точек контакта бокса с прямоугольником. + * + * @param box бокс + * @param rect прямоугольник + * @param vectorToBox вектор, направленный из центра прямоугольника в центр бокса + * @param faceAxisIdx индекс оси, идентифицирующей полскость столкновения (грань бокса или полскость прямоугольника) + * @param colInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, faceAxisIdx:int, colInfo:Contact):Boolean { + var pnum:int, i:int, v:Vector3, cp:ContactPoint; + if (faceAxisIdx == 0) { + // Столкновение с плоскостью прямоугольника + + // Проверим положение бокса относительно плоскости прямоугольника + rect.transform.getAxis(2, colAxis); + var offset:Number = colAxis.vDot(rPos); + if (bPos.vDot(colAxis) < offset) return false; + + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + box.transform.getAxis(axisIdx, axis); + var dot:Number = axis.vDot(colAxis); + if (dot < 0) dot = -dot; + if (dot > maxDot) { + maxDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Получаем список вершин грани бокса, переводим их в систему координат прямоугольника и выполняем обрезку + // по прямоугольнику. Таким образом получается список потенциальных точек контакта. + box.transform.getAxis(incFaceAxisIdx, axis); + getFaceVertsByAxis(box.hs, incFaceAxisIdx, axis.vDot(colAxis) > 0, verts1); + box.transform.transformVectors(verts1, verts2); + rect.transform.transformVectorsInverse(verts2, verts1); + pnum = clipByRect(rect.hs); + // Проверяем каждую потенциальную точку на принадлежность нижней полуплоскости прямоугольника и добавляем такие точки в список контактов + colInfo.pcount = 0; + for (i = 0; i < pnum; i++) { + v = verts1[i]; + if (v.z < tolerance) { + cp = colInfo.points[colInfo.pcount++]; + rect.transform.transformVector(v, cp.pos); + cp.r1.vDiff(cp.pos, bPos); + cp.r2.vDiff(cp.pos, rPos); + cp.penetration = -v.z; + } + } + colInfo.normal.vCopy(colAxis); + } else { + // Столкновение с гранью бокса + faceAxisIdx--; + box.transform.getAxis(faceAxisIdx, colAxis); + var faceReversed:Boolean = colAxis.vDot(vectorToBox) > 0; + if (!faceReversed) colAxis.vReverse(); + + rect.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) return false; + + getFaceVertsByAxis(rect.hs, 2, false, verts1); + rect.transform.transformVectors(verts1, verts2); + box.transform.transformVectorsInverse(verts2, verts1); + pnum = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + colInfo.pcount = 0; + for (i = 0; i < pnum; i++) { + if ((pen = getPointBoxPenetration(box.hs, verts1[i], faceAxisIdx, faceReversed)) > -tolerance) { + cp = colInfo.points[colInfo.pcount++]; + box.transform.transformVector(verts1[i], cp.pos); + cp.r1.vDiff(cp.pos, bPos); + cp.r2.vDiff(cp.pos, rPos); + cp.penetration = pen; + } + } + colInfo.normal.vCopy(colAxis); + } + return true; + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + private function getFaceVertsByAxis(hs:Vector3, axisIdx:int, reverse:Boolean, result:Vector.):void { + switch (axisIdx) { + case 0: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, -hs.y, hs.z); + } + break; + case 1: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, hs.y, hs.z); + } + break; + case 2: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + } else { + (result[0] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } + break; + } + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + return clipHighX(hs.x, pnum, verts2, verts1); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + } + return 0; + } + + /** + * + * @param hs + * @return + */ + private function clipByRect(hs:Vector3):int { + var pnum:int = 4; + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box первый бокс + * @param rect второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param colInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, axisIdx1:int, axisIdx2:int, colInfo:Contact):void { + box.transform.getAxis(axisIdx1, axis10); + rect.transform.getAxis(axisIdx2, axis20); + colAxis.vCross2(axis10, axis20).vNormalize(); + // Разворот оси в сторону бокса + if (colAxis.vDot(vectorToBox) < 0) colAxis.vReverse(); + + /* Кодирование рёбер + бит 0: 1 (тип контакта ребро-ребро) + биты 1-2: индекс направляющей оси ребра + бит 3: + бит 4: + бит 5: + */ + var edgeCode1:int = 1; + var edgeCode2:int = 1; + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + point1.vCopy(box.hs); + point2.vCopy(rect.hs); + // x + if (axisIdx1 == 0) { + point1.x = 0; + halfLen1 = box.hs.x; + } else { + box.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.x = -point1.x; + edgeCode1 |= 8; // 1 << 3 + } + } + if (axisIdx2 == 0) { + point2.x = 0; + halfLen2 = rect.hs.x; + } else { + rect.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.x = -point2.x; + edgeCode2 |= 8; // 1 << 3 + } + } + // y + if (axisIdx1 == 1) { + point1.y = 0; + halfLen1 = box.hs.y; + edgeCode1 |= 2; // 1 << 1 + } else { + box.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.y = -point1.y; + edgeCode1 |= 16; // 1 << 4 + } + } + if (axisIdx2 == 1) { + point2.y = 0; + halfLen2 = rect.hs.y; + edgeCode2 |= 2; // 1 << 1 + } else { + rect.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.y = -point2.y; + edgeCode2 |= 16; // 1 << 4 + } + } + // z + if (axisIdx1 == 2) { + point1.z = 0; + halfLen1 = box.hs.z; + edgeCode1 |= 4; // 2 << 1 + } else { + box.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.z = -point1.z; + edgeCode1 |= 32; // 1 << 5 + } + } + // Получаем глобальные координаты средних точек рёбер + point1.vTransformBy4(box.transform); + point2.vTransformBy4(rect.transform); + // Находим точку пересечения рёбер, решая систему уравнений + var k:Number = axis10.vDot(axis20); + var det:Number = k*k - 1; + vector.vDiff(point2, point1); + var c1:Number = axis10.vDot(vector); + var c2:Number = axis20.vDot(vector); + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + colInfo.normal.vCopy(colAxis); + colInfo.pcount = 1; + var cp:ContactPoint = colInfo.points[0]; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(point1.x + axis10.x*t1 + point2.x + axis20.x*t2); + cp.pos.y = 0.5*(point1.y + axis10.y*t1 + point2.y + axis20.y*t2); + cp.pos.z = 0.5*(point1.z + axis10.z*t1 + point2.z + axis20.z*t2); + cp.r1.vDiff(cp.pos, bPos); + cp.r2.vDiff(cp.pos, rPos); + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box + * @param rect + * @param axis + * @param axisIndex + * @param vectorToBox + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + if (axis.vLengthSqr() < 0.0001) return true; + axis.vNormalize(); + + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -tolerance) return false; + if (overlap + tolerance < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия вдоль заданной оси. + * + * @param box бокс + * @param rect прямоугольник + * @param axis ось + * @param vectorToBox вектор, соединяющий центр прямоугольника с центром бокса + * @return величина перекрытия вдоль оси + */ + public function overlapOnAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, vectorToBox:Vector3):Number { + var m:Matrix4 = box.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box.hs.z; + if (d < 0) d = -d; + projection += d; + + m = rect.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*rect.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*rect.hs.y; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox.x*axis.x + vectorToBox.y*axis.y + vectorToBox.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/collision/BoxSphereCollider.as b/0.0.2.0/src/alternativa/physics/collision/BoxSphereCollider.as new file mode 100644 index 0000000..2b166f3 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/BoxSphereCollider.as @@ -0,0 +1,104 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + use namespace altphysics; + /** + * + */ + public class BoxSphereCollider implements ICollider { + + private var center:Vector3 = new Vector3(); + private var closestPt:Vector3 = new Vector3(); + + private var bPos:Vector3 = new Vector3(); + private var sPos:Vector3 = new Vector3(); + + /** + * + */ + public function BoxSphereCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + // Зафиксированно столкновение + contact.body1 = sphere.body; + contact.body2 = box.body; + contact.normal.vCopy(closestPt).vTransformBy4(box.transform).vSubtract(sPos).vNormalize().vReverse(); + contact.pcount = 1; + + var cp:ContactPoint = contact.points[0]; + cp.penetration = sphere.r - Math.sqrt(distSqr); + cp.pos.vCopy(contact.normal).vScale(-sphere.r).vAdd(sPos); + cp.r1.vDiff(cp.pos, sPos); + cp.r2.vDiff(cp.pos, bPos); + + return true; + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/collision/BoxTriangleCollider.as b/0.0.2.0/src/alternativa/physics/collision/BoxTriangleCollider.as new file mode 100644 index 0000000..63c6efd --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/BoxTriangleCollider.as @@ -0,0 +1,14 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Contact; + import alternativa.physics.collision.primitives.CollisionPrimitive; + + public class BoxTriangleCollider implements ICollider { + public function BoxTriangleCollider() { + } + + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/collision/BruteForceCollisionDetector.as b/0.0.2.0/src/alternativa/physics/collision/BruteForceCollisionDetector.as new file mode 100644 index 0000000..d2c2e2a --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/BruteForceCollisionDetector.as @@ -0,0 +1,37 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + public class BruteForceCollisionDetector implements ICollisionDetector { + public function BruteForceCollisionDetector() { + } + + public function addPrimitive(primitive:CollisionPrimitive, isStatic:Boolean=true):Boolean { + return false; + } + + public function removePrimitive(primitive:CollisionPrimitive, isStatic:Boolean=true):Boolean { + return false; + } + + public function init():void { + } + + public function getAllCollisions(contacts:Vector.):int { + return 0; + } + + public function intersectRay(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, intersection:RayIntersection):Boolean { + return false; + } + + public function intersectRayWithStatic(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/collision/CollisionKdNode.as b/0.0.2.0/src/alternativa/physics/collision/CollisionKdNode.as new file mode 100644 index 0000000..e1a1657 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/CollisionKdNode.as @@ -0,0 +1,16 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.types.BoundBox; + + public class CollisionKdNode { + public var objects:Vector.; + public var boundBox:BoundBox; + public var parent:CollisionKdNode; + + public var axis:int = -1; // 0 - x, 1 - y, 2 - z + public var coord:Number; + public var positiveNode:CollisionKdNode; + public var negativeNode:CollisionKdNode; + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/collision/CollisionKdTree.as b/0.0.2.0/src/alternativa/physics/collision/CollisionKdTree.as new file mode 100644 index 0000000..6c62b79 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/CollisionKdTree.as @@ -0,0 +1,259 @@ +package alternativa.physics.collision { + + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + + /** + * @author mike + */ + public class CollisionKdTree { + + public var threshold:Number = 0.01; + public var minPrimitivesPerNode:int = 1; + + public var rootNode:CollisionKdNode; + public var staticChildren:Vector. = new Vector.(); + public var numStaticChildren:int; + private var staticBoundBoxes:Vector. = new Vector.(); + + /** + * @param primitive + */ + public function addStaticPrimitive(primitive:CollisionPrimitive):void { + staticChildren[numStaticChildren++] = primitive; + } + + /** + * @param primitive + * @return + */ + public function removeStaticPrimitive(primitive:CollisionPrimitive):Boolean { + var idx:int = staticChildren.indexOf(primitive); + if (idx < 0) return false; + staticChildren.splice(idx, 1); + numStaticChildren--; + return true; + } + + /** + * @param boundBox + */ + public function createTree(boundBox:BoundBox = null):void { + if (numStaticChildren == 0) return; + // Создаём корневую ноду + rootNode = new CollisionKdNode(); + rootNode.objects = new Vector.(); + // Расчитываем баунды объектов и рутовой ноды + var rootNodeBoundBox:BoundBox = rootNode.boundBox = (boundBox != null) ? boundBox : new BoundBox(); + for (var i:int = 0; i < numStaticChildren; i++) { + var child:CollisionPrimitive = staticChildren[i]; + var childBoundBox:BoundBox = staticBoundBoxes[i] = child.calculateAABB(); + rootNodeBoundBox.addBoundBox(childBoundBox); + rootNode.objects[i] = i; + } + staticBoundBoxes.length = numStaticChildren; + // Разделяем рутовую ноду + splitNode(rootNode); + } + + private var splitAxis:int; + private var splitCoord:Number; + private var splitCost:Number; + static private const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + static private const splitCoordsX:Vector. = new Vector.(); + static private const splitCoordsY:Vector. = new Vector.(); + static private const splitCoordsZ:Vector. = new Vector.(); + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + if (node.objects.length <= minPrimitivesPerNode) return; + + var objects:Vector. = node.objects; + var i:int, j:int, k:int, length:int = objects.length, c1:Number, c2:Number, ct1:Number, ct2:Number, area:Number, areaNegative:Number, areaPositive:Number, numNegative:int, numPositive:int, conflict:Boolean, cost:Number; + + var nodeBoundBox:BoundBox = node.boundBox; + + // Подготовка баунда с погрешностями + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + // Собираем опорные координаты + var numSplitCoordsX:int = 0, numSplitCoordsY:int = 0, numSplitCoordsZ:int = 0; + for (i = 0; i < length; i++) { + var boundBox:BoundBox = staticBoundBoxes[objects[i]]; + if (boundBox.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = boundBox.minX; + if (boundBox.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = boundBox.maxX; + + if (boundBox.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = boundBox.minY; + if (boundBox.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = boundBox.maxY; + + if (boundBox.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.minZ; + if (boundBox.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.maxZ; + } + + // Убираем дубликаты координат, ищем наилучший сплит + splitAxis = -1; splitCost = 1e308; + i = 0; area = (nodeBoundBox.maxY - nodeBoundBox.minY)*(nodeBoundBox.maxZ - nodeBoundBox.minZ); + while (i < numSplitCoordsX) { + if (isNaN(c1 = splitCoordsX[i++])) continue; + ct1 = c1 - threshold; + ct2 = c1 + threshold; + areaNegative = area*(c1 - nodeBoundBox.minX); + areaPositive = area*(nodeBoundBox.maxX - c1); + numNegative = numPositive = 0; + conflict = false; + // Проверяем объекты + for (j = 0; j < length; j++) { + boundBox = staticBoundBoxes[objects[j]]; + if (boundBox.maxX <= ct2) { + if (boundBox.minX < ct1) numNegative++; + } else { + if (boundBox.minX >= ct1) numPositive++; else {conflict = true; break;} + } + } + // Если хороший сплит, сохраняем + if (!conflict && (cost = areaNegative*numNegative + areaPositive*numPositive) < splitCost) { + splitCost = cost; + splitAxis = 0; + splitCoord = c1; + } + j = i; + while (++j < numSplitCoordsX) if ((c2 = splitCoordsX[j]) >= c1 - threshold && c2 <= c1 + threshold) splitCoordsX[j] = NaN; + } + i = 0; area = (nodeBoundBox.maxX - nodeBoundBox.minX)*(nodeBoundBox.maxZ - nodeBoundBox.minZ); + while (i < numSplitCoordsY) { + if (isNaN(c1 = splitCoordsY[i++])) continue; + ct1 = c1 - threshold; + ct2 = c1 + threshold; + areaNegative = area*(c1 - nodeBoundBox.minY); + areaPositive = area*(nodeBoundBox.maxY - c1); + numNegative = numPositive = 0; + conflict = false; + // Проверяем объекты + for (j = 0; j < length; j++) { + boundBox = staticBoundBoxes[objects[j]]; + if (boundBox.maxY <= ct2) { + if (boundBox.minY < ct1) numNegative++; + } else { + if (boundBox.minY >= ct1) numPositive++; else {conflict = true; break;} + } + } + // Если хороший сплит, сохраняем + if (!conflict && (cost = areaNegative*numNegative + areaPositive*numPositive) < splitCost) { + splitCost = cost; + splitAxis = 1; + splitCoord = c1; + } + j = i; + while (++j < numSplitCoordsY) if ((c2 = splitCoordsY[j]) >= c1 - threshold && c2 <= c1 + threshold) splitCoordsY[j] = NaN; + } + i = 0; area = (nodeBoundBox.maxX - nodeBoundBox.minX)*(nodeBoundBox.maxY - nodeBoundBox.minY); + while (i < numSplitCoordsZ) { + if (isNaN(c1 = splitCoordsZ[i++])) continue; + ct1 = c1 - threshold; + ct2 = c1 + threshold; + areaNegative = area*(c1 - nodeBoundBox.minZ); + areaPositive = area*(nodeBoundBox.maxZ - c1); + numNegative = numPositive = 0; + conflict = false; + // Проверяем объекты + for (j = 0; j < length; j++) { + boundBox = staticBoundBoxes[objects[j]]; + if (boundBox.maxZ <= ct2) { + if (boundBox.minZ < ct1) numNegative++; + } else { + if (boundBox.minZ >= ct1) numPositive++; else {conflict = true; break;} + } + } + // Если хороший сплит, сохраняем + if (!conflict && (cost = areaNegative*numNegative + areaPositive*numPositive) < splitCost) { + splitCost = cost; + splitAxis = 2; + splitCoord = c1; + } + j = i; + while (++j < numSplitCoordsZ) if ((c2 = splitCoordsZ[j]) >= c1 - threshold && c2 <= c1 + threshold) splitCoordsZ[j] = NaN; + } + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Разделяем ноду + var axisX:Boolean = splitAxis == 0, axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.positiveNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.positiveNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + ct1 = splitCoord - threshold; ct2 = splitCoord + threshold; + for (i = 0; i < length; i++) { + boundBox = staticBoundBoxes[objects[i]]; + var min:Number = axisX ? boundBox.minX : (axisY ? boundBox.minY : boundBox.minZ); + var max:Number = axisX ? boundBox.maxX : (axisY ? boundBox.maxY : boundBox.maxZ); + if (max <= ct2) { + // Объект в негативной стороне + if (node.negativeNode.objects == null) node.negativeNode.objects = new Vector.(); + node.negativeNode.objects.push(objects[i]); + objects[i] = -1; + } else { + if (min >= ct1) { + // Объект в положительной стороне + if (node.positiveNode.objects == null) node.positiveNode.objects = new Vector.(); + node.positiveNode.objects.push(objects[i]); + objects[i] = -1; + } else { + // Распилился + } + } + } + + // Очистка списка объектов + j = 0; + for (i = 0; i < length; i++) { + if (objects[i] >= 0) objects[j++] = objects[i]; + } + if (j > 0) objects.length = j; else node.objects = null; + + // Разделение дочерних нод + if (node.negativeNode.objects != null) splitNode(node.negativeNode); + if (node.positiveNode.objects != null) splitNode(node.positiveNode); + } + + /** + * + */ + public function traceTree():void { + traceNode("", rootNode); + } + + /** + * @param str + * @param node + */ + private function traceNode(str:String, node:CollisionKdNode):void { + if (node == null) return; + trace(str, node.axis == -1 ? "end" : ((node.axis == 0) ? "X" : ((node.axis == 1) ? "Y" : "Z")), "splitCoord=" + splitCoord, "bound", node.boundBox, "objs:", node.objects); + traceNode(str + "-", node.negativeNode); + traceNode(str + "+", node.positiveNode); + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/collision/ICollider.as b/0.0.2.0/src/alternativa/physics/collision/ICollider.as new file mode 100644 index 0000000..733231b --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/ICollider.as @@ -0,0 +1,12 @@ +package alternativa.physics.collision { + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + + /** + * Интерфейс определителя столкновений между двумя примитивами. + */ + public interface ICollider { + function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/collision/ICollisionDetector.as b/0.0.2.0/src/alternativa/physics/collision/ICollisionDetector.as new file mode 100644 index 0000000..83e94d7 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/ICollisionDetector.as @@ -0,0 +1,73 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + /** + * Интерфейс детектора столкновений. + */ + public interface ICollisionDetector { + + /** + * Добавляет физический примитив в коллайдер. + * + * @param primitive добавляемый примитив + * @param isStatic указывает тип примитива: статический или динамический + * @return true если примитив был успешно добавлен, иначе false + */ + function addPrimitive(primitive:CollisionPrimitive, isStatic:Boolean = true):Boolean; + + /** + * Удаляет физический примитив из коллайдера. + * + * @param primitive удаляемый примитив + * @param isStatic указывает тип примитива: статический или динамический + * @return true если примитив был успшено удалён, иначе false + */ + function removePrimitive(primitive:CollisionPrimitive, isStatic:Boolean = true):Boolean; + + /** + * Выполняет инициализацию детектора после обновления списка примитивов. + */ + function init():void; + + /** + * Получает все столкновения в текущей конфигурации физической геометрии. + * + * @param contacts список контактов, в кторые будет записана информация о столкновении + * @return количество найденных столкновений + */ + function getAllCollisions(contacts:Vector.):int; + + /** + * Тестирует луч на пересечение с физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param dir направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRay(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * Тестирует луч на пересечение со статической физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param dir направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRayWithStatic(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/collision/ICollisionPredicate.as b/0.0.2.0/src/alternativa/physics/collision/ICollisionPredicate.as new file mode 100644 index 0000000..bb1b756 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/ICollisionPredicate.as @@ -0,0 +1,9 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + + public interface ICollisionPredicate { + + function considerCollision(primitive:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/collision/IRayCollisionPredicate.as b/0.0.2.0/src/alternativa/physics/collision/IRayCollisionPredicate.as new file mode 100644 index 0000000..0f3d929 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/IRayCollisionPredicate.as @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Body; + + public interface IRayCollisionPredicate { + function considerBody(body:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/collision/KdTreeCollisionDetector.as b/0.0.2.0/src/alternativa/physics/collision/KdTreeCollisionDetector.as new file mode 100644 index 0000000..b12218e --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/KdTreeCollisionDetector.as @@ -0,0 +1,452 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Детектор, хранящий статическую геометрию в kD-дереве и использующий дерево для ускорения тестов на пересечения. + */ + public class KdTreeCollisionDetector implements ICollisionDetector { + + altphysics var tree:CollisionKdTree; + altphysics var dynamicPrimitives:Vector.; + 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.(); + + addCollider(CollisionPrimitive.BOX, CollisionPrimitive.BOX, new BoxBoxCollider()); +// addCollider(CollisionPrimitive.BOX, CollisionPrimitive.PLANE, new BoxPlaneCollider()); + addCollider(CollisionPrimitive.BOX, CollisionPrimitive.SPHERE, new BoxSphereCollider()); + addCollider(CollisionPrimitive.BOX, CollisionPrimitive.RECT, new BoxRectCollider()); + +// 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():void { + tree.createTree(); +// tree.traceTree(); + } + + /** + * + * @param contacts + * @return + */ + public function getAllCollisions(contacts:Vector.):int { + var colNum:int = 0; + for (var i:int = 0; i < dynamicPrimitivesNum; i++) { + var primitive:CollisionPrimitive = dynamicPrimitives[i]; + primitive.calculateAABB(); + if (primitive.body != null && primitive.body.frozen) continue; + colNum += getPrimitiveNodeCollisions(tree.rootNode, primitive, contacts, colNum); + + // Столкновения динамических примитивов между собой + // TODO: Попробовать различные оптимизации (сортировка по баундам, встраивание в дерево) + for (var j:int = i + 1; j < dynamicPrimitivesNum; j++) { + if (collide(primitive, dynamicPrimitives[j], contacts[colNum])) colNum++; + } + } + + return colNum; + } + + /** + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + public function collide(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.collide(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 = (contact.points[0] as ContactPoint).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 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 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:Vector., startIndex:int):int { + var colNum:int = 0; + if (node.objects != null) { + // Поиск столкновений со статическими примитивами узла + var primitives:Vector. = tree.staticChildren; + var indices:Vector. = node.objects; + for (var i:int = indices.length - 1; i >= 0; i--) + if (collide(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; + } + + 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.getSegmentIntersection(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.objects != null && getRayNodeIntersection(origin, dir, collisionGroup, tree.staticChildren, node.objects, 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., indices:Vector., 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.getSegmentIntersection(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; + } \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/collision/SpherePlaneCollider.as b/0.0.2.0/src/alternativa/physics/collision/SpherePlaneCollider.as new file mode 100644 index 0000000..9b527d8 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/SpherePlaneCollider.as @@ -0,0 +1,58 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + /** + * + */ + public class SpherePlaneCollider implements ICollider { + + private var normal:Vector3 = new Vector3(); + + /** + * + */ + public function SpherePlaneCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var sphere:RigidSphere = body1 as RigidSphere; +// var plane:RigidPlane; +// if (sphere == null) { +// sphere = body2 as RigidSphere; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// +// var dist:Number = sphere.state.pos.dot(normal) - offset; +// if (dist > sphere.r) return false; +// +// collisionInfo.body1 = sphere; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// collisionInfo.pcount = 1; +// +// var cp:ContactPoint = collisionInfo.points[0]; +// cp.penetration = sphere.r - dist; +// cp.pos.copy(normal).reverse().scale(sphere.r).add(sphere.state.pos); +// cp.r1.diff(cp.pos, sphere.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); + + return true; + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/collision/SphereSphereCollider.as b/0.0.2.0/src/alternativa/physics/collision/SphereSphereCollider.as new file mode 100644 index 0000000..9111a84 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/SphereSphereCollider.as @@ -0,0 +1,60 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + public class SphereSphereCollider implements ICollider { + + private var p1:Vector3 = new Vector3(); + private var p2:Vector3 = new Vector3(); + + public function SphereSphereCollider() { + } + + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var s1:CollisionSphere; + var s2:CollisionSphere; + if (prim1.body != null) { + s1 = prim1 as CollisionSphere; + s2 = prim2 as CollisionSphere; + } else { + s1 = prim2 as CollisionSphere; + s2 = prim1 as CollisionSphere; + } + + s1.transform.getAxis(3, p1); + s2.transform.getAxis(3, p2); + var dx:Number = p1.x - p2.x; + var dy:Number = p1.y - p2.y; + var dz:Number = p1.z - p2.z; + var len:Number = dx*dx + dy*dy + dz*dz; + var sum:Number = s1.r + s2.r; + if (len > sum*sum) return false; + len = Math.sqrt(len); + dx /= len; + dy /= len; + dz /= len; + + contact.body1 = s1.body; + contact.body2 = s2.body; + contact.normal.x = dx; + contact.normal.y = dy; + contact.normal.z = dz; + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = sum - len; + cp.pos.x = p1.x - dx*s1.r; + cp.pos.y = p1.y - dy*s1.r; + cp.pos.z = p1.z - dz*s1.r; + cp.r1.vDiff(cp.pos, p1); + cp.r2.vDiff(cp.pos, p2); + + return true; + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/collision/primitives/.svn/all-wcprops b/0.0.2.0/src/alternativa/physics/collision/primitives/.svn/all-wcprops new file mode 100644 index 0000000..03fa72c --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/primitives/.svn/all-wcprops @@ -0,0 +1,35 @@ +K 25 +svn:wc:ra_dav:version-url +V 124 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/collision/primitives +END +CollisionTriangle.as +K 25 +svn:wc:ra_dav:version-url +V 145 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/collision/primitives/CollisionTriangle.as +END +CollisionSphere.as +K 25 +svn:wc:ra_dav:version-url +V 143 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/collision/primitives/CollisionSphere.as +END +CollisionBox.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/collision/primitives/CollisionBox.as +END +CollisionPrimitive.as +K 25 +svn:wc:ra_dav:version-url +V 146 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/collision/primitives/CollisionPrimitive.as +END +CollisionRect.as +K 25 +svn:wc:ra_dav:version-url +V 141 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/collision/primitives/CollisionRect.as +END diff --git a/0.0.2.0/src/alternativa/physics/collision/primitives/.svn/entries b/0.0.2.0/src/alternativa/physics/collision/primitives/.svn/entries new file mode 100644 index 0000000..883001b --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/primitives/.svn/entries @@ -0,0 +1,88 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/collision/primitives +http://svndev.alternativaplatform.com + + + +2009-06-25T09:18:57.805913Z +15117 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +CollisionTriangle.as +file + + + + +2010-10-28T04:32:40.000000Z +a1ccd11eb25d172d2739fce7fef21ee2 +2009-06-08T06:46:18.310732Z +14099 +mike + +CollisionSphere.as +file + + + + +2010-10-28T04:32:40.000000Z +1ca67429cc8bf27435b6ec46f8d6f80a +2009-05-26T08:41:33.471001Z +13389 +mike + +CollisionBox.as +file + + + + +2010-10-28T04:32:40.000000Z +e6a441a4317416b0d13d365f5b26ace4 +2009-06-08T07:53:17.170410Z +14100 +mike + +CollisionPrimitive.as +file + + + + +2010-10-28T04:32:40.000000Z +ba2d42da95941781dfd027568c57888c +2009-06-25T09:18:57.805913Z +15117 +mike + +CollisionRect.as +file + + + + +2010-10-28T04:32:40.000000Z +c76d5cb0626964578f2df221c8072142 +2009-06-08T07:53:17.170410Z +14100 +mike + diff --git a/0.0.2.0/src/alternativa/physics/collision/primitives/.svn/format b/0.0.2.0/src/alternativa/physics/collision/primitives/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/primitives/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.2.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base b/0.0.2.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base new file mode 100644 index 0000000..e3d90cc --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base @@ -0,0 +1,178 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + /** + * Ориентированный бокс. + */ + public class CollisionBox extends CollisionPrimitive { + + // Половинные размеры вдоль каждой из осей + public var hs:Vector3 = new Vector3(); + + // Используются в определении пересечения с лучом + private static var _o:Vector3 = new Vector3(); + private static var _v:Vector3 = new Vector3(); + + /** + * @param hs + * @param collisionGroup + */ + public function CollisionBox(hs:Vector3, collisionGroup:int) { + super(BOX, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + var t:Matrix4 = transform; + aabb.maxX = hs.x*(t.a < 0 ? -t.a : t.a) + hs.y*(t.b < 0 ? -t.b : t.b) + hs.z*(t.c < 0 ? -t.c : t.c); + aabb.minX = -aabb.maxX; + + aabb.maxY = hs.x*(t.e < 0 ? -t.e : t.e) + hs.y*(t.f < 0 ? -t.f : t.f) + hs.z*(t.g < 0 ? -t.g : t.g); + aabb.minY = -aabb.maxY; + + aabb.maxZ = hs.x*(t.i < 0 ? -t.i : t.i) + hs.y*(t.j < 0 ? -t.j : t.j) + hs.z*(t.k < 0 ? -t.k : t.k); + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var box:CollisionBox = source as CollisionBox; + if (box == null) return this; + super.copyFrom(box); + hs.vCopy(box.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionBox(hs, collisionGroup); + } + + /** + * Вычисляет параметры пересечения заданного сегмента с примитивом. + * + * @param origin начальная точка сегмента в мировых координатах + * @param vector вектор сегмента в мировых координатах + * @param threshold погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную + * погрешность, считается равной нулю. + * @param normal возвращаемое значение. Нормаль к примитиву в точке пересечения с сегментом. + * @return в случае наличия пересечения возвращается время точки пересечения, в противном случае возвращается -1. + */ + override public function getSegmentIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var tMin:Number = -1, tMax:Number = 1e308, t1:Number, t2:Number; + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var xx:Number = origin.x - transform.d; + var yy:Number = origin.y - transform.h; + var zz:Number = origin.z - transform.l; + _o.x = transform.a*xx + transform.e*yy + transform.i*zz; + _o.y = transform.b*xx + transform.f*yy + transform.j*zz; + _o.z = transform.c*xx + transform.g*yy + transform.k*zz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + _v.x = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + _v.y = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + _v.z = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + // X + if (_v.x < threshold && _v.x > -threshold) { + if (_o.x < -hs.x || _o.x > hs.x) return -1; + } else { + t1 = (-hs.x - _o.x)/_v.x; + t2 = (hs.x - _o.x)/_v.x; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.x = -1; + normal.y = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.x = 1; + normal.y = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Y + if (_v.y < threshold && _v.y > -threshold) { + if (_o.y < -hs.y || _o.y > hs.y) return -1; + } else { + t1 = (-hs.y - _o.y)/_v.y; + t2 = (hs.y - _o.y)/_v.y; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.y = -1; + normal.x = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.y = 1; + normal.x = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Z + if (_v.z < threshold && _v.z > -threshold) { + if (_o.z < -hs.z || _o.z > hs.z) return -1; + } else { + t1 = (-hs.z - _o.z)/_v.z; + t2 = (hs.z - _o.z)/_v.z; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.z = -1; + normal.x = normal.y = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.z = 1; + normal.x = normal.y = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Перевод нормали в мировую систему координат + // Inlined normal.vDeltaTransformBy4(transform); + xx = normal.x, yy = normal.y, zz = normal.z; + normal.x = transform.a*xx + transform.b*yy + transform.c*zz; + normal.y = transform.e*xx + transform.f*yy + transform.g*zz; + normal.z = transform.i*xx + transform.j*yy + transform.k*zz; + + return tMin; + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionPrimitive.as.svn-base b/0.0.2.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionPrimitive.as.svn-base new file mode 100644 index 0000000..352a940 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionPrimitive.as.svn-base @@ -0,0 +1,130 @@ +package alternativa.physics.collision.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.ICollisionPredicate; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Базовый класс для примитивов, использующихся детектором столкновений. + */ + public class CollisionPrimitive { + // Константы типов примитива + public static const BOX:int = 1; + public static const PLANE:int = 2; + public static const SPHERE:int = 3; + public static const RECT:int = 4; + public static const TRIANGLE:int = 5; + + // Тип примитива + public var type:int; + // Группы примитива. Каждая группа определяется установленным битом. Столкновения проверяются только для примитивов, + // имеющих хотя бы одну общую группу. + public var collisionGroup:int; + // Предикат примитива, вызывающийся детектором при нахождении столкновения. В зависимости от возвращённого + // предикатом результата столкновение либо регистрируется, либо игнорируется. + // Nullable + public var postCollisionPredicate:ICollisionPredicate; + + // Тело, владеющее примитивом. + // Nullable + altphysics var body:Body; + // Трансформация примитива в системе координат тела, если оно указано. Не допускается масштабирование матрицы. + altphysics var localTransform:Matrix4 = new Matrix4(); + // Полная трансформация примитива. Не допускается масштабирование матрицы. + altphysics var transform:Matrix4 = new Matrix4(); + // AABB в мировой системе координат. Расчитывается системой вызовом функции calculateBoundBox(). + altphysics var aabb:BoundBox = new BoundBox(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param type тип примитива + * @param collisionGroup группа примитива + */ + public function CollisionPrimitive(type:int, collisionGroup:int) { + this.type = type; + this.collisionGroup = collisionGroup; + } + + /** + * Устанавливает тело, владеющее примитивом. + * + * @param body тело, к которому относится примитив. Если указано значение null, матрица локальной трансформации удаляется. + * @param localTransform трансформация примитива в системе координат тела. Указание значения null равносильно + * заданию единичной матрицы. + */ + public function setBody(body:Body, localTransform:Matrix4 = null):void { + if (this.body == body) return; + this.body = body; + if (body == null) this.localTransform = null; + else { + if (this.localTransform == null) this.localTransform = new Matrix4(); + if (localTransform != null) this.localTransform.copy(localTransform); + } + } + + /** + * Наследники должны переопределять этот метод, реализуя в нём корректный расчёт ограничивающего бокса, выравненного + * по осям мировой системы координат. + * + * @return ссылка на свой баунд бокс + */ + public function calculateAABB():BoundBox { + return aabb; + } + + /** + * Клонирует примитив. Переопределять не рекомендуется. Вместо этого переопределяются методы createPrimitive() и copyFrom(). + * + * @return клон примитива + */ + public function clone():CollisionPrimitive { + var p:CollisionPrimitive = createPrimitive(); + return p.copyFrom(this); + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + type = source.type; + transform.copy(source.transform); + collisionGroup = source.collisionGroup; + setBody(source.body, source.localTransform); + aabb.copyFrom(source.aabb); + return this; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + protected function createPrimitive():CollisionPrimitive { + return new CollisionPrimitive(type, collisionGroup); + } + + /** + * Метод должен вычислять параметры пересечения заданного сегмента с примитивом. + * + * @param origin начальная точка сегмента в мировых координатах + * @param vector вектор сегмента в мировых координатах + * @param threshold погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную + * погрешность, считается равной нулю. + * @param normal возвращаемое значение. Нормаль к примитиву в точке пересечения с сегментом. + * @return в случае наличия пересечения возвращается время точки пересечения, в противном случае возвращается -1. + */ + public function getSegmentIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + return -1; + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base b/0.0.2.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base new file mode 100644 index 0000000..7d6aa47 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base @@ -0,0 +1,126 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + /** + * Ориентированный прямоугольник. Задаётся половинами размеров вдоль осей X (ширина) и Y (длина) локальной системы + * координат примитива. Таким образом, прямоугольник лежит в плоскости XY, его стороны параллельны осям этой + * плоскости, а нормаль направлена вдоль локальной оси Z. + * + * Прямоугольник может быть одно- или двусторонним. В случае одностороннего прямоугольника, + * столкновения с внутренней стороны не регистрируются. + */ + public class CollisionRect extends CollisionPrimitive { + // Половинные размеры прямоугольника вдоль осей X и Y. Нормаль направлена вдоль оси Z. + public var hs:Vector3 = new Vector3(); + // Флаг указывает, является примитив одно- или двусторонним + public var twoSided:Boolean = true; + + // Малое значение. Используется для указания фиктивной высоты примитива и в функции определения пересечения с сегментом. + private static const EPSILON:Number = 0.01; + // Переменные используются в функции поиска пересечения с лучом + private static var _o:Vector3 = new Vector3(); + private static var _v:Vector3 = new Vector3(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param hs половинные размерв прямоугольника вдоль осей X и Y. Значение z игнорируется. + * @param collisionGroup группа примитива + */ + public function CollisionRect(hs:Vector3, collisionGroup:int) { + super(RECT, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * Расчитывает ограничивающий бокс прямоугольника. Для избежания проблем высота примитива принимается равной + * не нулю, а малому значению. + * + * @return + */ + override public function calculateAABB():BoundBox { + // Баунд бокс прямоугольника имеет минимальную высоту, отличную от нуля во избежание проблем с построением kd-дерева + var t:Matrix4 = transform; + aabb.maxX = hs.x*(t.a < 0 ? -t.a : t.a) + hs.y*(t.b < 0 ? -t.b : t.b) + EPSILON*(t.c < 0 ? -t.c : t.c); + aabb.minX = -aabb.maxX; + + aabb.maxY = hs.x*(t.e < 0 ? -t.e : t.e) + hs.y*(t.f < 0 ? -t.f : t.f) + EPSILON*(t.g < 0 ? -t.g : t.g); + aabb.minY = -aabb.maxY; + + aabb.maxZ = hs.x*(t.i < 0 ? -t.i : t.i) + hs.y*(t.j < 0 ? -t.j : t.j) + EPSILON*(t.k < 0 ? -t.k : t.k); + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var rect:CollisionRect = source as CollisionRect; + if (rect == null) return this; + super.copyFrom(rect); + hs.vCopy(rect.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionRect(hs, collisionGroup); + } + + /** + * @param origin + * @param vector + * @param collisionGroup + * @param threshold + * @param normal + * @return + */ + override public function getSegmentIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var xx:Number = origin.x - transform.d; + var yy:Number = origin.y - transform.h; + var zz:Number = origin.z - transform.l; + _o.x = transform.a*xx + transform.e*yy + transform.i*zz; + _o.y = transform.b*xx + transform.f*yy + transform.j*zz; + _o.z = transform.c*xx + transform.g*yy + transform.k*zz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + _v.x = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + _v.y = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + _v.z = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + + // Проверка параллельности сегмента и плоскости примитива + if (_v.z > -threshold && _v.z < threshold) return -1; + var t:Number = -_o.z/_v.z; + if (t < 0) return -1; + // Проверка вхождения точки пересечения в прямоугольник + _o.x += _v.x*t; + _o.y += _v.y*t; + _o.z = 0; + if (_o.x < (-hs.x - EPSILON) || _o.x > (hs.x + EPSILON) || _o.y < (-hs.y - EPSILON) || _o.y > (hs.y + EPSILON)) return -1; + + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + return t; + } + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base b/0.0.2.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base new file mode 100644 index 0000000..1a9e6d1 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base @@ -0,0 +1,80 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + + /** + * Сфера. + */ + public class CollisionSphere extends CollisionPrimitive { + + // Радиус сферы + public var r:Number = 0; + + /** + * + * @param r + * @param collisionGroup + */ + public function CollisionSphere(r:Number, collisionGroup:int) { + super(SPHERE, collisionGroup); + this.r = r; + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + aabb.maxX = transform.d + r; + aabb.minX = transform.d - r; + + aabb.maxY = transform.h + r; + aabb.minY = transform.h - r; + + aabb.maxZ = transform.l + r; + aabb.minZ = transform.l - r; + + return aabb; + } + + /** + * @param origin + * @param vector + * @param threshold + * @param normal + * @return + */ + override public function getSegmentIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var px:Number = origin.x - transform.d; + var py:Number = origin.y - transform.h; + var pz:Number = origin.z - transform.l; + var k:Number = vector.x*px + vector.y*py + vector.z*pz; + if (k > 0) return -1; + var a:Number = vector.x*vector.x + vector.y*vector.y + vector.z*vector.z; + var D:Number = k*k - a*(px*px + py*py + pz*pz - r*r); + if (D < 0) return -1; + return -(k + Math.sqrt(D))/a; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var sphere:CollisionSphere = source as CollisionSphere; + if (sphere == null) return this; + super.copyFrom(sphere); + r = sphere.r; + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionSphere(r, collisionGroup); + } + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base b/0.0.2.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base new file mode 100644 index 0000000..a48803a --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base @@ -0,0 +1,39 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + public class CollisionTriangle extends CollisionPrimitive { + + public var v0:Vector3 = new Vector3(); + public var v1:Vector3 = new Vector3(); + public var v2:Vector3 = new Vector3(); + public var e0:Vector3 = new Vector3(); + public var e1:Vector3 = new Vector3(); + public var e2:Vector3 = new Vector3(); + public var len0:Number; + public var len1:Number; + public var len2:Number; + + public function CollisionTriangle(v0:Vector3, v1:Vector3, v2:Vector3, collisionGroup:int) { + super(TRIANGLE, collisionGroup); + + this.v0.vCopy(v0); + this.v1.vCopy(v1); + this.v2.vCopy(v2); + + e0.vDiff(v1, v0); + len0 = e0.vLength(); + e0.vNormalize(); + + e1.vDiff(v2, v1); + len1 = e1.vLength(); + e1.vNormalize(); + + e2.vDiff(v0, v2); + len2 = e2.vLength(); + e2.vNormalize(); + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/collision/primitives/CollisionBox.as b/0.0.2.0/src/alternativa/physics/collision/primitives/CollisionBox.as new file mode 100644 index 0000000..e3d90cc --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/primitives/CollisionBox.as @@ -0,0 +1,178 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + /** + * Ориентированный бокс. + */ + public class CollisionBox extends CollisionPrimitive { + + // Половинные размеры вдоль каждой из осей + public var hs:Vector3 = new Vector3(); + + // Используются в определении пересечения с лучом + private static var _o:Vector3 = new Vector3(); + private static var _v:Vector3 = new Vector3(); + + /** + * @param hs + * @param collisionGroup + */ + public function CollisionBox(hs:Vector3, collisionGroup:int) { + super(BOX, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + var t:Matrix4 = transform; + aabb.maxX = hs.x*(t.a < 0 ? -t.a : t.a) + hs.y*(t.b < 0 ? -t.b : t.b) + hs.z*(t.c < 0 ? -t.c : t.c); + aabb.minX = -aabb.maxX; + + aabb.maxY = hs.x*(t.e < 0 ? -t.e : t.e) + hs.y*(t.f < 0 ? -t.f : t.f) + hs.z*(t.g < 0 ? -t.g : t.g); + aabb.minY = -aabb.maxY; + + aabb.maxZ = hs.x*(t.i < 0 ? -t.i : t.i) + hs.y*(t.j < 0 ? -t.j : t.j) + hs.z*(t.k < 0 ? -t.k : t.k); + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var box:CollisionBox = source as CollisionBox; + if (box == null) return this; + super.copyFrom(box); + hs.vCopy(box.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionBox(hs, collisionGroup); + } + + /** + * Вычисляет параметры пересечения заданного сегмента с примитивом. + * + * @param origin начальная точка сегмента в мировых координатах + * @param vector вектор сегмента в мировых координатах + * @param threshold погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную + * погрешность, считается равной нулю. + * @param normal возвращаемое значение. Нормаль к примитиву в точке пересечения с сегментом. + * @return в случае наличия пересечения возвращается время точки пересечения, в противном случае возвращается -1. + */ + override public function getSegmentIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var tMin:Number = -1, tMax:Number = 1e308, t1:Number, t2:Number; + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var xx:Number = origin.x - transform.d; + var yy:Number = origin.y - transform.h; + var zz:Number = origin.z - transform.l; + _o.x = transform.a*xx + transform.e*yy + transform.i*zz; + _o.y = transform.b*xx + transform.f*yy + transform.j*zz; + _o.z = transform.c*xx + transform.g*yy + transform.k*zz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + _v.x = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + _v.y = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + _v.z = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + // X + if (_v.x < threshold && _v.x > -threshold) { + if (_o.x < -hs.x || _o.x > hs.x) return -1; + } else { + t1 = (-hs.x - _o.x)/_v.x; + t2 = (hs.x - _o.x)/_v.x; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.x = -1; + normal.y = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.x = 1; + normal.y = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Y + if (_v.y < threshold && _v.y > -threshold) { + if (_o.y < -hs.y || _o.y > hs.y) return -1; + } else { + t1 = (-hs.y - _o.y)/_v.y; + t2 = (hs.y - _o.y)/_v.y; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.y = -1; + normal.x = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.y = 1; + normal.x = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Z + if (_v.z < threshold && _v.z > -threshold) { + if (_o.z < -hs.z || _o.z > hs.z) return -1; + } else { + t1 = (-hs.z - _o.z)/_v.z; + t2 = (hs.z - _o.z)/_v.z; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.z = -1; + normal.x = normal.y = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.z = 1; + normal.x = normal.y = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Перевод нормали в мировую систему координат + // Inlined normal.vDeltaTransformBy4(transform); + xx = normal.x, yy = normal.y, zz = normal.z; + normal.x = transform.a*xx + transform.b*yy + transform.c*zz; + normal.y = transform.e*xx + transform.f*yy + transform.g*zz; + normal.z = transform.i*xx + transform.j*yy + transform.k*zz; + + return tMin; + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/collision/primitives/CollisionPrimitive.as b/0.0.2.0/src/alternativa/physics/collision/primitives/CollisionPrimitive.as new file mode 100644 index 0000000..352a940 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/primitives/CollisionPrimitive.as @@ -0,0 +1,130 @@ +package alternativa.physics.collision.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.ICollisionPredicate; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Базовый класс для примитивов, использующихся детектором столкновений. + */ + public class CollisionPrimitive { + // Константы типов примитива + public static const BOX:int = 1; + public static const PLANE:int = 2; + public static const SPHERE:int = 3; + public static const RECT:int = 4; + public static const TRIANGLE:int = 5; + + // Тип примитива + public var type:int; + // Группы примитива. Каждая группа определяется установленным битом. Столкновения проверяются только для примитивов, + // имеющих хотя бы одну общую группу. + public var collisionGroup:int; + // Предикат примитива, вызывающийся детектором при нахождении столкновения. В зависимости от возвращённого + // предикатом результата столкновение либо регистрируется, либо игнорируется. + // Nullable + public var postCollisionPredicate:ICollisionPredicate; + + // Тело, владеющее примитивом. + // Nullable + altphysics var body:Body; + // Трансформация примитива в системе координат тела, если оно указано. Не допускается масштабирование матрицы. + altphysics var localTransform:Matrix4 = new Matrix4(); + // Полная трансформация примитива. Не допускается масштабирование матрицы. + altphysics var transform:Matrix4 = new Matrix4(); + // AABB в мировой системе координат. Расчитывается системой вызовом функции calculateBoundBox(). + altphysics var aabb:BoundBox = new BoundBox(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param type тип примитива + * @param collisionGroup группа примитива + */ + public function CollisionPrimitive(type:int, collisionGroup:int) { + this.type = type; + this.collisionGroup = collisionGroup; + } + + /** + * Устанавливает тело, владеющее примитивом. + * + * @param body тело, к которому относится примитив. Если указано значение null, матрица локальной трансформации удаляется. + * @param localTransform трансформация примитива в системе координат тела. Указание значения null равносильно + * заданию единичной матрицы. + */ + public function setBody(body:Body, localTransform:Matrix4 = null):void { + if (this.body == body) return; + this.body = body; + if (body == null) this.localTransform = null; + else { + if (this.localTransform == null) this.localTransform = new Matrix4(); + if (localTransform != null) this.localTransform.copy(localTransform); + } + } + + /** + * Наследники должны переопределять этот метод, реализуя в нём корректный расчёт ограничивающего бокса, выравненного + * по осям мировой системы координат. + * + * @return ссылка на свой баунд бокс + */ + public function calculateAABB():BoundBox { + return aabb; + } + + /** + * Клонирует примитив. Переопределять не рекомендуется. Вместо этого переопределяются методы createPrimitive() и copyFrom(). + * + * @return клон примитива + */ + public function clone():CollisionPrimitive { + var p:CollisionPrimitive = createPrimitive(); + return p.copyFrom(this); + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + type = source.type; + transform.copy(source.transform); + collisionGroup = source.collisionGroup; + setBody(source.body, source.localTransform); + aabb.copyFrom(source.aabb); + return this; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + protected function createPrimitive():CollisionPrimitive { + return new CollisionPrimitive(type, collisionGroup); + } + + /** + * Метод должен вычислять параметры пересечения заданного сегмента с примитивом. + * + * @param origin начальная точка сегмента в мировых координатах + * @param vector вектор сегмента в мировых координатах + * @param threshold погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную + * погрешность, считается равной нулю. + * @param normal возвращаемое значение. Нормаль к примитиву в точке пересечения с сегментом. + * @return в случае наличия пересечения возвращается время точки пересечения, в противном случае возвращается -1. + */ + public function getSegmentIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + return -1; + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/collision/primitives/CollisionRect.as b/0.0.2.0/src/alternativa/physics/collision/primitives/CollisionRect.as new file mode 100644 index 0000000..7d6aa47 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/primitives/CollisionRect.as @@ -0,0 +1,126 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + /** + * Ориентированный прямоугольник. Задаётся половинами размеров вдоль осей X (ширина) и Y (длина) локальной системы + * координат примитива. Таким образом, прямоугольник лежит в плоскости XY, его стороны параллельны осям этой + * плоскости, а нормаль направлена вдоль локальной оси Z. + * + * Прямоугольник может быть одно- или двусторонним. В случае одностороннего прямоугольника, + * столкновения с внутренней стороны не регистрируются. + */ + public class CollisionRect extends CollisionPrimitive { + // Половинные размеры прямоугольника вдоль осей X и Y. Нормаль направлена вдоль оси Z. + public var hs:Vector3 = new Vector3(); + // Флаг указывает, является примитив одно- или двусторонним + public var twoSided:Boolean = true; + + // Малое значение. Используется для указания фиктивной высоты примитива и в функции определения пересечения с сегментом. + private static const EPSILON:Number = 0.01; + // Переменные используются в функции поиска пересечения с лучом + private static var _o:Vector3 = new Vector3(); + private static var _v:Vector3 = new Vector3(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param hs половинные размерв прямоугольника вдоль осей X и Y. Значение z игнорируется. + * @param collisionGroup группа примитива + */ + public function CollisionRect(hs:Vector3, collisionGroup:int) { + super(RECT, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * Расчитывает ограничивающий бокс прямоугольника. Для избежания проблем высота примитива принимается равной + * не нулю, а малому значению. + * + * @return + */ + override public function calculateAABB():BoundBox { + // Баунд бокс прямоугольника имеет минимальную высоту, отличную от нуля во избежание проблем с построением kd-дерева + var t:Matrix4 = transform; + aabb.maxX = hs.x*(t.a < 0 ? -t.a : t.a) + hs.y*(t.b < 0 ? -t.b : t.b) + EPSILON*(t.c < 0 ? -t.c : t.c); + aabb.minX = -aabb.maxX; + + aabb.maxY = hs.x*(t.e < 0 ? -t.e : t.e) + hs.y*(t.f < 0 ? -t.f : t.f) + EPSILON*(t.g < 0 ? -t.g : t.g); + aabb.minY = -aabb.maxY; + + aabb.maxZ = hs.x*(t.i < 0 ? -t.i : t.i) + hs.y*(t.j < 0 ? -t.j : t.j) + EPSILON*(t.k < 0 ? -t.k : t.k); + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var rect:CollisionRect = source as CollisionRect; + if (rect == null) return this; + super.copyFrom(rect); + hs.vCopy(rect.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionRect(hs, collisionGroup); + } + + /** + * @param origin + * @param vector + * @param collisionGroup + * @param threshold + * @param normal + * @return + */ + override public function getSegmentIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var xx:Number = origin.x - transform.d; + var yy:Number = origin.y - transform.h; + var zz:Number = origin.z - transform.l; + _o.x = transform.a*xx + transform.e*yy + transform.i*zz; + _o.y = transform.b*xx + transform.f*yy + transform.j*zz; + _o.z = transform.c*xx + transform.g*yy + transform.k*zz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + _v.x = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + _v.y = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + _v.z = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + + // Проверка параллельности сегмента и плоскости примитива + if (_v.z > -threshold && _v.z < threshold) return -1; + var t:Number = -_o.z/_v.z; + if (t < 0) return -1; + // Проверка вхождения точки пересечения в прямоугольник + _o.x += _v.x*t; + _o.y += _v.y*t; + _o.z = 0; + if (_o.x < (-hs.x - EPSILON) || _o.x > (hs.x + EPSILON) || _o.y < (-hs.y - EPSILON) || _o.y > (hs.y + EPSILON)) return -1; + + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + return t; + } + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/collision/primitives/CollisionSphere.as b/0.0.2.0/src/alternativa/physics/collision/primitives/CollisionSphere.as new file mode 100644 index 0000000..1a9e6d1 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/primitives/CollisionSphere.as @@ -0,0 +1,80 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + + /** + * Сфера. + */ + public class CollisionSphere extends CollisionPrimitive { + + // Радиус сферы + public var r:Number = 0; + + /** + * + * @param r + * @param collisionGroup + */ + public function CollisionSphere(r:Number, collisionGroup:int) { + super(SPHERE, collisionGroup); + this.r = r; + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + aabb.maxX = transform.d + r; + aabb.minX = transform.d - r; + + aabb.maxY = transform.h + r; + aabb.minY = transform.h - r; + + aabb.maxZ = transform.l + r; + aabb.minZ = transform.l - r; + + return aabb; + } + + /** + * @param origin + * @param vector + * @param threshold + * @param normal + * @return + */ + override public function getSegmentIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var px:Number = origin.x - transform.d; + var py:Number = origin.y - transform.h; + var pz:Number = origin.z - transform.l; + var k:Number = vector.x*px + vector.y*py + vector.z*pz; + if (k > 0) return -1; + var a:Number = vector.x*vector.x + vector.y*vector.y + vector.z*vector.z; + var D:Number = k*k - a*(px*px + py*py + pz*pz - r*r); + if (D < 0) return -1; + return -(k + Math.sqrt(D))/a; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var sphere:CollisionSphere = source as CollisionSphere; + if (sphere == null) return this; + super.copyFrom(sphere); + r = sphere.r; + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionSphere(r, collisionGroup); + } + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/collision/primitives/CollisionTriangle.as b/0.0.2.0/src/alternativa/physics/collision/primitives/CollisionTriangle.as new file mode 100644 index 0000000..a48803a --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/primitives/CollisionTriangle.as @@ -0,0 +1,39 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + public class CollisionTriangle extends CollisionPrimitive { + + public var v0:Vector3 = new Vector3(); + public var v1:Vector3 = new Vector3(); + public var v2:Vector3 = new Vector3(); + public var e0:Vector3 = new Vector3(); + public var e1:Vector3 = new Vector3(); + public var e2:Vector3 = new Vector3(); + public var len0:Number; + public var len1:Number; + public var len2:Number; + + public function CollisionTriangle(v0:Vector3, v1:Vector3, v2:Vector3, collisionGroup:int) { + super(TRIANGLE, collisionGroup); + + this.v0.vCopy(v0); + this.v1.vCopy(v1); + this.v2.vCopy(v2); + + e0.vDiff(v1, v0); + len0 = e0.vLength(); + e0.vNormalize(); + + e1.vDiff(v2, v1); + len1 = e1.vLength(); + e1.vNormalize(); + + e2.vDiff(v0, v2); + len2 = e2.vLength(); + e2.vNormalize(); + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/collision/types/.svn/all-wcprops b/0.0.2.0/src/alternativa/physics/collision/types/.svn/all-wcprops new file mode 100644 index 0000000..3a52ee0 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/types/.svn/all-wcprops @@ -0,0 +1,23 @@ +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/collision/types +END +BoundBox.as +K 25 +svn:wc:ra_dav:version-url +V 131 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/collision/types/BoundBox.as +END +RayIntersection.as +K 25 +svn:wc:ra_dav:version-url +V 138 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/collision/types/RayIntersection.as +END +Ray.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/collision/types/Ray.as +END diff --git a/0.0.2.0/src/alternativa/physics/collision/types/.svn/entries b/0.0.2.0/src/alternativa/physics/collision/types/.svn/entries new file mode 100644 index 0000000..598bf46 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/types/.svn/entries @@ -0,0 +1,64 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/collision/types +http://svndev.alternativaplatform.com + + + +2009-06-08T06:46:18.310732Z +14099 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +BoundBox.as +file + + + + +2010-10-28T04:32:40.000000Z +13aaaf1b1923de8a2579543651734ab7 +2009-05-18T16:37:02.465501Z +13087 +mike + +RayIntersection.as +file + + + + +2010-10-28T04:32:40.000000Z +a0a4cf3dd94468fdf44cd06734927c94 +2009-06-08T06:46:18.310732Z +14099 +mike + +Ray.as +file + + + + +2010-10-28T04:32:40.000000Z +55eeb1cf79a416bb863bee95f089dbcd +2009-04-17T17:39:21.367569Z +11341 +mike + diff --git a/0.0.2.0/src/alternativa/physics/collision/types/.svn/format b/0.0.2.0/src/alternativa/physics/collision/types/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/types/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.2.0/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base b/0.0.2.0/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base new file mode 100644 index 0000000..6efc097 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base @@ -0,0 +1,72 @@ +package alternativa.physics.collision.types { + + public class BoundBox { + + public var minX:Number = 1e308; + public var minY:Number = 1e308; + public var minZ:Number = 1e308; + public var maxX:Number = -1e308; + public var maxY:Number = -1e308; + public var maxZ:Number = -1e308; + + public function setSize(minX:Number, minY:Number, minZ:Number, maxX:Number, maxY:Number, maxZ:Number):void { + this.minX = minX; + this.minY = minY; + this.minZ = minZ; + this.maxX = maxX; + this.maxY = maxY; + this.maxZ = maxZ; + } + + public function addBoundBox(boundBox:BoundBox):void { + minX = (boundBox.minX < minX) ? boundBox.minX : minX; + minY = (boundBox.minY < minY) ? boundBox.minY : minY; + minZ = (boundBox.minZ < minZ) ? boundBox.minZ : minZ; + maxX = (boundBox.maxX > maxX) ? boundBox.maxX : maxX; + maxY = (boundBox.maxY > maxY) ? boundBox.maxY : maxY; + maxZ = (boundBox.maxZ > maxZ) ? boundBox.maxZ : maxZ; + } + + public function addPoint(x:Number, y:Number, z:Number):void { + if (x < minX) minX = x; + if (x > maxX) maxX = x; + if (y < minY) minY = y; + if (y > maxY) maxY = y; + if (z < minZ) minZ = z; + if (z > maxZ) maxZ = z; + } + + public function infinity():void { + minX = 1e308; + minY = 1e308; + minZ = 1e308; + maxX = -1e308; + maxY = -1e308; + maxZ = -1e308; + } + + public function intersects(bb:BoundBox, tolerance:Number):Boolean { + return !(minX > bb.maxX + tolerance || maxX < bb.minX - tolerance || minY > bb.maxY + tolerance || maxY < bb.minY - tolerance || minZ > bb.maxZ + tolerance || maxZ < bb.minZ - tolerance); + } + + public function copyFrom(boundBox:BoundBox):void { + minX = boundBox.minX; + minY = boundBox.minY; + minZ = boundBox.minZ; + maxX = boundBox.maxX; + maxY = boundBox.maxY; + maxZ = boundBox.maxZ; + } + + public function clone():BoundBox { + var clone:BoundBox = new BoundBox(); + clone.copyFrom(this); + return clone; + } + + public function toString():String { + return "BoundBox [" + minX + ", " + minY + ", " + minZ + " : " + maxX + ", " + maxY + ", " + maxZ + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base b/0.0.2.0/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base new file mode 100644 index 0000000..f80620a --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base @@ -0,0 +1,13 @@ +package alternativa.physics.collision.types { + import alternativa.physics.types.Vector3; + + /** + * Структура, описывающая луч и сегмент прямой в пространстве. + */ + public class Ray { + // Начало луча. Начальная точка сегмента на прямой луча. + public var origin:Vector3 = new Vector3(); + // Направляющий вектор луча. Длина вектора задаёт длину сегмента на луче. Конечная точка сегмента вычисляется как origin + dir. + public var dir:Vector3 = new Vector3(); + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base b/0.0.2.0/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base new file mode 100644 index 0000000..8a89f75 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base @@ -0,0 +1,25 @@ +package alternativa.physics.collision.types { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.types.Vector3; + + /** + * Структура описывает точку пересечения луча с физической геометрией. + */ + public class RayIntersection { + // Физический примитив, с которым пересекается луч + public var primitive:CollisionPrimitive; + // Положенеи точки пересечения + public var pos:Vector3 = new Vector3(); + // Нормаль поверхности физического примитива в точке пересечения + public var normal:Vector3 = new Vector3(); + // Время пересечения (|pos - ray.origin|/|ray.dir|) + public var t:Number = 0; + + public function copy(source:RayIntersection):void { + primitive = source.primitive; + pos.vCopy(source.pos); + normal.vCopy(source.normal); + t = source.t; + } + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/collision/types/BoundBox.as b/0.0.2.0/src/alternativa/physics/collision/types/BoundBox.as new file mode 100644 index 0000000..6efc097 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/types/BoundBox.as @@ -0,0 +1,72 @@ +package alternativa.physics.collision.types { + + public class BoundBox { + + public var minX:Number = 1e308; + public var minY:Number = 1e308; + public var minZ:Number = 1e308; + public var maxX:Number = -1e308; + public var maxY:Number = -1e308; + public var maxZ:Number = -1e308; + + public function setSize(minX:Number, minY:Number, minZ:Number, maxX:Number, maxY:Number, maxZ:Number):void { + this.minX = minX; + this.minY = minY; + this.minZ = minZ; + this.maxX = maxX; + this.maxY = maxY; + this.maxZ = maxZ; + } + + public function addBoundBox(boundBox:BoundBox):void { + minX = (boundBox.minX < minX) ? boundBox.minX : minX; + minY = (boundBox.minY < minY) ? boundBox.minY : minY; + minZ = (boundBox.minZ < minZ) ? boundBox.minZ : minZ; + maxX = (boundBox.maxX > maxX) ? boundBox.maxX : maxX; + maxY = (boundBox.maxY > maxY) ? boundBox.maxY : maxY; + maxZ = (boundBox.maxZ > maxZ) ? boundBox.maxZ : maxZ; + } + + public function addPoint(x:Number, y:Number, z:Number):void { + if (x < minX) minX = x; + if (x > maxX) maxX = x; + if (y < minY) minY = y; + if (y > maxY) maxY = y; + if (z < minZ) minZ = z; + if (z > maxZ) maxZ = z; + } + + public function infinity():void { + minX = 1e308; + minY = 1e308; + minZ = 1e308; + maxX = -1e308; + maxY = -1e308; + maxZ = -1e308; + } + + public function intersects(bb:BoundBox, tolerance:Number):Boolean { + return !(minX > bb.maxX + tolerance || maxX < bb.minX - tolerance || minY > bb.maxY + tolerance || maxY < bb.minY - tolerance || minZ > bb.maxZ + tolerance || maxZ < bb.minZ - tolerance); + } + + public function copyFrom(boundBox:BoundBox):void { + minX = boundBox.minX; + minY = boundBox.minY; + minZ = boundBox.minZ; + maxX = boundBox.maxX; + maxY = boundBox.maxY; + maxZ = boundBox.maxZ; + } + + public function clone():BoundBox { + var clone:BoundBox = new BoundBox(); + clone.copyFrom(this); + return clone; + } + + public function toString():String { + return "BoundBox [" + minX + ", " + minY + ", " + minZ + " : " + maxX + ", " + maxY + ", " + maxZ + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/collision/types/Ray.as b/0.0.2.0/src/alternativa/physics/collision/types/Ray.as new file mode 100644 index 0000000..f80620a --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/types/Ray.as @@ -0,0 +1,13 @@ +package alternativa.physics.collision.types { + import alternativa.physics.types.Vector3; + + /** + * Структура, описывающая луч и сегмент прямой в пространстве. + */ + public class Ray { + // Начало луча. Начальная точка сегмента на прямой луча. + public var origin:Vector3 = new Vector3(); + // Направляющий вектор луча. Длина вектора задаёт длину сегмента на луче. Конечная точка сегмента вычисляется как origin + dir. + public var dir:Vector3 = new Vector3(); + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/collision/types/RayIntersection.as b/0.0.2.0/src/alternativa/physics/collision/types/RayIntersection.as new file mode 100644 index 0000000..8a89f75 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/collision/types/RayIntersection.as @@ -0,0 +1,25 @@ +package alternativa.physics.collision.types { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.types.Vector3; + + /** + * Структура описывает точку пересечения луча с физической геометрией. + */ + public class RayIntersection { + // Физический примитив, с которым пересекается луч + public var primitive:CollisionPrimitive; + // Положенеи точки пересечения + public var pos:Vector3 = new Vector3(); + // Нормаль поверхности физического примитива в точке пересечения + public var normal:Vector3 = new Vector3(); + // Время пересечения (|pos - ray.origin|/|ray.dir|) + public var t:Number = 0; + + public function copy(source:RayIntersection):void { + primitive = source.primitive; + pos.vCopy(source.pos); + normal.vCopy(source.normal); + t = source.t; + } + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/rigid/.svn/all-wcprops b/0.0.2.0/src/alternativa/physics/rigid/.svn/all-wcprops new file mode 100644 index 0000000..9e8fcc6 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/rigid/.svn/all-wcprops @@ -0,0 +1,47 @@ +K 25 +svn:wc:ra_dav:version-url +V 109 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/rigid +END +Body.as +K 25 +svn:wc:ra_dav:version-url +V 117 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/rigid/Body.as +END +BodyState.as +K 25 +svn:wc:ra_dav:version-url +V 122 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/rigid/BodyState.as +END +PhysicsUtils.as +K 25 +svn:wc:ra_dav:version-url +V 125 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/rigid/PhysicsUtils.as +END +ContactPoint.as +K 25 +svn:wc:ra_dav:version-url +V 125 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/rigid/ContactPoint.as +END +RigidWorld.as +K 25 +svn:wc:ra_dav:version-url +V 123 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/rigid/RigidWorld.as +END +Contact.as +K 25 +svn:wc:ra_dav:version-url +V 120 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/rigid/Contact.as +END +BodyMaterial.as +K 25 +svn:wc:ra_dav:version-url +V 125 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/rigid/BodyMaterial.as +END diff --git a/0.0.2.0/src/alternativa/physics/rigid/.svn/entries b/0.0.2.0/src/alternativa/physics/rigid/.svn/entries new file mode 100644 index 0000000..6c25616 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/rigid/.svn/entries @@ -0,0 +1,118 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/rigid +http://svndev.alternativaplatform.com + + + +2009-06-29T03:58:47.301371Z +15198 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Body.as +file + + + + +2010-10-28T04:32:40.000000Z +eba89313629401d995ce3230ac59cfa8 +2009-06-29T03:52:30.472906Z +15194 +mike + +BodyState.as +file + + + + +2010-10-28T04:32:40.000000Z +8a089205beccac13a05eccfb9cfc6387 +2009-06-08T06:46:18.310732Z +14099 +mike + +PhysicsUtils.as +file + + + + +2010-10-28T04:32:40.000000Z +6d88004ddab2ae19e0d15f5b1b7c016a +2009-04-26T21:40:17.813142Z +12023 +mike + +constraints +dir + +ContactPoint.as +file + + + + +2010-10-28T04:32:40.000000Z +a94e73e3bfc85be2a79d6cc939512d91 +2009-06-08T06:46:18.310732Z +14099 +mike + +RigidWorld.as +file + + + + +2010-10-28T04:32:40.000000Z +f2034cedeb3afa045c7c80c4ef623ae4 +2009-06-29T03:52:30.472906Z +15194 +mike + +primitives +dir + +Contact.as +file + + + + +2010-10-28T04:32:40.000000Z +d65ad3d488d4a06addcbfa4aaf958f10 +2009-04-13T03:04:08.898183Z +11018 +mike + +BodyMaterial.as +file + + + + +2010-10-28T04:32:40.000000Z +49c7a55b5b26fdb03a60f24bac460f53 +2009-04-07T08:34:06.342994Z +10587 +mike + diff --git a/0.0.2.0/src/alternativa/physics/rigid/.svn/format b/0.0.2.0/src/alternativa/physics/rigid/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/rigid/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.2.0/src/alternativa/physics/rigid/.svn/text-base/Body.as.svn-base b/0.0.2.0/src/alternativa/physics/rigid/.svn/text-base/Body.as.svn-base new file mode 100644 index 0000000..d9594df --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/rigid/.svn/text-base/Body.as.svn-base @@ -0,0 +1,338 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Quaternion; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class Body { + + public static var linDamping:Number = 0.997; + public static var rotDamping:Number = 0.997; + + public var name:String; + public var movable:Boolean = true; + public var canFreeze:Boolean = false; + + public var freezeCounter:int; + public var frozen:Boolean = false; + public var aabb:BoundBox = new BoundBox(); + + altphysics var id:int; + // Мир, в котором находится тело + altphysics var world:RigidWorld; + // Текущее и предыдущее состояние тела. Промежуточное состояние вычисляется линейной интерполяцией. + altphysics var state:BodyState = new BodyState(); + altphysics var prevState:BodyState = new BodyState(); + // Линейное и угловое ускорение тела на текущем шаге симуляции + altphysics var accel:Vector3 = new Vector3(); + altphysics var angleAccel:Vector3 = new Vector3(); + // Материал тела + altphysics var material:BodyMaterial = new BodyMaterial(); + + altphysics var invMass:Number = 1; + altphysics var invInertia:Matrix3 = new Matrix3(); + altphysics var invInertiaWorld:Matrix3 = new Matrix3(); + altphysics var baseMatrix:Matrix3 = new Matrix3(); + + altphysics const MAX_CONTACTS:int = 20; + altphysics var contacts:Vector. = new Vector.(MAX_CONTACTS); + altphysics var contactsNum:int; + + altphysics var collisionPrimitives:Vector.; + altphysics var collisionPrimitivesNum:int; + + // Аккумулятор сил + altphysics var forceAccum:Vector3 = new Vector3(); + // Аккумулятор моментов + altphysics var torqueAccum:Vector3 = new Vector3(); + + // Внутренние переменные для избежания создания экземпляров + private static var _r:Vector3 = new Vector3(); + private static var _f:Vector3 = new Vector3(); + + /** + * + * @param invMass + * @param invInertia + */ + public function Body(invMass:Number, invInertia:Matrix3) { + this.invMass = invMass; + this.invInertia.copy(invInertia); + } + + /** + * @param primitive + * @param localTransform + */ + public function addCollisionPrimitive(primitive:CollisionPrimitive, localTransform:Matrix4 = null):void { + if (primitive == null) throw new ArgumentError("Primitive cannot be null"); + if (collisionPrimitives == null) collisionPrimitives = new Vector.(); + collisionPrimitives[collisionPrimitivesNum++] = primitive; + primitive.setBody(this, localTransform); + } + + /** + * @param t + * @param result + */ + public function interpolate(t:Number, pos:Vector3, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.w = prevState.orientation.w*t1 + state.orientation.w*t; + orientation.x = prevState.orientation.x*t1 + state.orientation.x*t; + orientation.y = prevState.orientation.y*t1 + state.orientation.y*t; + orientation.z = prevState.orientation.z*t1 + state.orientation.z*t; + } + + /** + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + state.pos.vCopy(pos); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setPositionXYZ(x:Number, y:Number, z:Number):void { + state.pos.vReset(x, y, z); + } + + /** + * + * @param vel + */ + public function setVelocity(vel:Vector3):void { + state.velocity.vCopy(vel); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setVelocityXYZ(x:Number, y:Number, z:Number):void { + state.velocity.vReset(x, y, z); + } + + /** + * + * @param rot + */ + public function setRotation(rot:Vector3):void { + state.rotation.vCopy(rot); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setRotationXYZ(x:Number, y:Number, z:Number):void { + state.rotation.vReset(x, y, z); + } + + /** + * + * @param q + */ + public function setOrientation(q:Quaternion):void { + state.orientation.copy(q); + } + + /** + * @param r + * @param dir + * @param magnitude + */ + public function applyRelPosWorldImpulse(r:Vector3, dir:Vector3, magnitude:Number):void { + var d:Number = magnitude*invMass; + // Линейная часть + state.velocity.x += d*dir.x; + state.velocity.y += d*dir.y; + state.velocity.z += d*dir.z; + + // Вращательная часть + var rx:Number = r.x; + var ry:Number = r.y; + var rz:Number = r.z; + _r.x = (ry*dir.z - rz*dir.y)*magnitude; + _r.y = (rz*dir.x - rx*dir.z)*magnitude; + _r.z = (rx*dir.y - ry*dir.x)*magnitude; + _r.vTransformBy3(invInertiaWorld); + state.rotation.x += _r.x; + state.rotation.y += _r.y; + state.rotation.z += _r.z; + } + + /** + * @param f + */ + public function addForce(f:Vector3):void { + forceAccum.vAdd(f); + } + + /** + * @param pos + * @param f + */ + public function addWorldForce(pos:Vector3, force:Vector3):void { + forceAccum.vAdd(force); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(force)); + } + + /** + * @param pos + * @param f + */ + public function addWorldForceScaled(pos:Vector3, force:Vector3, scale:Number):void { + _f.x = scale*force.x, _f.y = scale*force.y, _f.z = scale*force.z; + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(_f)); + } + + /** + * @param pos + * @param f + */ + public function addLocalForce(pos:Vector3, force:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(pos, _r); + // Трансформируем вектор силы в мировую систему + baseMatrix.transformVector(force, _f); + // Добавляем силу и момент + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vCross(_f)); + } + + /** + * + * @param localPos + * @param worldForce + */ + public function addWorldForceAtLocalPoint(localPos:Vector3, worldForce:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(localPos, _r); + // Добавляем силу и момент + forceAccum.vAdd(worldForce); + torqueAccum.vAdd(_r.vCross(worldForce)); + } + + /** + * @param dt + */ + public function addExternalForces(dt:Number):void { + } + + /** + * @param t + */ + public function addTorque(t:Vector3):void { + torqueAccum.vAdd(t); + } + + /** + * + */ + altphysics function clearAccumulators():void { + forceAccum.vReset(); + torqueAccum.vReset(); + } + + /** + * + */ + altphysics function calcAccelerations():void { + accel.x = forceAccum.x*invMass; + accel.y = forceAccum.y*invMass; + accel.z = forceAccum.z*invMass; + invInertiaWorld.transformVector(torqueAccum, angleAccel); + } + + /** + * Вычисляет производные данные. + */ + altphysics function calcDerivedData():void { + // Вычисление базисной матрицы и обратного тензора инерции в мировых координатах + state.orientation.toMatrix3(baseMatrix); + invInertiaWorld.copy(invInertia).append(baseMatrix).prependTransposed(baseMatrix); + if (collisionPrimitives != null) { + aabb.infinity(); + for (var i:int = 0; i < collisionPrimitivesNum; i++) { + var primitive:CollisionPrimitive = collisionPrimitives[i]; + primitive.transform.setFromMatrix3(baseMatrix, state.pos); + if (primitive.localTransform != null) primitive.transform.prepend(primitive.localTransform); + primitive.calculateAABB(); + aabb.addBoundBox(primitive.aabb); + } + } + } + + /** + * + */ + altphysics function saveState():void { + prevState.copy(state); + } + + /** + * + */ + altphysics function restoreState():void { + state.copy(prevState); + } + + /** + * @param dt + */ + altphysics function integrateVelocity(dt:Number):void { + // v = v + a*t + state.velocity.x += accel.x*dt; + state.velocity.y += accel.y*dt; + state.velocity.z += accel.z*dt; + // rot = rot + eps*t + state.rotation.x += angleAccel.x*dt; + state.rotation.y += angleAccel.y*dt; + state.rotation.z += angleAccel.z*dt; + + state.velocity.x *= linDamping; + state.velocity.y *= linDamping; + state.velocity.z *= linDamping; + + state.rotation.x *= rotDamping; + state.rotation.y *= rotDamping; + state.rotation.z *= rotDamping; + } + + /** + * + */ + altphysics function integratePosition(dt:Number):void { + // pos = pos + v*t + state.pos.x += state.velocity.x*dt; + state.pos.y += state.velocity.y*dt; + state.pos.z += state.velocity.z*dt; + // q = q + 0.5*rot*q + state.orientation.addScaledVector(state.rotation, dt); + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/rigid/.svn/text-base/BodyMaterial.as.svn-base b/0.0.2.0/src/alternativa/physics/rigid/.svn/text-base/BodyMaterial.as.svn-base new file mode 100644 index 0000000..61d4382 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/rigid/.svn/text-base/BodyMaterial.as.svn-base @@ -0,0 +1,11 @@ +package alternativa.physics.rigid { + + public class BodyMaterial { + + public var restitution:Number = 0; + public var friction:Number = 0.3; +// public var dynamicFriction:Number = 0.2; +// public var dynamicFriction:Number = 0.2; + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/rigid/.svn/text-base/BodyState.as.svn-base b/0.0.2.0/src/alternativa/physics/rigid/.svn/text-base/BodyState.as.svn-base new file mode 100644 index 0000000..408b0ea --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/rigid/.svn/text-base/BodyState.as.svn-base @@ -0,0 +1,39 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Quaternion; + import alternativa.physics.types.Vector3; + + /** + * Класс описывает состояние твёрдого тела. + */ + public class BodyState { + /** + * Положение тела. + */ + public var pos:Vector3 = new Vector3(); + /** + * Ориентация тела. + */ + public var orientation:Quaternion = new Quaternion(); + /** + * Скорость тела. + */ + public var velocity:Vector3 = new Vector3(); + /** + * Угловая скорость тела. + */ + public var rotation:Vector3 = new Vector3(); + + /** + * Копирует значение указанного объекта. + * + * @param state + */ + public function copy(state:BodyState):void { + pos.vCopy(state.pos); + orientation.copy(state.orientation); + velocity.vCopy(state.velocity); + rotation.vCopy(state.rotation); + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/rigid/.svn/text-base/Contact.as.svn-base b/0.0.2.0/src/alternativa/physics/rigid/.svn/text-base/Contact.as.svn-base new file mode 100644 index 0000000..b73fc55 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/rigid/.svn/text-base/Contact.as.svn-base @@ -0,0 +1,53 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.types.Vector3; + + /** + * Информация о контакте между двумя телами. Содержит нормаль, множество точек контакта и прочие величины. + */ + public class Contact { + +// private static var pool:Vector. = new Vector.(); +// +// public static function create():ContactInfo { +// if (pool.length > 0) { +// return pool.pop(); +// } +// return new ContactInfo(); +// } +// +// public static function destroy(collInfo:ContactInfo):void { +// collInfo.body1 = collInfo.body2 = null; +// pool.push(collInfo); +// } + + // Максимальное количество точек контакта + private const N:int = 8; + + public var body1:Body; + public var body2:Body; + // Взаимный коэффициент отскока + public var restitution:Number; + // Взаимный коэффициент трения + public var friction:Number; + // Нормаль контакта. Направлена от второго тела к первому. + public var normal:Vector3 = new Vector3(); + // Список точек контакта + public var points:Vector. = new Vector.(N, true); + // Количество точек контакта + public var pcount:int; + // Максимальная глубина пересечения тел + public var maxPenetration:Number = 0; + + public var satisfied:Boolean; + + /** + * + */ + public function Contact() { + for (var i:int = 0; i < N; i++) points[i] = new ContactPoint(); + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/rigid/.svn/text-base/ContactPoint.as.svn-base b/0.0.2.0/src/alternativa/physics/rigid/.svn/text-base/ContactPoint.as.svn-base new file mode 100644 index 0000000..c36f958 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/rigid/.svn/text-base/ContactPoint.as.svn-base @@ -0,0 +1,52 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Vector3; + + public class ContactPoint { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + + // Величины, расчитываемые перед началом фазы решения контактов + + // Требуемая проекция конечной скорости на нормаль для упругого контакта + public var normalVel:Number; + // Минимальная скорость разделения неупругого контакта + public var minSepVel:Number; + // Изменение проекции скорости на единицу нормального импульса + public var velByUnitImpulseN:Number; + + public var angularInertia1:Number; + public var angularInertia2:Number; + + // Радиус-вектор точки контакта относительно центра первого тела + public var r1:Vector3 = new Vector3(); + // Радиус-вектор точки контакта относительно центра второго тела + public var r2:Vector3 = new Vector3(); + + // Величины, накапливаемые во время фазы решения контактов + + // Накопленный импульс, применяемый для получения требуемой относительной скорости в точке контакта + public var accumImpulseN:Number; + // Накопленный импульс, применяемый для разделения тел в точке контакта. Не создаёт момента. +// public var accumSepImpulse:Number; + + public var satisfied:Boolean; + + /** + * + * @param cp + */ + public function copyFrom(cp:ContactPoint):void { + pos.vCopy(cp.pos); + penetration = cp.penetration; + feature1 = cp.feature1; + feature2 = cp.feature2; + r1.vCopy(cp.r1); + r2.vCopy(cp.r2); + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/rigid/.svn/text-base/PhysicsUtils.as.svn-base b/0.0.2.0/src/alternativa/physics/rigid/.svn/text-base/PhysicsUtils.as.svn-base new file mode 100644 index 0000000..a9aaa99 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/rigid/.svn/text-base/PhysicsUtils.as.svn-base @@ -0,0 +1,49 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + public class PhysicsUtils { + + /** + * @param mass + * @param halfSize + * @param result + */ + public static function getBoxInvInertia(mass:Number, halfSize:Vector3, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + result.a = 3/(mass*(yy + zz)); + result.f = 3/(mass*(zz + xx)); + result.k = 3/(mass*(xx + yy)); + } + + /** + * @param mass + * @param r + * @param h + * @param result + */ + public static function getCylinderInvInertia(mass:Number, r:Number, h:Number, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + + result.a = result.f = 1/(mass*(h*h/12 + r*r/4)); + result.k = 2/(mass*r*r); + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/rigid/.svn/text-base/RigidWorld.as.svn-base b/0.0.2.0/src/alternativa/physics/rigid/.svn/text-base/RigidWorld.as.svn-base new file mode 100644 index 0000000..f6de141 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/rigid/.svn/text-base/RigidWorld.as.svn-base @@ -0,0 +1,656 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.ICollisionDetector; + import alternativa.physics.collision.KdTreeCollisionDetector; + import alternativa.physics.rigid.constraints.Constraint; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Класс реализует физическую симуляцию поведения твёрдых тел. + */ + public class RigidWorld { + + private static var lastBodyId:int; + + // Максимальное количество контактов + altphysics const MAX_CONTACTS:int = 1000; + + ////////////////////////////////////////////////////////////////// + // Настроечные параметры симуляции. Могут быть изменены в любой + // момент времени без нарушения корректной работы симуляции. + ////////////////////////////////////////////////////////////////// + // Количество шагов, за которое пересекающиеся тела должны разделиться + public var penResolutionSteps:int = 10; + // Величина допустимой глубины пересечения + public var allowedPenetration:Number = 0.1; + // Максимальная скорость, добавляемая с целью разделения тел + public var maxPenResolutionSpeed:Number = 0.5; + // Количество итераций для обработки упругих контактов + public var collisionIterations:int = 5; + // Количество итераций для обработки неупругих контактов + public var contactIterations:int = 5; + // Флаг использования предсказания состояний + public var usePrediction:Boolean = false; + + public var freezeSteps:int = 10; + public var linSpeedFreezeLimit:Number = 1; + public var angSpeedFreezeLimit:Number = 0.01; + + // Переменные для процедуры разделения тел путём непосредственного их перемещенеия. + // !!!!!!!!!!!!!!!!!!!!!!!! + // !!! Экспериментально !!! + // !!!!!!!!!!!!!!!!!!!!!!!! + public var staticSeparationIterations:int = 10; + public var staticSeparationSteps:int = 10; + public var maxAngleMove:Number = 10; + public var useStaticSeparation:Boolean = false; + + // Вектор гравитации + altphysics var _gravity:Vector3 = new Vector3(0, 0, -9.8); + // Использующийся детектор столкновений + altphysics var collisionDetector:ICollisionDetector; + // Список тел, участвующих в симуляции + altphysics var bodies:Vector. = new Vector.(); + // Количество тел, участвующих в симуляции + altphysics var bodiesNum:int; + // Список контактов на текущем шаге симуляции + altphysics var contacts:Vector. = new Vector.(MAX_CONTACTS, true); + // Количество контактов на текущем шаге симуляции + altphysics var contactsNum:int; + // Список ограничений + altphysics var constraints:Vector. = new Vector.(); + // Количество ограничений + altphysics var constraintsNum:int; + // Временная метка. Число прошедших шагов с начала симуляции. + altphysics var timeStamp:uint; + + // Временные переменные для избежания создания экземпляров + private var _r:Vector3 = new Vector3(); + private var _t:Vector3 = new Vector3(); + private var _v:Vector3 = new Vector3(); + private var _v1:Vector3 = new Vector3(); + private var _v2:Vector3 = new Vector3(); + + /** + * + */ + public function RigidWorld() { + for (var i:int = 0; i < MAX_CONTACTS; i++) contacts[i] = new Contact(); + collisionDetector = new KdTreeCollisionDetector(); + } + + /** + * Вектор гравитации. + */ + public function get gravity():Vector3 { + return _gravity.vClone(); + } + + /** + * @private + */ + public function set gravity(value:Vector3):void { + _gravity.vCopy(value); + } + + /** + * Добавляет тело в симуляцию. + * @param body + * @return + */ + public function addBody(body:Body, isStatic:Boolean):Boolean { + var i:int = bodies.indexOf(body); + if (i > -1) return false; + bodies[bodiesNum++] = body; + body.world = this; + body.id = lastBodyId++; +// if (body.collisionPrimitives != null) { +// for (i = 0; i < body.collisionPrimitivesNum; i++) collisionDetector.addPrimitive(body.collisionPrimitives[i], isStatic); +// } + return true; + } + + /** + * Удаляет тело из симуляции. + * @param body + * @return + */ + public function removeBody(body:Body, isStatic:Boolean):Boolean { + var i:int = bodies.indexOf(body); + if (i == -1) return false; + bodies.splice(i, 1); + body.world = null; +// if (body.collisionPrimitives != null) { +// for (i = 0; i < body.collisionPrimitivesNum; i++) collisionDetector.removePrimitive(body.collisionPrimitives[i], isStatic); +// } + bodiesNum--; + return true; + } + + /** + * Добавляет ограничение. + * @param c + */ + public function addConstraint(c:Constraint):void { + constraints[constraintsNum++] = c; + c.world = this; + } + + /** + * Удаляет ограничение. + * @param c + */ + public function removeConstraint(c:Constraint):Boolean { + var idx:int = constraints.indexOf(c); + if (idx < 0) return false; + constraints.splice(idx, 1) + constraintsNum--; + c.world = null; + return true; + } + + /** + * Применяет к телам действующте на них силы. Аккумуляторы сил и моментов тел очищаются после завершения шага + * симуляции, поэтому на момент вызова метода могут уже содержать некоторые значения. + * + * @param dt промежуток времени, в течении которого действуют силы + */ + private function applyForces(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i]; + body.addExternalForces(dt); + body.calcAccelerations(); + // Ускорение свободного падения применяется только к подвижным телам во избежание некорректного изменения + // фиктивной скорости неподвижных тел. + if (body.movable && !body.frozen) { + body.accel.x += _gravity.x; + body.accel.y += _gravity.y; + body.accel.z += _gravity.z; + } + } + } + + /** + * Определяет все столкновения на текущем шаге симуляции и заполняет список получившихся контактов. + * + * @param dt длительность шага симуляции + */ + private function detectCollisions(dt:Number):void { + var i:int; + var j:int; + var body:Body; + + for (i = 0; i < bodiesNum; i++) { + body = bodies[i]; + if (body.frozen) continue; + body.contactsNum = 0; + body.saveState(); + // При включённом режиме предсказания состояние тел интегрируется на один шаг вперёд + if (usePrediction) { + body.integrateVelocity(dt); + body.integratePosition(dt); + } + body.calcDerivedData(); + } + + contactsNum = collisionDetector.getAllCollisions(contacts); + + // Расчёт относительных векторов точки контакта вынесен сюда из-за необходимости учитывать + // положение тел в предсказанном состоянии + for (i = 0; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + var b1:Body = contact.body1; + var b2:Body = contact.body2; + for (j = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.r1.x = cp.pos.x - b1.state.pos.x; + cp.r1.y = cp.pos.y - b1.state.pos.y; + cp.r1.z = cp.pos.z - b1.state.pos.z; + if (b2 != null) { + cp.r2.x = cp.pos.x - b2.state.pos.x; + cp.r2.y = cp.pos.y - b2.state.pos.y; + cp.r2.z = cp.pos.z - b2.state.pos.z; + } + } + } + + // Восстановление состояния тел + if (usePrediction) { + for (i = 0; i < bodiesNum; i++) { + body = bodies[i]; + if (body.frozen) continue; + body.restoreState(); + body.calcDerivedData(); + } + } + } + + /** + * Подготваливает полученные из детектора столкновений контакты, расчитывая значения, не меняющиеся + * в ходе шага симуляции. + */ + private function preProcessContacts(dt:Number):void { + var i:int; + for (i = 0; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + var b1:Body = contact.body1; + var b2:Body = contact.body2; + // Столкнувшиеся тела размораживаются + if (b1.frozen) { + b1.frozen = false; + b1.freezeCounter = 0; + } + if (b2 != null && b2.frozen) { + b2.frozen = false; + b2.freezeCounter = 0; + } + contact.restitution = b1.material.restitution; + if (b2 != null && b2.material.restitution < contact.restitution) contact.restitution = b2.material.restitution; + contact.friction = b1.material.friction; + if (b2 != null && b2.material.friction < contact.friction) contact.friction = b2.material.friction; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.accumImpulseN = 0; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + cp.velByUnitImpulseN = 0; + if (b1.movable) { + cp.angularInertia1 = _v.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1).vDot(contact.normal); + cp.velByUnitImpulseN += b1.invMass + cp.angularInertia1; + } + if (b2 != null && b2.movable) { + cp.angularInertia2 = _v.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2).vDot(contact.normal); + cp.velByUnitImpulseN += b2.invMass + cp.angularInertia2; + } + // Расчёт требуемой конечной скорости для упругого контакта + calcSepVelocity(b1, b2, cp, _v); + cp.normalVel = _v.vDot(contact.normal); + if (cp.normalVel < 0) cp.normalVel = - contact.restitution*cp.normalVel; + // Скорость разделения неупругого контакта + cp.minSepVel = cp.penetration > allowedPenetration ? (cp.penetration - allowedPenetration)/(penResolutionSteps*dt) : 0; + if (cp.minSepVel > maxPenResolutionSpeed) cp.minSepVel = maxPenResolutionSpeed; + } + } + for (i = 0; i < constraintsNum; i++) Constraint(constraints[i]).preProcess(dt); + } + + /** + * + * @param dt + * @param forceInelastic + */ + private function processContacts(dt:Number, forceInelastic:Boolean):void { + var iterNum:int = forceInelastic ? contactIterations : collisionIterations; + var i:int; + var forwardLoop:Boolean = false; + for (var iter:int = 0; iter < iterNum; iter++) { + forwardLoop = !forwardLoop; + for (i = 0; i < contactsNum; i++) resolveContact(contacts[i], forceInelastic, forwardLoop); + // Ограничения + for (i = 0; i < constraintsNum; i++) Constraint(constraints[i]).apply(dt); + } + + // Разделение контактов путём непосредственного изменения координат и ориентации. Экспериментально. +// if (forceInelastic && useStaticSeparation) performStaticSeparation(); + } + + /** + * + */ + private function resolveContact(contactInfo:Contact, forceInelastic:Boolean, forwardLoop:Boolean):void { + var b1:Body = contactInfo.body1; + var b2:Body = contactInfo.body2; + var normal:Vector3 = contactInfo.normal; + var i:int; + if (forwardLoop) { + for (i = 0; i < contactInfo.pcount; i++) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } else { + for (i = contactInfo.pcount - 1; i >= 0; i--) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } + } + + /** + * + * @param idx + * @param b1 + * @param b2 + * @param colInfo + * @param normal + * @param forceInelastic + */ + private function resolveContactPoint(idx:int, b1:Body, b2:Body, contact:Contact, normal:Vector3, forceInelastic:Boolean):void { + var cp:ContactPoint = contact.points[idx]; + if (!forceInelastic) cp.satisfied = true; + + var newVel:Number = 0; + calcSepVelocity(b1, b2, cp, _v); + var sepVel:Number = _v.vDot(contact.normal); + if (forceInelastic) { + var minSpeVel:Number = useStaticSeparation ? 0 : cp.minSepVel; + if (sepVel < minSpeVel) cp.satisfied = false; + else if (cp.satisfied) return; + newVel = minSpeVel; + } else { + newVel = cp.normalVel; + } + var deltaVel:Number = newVel - sepVel; + var impulse:Number = deltaVel/cp.velByUnitImpulseN; + var accumImpulse:Number = cp.accumImpulseN + impulse; + if (accumImpulse < 0) accumImpulse = 0; + var deltaImpulse:Number = accumImpulse - cp.accumImpulseN; + cp.accumImpulseN = accumImpulse; + // Применяем импульс к телам + if (b1.movable) b1.applyRelPosWorldImpulse(cp.r1, normal, deltaImpulse); + if (b2 != null && b2.movable) b2.applyRelPosWorldImpulse(cp.r2, normal, -deltaImpulse); + + // Учёт силы трения + calcSepVelocity(b1, b2, cp, _v); + // Расчитываем изменение касательной скорости на единицу касательного импульса + var tanSpeedByUnitImpulse:Number = 0; + _v.vAddScaled(-_v.vDot(contact.normal), contact.normal); + var tanSpeed:Number = _v.vLength(); + if (tanSpeed < 0.001) return; + _t.vCopy(_v).vNormalize().vReverse(); + // dV = b.invMass + ((invI * (r % t)) % r) * t + if (b1.movable) { + _v.vCross2(cp.r1, _t).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1); + tanSpeedByUnitImpulse += b1.invMass + _v.vDot(_t); + } + if (b2 != null && b2.movable) { + _v.vCross2(cp.r2, _t).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2); + tanSpeedByUnitImpulse += b2.invMass + _v.vDot(_t); + } + + var tanImpulse:Number = tanSpeed/tanSpeedByUnitImpulse; + var max:Number = contact.friction*cp.accumImpulseN; + if (max < 0) { + if (tanImpulse < max) tanImpulse = max; + } else { + if (tanImpulse > max) tanImpulse = max; + } + + // Применяем импульс к телам + if (b1.movable) b1.applyRelPosWorldImpulse(cp.r1, _t, tanImpulse); + if (b2 != null && b2.movable) b2.applyRelPosWorldImpulse(cp.r2, _t, -tanImpulse); + } + + /** + * + * @param cp + * @param normal + * @return + */ + private function calcSepVelocity(body1:Body, body2:Body, cp:ContactPoint, result:Vector3):void { + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 +// result.vCopy(body1.state.velocity).vAdd(_v1.vCross2(body1.state.rotation, cp.r1)); + var rot:Vector3 = body1.state.rotation; + var v:Vector3 = cp.r1; + var x:Number = rot.y*v.z - rot.z*v.y; + var y:Number = rot.z*v.x - rot.x*v.z; + var z:Number = rot.x*v.y - rot.y*v.x; + v = body1.state.velocity; + result.x = v.x + x; + result.y = v.y + y; + result.z = v.z + z; + // V2 = V2_c + w2%r2 + if (body2 != null) { +// result.vSubtract(body2.state.velocity).vSubtract(_v2.vCross2(body2.state.rotation, cp.r2)); + rot = body2.state.rotation; + v = cp.r2; + x = rot.y*v.z - rot.z*v.y; + y = rot.z*v.x - rot.x*v.z; + z = rot.x*v.y - rot.y*v.x; + v = body2.state.velocity; + result.x -= v.x + x; + result.y -= v.y + y; + result.z -= v.z + z; + } + } + + /** + * + * @param dt + */ + private function intergateVelocities(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + (bodies[i] as Body).integrateVelocity(dt); + } + } + + /** + * + * @param dt + */ + private function integratePositions(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i] as Body; + if (body.movable && !body.frozen) body.integratePosition(dt); + } + } + + /** + * + */ + private function performStaticSeparation():void { + var iterNum:int = staticSeparationIterations; +// iterNum = 100; + + // 1. В начале каждой итерации для всех контактов сбрасывается флаг satisfied с одновременным поиском наихудшего контакта, т.е. имеющего наибольшую величину пересечения тел. + // 2. Если найденный контакт имеет величину пересечения меньше предельно допустимой процедура прерывается, т.к. разделение контактов не требуется. + // 3. Если процедура продолжается, то выполняется разделение найденного контакта и установка его флага satisfied в true, после чего запускается внутренний цикл, состоящий + // из contactsNum - 1 итераций. На каждой итерации ищется наихудший контакт среди оставшихся (satisfied == false). Если такой контакт найден, то выполняется его разделение + // и переход к следующей итерации внутреннего цикла, иначе внутренний цикл прерывается и выполняется переход к следующей итерации внешнего цикла (шаг 1.). + for (var iter:int = 0; iter < iterNum; iter++) { + // Ищем контакт с максимальной величиной пересечения тел, одновременно сбрасывая флаги satisfied в false + var worstContact:Contact = contacts[0]; + var i:int; + for (i = 1; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + contact.satisfied = false; + if (contact.maxPenetration > worstContact.maxPenetration) worstContact = contact; + } + if (worstContact.maxPenetration <= allowedPenetration) return; + + resolveInterpenetration(worstContact); + // Внутренний цикл по оставшимся контактам + for (i = 1; i < contactsNum; i++) { + worstContact = getWorstContact(); + if (worstContact == null) break; + resolveInterpenetration(worstContact); + } + } + } + + /** + * + */ + private function getWorstContact():Contact { + var maxPen:Number = 0; + var worst:Contact = null; + for (var i:int = 0; i < contactsNum; i++) { + var c:Contact = contacts[i]; + if (!c.satisfied && c.maxPenetration > maxPen) { + worst = c; + maxPen = c.maxPenetration; + } + } + return maxPen > allowedPenetration ? worst : null; + } + + /** + * Разделяет указанный контакт, выполняя staticSeparationSteps итераций по списку точек контакта. + * 1. В начале каждой итерации ищется точка с наибольшим пересечением, одновременно сбрасываются флаги satisfied у точек. + * 2. Для найденной точки выполняется процедура разделения. + * 3. После выполняется pcount - 1 итерация по списку точек, каждый раз ищется наихудшая среди имеющих satisfied == false, для которой выполняется процедура разделения. + * Если наихудшая точка не найдена, то выполнется переход к следующей итерации внешнего цикла (шаг 1). + * + * В конце процедуры разделения обновляются значения пересечений для остальных точек контакта, а также для всех прочих контактов, относящихся к телам текущего. + * + * @param contact контакт для разделения + */ + private function resolveInterpenetration(contact:Contact):void { + contact.satisfied = true; + + for (var step:int = 0; step < staticSeparationSteps; step++) { + var worstCp:ContactPoint = contact.points[0]; + var cp:ContactPoint; + var i:int; + for (i = 1; i < contact.pcount; i++) { + cp = contact.points[i]; + cp.satisfied = false; + if (cp.penetration > worstCp.penetration) worstCp = cp; + } + if (worstCp.penetration <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + // Разделяем оставшиеся точки + var maxPen:Number = 0; + for (i = 1; i < contact.pcount; i++) { + // Поиск наихудшей точки + for (var j:int = 0; j < contact.pcount; j++) { + cp = contact.points[j]; + if (cp.satisfied) continue; + if (cp.penetration > maxPen) { + maxPen = cp.penetration; + worstCp = cp; + } + } + if (maxPen <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + } + } + } + + /** + * + * @param cp + * @param contact + */ + private function separateContactPoint(cp:ContactPoint, contact:Contact):void { + cp.satisfied = true; + + var b1:Body = contact.body1; + var b2:Body = contact.body2; + var totalMove:Number = cp.penetration - allowedPenetration; + var moveCoeff:Number = totalMove/cp.velByUnitImpulseN; + var linMove1:Number; + var angleMove1:Number; + if (b1.movable) { + linMove1 = b1.invMass*moveCoeff; + angleMove1 = cp.angularInertia1*moveCoeff; + if (angleMove1 > maxAngleMove) { + linMove1 += angleMove1 - maxAngleMove; + angleMove1 -= maxAngleMove; + } + b1.state.pos.vAddScaled(linMove1, contact.normal); + _v1.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vScale(angleMove1); + b1.state.orientation.addScaledVector(_v1, 1); + } + var linMove2:Number; + var angleMove2:Number; + if (b2 != null && b2.movable) { + linMove2 = b2.invMass*moveCoeff; + angleMove2 = cp.angularInertia2*moveCoeff; + if (angleMove2 > maxAngleMove) { + linMove2 += angleMove2 - maxAngleMove; + angleMove2 -= maxAngleMove; + } + b2.state.pos.vAddScaled(-linMove2, contact.normal); + _v2.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vScale(angleMove2); + _v2.vReverse(); + b2.state.orientation.addScaledVector(_v2, 1); + } + cp.penetration = allowedPenetration; + // Обновляем пересечения в других точках + var i:int; + for (i = 0; i < contact.pcount; i++) { + var cp1:ContactPoint = contact.points[i]; + if (cp1 == cp) continue; + var angularMove:Number; + if (b1.movable) { + angularMove = _v.vCross2(_v1, cp1.r1).vDot(contact.normal); + cp1.penetration -= linMove1 + angularMove; + } + if (b2 != null && b2.movable) { + angularMove = _v.vCross2(_v2, cp1.r2).vDot(contact.normal); + cp1.penetration -= linMove2 - angularMove; + } + // Обновление максимального значения для контакта + if (cp1.penetration > contact.maxPenetration) contact.maxPenetration = cp1.penetration; + } + // Обновляем пересечения для других контактов + var c:Contact; + var j:int; + if (b1.movable) { + for (i = 0; i < b1.contactsNum; i++) { + c = b1.contacts[i]; + if (c == contact) continue; + for (j = 0; j < c.pcount; j++) { + cp1 = c.points[j]; + if (b1 == c.body1) cp1.penetration -= linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r1).vDot(c.normal); + else cp1.penetration += linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r2).vDot(c.normal); + if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; + } + } + } + if (b2 != null && b2.movable) { + for (i = 0; i < b2.contactsNum; i++) { + c = b2.contacts[i]; + if (c == contact) continue; + for (j = 0; j < c.pcount; j++) { + cp1 = c.points[j]; + if (b2 == c.body1) cp1.penetration -= linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r1).vDot(c.normal); + else cp1.penetration += linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r2).vDot(c.normal); + if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; + } + } + } + } + + /** + * + */ + private function postPhysics():void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i]; + body.clearAccumulators(); + body.calcDerivedData(); + if (body.canFreeze) { + if (body.state.velocity.vLength() < linSpeedFreezeLimit && body.state.rotation.vLength() < angSpeedFreezeLimit) { + if (!body.frozen) { + body.freezeCounter++; + if (body.freezeCounter >= freezeSteps) { + body.frozen = true; + trace("FROZEN"); + } + } + } else { + body.freezeCounter = 0; + body.frozen = false; + } + } + } + } + + /** + * + * @param dt + */ + public function runPhysics(dt:Number):void { + timeStamp++; + applyForces(dt); + detectCollisions(dt); + preProcessContacts(dt); + processContacts(dt, false); + intergateVelocities(dt); + processContacts(dt, true); + integratePositions(dt); + postPhysics(); + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/rigid/Body.as b/0.0.2.0/src/alternativa/physics/rigid/Body.as new file mode 100644 index 0000000..d9594df --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/rigid/Body.as @@ -0,0 +1,338 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Quaternion; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class Body { + + public static var linDamping:Number = 0.997; + public static var rotDamping:Number = 0.997; + + public var name:String; + public var movable:Boolean = true; + public var canFreeze:Boolean = false; + + public var freezeCounter:int; + public var frozen:Boolean = false; + public var aabb:BoundBox = new BoundBox(); + + altphysics var id:int; + // Мир, в котором находится тело + altphysics var world:RigidWorld; + // Текущее и предыдущее состояние тела. Промежуточное состояние вычисляется линейной интерполяцией. + altphysics var state:BodyState = new BodyState(); + altphysics var prevState:BodyState = new BodyState(); + // Линейное и угловое ускорение тела на текущем шаге симуляции + altphysics var accel:Vector3 = new Vector3(); + altphysics var angleAccel:Vector3 = new Vector3(); + // Материал тела + altphysics var material:BodyMaterial = new BodyMaterial(); + + altphysics var invMass:Number = 1; + altphysics var invInertia:Matrix3 = new Matrix3(); + altphysics var invInertiaWorld:Matrix3 = new Matrix3(); + altphysics var baseMatrix:Matrix3 = new Matrix3(); + + altphysics const MAX_CONTACTS:int = 20; + altphysics var contacts:Vector. = new Vector.(MAX_CONTACTS); + altphysics var contactsNum:int; + + altphysics var collisionPrimitives:Vector.; + altphysics var collisionPrimitivesNum:int; + + // Аккумулятор сил + altphysics var forceAccum:Vector3 = new Vector3(); + // Аккумулятор моментов + altphysics var torqueAccum:Vector3 = new Vector3(); + + // Внутренние переменные для избежания создания экземпляров + private static var _r:Vector3 = new Vector3(); + private static var _f:Vector3 = new Vector3(); + + /** + * + * @param invMass + * @param invInertia + */ + public function Body(invMass:Number, invInertia:Matrix3) { + this.invMass = invMass; + this.invInertia.copy(invInertia); + } + + /** + * @param primitive + * @param localTransform + */ + public function addCollisionPrimitive(primitive:CollisionPrimitive, localTransform:Matrix4 = null):void { + if (primitive == null) throw new ArgumentError("Primitive cannot be null"); + if (collisionPrimitives == null) collisionPrimitives = new Vector.(); + collisionPrimitives[collisionPrimitivesNum++] = primitive; + primitive.setBody(this, localTransform); + } + + /** + * @param t + * @param result + */ + public function interpolate(t:Number, pos:Vector3, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.w = prevState.orientation.w*t1 + state.orientation.w*t; + orientation.x = prevState.orientation.x*t1 + state.orientation.x*t; + orientation.y = prevState.orientation.y*t1 + state.orientation.y*t; + orientation.z = prevState.orientation.z*t1 + state.orientation.z*t; + } + + /** + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + state.pos.vCopy(pos); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setPositionXYZ(x:Number, y:Number, z:Number):void { + state.pos.vReset(x, y, z); + } + + /** + * + * @param vel + */ + public function setVelocity(vel:Vector3):void { + state.velocity.vCopy(vel); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setVelocityXYZ(x:Number, y:Number, z:Number):void { + state.velocity.vReset(x, y, z); + } + + /** + * + * @param rot + */ + public function setRotation(rot:Vector3):void { + state.rotation.vCopy(rot); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setRotationXYZ(x:Number, y:Number, z:Number):void { + state.rotation.vReset(x, y, z); + } + + /** + * + * @param q + */ + public function setOrientation(q:Quaternion):void { + state.orientation.copy(q); + } + + /** + * @param r + * @param dir + * @param magnitude + */ + public function applyRelPosWorldImpulse(r:Vector3, dir:Vector3, magnitude:Number):void { + var d:Number = magnitude*invMass; + // Линейная часть + state.velocity.x += d*dir.x; + state.velocity.y += d*dir.y; + state.velocity.z += d*dir.z; + + // Вращательная часть + var rx:Number = r.x; + var ry:Number = r.y; + var rz:Number = r.z; + _r.x = (ry*dir.z - rz*dir.y)*magnitude; + _r.y = (rz*dir.x - rx*dir.z)*magnitude; + _r.z = (rx*dir.y - ry*dir.x)*magnitude; + _r.vTransformBy3(invInertiaWorld); + state.rotation.x += _r.x; + state.rotation.y += _r.y; + state.rotation.z += _r.z; + } + + /** + * @param f + */ + public function addForce(f:Vector3):void { + forceAccum.vAdd(f); + } + + /** + * @param pos + * @param f + */ + public function addWorldForce(pos:Vector3, force:Vector3):void { + forceAccum.vAdd(force); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(force)); + } + + /** + * @param pos + * @param f + */ + public function addWorldForceScaled(pos:Vector3, force:Vector3, scale:Number):void { + _f.x = scale*force.x, _f.y = scale*force.y, _f.z = scale*force.z; + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(_f)); + } + + /** + * @param pos + * @param f + */ + public function addLocalForce(pos:Vector3, force:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(pos, _r); + // Трансформируем вектор силы в мировую систему + baseMatrix.transformVector(force, _f); + // Добавляем силу и момент + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vCross(_f)); + } + + /** + * + * @param localPos + * @param worldForce + */ + public function addWorldForceAtLocalPoint(localPos:Vector3, worldForce:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(localPos, _r); + // Добавляем силу и момент + forceAccum.vAdd(worldForce); + torqueAccum.vAdd(_r.vCross(worldForce)); + } + + /** + * @param dt + */ + public function addExternalForces(dt:Number):void { + } + + /** + * @param t + */ + public function addTorque(t:Vector3):void { + torqueAccum.vAdd(t); + } + + /** + * + */ + altphysics function clearAccumulators():void { + forceAccum.vReset(); + torqueAccum.vReset(); + } + + /** + * + */ + altphysics function calcAccelerations():void { + accel.x = forceAccum.x*invMass; + accel.y = forceAccum.y*invMass; + accel.z = forceAccum.z*invMass; + invInertiaWorld.transformVector(torqueAccum, angleAccel); + } + + /** + * Вычисляет производные данные. + */ + altphysics function calcDerivedData():void { + // Вычисление базисной матрицы и обратного тензора инерции в мировых координатах + state.orientation.toMatrix3(baseMatrix); + invInertiaWorld.copy(invInertia).append(baseMatrix).prependTransposed(baseMatrix); + if (collisionPrimitives != null) { + aabb.infinity(); + for (var i:int = 0; i < collisionPrimitivesNum; i++) { + var primitive:CollisionPrimitive = collisionPrimitives[i]; + primitive.transform.setFromMatrix3(baseMatrix, state.pos); + if (primitive.localTransform != null) primitive.transform.prepend(primitive.localTransform); + primitive.calculateAABB(); + aabb.addBoundBox(primitive.aabb); + } + } + } + + /** + * + */ + altphysics function saveState():void { + prevState.copy(state); + } + + /** + * + */ + altphysics function restoreState():void { + state.copy(prevState); + } + + /** + * @param dt + */ + altphysics function integrateVelocity(dt:Number):void { + // v = v + a*t + state.velocity.x += accel.x*dt; + state.velocity.y += accel.y*dt; + state.velocity.z += accel.z*dt; + // rot = rot + eps*t + state.rotation.x += angleAccel.x*dt; + state.rotation.y += angleAccel.y*dt; + state.rotation.z += angleAccel.z*dt; + + state.velocity.x *= linDamping; + state.velocity.y *= linDamping; + state.velocity.z *= linDamping; + + state.rotation.x *= rotDamping; + state.rotation.y *= rotDamping; + state.rotation.z *= rotDamping; + } + + /** + * + */ + altphysics function integratePosition(dt:Number):void { + // pos = pos + v*t + state.pos.x += state.velocity.x*dt; + state.pos.y += state.velocity.y*dt; + state.pos.z += state.velocity.z*dt; + // q = q + 0.5*rot*q + state.orientation.addScaledVector(state.rotation, dt); + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/rigid/BodyMaterial.as b/0.0.2.0/src/alternativa/physics/rigid/BodyMaterial.as new file mode 100644 index 0000000..61d4382 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/rigid/BodyMaterial.as @@ -0,0 +1,11 @@ +package alternativa.physics.rigid { + + public class BodyMaterial { + + public var restitution:Number = 0; + public var friction:Number = 0.3; +// public var dynamicFriction:Number = 0.2; +// public var dynamicFriction:Number = 0.2; + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/rigid/BodyState.as b/0.0.2.0/src/alternativa/physics/rigid/BodyState.as new file mode 100644 index 0000000..408b0ea --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/rigid/BodyState.as @@ -0,0 +1,39 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Quaternion; + import alternativa.physics.types.Vector3; + + /** + * Класс описывает состояние твёрдого тела. + */ + public class BodyState { + /** + * Положение тела. + */ + public var pos:Vector3 = new Vector3(); + /** + * Ориентация тела. + */ + public var orientation:Quaternion = new Quaternion(); + /** + * Скорость тела. + */ + public var velocity:Vector3 = new Vector3(); + /** + * Угловая скорость тела. + */ + public var rotation:Vector3 = new Vector3(); + + /** + * Копирует значение указанного объекта. + * + * @param state + */ + public function copy(state:BodyState):void { + pos.vCopy(state.pos); + orientation.copy(state.orientation); + velocity.vCopy(state.velocity); + rotation.vCopy(state.rotation); + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/rigid/Contact.as b/0.0.2.0/src/alternativa/physics/rigid/Contact.as new file mode 100644 index 0000000..b73fc55 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/rigid/Contact.as @@ -0,0 +1,53 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.types.Vector3; + + /** + * Информация о контакте между двумя телами. Содержит нормаль, множество точек контакта и прочие величины. + */ + public class Contact { + +// private static var pool:Vector. = new Vector.(); +// +// public static function create():ContactInfo { +// if (pool.length > 0) { +// return pool.pop(); +// } +// return new ContactInfo(); +// } +// +// public static function destroy(collInfo:ContactInfo):void { +// collInfo.body1 = collInfo.body2 = null; +// pool.push(collInfo); +// } + + // Максимальное количество точек контакта + private const N:int = 8; + + public var body1:Body; + public var body2:Body; + // Взаимный коэффициент отскока + public var restitution:Number; + // Взаимный коэффициент трения + public var friction:Number; + // Нормаль контакта. Направлена от второго тела к первому. + public var normal:Vector3 = new Vector3(); + // Список точек контакта + public var points:Vector. = new Vector.(N, true); + // Количество точек контакта + public var pcount:int; + // Максимальная глубина пересечения тел + public var maxPenetration:Number = 0; + + public var satisfied:Boolean; + + /** + * + */ + public function Contact() { + for (var i:int = 0; i < N; i++) points[i] = new ContactPoint(); + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/rigid/ContactPoint.as b/0.0.2.0/src/alternativa/physics/rigid/ContactPoint.as new file mode 100644 index 0000000..c36f958 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/rigid/ContactPoint.as @@ -0,0 +1,52 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Vector3; + + public class ContactPoint { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + + // Величины, расчитываемые перед началом фазы решения контактов + + // Требуемая проекция конечной скорости на нормаль для упругого контакта + public var normalVel:Number; + // Минимальная скорость разделения неупругого контакта + public var minSepVel:Number; + // Изменение проекции скорости на единицу нормального импульса + public var velByUnitImpulseN:Number; + + public var angularInertia1:Number; + public var angularInertia2:Number; + + // Радиус-вектор точки контакта относительно центра первого тела + public var r1:Vector3 = new Vector3(); + // Радиус-вектор точки контакта относительно центра второго тела + public var r2:Vector3 = new Vector3(); + + // Величины, накапливаемые во время фазы решения контактов + + // Накопленный импульс, применяемый для получения требуемой относительной скорости в точке контакта + public var accumImpulseN:Number; + // Накопленный импульс, применяемый для разделения тел в точке контакта. Не создаёт момента. +// public var accumSepImpulse:Number; + + public var satisfied:Boolean; + + /** + * + * @param cp + */ + public function copyFrom(cp:ContactPoint):void { + pos.vCopy(cp.pos); + penetration = cp.penetration; + feature1 = cp.feature1; + feature2 = cp.feature2; + r1.vCopy(cp.r1); + r2.vCopy(cp.r2); + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/rigid/PhysicsUtils.as b/0.0.2.0/src/alternativa/physics/rigid/PhysicsUtils.as new file mode 100644 index 0000000..a9aaa99 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/rigid/PhysicsUtils.as @@ -0,0 +1,49 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + public class PhysicsUtils { + + /** + * @param mass + * @param halfSize + * @param result + */ + public static function getBoxInvInertia(mass:Number, halfSize:Vector3, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + result.a = 3/(mass*(yy + zz)); + result.f = 3/(mass*(zz + xx)); + result.k = 3/(mass*(xx + yy)); + } + + /** + * @param mass + * @param r + * @param h + * @param result + */ + public static function getCylinderInvInertia(mass:Number, r:Number, h:Number, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + + result.a = result.f = 1/(mass*(h*h/12 + r*r/4)); + result.k = 2/(mass*r*r); + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/rigid/RigidWorld.as b/0.0.2.0/src/alternativa/physics/rigid/RigidWorld.as new file mode 100644 index 0000000..f6de141 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/rigid/RigidWorld.as @@ -0,0 +1,656 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.ICollisionDetector; + import alternativa.physics.collision.KdTreeCollisionDetector; + import alternativa.physics.rigid.constraints.Constraint; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Класс реализует физическую симуляцию поведения твёрдых тел. + */ + public class RigidWorld { + + private static var lastBodyId:int; + + // Максимальное количество контактов + altphysics const MAX_CONTACTS:int = 1000; + + ////////////////////////////////////////////////////////////////// + // Настроечные параметры симуляции. Могут быть изменены в любой + // момент времени без нарушения корректной работы симуляции. + ////////////////////////////////////////////////////////////////// + // Количество шагов, за которое пересекающиеся тела должны разделиться + public var penResolutionSteps:int = 10; + // Величина допустимой глубины пересечения + public var allowedPenetration:Number = 0.1; + // Максимальная скорость, добавляемая с целью разделения тел + public var maxPenResolutionSpeed:Number = 0.5; + // Количество итераций для обработки упругих контактов + public var collisionIterations:int = 5; + // Количество итераций для обработки неупругих контактов + public var contactIterations:int = 5; + // Флаг использования предсказания состояний + public var usePrediction:Boolean = false; + + public var freezeSteps:int = 10; + public var linSpeedFreezeLimit:Number = 1; + public var angSpeedFreezeLimit:Number = 0.01; + + // Переменные для процедуры разделения тел путём непосредственного их перемещенеия. + // !!!!!!!!!!!!!!!!!!!!!!!! + // !!! Экспериментально !!! + // !!!!!!!!!!!!!!!!!!!!!!!! + public var staticSeparationIterations:int = 10; + public var staticSeparationSteps:int = 10; + public var maxAngleMove:Number = 10; + public var useStaticSeparation:Boolean = false; + + // Вектор гравитации + altphysics var _gravity:Vector3 = new Vector3(0, 0, -9.8); + // Использующийся детектор столкновений + altphysics var collisionDetector:ICollisionDetector; + // Список тел, участвующих в симуляции + altphysics var bodies:Vector. = new Vector.(); + // Количество тел, участвующих в симуляции + altphysics var bodiesNum:int; + // Список контактов на текущем шаге симуляции + altphysics var contacts:Vector. = new Vector.(MAX_CONTACTS, true); + // Количество контактов на текущем шаге симуляции + altphysics var contactsNum:int; + // Список ограничений + altphysics var constraints:Vector. = new Vector.(); + // Количество ограничений + altphysics var constraintsNum:int; + // Временная метка. Число прошедших шагов с начала симуляции. + altphysics var timeStamp:uint; + + // Временные переменные для избежания создания экземпляров + private var _r:Vector3 = new Vector3(); + private var _t:Vector3 = new Vector3(); + private var _v:Vector3 = new Vector3(); + private var _v1:Vector3 = new Vector3(); + private var _v2:Vector3 = new Vector3(); + + /** + * + */ + public function RigidWorld() { + for (var i:int = 0; i < MAX_CONTACTS; i++) contacts[i] = new Contact(); + collisionDetector = new KdTreeCollisionDetector(); + } + + /** + * Вектор гравитации. + */ + public function get gravity():Vector3 { + return _gravity.vClone(); + } + + /** + * @private + */ + public function set gravity(value:Vector3):void { + _gravity.vCopy(value); + } + + /** + * Добавляет тело в симуляцию. + * @param body + * @return + */ + public function addBody(body:Body, isStatic:Boolean):Boolean { + var i:int = bodies.indexOf(body); + if (i > -1) return false; + bodies[bodiesNum++] = body; + body.world = this; + body.id = lastBodyId++; +// if (body.collisionPrimitives != null) { +// for (i = 0; i < body.collisionPrimitivesNum; i++) collisionDetector.addPrimitive(body.collisionPrimitives[i], isStatic); +// } + return true; + } + + /** + * Удаляет тело из симуляции. + * @param body + * @return + */ + public function removeBody(body:Body, isStatic:Boolean):Boolean { + var i:int = bodies.indexOf(body); + if (i == -1) return false; + bodies.splice(i, 1); + body.world = null; +// if (body.collisionPrimitives != null) { +// for (i = 0; i < body.collisionPrimitivesNum; i++) collisionDetector.removePrimitive(body.collisionPrimitives[i], isStatic); +// } + bodiesNum--; + return true; + } + + /** + * Добавляет ограничение. + * @param c + */ + public function addConstraint(c:Constraint):void { + constraints[constraintsNum++] = c; + c.world = this; + } + + /** + * Удаляет ограничение. + * @param c + */ + public function removeConstraint(c:Constraint):Boolean { + var idx:int = constraints.indexOf(c); + if (idx < 0) return false; + constraints.splice(idx, 1) + constraintsNum--; + c.world = null; + return true; + } + + /** + * Применяет к телам действующте на них силы. Аккумуляторы сил и моментов тел очищаются после завершения шага + * симуляции, поэтому на момент вызова метода могут уже содержать некоторые значения. + * + * @param dt промежуток времени, в течении которого действуют силы + */ + private function applyForces(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i]; + body.addExternalForces(dt); + body.calcAccelerations(); + // Ускорение свободного падения применяется только к подвижным телам во избежание некорректного изменения + // фиктивной скорости неподвижных тел. + if (body.movable && !body.frozen) { + body.accel.x += _gravity.x; + body.accel.y += _gravity.y; + body.accel.z += _gravity.z; + } + } + } + + /** + * Определяет все столкновения на текущем шаге симуляции и заполняет список получившихся контактов. + * + * @param dt длительность шага симуляции + */ + private function detectCollisions(dt:Number):void { + var i:int; + var j:int; + var body:Body; + + for (i = 0; i < bodiesNum; i++) { + body = bodies[i]; + if (body.frozen) continue; + body.contactsNum = 0; + body.saveState(); + // При включённом режиме предсказания состояние тел интегрируется на один шаг вперёд + if (usePrediction) { + body.integrateVelocity(dt); + body.integratePosition(dt); + } + body.calcDerivedData(); + } + + contactsNum = collisionDetector.getAllCollisions(contacts); + + // Расчёт относительных векторов точки контакта вынесен сюда из-за необходимости учитывать + // положение тел в предсказанном состоянии + for (i = 0; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + var b1:Body = contact.body1; + var b2:Body = contact.body2; + for (j = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.r1.x = cp.pos.x - b1.state.pos.x; + cp.r1.y = cp.pos.y - b1.state.pos.y; + cp.r1.z = cp.pos.z - b1.state.pos.z; + if (b2 != null) { + cp.r2.x = cp.pos.x - b2.state.pos.x; + cp.r2.y = cp.pos.y - b2.state.pos.y; + cp.r2.z = cp.pos.z - b2.state.pos.z; + } + } + } + + // Восстановление состояния тел + if (usePrediction) { + for (i = 0; i < bodiesNum; i++) { + body = bodies[i]; + if (body.frozen) continue; + body.restoreState(); + body.calcDerivedData(); + } + } + } + + /** + * Подготваливает полученные из детектора столкновений контакты, расчитывая значения, не меняющиеся + * в ходе шага симуляции. + */ + private function preProcessContacts(dt:Number):void { + var i:int; + for (i = 0; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + var b1:Body = contact.body1; + var b2:Body = contact.body2; + // Столкнувшиеся тела размораживаются + if (b1.frozen) { + b1.frozen = false; + b1.freezeCounter = 0; + } + if (b2 != null && b2.frozen) { + b2.frozen = false; + b2.freezeCounter = 0; + } + contact.restitution = b1.material.restitution; + if (b2 != null && b2.material.restitution < contact.restitution) contact.restitution = b2.material.restitution; + contact.friction = b1.material.friction; + if (b2 != null && b2.material.friction < contact.friction) contact.friction = b2.material.friction; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.accumImpulseN = 0; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + cp.velByUnitImpulseN = 0; + if (b1.movable) { + cp.angularInertia1 = _v.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1).vDot(contact.normal); + cp.velByUnitImpulseN += b1.invMass + cp.angularInertia1; + } + if (b2 != null && b2.movable) { + cp.angularInertia2 = _v.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2).vDot(contact.normal); + cp.velByUnitImpulseN += b2.invMass + cp.angularInertia2; + } + // Расчёт требуемой конечной скорости для упругого контакта + calcSepVelocity(b1, b2, cp, _v); + cp.normalVel = _v.vDot(contact.normal); + if (cp.normalVel < 0) cp.normalVel = - contact.restitution*cp.normalVel; + // Скорость разделения неупругого контакта + cp.minSepVel = cp.penetration > allowedPenetration ? (cp.penetration - allowedPenetration)/(penResolutionSteps*dt) : 0; + if (cp.minSepVel > maxPenResolutionSpeed) cp.minSepVel = maxPenResolutionSpeed; + } + } + for (i = 0; i < constraintsNum; i++) Constraint(constraints[i]).preProcess(dt); + } + + /** + * + * @param dt + * @param forceInelastic + */ + private function processContacts(dt:Number, forceInelastic:Boolean):void { + var iterNum:int = forceInelastic ? contactIterations : collisionIterations; + var i:int; + var forwardLoop:Boolean = false; + for (var iter:int = 0; iter < iterNum; iter++) { + forwardLoop = !forwardLoop; + for (i = 0; i < contactsNum; i++) resolveContact(contacts[i], forceInelastic, forwardLoop); + // Ограничения + for (i = 0; i < constraintsNum; i++) Constraint(constraints[i]).apply(dt); + } + + // Разделение контактов путём непосредственного изменения координат и ориентации. Экспериментально. +// if (forceInelastic && useStaticSeparation) performStaticSeparation(); + } + + /** + * + */ + private function resolveContact(contactInfo:Contact, forceInelastic:Boolean, forwardLoop:Boolean):void { + var b1:Body = contactInfo.body1; + var b2:Body = contactInfo.body2; + var normal:Vector3 = contactInfo.normal; + var i:int; + if (forwardLoop) { + for (i = 0; i < contactInfo.pcount; i++) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } else { + for (i = contactInfo.pcount - 1; i >= 0; i--) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } + } + + /** + * + * @param idx + * @param b1 + * @param b2 + * @param colInfo + * @param normal + * @param forceInelastic + */ + private function resolveContactPoint(idx:int, b1:Body, b2:Body, contact:Contact, normal:Vector3, forceInelastic:Boolean):void { + var cp:ContactPoint = contact.points[idx]; + if (!forceInelastic) cp.satisfied = true; + + var newVel:Number = 0; + calcSepVelocity(b1, b2, cp, _v); + var sepVel:Number = _v.vDot(contact.normal); + if (forceInelastic) { + var minSpeVel:Number = useStaticSeparation ? 0 : cp.minSepVel; + if (sepVel < minSpeVel) cp.satisfied = false; + else if (cp.satisfied) return; + newVel = minSpeVel; + } else { + newVel = cp.normalVel; + } + var deltaVel:Number = newVel - sepVel; + var impulse:Number = deltaVel/cp.velByUnitImpulseN; + var accumImpulse:Number = cp.accumImpulseN + impulse; + if (accumImpulse < 0) accumImpulse = 0; + var deltaImpulse:Number = accumImpulse - cp.accumImpulseN; + cp.accumImpulseN = accumImpulse; + // Применяем импульс к телам + if (b1.movable) b1.applyRelPosWorldImpulse(cp.r1, normal, deltaImpulse); + if (b2 != null && b2.movable) b2.applyRelPosWorldImpulse(cp.r2, normal, -deltaImpulse); + + // Учёт силы трения + calcSepVelocity(b1, b2, cp, _v); + // Расчитываем изменение касательной скорости на единицу касательного импульса + var tanSpeedByUnitImpulse:Number = 0; + _v.vAddScaled(-_v.vDot(contact.normal), contact.normal); + var tanSpeed:Number = _v.vLength(); + if (tanSpeed < 0.001) return; + _t.vCopy(_v).vNormalize().vReverse(); + // dV = b.invMass + ((invI * (r % t)) % r) * t + if (b1.movable) { + _v.vCross2(cp.r1, _t).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1); + tanSpeedByUnitImpulse += b1.invMass + _v.vDot(_t); + } + if (b2 != null && b2.movable) { + _v.vCross2(cp.r2, _t).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2); + tanSpeedByUnitImpulse += b2.invMass + _v.vDot(_t); + } + + var tanImpulse:Number = tanSpeed/tanSpeedByUnitImpulse; + var max:Number = contact.friction*cp.accumImpulseN; + if (max < 0) { + if (tanImpulse < max) tanImpulse = max; + } else { + if (tanImpulse > max) tanImpulse = max; + } + + // Применяем импульс к телам + if (b1.movable) b1.applyRelPosWorldImpulse(cp.r1, _t, tanImpulse); + if (b2 != null && b2.movable) b2.applyRelPosWorldImpulse(cp.r2, _t, -tanImpulse); + } + + /** + * + * @param cp + * @param normal + * @return + */ + private function calcSepVelocity(body1:Body, body2:Body, cp:ContactPoint, result:Vector3):void { + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 +// result.vCopy(body1.state.velocity).vAdd(_v1.vCross2(body1.state.rotation, cp.r1)); + var rot:Vector3 = body1.state.rotation; + var v:Vector3 = cp.r1; + var x:Number = rot.y*v.z - rot.z*v.y; + var y:Number = rot.z*v.x - rot.x*v.z; + var z:Number = rot.x*v.y - rot.y*v.x; + v = body1.state.velocity; + result.x = v.x + x; + result.y = v.y + y; + result.z = v.z + z; + // V2 = V2_c + w2%r2 + if (body2 != null) { +// result.vSubtract(body2.state.velocity).vSubtract(_v2.vCross2(body2.state.rotation, cp.r2)); + rot = body2.state.rotation; + v = cp.r2; + x = rot.y*v.z - rot.z*v.y; + y = rot.z*v.x - rot.x*v.z; + z = rot.x*v.y - rot.y*v.x; + v = body2.state.velocity; + result.x -= v.x + x; + result.y -= v.y + y; + result.z -= v.z + z; + } + } + + /** + * + * @param dt + */ + private function intergateVelocities(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + (bodies[i] as Body).integrateVelocity(dt); + } + } + + /** + * + * @param dt + */ + private function integratePositions(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i] as Body; + if (body.movable && !body.frozen) body.integratePosition(dt); + } + } + + /** + * + */ + private function performStaticSeparation():void { + var iterNum:int = staticSeparationIterations; +// iterNum = 100; + + // 1. В начале каждой итерации для всех контактов сбрасывается флаг satisfied с одновременным поиском наихудшего контакта, т.е. имеющего наибольшую величину пересечения тел. + // 2. Если найденный контакт имеет величину пересечения меньше предельно допустимой процедура прерывается, т.к. разделение контактов не требуется. + // 3. Если процедура продолжается, то выполняется разделение найденного контакта и установка его флага satisfied в true, после чего запускается внутренний цикл, состоящий + // из contactsNum - 1 итераций. На каждой итерации ищется наихудший контакт среди оставшихся (satisfied == false). Если такой контакт найден, то выполняется его разделение + // и переход к следующей итерации внутреннего цикла, иначе внутренний цикл прерывается и выполняется переход к следующей итерации внешнего цикла (шаг 1.). + for (var iter:int = 0; iter < iterNum; iter++) { + // Ищем контакт с максимальной величиной пересечения тел, одновременно сбрасывая флаги satisfied в false + var worstContact:Contact = contacts[0]; + var i:int; + for (i = 1; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + contact.satisfied = false; + if (contact.maxPenetration > worstContact.maxPenetration) worstContact = contact; + } + if (worstContact.maxPenetration <= allowedPenetration) return; + + resolveInterpenetration(worstContact); + // Внутренний цикл по оставшимся контактам + for (i = 1; i < contactsNum; i++) { + worstContact = getWorstContact(); + if (worstContact == null) break; + resolveInterpenetration(worstContact); + } + } + } + + /** + * + */ + private function getWorstContact():Contact { + var maxPen:Number = 0; + var worst:Contact = null; + for (var i:int = 0; i < contactsNum; i++) { + var c:Contact = contacts[i]; + if (!c.satisfied && c.maxPenetration > maxPen) { + worst = c; + maxPen = c.maxPenetration; + } + } + return maxPen > allowedPenetration ? worst : null; + } + + /** + * Разделяет указанный контакт, выполняя staticSeparationSteps итераций по списку точек контакта. + * 1. В начале каждой итерации ищется точка с наибольшим пересечением, одновременно сбрасываются флаги satisfied у точек. + * 2. Для найденной точки выполняется процедура разделения. + * 3. После выполняется pcount - 1 итерация по списку точек, каждый раз ищется наихудшая среди имеющих satisfied == false, для которой выполняется процедура разделения. + * Если наихудшая точка не найдена, то выполнется переход к следующей итерации внешнего цикла (шаг 1). + * + * В конце процедуры разделения обновляются значения пересечений для остальных точек контакта, а также для всех прочих контактов, относящихся к телам текущего. + * + * @param contact контакт для разделения + */ + private function resolveInterpenetration(contact:Contact):void { + contact.satisfied = true; + + for (var step:int = 0; step < staticSeparationSteps; step++) { + var worstCp:ContactPoint = contact.points[0]; + var cp:ContactPoint; + var i:int; + for (i = 1; i < contact.pcount; i++) { + cp = contact.points[i]; + cp.satisfied = false; + if (cp.penetration > worstCp.penetration) worstCp = cp; + } + if (worstCp.penetration <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + // Разделяем оставшиеся точки + var maxPen:Number = 0; + for (i = 1; i < contact.pcount; i++) { + // Поиск наихудшей точки + for (var j:int = 0; j < contact.pcount; j++) { + cp = contact.points[j]; + if (cp.satisfied) continue; + if (cp.penetration > maxPen) { + maxPen = cp.penetration; + worstCp = cp; + } + } + if (maxPen <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + } + } + } + + /** + * + * @param cp + * @param contact + */ + private function separateContactPoint(cp:ContactPoint, contact:Contact):void { + cp.satisfied = true; + + var b1:Body = contact.body1; + var b2:Body = contact.body2; + var totalMove:Number = cp.penetration - allowedPenetration; + var moveCoeff:Number = totalMove/cp.velByUnitImpulseN; + var linMove1:Number; + var angleMove1:Number; + if (b1.movable) { + linMove1 = b1.invMass*moveCoeff; + angleMove1 = cp.angularInertia1*moveCoeff; + if (angleMove1 > maxAngleMove) { + linMove1 += angleMove1 - maxAngleMove; + angleMove1 -= maxAngleMove; + } + b1.state.pos.vAddScaled(linMove1, contact.normal); + _v1.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vScale(angleMove1); + b1.state.orientation.addScaledVector(_v1, 1); + } + var linMove2:Number; + var angleMove2:Number; + if (b2 != null && b2.movable) { + linMove2 = b2.invMass*moveCoeff; + angleMove2 = cp.angularInertia2*moveCoeff; + if (angleMove2 > maxAngleMove) { + linMove2 += angleMove2 - maxAngleMove; + angleMove2 -= maxAngleMove; + } + b2.state.pos.vAddScaled(-linMove2, contact.normal); + _v2.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vScale(angleMove2); + _v2.vReverse(); + b2.state.orientation.addScaledVector(_v2, 1); + } + cp.penetration = allowedPenetration; + // Обновляем пересечения в других точках + var i:int; + for (i = 0; i < contact.pcount; i++) { + var cp1:ContactPoint = contact.points[i]; + if (cp1 == cp) continue; + var angularMove:Number; + if (b1.movable) { + angularMove = _v.vCross2(_v1, cp1.r1).vDot(contact.normal); + cp1.penetration -= linMove1 + angularMove; + } + if (b2 != null && b2.movable) { + angularMove = _v.vCross2(_v2, cp1.r2).vDot(contact.normal); + cp1.penetration -= linMove2 - angularMove; + } + // Обновление максимального значения для контакта + if (cp1.penetration > contact.maxPenetration) contact.maxPenetration = cp1.penetration; + } + // Обновляем пересечения для других контактов + var c:Contact; + var j:int; + if (b1.movable) { + for (i = 0; i < b1.contactsNum; i++) { + c = b1.contacts[i]; + if (c == contact) continue; + for (j = 0; j < c.pcount; j++) { + cp1 = c.points[j]; + if (b1 == c.body1) cp1.penetration -= linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r1).vDot(c.normal); + else cp1.penetration += linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r2).vDot(c.normal); + if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; + } + } + } + if (b2 != null && b2.movable) { + for (i = 0; i < b2.contactsNum; i++) { + c = b2.contacts[i]; + if (c == contact) continue; + for (j = 0; j < c.pcount; j++) { + cp1 = c.points[j]; + if (b2 == c.body1) cp1.penetration -= linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r1).vDot(c.normal); + else cp1.penetration += linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r2).vDot(c.normal); + if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; + } + } + } + } + + /** + * + */ + private function postPhysics():void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i]; + body.clearAccumulators(); + body.calcDerivedData(); + if (body.canFreeze) { + if (body.state.velocity.vLength() < linSpeedFreezeLimit && body.state.rotation.vLength() < angSpeedFreezeLimit) { + if (!body.frozen) { + body.freezeCounter++; + if (body.freezeCounter >= freezeSteps) { + body.frozen = true; + trace("FROZEN"); + } + } + } else { + body.freezeCounter = 0; + body.frozen = false; + } + } + } + } + + /** + * + * @param dt + */ + public function runPhysics(dt:Number):void { + timeStamp++; + applyForces(dt); + detectCollisions(dt); + preProcessContacts(dt); + processContacts(dt, false); + intergateVelocities(dt); + processContacts(dt, true); + integratePositions(dt); + postPhysics(); + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/rigid/constraints/.svn/all-wcprops b/0.0.2.0/src/alternativa/physics/rigid/constraints/.svn/all-wcprops new file mode 100644 index 0000000..3524bca --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/rigid/constraints/.svn/all-wcprops @@ -0,0 +1,17 @@ +K 25 +svn:wc:ra_dav:version-url +V 121 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/rigid/constraints +END +Constraint.as +K 25 +svn:wc:ra_dav:version-url +V 135 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/rigid/constraints/Constraint.as +END +MaxDistanceConstraint.as +K 25 +svn:wc:ra_dav:version-url +V 146 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/rigid/constraints/MaxDistanceConstraint.as +END diff --git a/0.0.2.0/src/alternativa/physics/rigid/constraints/.svn/entries b/0.0.2.0/src/alternativa/physics/rigid/constraints/.svn/entries new file mode 100644 index 0000000..0aa13b6 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/rigid/constraints/.svn/entries @@ -0,0 +1,52 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/rigid/constraints +http://svndev.alternativaplatform.com + + + +2009-06-29T03:58:47.301371Z +15198 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Constraint.as +file + + + + +2010-10-28T04:32:40.000000Z +757824f9eeb3c89243d104910e0d0529 +2009-04-23T13:43:38.903278Z +11779 +mike + +MaxDistanceConstraint.as +file + + + + +2010-10-28T04:32:40.000000Z +025a27c81aeaf18c860f3a4c4c63cf96 +2009-06-29T03:58:47.301371Z +15198 +mike + diff --git a/0.0.2.0/src/alternativa/physics/rigid/constraints/.svn/format b/0.0.2.0/src/alternativa/physics/rigid/constraints/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/rigid/constraints/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.2.0/src/alternativa/physics/rigid/constraints/.svn/text-base/Constraint.as.svn-base b/0.0.2.0/src/alternativa/physics/rigid/constraints/.svn/text-base/Constraint.as.svn-base new file mode 100644 index 0000000..c2cd912 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/rigid/constraints/.svn/text-base/Constraint.as.svn-base @@ -0,0 +1,35 @@ +package alternativa.physics.rigid.constraints { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.RigidWorld; + + use namespace altphysics; + + /** + * + */ + public class Constraint { + + altphysics var satisfied:Boolean; + altphysics var world:RigidWorld; + + /** + * + */ + public function Constraint() { + } + + /** + * @param dt + */ + public function preProcess(dt:Number):void { + } + + /** + * @param dt + */ + public function apply(dt:Number):void { + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/rigid/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base b/0.0.2.0/src/alternativa/physics/rigid/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base new file mode 100644 index 0000000..008d4a4 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/rigid/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base @@ -0,0 +1,155 @@ +package alternativa.physics.rigid.constraints { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.BodyState; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class MaxDistanceConstraint extends Constraint { + // Первое тело ограничения + altphysics var body1:Body; + // Второе тело ограничения. Может быть null. + altphysics var body2:Body; + // Первая точка крепления в системе координат первого тела + altphysics var r1:Vector3; + // Вторая точка привязки в системе координат второго тела. В случае отсутствия второго тела, координаты задаются в мировой системе. + altphysics var r2:Vector3; + // Максимально допустимое расстояние между точками крепления + altphysics var maxDistance:Number; + // Радиус-вектор первой точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr1:Vector3 = new Vector3(); + // Радиус-вектор второй точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr2:Vector3 = new Vector3(); + // Минимальная скорость сближения для удовлетворения ограничения + private var minClosingVel:Number; + // Изменение относительной скорости на единицу импульса + private var velByUnitImpulseN:Number; + // Направление корректирующего импульса + private var impulseDirection:Vector3 = new Vector3(); + + // Вспомогательные переменные + private static var _v:Vector3 = new Vector3(); + private static var _v1:Vector3 = new Vector3(); + private static var _v2:Vector3 = new Vector3(); + + /** + * @param body1 + * @param body2 + * @param r1 + * @param r2 + * @param maxDistance + */ + public function MaxDistanceConstraint(body1:Body, body2:Body, r1:Vector3, r2:Vector3, maxDistance:Number) { + super(); + this.body1 = body1; + this.body2 = body2; + this.r1 = r1.vClone(); + this.r2 = r2.vClone(); + this.maxDistance = maxDistance; + } + + /** + * @param dt + */ + override public function preProcess(dt:Number):void { + // Вычислим расстояние между точками + body1.baseMatrix.transformVector(r1, wr1); + if (body2 != null) body2.baseMatrix.transformVector(r2, wr2); + else { + wr2.x = r2.x; + wr2.y = r2.y; + wr2.z = r2.z; + } + var p1:Vector3 = body1.state.pos; + impulseDirection.x = wr2.x - wr1.x - p1.x; + impulseDirection.y = wr2.y - wr1.y - p1.y; + impulseDirection.z = wr2.z - wr1.z - p1.z; + if (body2 != null) { + var p2:Vector3 = body2.state.pos; + impulseDirection.x += p2.x; + impulseDirection.y += p2.y; + impulseDirection.z += p2.z; + } + + var len:Number = Math.sqrt(impulseDirection.x*impulseDirection.x + impulseDirection.y*impulseDirection.y + impulseDirection.z*impulseDirection.z); + var delta:Number = len - maxDistance; + if (delta > 0) { + satisfied = false; + if (len == 0) impulseDirection.x = 1; + else { + impulseDirection.x /= len; + impulseDirection.y /= len; + impulseDirection.z /= len; + } + minClosingVel = delta/(world.penResolutionSteps*dt); + if (minClosingVel > world.maxPenResolutionSpeed) minClosingVel = world.maxPenResolutionSpeed; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + velByUnitImpulseN = 0; + if (body1.movable) { +// velByUnitImpulseN += body1.invMass + _v.vCross2(wr1, impulseDirection).vTransformBy3(body1.invInertiaWorld).vCross(wr1).vDot(impulseDirection); + _v.x = wr1.y*impulseDirection.z - wr1.z*impulseDirection.y; + _v.y = wr1.z*impulseDirection.x - wr1.x*impulseDirection.z; + _v.z = wr1.x*impulseDirection.y - wr1.y*impulseDirection.x; + _v.vTransformBy3(body1.invInertiaWorld); + _v1.x = _v.y*wr1.z - _v.z*wr1.y; + _v1.y = _v.z*wr1.x - _v.x*wr1.z; + _v1.z = _v.x*wr1.y - _v.y*wr1.x; + velByUnitImpulseN += body1.invMass + _v1.x*impulseDirection.x + _v1.y*impulseDirection.y + _v1.z*impulseDirection.z; + } + if (body2 != null && body2.movable) { +// velByUnitImpulseN += body2.invMass + _v.vCross2(wr2, impulseDirection).vTransformBy3(body2.invInertiaWorld).vCross(wr2).vDot(impulseDirection); + _v.x = wr2.y*impulseDirection.z - wr2.z*impulseDirection.y; + _v.y = wr2.z*impulseDirection.x - wr2.x*impulseDirection.z; + _v.z = wr2.x*impulseDirection.y - wr2.y*impulseDirection.x; + _v.vTransformBy3(body2.invInertiaWorld); + _v1.x = _v.y*wr2.z - _v.z*wr2.y; + _v1.y = _v.z*wr2.x - _v.x*wr2.z; + _v1.z = _v.x*wr2.y - _v.y*wr2.x; + velByUnitImpulseN += body2.invMass + _v1.x*impulseDirection.x + _v1.y*impulseDirection.y + _v1.z*impulseDirection.z; + } + } else satisfied = true; + } + + /** + * @param dt + */ + override public function apply(dt:Number):void { + if (satisfied) return; + // Расчитываем проекцию скорости относительной скорости + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 + var state:BodyState = body1.state; + var rot:Vector3 = state.rotation; +// _v1.vCross2(state.rotation, wr1); + _v1.x = rot.y*wr1.z - rot.z*wr1.y; + _v1.y = rot.z*wr1.x - rot.x*wr1.z; + _v1.z = rot.x*wr1.y - rot.y*wr1.x; + _v.x = state.velocity.x + _v1.x; + _v.y = state.velocity.y + _v1.y; + _v.z = state.velocity.z + _v1.z; + // V2 = V2_c + w2%r2 + if (body2 != null) { + state = body2.state; +// _v2.vCross2(state.rotation, wr2); + _v2.x = rot.y*wr2.z - rot.z*wr2.y; + _v2.y = rot.z*wr2.x - rot.x*wr2.z; + _v2.z = rot.x*wr2.y - rot.y*wr2.x; + _v.x -= state.velocity.x - _v2.x; + _v.y -= state.velocity.y - _v2.y; + _v.z -= state.velocity.z - _v2.z; + } + var closingVel:Number = _v.x*impulseDirection.x + _v.y*impulseDirection.y + _v.z*impulseDirection.z; + if (closingVel > minClosingVel) return; + var impulse:Number = (minClosingVel - closingVel)/velByUnitImpulseN; + if (body1.movable) body1.applyRelPosWorldImpulse(wr1, impulseDirection, impulse); + if (body2 != null && body2.movable) body2.applyRelPosWorldImpulse(wr2, impulseDirection, -impulse); + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/rigid/constraints/Constraint.as b/0.0.2.0/src/alternativa/physics/rigid/constraints/Constraint.as new file mode 100644 index 0000000..c2cd912 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/rigid/constraints/Constraint.as @@ -0,0 +1,35 @@ +package alternativa.physics.rigid.constraints { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.RigidWorld; + + use namespace altphysics; + + /** + * + */ + public class Constraint { + + altphysics var satisfied:Boolean; + altphysics var world:RigidWorld; + + /** + * + */ + public function Constraint() { + } + + /** + * @param dt + */ + public function preProcess(dt:Number):void { + } + + /** + * @param dt + */ + public function apply(dt:Number):void { + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/rigid/constraints/MaxDistanceConstraint.as b/0.0.2.0/src/alternativa/physics/rigid/constraints/MaxDistanceConstraint.as new file mode 100644 index 0000000..008d4a4 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/rigid/constraints/MaxDistanceConstraint.as @@ -0,0 +1,155 @@ +package alternativa.physics.rigid.constraints { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.BodyState; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class MaxDistanceConstraint extends Constraint { + // Первое тело ограничения + altphysics var body1:Body; + // Второе тело ограничения. Может быть null. + altphysics var body2:Body; + // Первая точка крепления в системе координат первого тела + altphysics var r1:Vector3; + // Вторая точка привязки в системе координат второго тела. В случае отсутствия второго тела, координаты задаются в мировой системе. + altphysics var r2:Vector3; + // Максимально допустимое расстояние между точками крепления + altphysics var maxDistance:Number; + // Радиус-вектор первой точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr1:Vector3 = new Vector3(); + // Радиус-вектор второй точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr2:Vector3 = new Vector3(); + // Минимальная скорость сближения для удовлетворения ограничения + private var minClosingVel:Number; + // Изменение относительной скорости на единицу импульса + private var velByUnitImpulseN:Number; + // Направление корректирующего импульса + private var impulseDirection:Vector3 = new Vector3(); + + // Вспомогательные переменные + private static var _v:Vector3 = new Vector3(); + private static var _v1:Vector3 = new Vector3(); + private static var _v2:Vector3 = new Vector3(); + + /** + * @param body1 + * @param body2 + * @param r1 + * @param r2 + * @param maxDistance + */ + public function MaxDistanceConstraint(body1:Body, body2:Body, r1:Vector3, r2:Vector3, maxDistance:Number) { + super(); + this.body1 = body1; + this.body2 = body2; + this.r1 = r1.vClone(); + this.r2 = r2.vClone(); + this.maxDistance = maxDistance; + } + + /** + * @param dt + */ + override public function preProcess(dt:Number):void { + // Вычислим расстояние между точками + body1.baseMatrix.transformVector(r1, wr1); + if (body2 != null) body2.baseMatrix.transformVector(r2, wr2); + else { + wr2.x = r2.x; + wr2.y = r2.y; + wr2.z = r2.z; + } + var p1:Vector3 = body1.state.pos; + impulseDirection.x = wr2.x - wr1.x - p1.x; + impulseDirection.y = wr2.y - wr1.y - p1.y; + impulseDirection.z = wr2.z - wr1.z - p1.z; + if (body2 != null) { + var p2:Vector3 = body2.state.pos; + impulseDirection.x += p2.x; + impulseDirection.y += p2.y; + impulseDirection.z += p2.z; + } + + var len:Number = Math.sqrt(impulseDirection.x*impulseDirection.x + impulseDirection.y*impulseDirection.y + impulseDirection.z*impulseDirection.z); + var delta:Number = len - maxDistance; + if (delta > 0) { + satisfied = false; + if (len == 0) impulseDirection.x = 1; + else { + impulseDirection.x /= len; + impulseDirection.y /= len; + impulseDirection.z /= len; + } + minClosingVel = delta/(world.penResolutionSteps*dt); + if (minClosingVel > world.maxPenResolutionSpeed) minClosingVel = world.maxPenResolutionSpeed; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + velByUnitImpulseN = 0; + if (body1.movable) { +// velByUnitImpulseN += body1.invMass + _v.vCross2(wr1, impulseDirection).vTransformBy3(body1.invInertiaWorld).vCross(wr1).vDot(impulseDirection); + _v.x = wr1.y*impulseDirection.z - wr1.z*impulseDirection.y; + _v.y = wr1.z*impulseDirection.x - wr1.x*impulseDirection.z; + _v.z = wr1.x*impulseDirection.y - wr1.y*impulseDirection.x; + _v.vTransformBy3(body1.invInertiaWorld); + _v1.x = _v.y*wr1.z - _v.z*wr1.y; + _v1.y = _v.z*wr1.x - _v.x*wr1.z; + _v1.z = _v.x*wr1.y - _v.y*wr1.x; + velByUnitImpulseN += body1.invMass + _v1.x*impulseDirection.x + _v1.y*impulseDirection.y + _v1.z*impulseDirection.z; + } + if (body2 != null && body2.movable) { +// velByUnitImpulseN += body2.invMass + _v.vCross2(wr2, impulseDirection).vTransformBy3(body2.invInertiaWorld).vCross(wr2).vDot(impulseDirection); + _v.x = wr2.y*impulseDirection.z - wr2.z*impulseDirection.y; + _v.y = wr2.z*impulseDirection.x - wr2.x*impulseDirection.z; + _v.z = wr2.x*impulseDirection.y - wr2.y*impulseDirection.x; + _v.vTransformBy3(body2.invInertiaWorld); + _v1.x = _v.y*wr2.z - _v.z*wr2.y; + _v1.y = _v.z*wr2.x - _v.x*wr2.z; + _v1.z = _v.x*wr2.y - _v.y*wr2.x; + velByUnitImpulseN += body2.invMass + _v1.x*impulseDirection.x + _v1.y*impulseDirection.y + _v1.z*impulseDirection.z; + } + } else satisfied = true; + } + + /** + * @param dt + */ + override public function apply(dt:Number):void { + if (satisfied) return; + // Расчитываем проекцию скорости относительной скорости + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 + var state:BodyState = body1.state; + var rot:Vector3 = state.rotation; +// _v1.vCross2(state.rotation, wr1); + _v1.x = rot.y*wr1.z - rot.z*wr1.y; + _v1.y = rot.z*wr1.x - rot.x*wr1.z; + _v1.z = rot.x*wr1.y - rot.y*wr1.x; + _v.x = state.velocity.x + _v1.x; + _v.y = state.velocity.y + _v1.y; + _v.z = state.velocity.z + _v1.z; + // V2 = V2_c + w2%r2 + if (body2 != null) { + state = body2.state; +// _v2.vCross2(state.rotation, wr2); + _v2.x = rot.y*wr2.z - rot.z*wr2.y; + _v2.y = rot.z*wr2.x - rot.x*wr2.z; + _v2.z = rot.x*wr2.y - rot.y*wr2.x; + _v.x -= state.velocity.x - _v2.x; + _v.y -= state.velocity.y - _v2.y; + _v.z -= state.velocity.z - _v2.z; + } + var closingVel:Number = _v.x*impulseDirection.x + _v.y*impulseDirection.y + _v.z*impulseDirection.z; + if (closingVel > minClosingVel) return; + var impulse:Number = (minClosingVel - closingVel)/velByUnitImpulseN; + if (body1.movable) body1.applyRelPosWorldImpulse(wr1, impulseDirection, impulse); + if (body2 != null && body2.movable) body2.applyRelPosWorldImpulse(wr2, impulseDirection, -impulse); + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/rigid/primitives/.svn/all-wcprops b/0.0.2.0/src/alternativa/physics/rigid/primitives/.svn/all-wcprops new file mode 100644 index 0000000..c1cf63f --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/rigid/primitives/.svn/all-wcprops @@ -0,0 +1,35 @@ +K 25 +svn:wc:ra_dav:version-url +V 120 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/rigid/primitives +END +RigidPlane.as +K 25 +svn:wc:ra_dav:version-url +V 134 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/rigid/primitives/RigidPlane.as +END +RigidSphere.as +K 25 +svn:wc:ra_dav:version-url +V 135 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/rigid/primitives/RigidSphere.as +END +RigidBox.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/rigid/primitives/RigidBox.as +END +RigidCylinder.as +K 25 +svn:wc:ra_dav:version-url +V 137 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/rigid/primitives/RigidCylinder.as +END +RigidRect.as +K 25 +svn:wc:ra_dav:version-url +V 133 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/rigid/primitives/RigidRect.as +END diff --git a/0.0.2.0/src/alternativa/physics/rigid/primitives/.svn/entries b/0.0.2.0/src/alternativa/physics/rigid/primitives/.svn/entries new file mode 100644 index 0000000..c63c147 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/rigid/primitives/.svn/entries @@ -0,0 +1,88 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/rigid/primitives +http://svndev.alternativaplatform.com + + + +2009-06-08T06:46:18.310732Z +14099 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +RigidPlane.as +file + + + + +2010-10-28T04:32:40.000000Z +c4713421e357ee1967dca9ab3ffa7290 +2009-06-08T06:46:18.310732Z +14099 +mike + +RigidSphere.as +file + + + + +2010-10-28T04:32:40.000000Z +79ef38eab05657ad9627bdac4ade7271 +2009-04-23T13:43:38.903278Z +11779 +mike + +RigidBox.as +file + + + + +2010-10-28T04:32:40.000000Z +da834a5d208ff30f3a74321f75301cd7 +2009-06-08T06:46:18.310732Z +14099 +mike + +RigidCylinder.as +file + + + + +2010-10-28T04:32:40.000000Z +61512d31f9e211dcb0864164aa6f5210 +2009-04-23T13:43:38.903278Z +11779 +mike + +RigidRect.as +file + + + + +2010-10-28T04:32:40.000000Z +4cff4e313a1f41d4ccfa6480ea78173e +2009-04-23T13:43:38.903278Z +11779 +mike + diff --git a/0.0.2.0/src/alternativa/physics/rigid/primitives/.svn/format b/0.0.2.0/src/alternativa/physics/rigid/primitives/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/rigid/primitives/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.2.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidBox.as.svn-base b/0.0.2.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidBox.as.svn-base new file mode 100644 index 0000000..c01ff61 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidBox.as.svn-base @@ -0,0 +1,56 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class RigidBox extends Body { + + /** + * + * @param halfSize + * @param mass + */ + public function RigidBox(halfSize:Vector3, mass:Number) { + super(0, Matrix3.ZERO); + addCollisionPrimitive(new CollisionBox(halfSize, 1)); + setParams(halfSize, mass); + } + + /** + * @param halfSize + * @param mass + */ + public function setParams(halfSize:Vector3, mass:Number):void { + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + invInertia.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + invInertia.a = 3*invMass/(yy + zz); + invInertia.f = 3*invMass/(zz + xx); + invInertia.k = 3*invMass/(xx + yy); + } + var prim:CollisionBox = collisionPrimitives[0] as CollisionBox; + prim.hs.vCopy(halfSize); + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidCylinder.as.svn-base b/0.0.2.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidCylinder.as.svn-base new file mode 100644 index 0000000..0f9f2b3 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidCylinder.as.svn-base @@ -0,0 +1,32 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + + use namespace altphysics; + + public class RigidCylinder extends Body { + + altphysics var r:Number; + altphysics var h:Number; + + public function RigidCylinder(mass:Number, radius:Number, height:Number) { + super(0, Matrix3.ZERO); + setParams(mass, radius, height); + } + + public function setParams(mass:Number, radius:Number, height:Number):void { + r = radius; + h = height; + invInertiaWorld.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + invInertia.a = invInertia.f = mass*(h*h/12 + r*r/4); + invInertia.k = 0.5*mass*r*r; + invInertia.invert(); + } + } + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidPlane.as.svn-base b/0.0.2.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidPlane.as.svn-base new file mode 100644 index 0000000..26f74b2 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidPlane.as.svn-base @@ -0,0 +1,23 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + public class RigidPlane extends Body { + + public var normal:Vector3 = new Vector3(); + public var offset:Number; + + public function RigidPlane() { + super(0, Matrix3.ZERO); + this.normal.vCopy(Vector3.Z_AXIS); + this.offset = 0; + movable = false; + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidRect.as.svn-base b/0.0.2.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidRect.as.svn-base new file mode 100644 index 0000000..3c3913d --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidRect.as.svn-base @@ -0,0 +1,20 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + public class RigidRect extends Body { + + public function RigidRect(halfWidth:Number, halfLength:Number) { + super(0, Matrix3.ZERO); + movable = false; + addCollisionPrimitive(new CollisionRect(new Vector3(halfWidth, halfLength, 0), 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidSphere.as.svn-base b/0.0.2.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidSphere.as.svn-base new file mode 100644 index 0000000..9668716 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidSphere.as.svn-base @@ -0,0 +1,29 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + + use namespace altphysics; + + public class RigidSphere extends Body { + + public function RigidSphere(radius:Number, mass:Number) { + var m:Matrix3; + if (mass == Infinity) { + mass = 0; + m = Matrix3.ZERO; + } else { + mass = 1/mass; + var r2:Number = radius*radius; + m = new Matrix3(); + m.a = m.f = m.k = 2.5*mass/r2; + } + super(mass, m); + if (mass == 0) movable = false; + addCollisionPrimitive(new CollisionSphere(radius, 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/rigid/primitives/RigidBox.as b/0.0.2.0/src/alternativa/physics/rigid/primitives/RigidBox.as new file mode 100644 index 0000000..c01ff61 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/rigid/primitives/RigidBox.as @@ -0,0 +1,56 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class RigidBox extends Body { + + /** + * + * @param halfSize + * @param mass + */ + public function RigidBox(halfSize:Vector3, mass:Number) { + super(0, Matrix3.ZERO); + addCollisionPrimitive(new CollisionBox(halfSize, 1)); + setParams(halfSize, mass); + } + + /** + * @param halfSize + * @param mass + */ + public function setParams(halfSize:Vector3, mass:Number):void { + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + invInertia.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + invInertia.a = 3*invMass/(yy + zz); + invInertia.f = 3*invMass/(zz + xx); + invInertia.k = 3*invMass/(xx + yy); + } + var prim:CollisionBox = collisionPrimitives[0] as CollisionBox; + prim.hs.vCopy(halfSize); + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/rigid/primitives/RigidCylinder.as b/0.0.2.0/src/alternativa/physics/rigid/primitives/RigidCylinder.as new file mode 100644 index 0000000..0f9f2b3 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/rigid/primitives/RigidCylinder.as @@ -0,0 +1,32 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + + use namespace altphysics; + + public class RigidCylinder extends Body { + + altphysics var r:Number; + altphysics var h:Number; + + public function RigidCylinder(mass:Number, radius:Number, height:Number) { + super(0, Matrix3.ZERO); + setParams(mass, radius, height); + } + + public function setParams(mass:Number, radius:Number, height:Number):void { + r = radius; + h = height; + invInertiaWorld.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + invInertia.a = invInertia.f = mass*(h*h/12 + r*r/4); + invInertia.k = 0.5*mass*r*r; + invInertia.invert(); + } + } + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/rigid/primitives/RigidPlane.as b/0.0.2.0/src/alternativa/physics/rigid/primitives/RigidPlane.as new file mode 100644 index 0000000..26f74b2 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/rigid/primitives/RigidPlane.as @@ -0,0 +1,23 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + public class RigidPlane extends Body { + + public var normal:Vector3 = new Vector3(); + public var offset:Number; + + public function RigidPlane() { + super(0, Matrix3.ZERO); + this.normal.vCopy(Vector3.Z_AXIS); + this.offset = 0; + movable = false; + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/rigid/primitives/RigidRect.as b/0.0.2.0/src/alternativa/physics/rigid/primitives/RigidRect.as new file mode 100644 index 0000000..3c3913d --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/rigid/primitives/RigidRect.as @@ -0,0 +1,20 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + public class RigidRect extends Body { + + public function RigidRect(halfWidth:Number, halfLength:Number) { + super(0, Matrix3.ZERO); + movable = false; + addCollisionPrimitive(new CollisionRect(new Vector3(halfWidth, halfLength, 0), 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/rigid/primitives/RigidSphere.as b/0.0.2.0/src/alternativa/physics/rigid/primitives/RigidSphere.as new file mode 100644 index 0000000..9668716 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/rigid/primitives/RigidSphere.as @@ -0,0 +1,29 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + + use namespace altphysics; + + public class RigidSphere extends Body { + + public function RigidSphere(radius:Number, mass:Number) { + var m:Matrix3; + if (mass == Infinity) { + mass = 0; + m = Matrix3.ZERO; + } else { + mass = 1/mass; + var r2:Number = radius*radius; + m = new Matrix3(); + m.a = m.f = m.k = 2.5*mass/r2; + } + super(mass, m); + if (mass == 0) movable = false; + addCollisionPrimitive(new CollisionSphere(radius, 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/types/.svn/all-wcprops b/0.0.2.0/src/alternativa/physics/types/.svn/all-wcprops new file mode 100644 index 0000000..bea75e4 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/types/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 109 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/types +END +Quaternion.as +K 25 +svn:wc:ra_dav:version-url +V 123 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/types/Quaternion.as +END +Vector3.as +K 25 +svn:wc:ra_dav:version-url +V 120 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/types/Vector3.as +END +Matrix3.as +K 25 +svn:wc:ra_dav:version-url +V 120 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/types/Matrix3.as +END +Matrix4.as +K 25 +svn:wc:ra_dav:version-url +V 120 +/!svn/ver/15305/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/types/Matrix4.as +END diff --git a/0.0.2.0/src/alternativa/physics/types/.svn/entries b/0.0.2.0/src/alternativa/physics/types/.svn/entries new file mode 100644 index 0000000..b18dfa1 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/types/.svn/entries @@ -0,0 +1,76 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.2.0/src/alternativa/physics/types +http://svndev.alternativaplatform.com + + + +2009-06-25T09:18:57.805913Z +15117 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Quaternion.as +file + + + + +2010-10-28T04:32:40.000000Z +595f36fdfea8e82e752bb989e97e3a9a +2009-05-04T04:55:56.466789Z +12306 +mike + +Vector3.as +file + + + + +2010-10-28T04:32:40.000000Z +c8476935b1206abe041e8822204346cd +2009-06-25T09:18:57.805913Z +15117 +mike + +Matrix3.as +file + + + + +2010-10-28T04:32:40.000000Z +3fde68593cd3eb5a7046c441a76c9c5c +2009-06-25T09:18:57.805913Z +15117 +mike + +Matrix4.as +file + + + + +2010-10-28T04:32:40.000000Z +b2ed6277a2052c2568a1b4f19c469223 +2009-05-04T04:55:56.466789Z +12306 +mike + diff --git a/0.0.2.0/src/alternativa/physics/types/.svn/format b/0.0.2.0/src/alternativa/physics/types/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/types/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.2.0/src/alternativa/physics/types/.svn/text-base/Matrix3.as.svn-base b/0.0.2.0/src/alternativa/physics/types/.svn/text-base/Matrix3.as.svn-base new file mode 100644 index 0000000..e07d0d1 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/types/.svn/text-base/Matrix3.as.svn-base @@ -0,0 +1,396 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + /** + * + */ + public class Matrix3 { + + public static const ZERO:Matrix3 = new Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0); + public static const IDENTITY:Matrix3 = new Matrix3(); + + public var a:Number; + public var b:Number; + public var c:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix3(a:Number = 1, b:Number = 0, c:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1) { + this.a = a; + this.b = b; + this.c = c; + + this.e = e; + this.f = f; + this.g = g; + + this.i = i; + this.j = j; + this.k = k; + } + + /** + * + * @return + */ + public function toIdentity():Matrix3 { + a = f = k = 1; + b = c = e = g = i = j = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + var det:Number = 1/(-cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk); + + a = (ff*kk - gg*jj)*det; + b = (cc*jj - bb*kk)*det; + c = (bb*gg - cc*ff)*det; + e = (gg*ii - ee*kk)*det; + f = (aa*kk - cc*ii)*det; + g = (cc*ee - aa*gg)*det; + i = (ee*jj - ff*ii)*det; + j = (bb*ii - aa*jj)*det; + k = (aa*ff - bb*ee)*det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prependTransposed(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.b + cc*m.c; + b = aa*m.e + bb*m.f + cc*m.g; + c = aa*m.i + bb*m.j + cc*m.k; + e = ee*m.a + ff*m.b + gg*m.c; + f = ee*m.e + ff*m.f + gg*m.g; + g = ee*m.i + ff*m.j + gg*m.k; + i = ii*m.a + jj*m.b + kk*m.c; + j = ii*m.e + jj*m.f + kk*m.g; + k = ii*m.i + jj*m.j + kk*m.k; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix3):Matrix3 { + a += m.a; + b += m.b; + c += m.c; + e += m.e; + f += m.f; + g += m.g; + i += m.i; + j += m.j; + k += m.k; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix3):Matrix3 { + a -= m.a; + b -= m.b; + c -= m.c; + e -= m.e; + f -= m.f; + g -= m.g; + i -= m.i; + j -= m.j; + k -= m.k; + + return this; + } + + /** + * + * @return + */ + public function transpose():Matrix3 { + var tmp:Number = b; + b = e; + e = tmp; + tmp = c; + c = i; + i = tmp; + tmp = g; + g = j; + j = tmp; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector3To3D(vin:Vector3, vout:Vector3D):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * + * @param matrix + */ + public function createSkewSymmetric(v:Vector3):Matrix3 { + a = f = k = 0; + b = -v.z; + c = v.y; + e = v.z; + g = -v.x; + i = -v.y; + j = v.x; + return this; + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(i:int, axis:Vector3):void { + switch (i) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + } + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix3):Matrix3 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @return + */ + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):Matrix3 { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + + return this; + } + + /** + * Метод формирует матрицу поворота на заданный угол относительно заданной оси. + * + * @param axis нормализованный вектор, задающий ось, относительно которой выполняется поворот + * @param angle угол поворота в радианах + */ + public function fromAxisAngle(axis:Vector3, angle:Number):void { + var c1:Number = Math.cos(angle); + var s:Number = Math.sin(angle); + var t:Number = 1 - c1; + var x:Number = axis.x; + var y:Number = axis.y; + var z:Number = axis.z; + + a = t*x*x + c1; + b = t*x*y - z*s; + c = t*x*z + y*s; + + e = t*x*y + z*s; + f = t*y*y + c1; + g = t*y*z - x*s; + + i = t*x*z - y*s; + j = t*y*z + x*s; + k = t*z*z + c1; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix3 { + return new Matrix3(a, b, c, e, f, g, i, j, k); + } + + public function toString():String { + return "[Matrix3 (" + a + ", " + b + ", " + c + "), (" + e + ", " + f + ", " + g + "), (" + i + ", " + j + ", " + k + ")]"; + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/types/.svn/text-base/Matrix4.as.svn-base b/0.0.2.0/src/alternativa/physics/types/.svn/text-base/Matrix4.as.svn-base new file mode 100644 index 0000000..8cc23b2 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/types/.svn/text-base/Matrix4.as.svn-base @@ -0,0 +1,402 @@ +package alternativa.physics.types { + import __AS3__.vec.Vector; + + /** + * + */ + public class Matrix4 { + + public var a:Number; + public var b:Number; + public var c:Number; + public var d:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + public var h:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + public var l:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix4(a:Number = 1, b:Number = 0, c:Number = 0, d:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, h:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1, l:Number = 0) { + this.a = a; + this.b = b; + this.c = c; + this.d = h; + + this.e = e; + this.f = f; + this.g = g; + this.h = h; + + this.i = i; + this.j = j; + this.k = k; + this.l = l; + } + + /** + * + * @return + */ + public function toIdentity():Matrix4 { + a = f = k = 1; + b = c = e = g = i = j = d = h = l = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + var det:Number = -cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk; + + a = (-gg*jj + ff*kk)/det; + b = (cc*jj - bb*kk)/det; + c = (-cc*ff + bb*gg)/det; + d = (dd*gg*jj - cc*hh*jj - dd*ff*kk + bb*hh*kk + cc*ff*ll - bb*gg*ll)/det; + e = (gg*ii - ee*kk)/det; + f = (-cc*ii + aa*kk)/det; + g = (cc*ee - aa*gg)/det; + h = (cc*hh*ii - dd*gg*ii + dd*ee*kk - aa*hh*kk - cc*ee*ll + aa*gg*ll)/det; + i = (-ff*ii + ee*jj)/det; + j = (bb*ii - aa*jj)/det; + k = (-bb*ee + aa*ff)/det; + l = (dd*ff*ii - bb*hh*ii - dd*ee*jj + aa*hh*jj + bb*ee*ll - aa*ff*ll)/det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + d = m.a*dd + m.b*hh + m.c*ll + m.d; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + h = m.e*dd + m.f*hh + m.g*ll + m.h; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + l = m.i*dd + m.j*hh + m.k*ll + m.l; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + d = aa*m.d + bb*m.h + cc*m.l + dd; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + h = ee*m.d + ff*m.h + gg*m.l + hh; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + l = ii*m.d + jj*m.h + kk*m.l + ll; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix4):Matrix4 { + a += m.a; + b += m.b; + c += m.c; + d += m.d; + e += m.e; + f += m.f; + g += m.g; + h += m.h; + i += m.i; + j += m.j; + k += m.k; + l += m.l; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix4):Matrix4 { + a -= m.a; + b -= m.b; + c -= m.c; + d -= m.d; + e -= m.e; + f -= m.f; + g -= m.g; + h -= m.h; + i -= m.i; + j -= m.j; + k -= m.k; + l -= m.l; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectors(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverse(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(idx:int, axis:Vector3):void { + switch (idx) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + case 3: + axis.x = d; + axis.y = h; + axis.z = l; + return; + } + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix4):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = m.d; + e = m.e; + f = m.f; + g = m.g; + h = m.h; + i = m.i; + j = m.j; + k = m.k; + l = m.l; + + return this; + } + + public function setFromMatrix3(m:Matrix3, offset:Vector3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = offset.x; + e = m.e; + f = m.f; + g = m.g; + h = offset.y; + i = m.i; + j = m.j; + k = m.k; + l = offset.z; + + return this; + } + + /** + * @param rotations + */ + public function getEulerAngles(rotations:Vector3):void { + if (-1 < i && i < 1) { + rotations.x = Math.atan2(j, k); + rotations.y = -Math.asin(i); + rotations.z = Math.atan2(e, a); + } else { + rotations.x = 0; + rotations.y = 0.5*((i <= -1) ? Math.PI : -Math.PI); + rotations.z = Math.atan2(-b, f); + } + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix4 { + return new Matrix4(a, b, c, d, e, f, g, h, i, j, k, l); + } + + public function toString():String { + return "[Matrix4 [" + a.toFixed(3) + " " + b.toFixed(3) + " " + c.toFixed(3) + " " + d.toFixed(3) + "] [" + e.toFixed(3) + " " + f.toFixed(3) + " " + g.toFixed(3) + " " + h.toFixed(3) + "] [" + i.toFixed(3) + " " + j.toFixed(3) + " " + k.toFixed(3) + " " + l.toFixed(3) + "]]"; + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/types/.svn/text-base/Quaternion.as.svn-base b/0.0.2.0/src/alternativa/physics/types/.svn/text-base/Quaternion.as.svn-base new file mode 100644 index 0000000..7ad2af0 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/types/.svn/text-base/Quaternion.as.svn-base @@ -0,0 +1,396 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + public class Quaternion { + + public var w:Number; + public var x:Number; + public var y:Number; + public var z:Number; + + public static function multiply(q1:Quaternion, q2:Quaternion, result:Quaternion):void { + result.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z; + result.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y; + result.y = q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z; + result.z = q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x; + } + + public static function createFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngle(axis, angle); + return q; + } + + public static function createFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngleComponents(x, y, z, angle); + return q; + } + + public function Quaternion(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0) { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + } + + public function reset(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0):Quaternion { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + return this; + } + + public function normalize():Quaternion { + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * Умножает на указанный кватернион слева: this * q + * + * @param q множитель + */ + public function prepend(q:Quaternion):Quaternion { + var ww:Number = w*q.w - x*q.x - y*q.y - z*q.z; + var xx:Number = w*q.x + x*q.w + y*q.z - z*q.y; + var yy:Number = w*q.y + y*q.w + z*q.x - x*q.z; + var zz:Number = w*q.z + z*q.w + x*q.y - y*q.x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Умножает на указанный кватернион справа: q * this + * + * @param q множитель + */ + public function append(q:Quaternion):Quaternion { + var ww:Number = q.w*w - q.x*x - q.y*y - q.z*z; + var xx:Number = q.w*x + q.x*w + q.y*z - q.z*y; + var yy:Number = q.w*y + q.y*w + q.z*x - q.x*z; + var zz:Number = q.w*z + q.z*w + q.x*y - q.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * + * @param vector + */ + public function rotateByVector(v:Vector3):Quaternion { + var ww:Number = -v.x*x - v.y*y - v.z*z; + var xx:Number = v.x*w + v.y*z - v.z*y; + var yy:Number = v.y*w + v.z*x - v.x*z; + var zz:Number = v.z*w + v.x*y - v.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Добавляет вращение, приданное вектором угловой скорости за указанное время. + * + * @param v + * @param scale + */ + public function addScaledVector(v:Vector3, scale:Number):Quaternion { + var vx:Number = v.x*scale; + var vy:Number = v.y*scale; + var vz:Number = v.z*scale; + var ww:Number = -x*vx - y*vy - z*vz; + var xx:Number = vx*w + vy*z - vz*y; + var yy:Number = vy*w + vz*x - vx*z; + var zz:Number = vz*w + vx*y - vy*x; + w += 0.5*ww; + x += 0.5*xx; + y += 0.5*yy; + z += 0.5*zz; + // inlined normalize + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param m + * @return + */ + public function toMatrix3(m:Matrix3):Quaternion { + var xx2:Number = 2*x*x; + var yy2:Number = 2*y*y; + var zz2:Number = 2*z*z; + var xy2:Number = 2*x*y; + var yz2:Number = 2*y*z; + var zx2:Number = 2*z*x; + var wx2:Number = 2*w*x; + var wy2:Number = 2*w*y; + var wz2:Number = 2*w*z; + + m.a = 1 - yy2 - zz2; + m.b = xy2 - wz2; + m.c = zx2 + wy2; + + m.e = xy2 + wz2; + m.f = 1 - xx2 - zz2; + m.g = yz2 - wx2; + + m.i = zx2 - wy2; + m.j = yz2 + wx2; + m.k = 1 - xx2 - yy2; + return this; + } + + /** + * + * @return + */ + public function length():Number { + return Math.sqrt(w*w + x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function lengthSqr():Number { + return w*w + x*x + y*y + z*z; + } + + /** + * + * @param axis + * @param angle + * @return + */ + public function setFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z); + x = axis.x*k; + y = axis.y*k; + z = axis.z*k; + return this; + } + + /** + * + * @param x + * @param y + * @param z + * @param angle + */ + public function setFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(x*x + y*y + z*z); + this.x = x*k; + this.y = y*k; + this.z = z*k; + return this; + } + + /** + * + * @param vector + */ + public function toAxisVector(v:Vector3 = null):Vector3 { + if (w < -1 || w > 1) { + normalize(); + } + if (v == null) { + v = new Vector3(); + } + if (w > -1 && w < 1) { + if (w == 0) { + v.x = x; + v.y = y; + v.z = z; + } else { + var angle:Number = 2*Math.acos(w); + var coeff:Number = 1/Math.sqrt(1 - w*w); + v.x = x*coeff*angle; + v.y = y*coeff*angle; + v.z = z*coeff*angle; + } + } else { + v.x = 0; + v.y = 0; + v.z = 0; + } + return v; + } + + /** + * + * @param rotations + */ + public function getEulerAngles(angles:Vector3):Vector3 { + var qi2:Number = 2*x*x; + var qj2:Number = 2*y*y; + var qk2:Number = 2*z*z; + var qij:Number = 2*x*y; + var qjk:Number = 2*y*z; + var qki:Number = 2*z*x; + var qri:Number = 2*w*x; + var qrj:Number = 2*w*y; + var qrk:Number = 2*w*z; + + var aa:Number = 1 - qj2 - qk2; + var bb:Number = qij - qrk; + var ee:Number = qij + qrk; + var ff:Number = 1 - qi2 - qk2; + var ii:Number = qki - qrj; + var jj:Number = qjk + qri; + var kk:Number = 1 - qi2 - qj2; + + if (-1 < ii && ii < 1) { + if (angles == null) { + angles = new Vector3(Math.atan2(jj, kk), -Math.asin(ii), Math.atan2(ee, aa)); + } else { + angles.x = Math.atan2(jj, kk); + angles.y = -Math.asin(ii); + angles.z = Math.atan2(ee, aa); + } + } else { + if (angles == null) { + angles = new Vector3(0, 0.5*((ii <= -1) ? Math.PI : -Math.PI), Math.atan2(-bb, ff)); + } else { + angles.x = 0; + angles.y = 0.5*((ii <= -1) ? Math.PI : -Math.PI); + angles.z = Math.atan2(-bb, ff); + } + } + return angles; + } + + /** + * + */ + public function conjugate():void { + x = -x; + y = -y; + z = -z; + } + + /** + * Выполняет линейную интерполяцию. + * + * @param q1 начало отрезка + * @param q2 конец отрезка + * @param t время, обычно задаётся в интервале [0, 1] + */ + public function nlerp(q1:Quaternion, q2:Quaternion, t:Number):Quaternion { + var d:Number = 1 - t; + w = q1.w*d + q2.w*t; + x = q1.x*d + q2.x*t; + y = q1.y*d + q2.y*t; + z = q1.z*d + q2.z*t; + // inlined normalize + d = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param q + * @return + */ + public function subtract(q:Quaternion):Quaternion { + w -= q.w; + x -= q.x; + y -= q.y; + z -= q.z; + return this; + } + + /** + * + * @param q1 + * @param q2 + * @return + */ + public function diff(q1:Quaternion, q2:Quaternion):Quaternion { + w = q2.w - q1.w; + x = q2.x - q1.x; + y = q2.y - q1.y; + z = q2.z - q1.z; + return this; + } + + /** + * + * @param q + * @return + */ + public function copy(q:Quaternion):Quaternion { + w = q.w; + x = q.x; + y = q.y; + z = q.z; + return this; + } + + /** + * @param reslult + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + result.w = w; + return result; + } + + /** + * + * @return + */ + public function clone():Quaternion { + return new Quaternion(w, x, y, z); + } + + /** + * + * @return + */ + public function toString():String { + return "[" + w + ", " + x + ", " + y + ", " + z + "]"; + } + } +} diff --git a/0.0.2.0/src/alternativa/physics/types/.svn/text-base/Vector3.as.svn-base b/0.0.2.0/src/alternativa/physics/types/.svn/text-base/Vector3.as.svn-base new file mode 100644 index 0000000..13ffda0 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/types/.svn/text-base/Vector3.as.svn-base @@ -0,0 +1,331 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + /** + * + */ + public class Vector3 extends Vector3D { + + public static const ZERO:Vector3 = new Vector3(0, 0, 0); + public static const X_AXIS:Vector3 = new Vector3(1, 0, 0); + public static const Y_AXIS:Vector3 = new Vector3(0, 1, 0); + public static const Z_AXIS:Vector3 = new Vector3(0, 0, 1); + + /** + * + * @param x + * @param y + * @param z + */ + public function Vector3(x:Number = 0, y:Number = 0, z:Number = 0) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * + * @return + */ + public function vLength():Number { + return Math.sqrt(x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function vLengthSqr():Number { + return x*x + y*y + z*z; + } + + /** + * + * @param length + * @return + */ + public function vSetLength(length:Number):Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = length; + } else { + var k:Number = length/Math.sqrt(x*x + y*y + z*z); + x *= k; + y *= k; + z *= k; + } + return this; + } + + /** + * + * @return + */ + public function vNormalize():Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = 1; + } else { + d = Math.sqrt(d); + x /= d; + y /= d; + z /= d; + } + return this; + } + + /** + * Сложение координат. + * + * @param v точка, координаты которой прибавляются к собственным + */ + public function vAdd(v:Vector3):Vector3 { + x += v.x; + y += v.y; + z += v.z; + return this; + } + + /** + * + * @param k + * @param v + * @return + */ + public function vAddScaled(k:Number, v:Vector3):Vector3 { + x += k*v.x; + y += k*v.y; + z += k*v.z; + return this; + } + + /** + * Вычитание координат. + * + * @param v точка, координаты которой вычитаются из собственных + */ + public function vSubtract(v:Vector3):Vector3 { + x -= v.x; + y -= v.y; + z -= v.z; + return this; + } + + /** + * Вычисляет сумму векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + * @return this + */ + public function vSum(a:Vector3, b:Vector3):Vector3 { + x = a.x + b.x; + y = a.y + b.y; + z = a.z + b.z; + return this; + } + + /** + * Вычисление разности векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + */ + public function vDiff(a:Vector3, b:Vector3):Vector3 { + x = a.x - b.x; + y = a.y - b.y; + z = a.z - b.z; + return this; + } + + /** + * Умножение на скаляр. + * + * @param k число, на которое умножаются координаты + */ + public function vScale(k:Number):Vector3 { + x *= k; + y *= k; + z *= k; + return this; + } + + /** + * Инвертирование вектора. + */ + public function vReverse():Vector3 { + x = -x; + y = -y; + z = -z; + return this; + } + + /** + * + * @param v + * @return + */ + public function vDot(v:Vector3):Number { + return x*v.x + y*v.y + z*v.z; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross(v:Vector3):Vector3 { + var xx:Number = y*v.z - z*v.y; + var yy:Number = z*v.x - x*v.z; + var zz:Number = x*v.y - y*v.x; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross2(a:Vector3, b:Vector3):Vector3 { + x = a.y*b.z - a.z*b.y; + y = a.z*b.x - a.x*b.z; + z = a.x*b.y - a.y*b.x; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy3(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * + * @param matrix + */ + public function vTransformBy3Tr(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz + m.d; + y = m.e*xx + m.f*yy + m.g*zz + m.h; + z = m.i*xx + m.j*yy + m.k*zz + m.l; + return this; + } + + /** + * + * @param m + * @return + */ + public function vTransformInverseBy4(m:Matrix4):Vector3 { + var xx:Number = x - m.d; + var yy:Number = y - m.h; + var zz:Number = z - m.l; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * + * @param m + */ + public function vDeltaTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * Установка координат. + */ + public function vReset(x:Number = 0, y:Number = 0, z:Number = 0):Vector3 { + this.x = x; + this.y = y; + this.z = z; + return this; + } + + /** + * Копирование координат точки. + * + * @param v точка, координаты которой копируются + */ + public function vCopy(v:Vector3):Vector3 { + x = v.x; + y = v.y; + z = v.z; + return this; + } + + /** + * Клонирование точки. + * + * @return клонированная точка + */ + public function vClone():Vector3 { + return new Vector3(x, y, z); + } + + /** + * Копирует координаты вектора в экземпляр класса Vector3D. + * + * @param result экземпляр, в который копируются координаты + * @return переданный параметр + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + return result; + } + + /** + * @param source + * @return + */ + public function copyFromVector3D(source:Vector3D):Vector3 { + x = source.x; + y = source.y; + z = source.z; + return this; + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/types/Matrix3.as b/0.0.2.0/src/alternativa/physics/types/Matrix3.as new file mode 100644 index 0000000..e07d0d1 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/types/Matrix3.as @@ -0,0 +1,396 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + /** + * + */ + public class Matrix3 { + + public static const ZERO:Matrix3 = new Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0); + public static const IDENTITY:Matrix3 = new Matrix3(); + + public var a:Number; + public var b:Number; + public var c:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix3(a:Number = 1, b:Number = 0, c:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1) { + this.a = a; + this.b = b; + this.c = c; + + this.e = e; + this.f = f; + this.g = g; + + this.i = i; + this.j = j; + this.k = k; + } + + /** + * + * @return + */ + public function toIdentity():Matrix3 { + a = f = k = 1; + b = c = e = g = i = j = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + var det:Number = 1/(-cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk); + + a = (ff*kk - gg*jj)*det; + b = (cc*jj - bb*kk)*det; + c = (bb*gg - cc*ff)*det; + e = (gg*ii - ee*kk)*det; + f = (aa*kk - cc*ii)*det; + g = (cc*ee - aa*gg)*det; + i = (ee*jj - ff*ii)*det; + j = (bb*ii - aa*jj)*det; + k = (aa*ff - bb*ee)*det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prependTransposed(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.b + cc*m.c; + b = aa*m.e + bb*m.f + cc*m.g; + c = aa*m.i + bb*m.j + cc*m.k; + e = ee*m.a + ff*m.b + gg*m.c; + f = ee*m.e + ff*m.f + gg*m.g; + g = ee*m.i + ff*m.j + gg*m.k; + i = ii*m.a + jj*m.b + kk*m.c; + j = ii*m.e + jj*m.f + kk*m.g; + k = ii*m.i + jj*m.j + kk*m.k; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix3):Matrix3 { + a += m.a; + b += m.b; + c += m.c; + e += m.e; + f += m.f; + g += m.g; + i += m.i; + j += m.j; + k += m.k; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix3):Matrix3 { + a -= m.a; + b -= m.b; + c -= m.c; + e -= m.e; + f -= m.f; + g -= m.g; + i -= m.i; + j -= m.j; + k -= m.k; + + return this; + } + + /** + * + * @return + */ + public function transpose():Matrix3 { + var tmp:Number = b; + b = e; + e = tmp; + tmp = c; + c = i; + i = tmp; + tmp = g; + g = j; + j = tmp; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector3To3D(vin:Vector3, vout:Vector3D):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * + * @param matrix + */ + public function createSkewSymmetric(v:Vector3):Matrix3 { + a = f = k = 0; + b = -v.z; + c = v.y; + e = v.z; + g = -v.x; + i = -v.y; + j = v.x; + return this; + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(i:int, axis:Vector3):void { + switch (i) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + } + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix3):Matrix3 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @return + */ + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):Matrix3 { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + + return this; + } + + /** + * Метод формирует матрицу поворота на заданный угол относительно заданной оси. + * + * @param axis нормализованный вектор, задающий ось, относительно которой выполняется поворот + * @param angle угол поворота в радианах + */ + public function fromAxisAngle(axis:Vector3, angle:Number):void { + var c1:Number = Math.cos(angle); + var s:Number = Math.sin(angle); + var t:Number = 1 - c1; + var x:Number = axis.x; + var y:Number = axis.y; + var z:Number = axis.z; + + a = t*x*x + c1; + b = t*x*y - z*s; + c = t*x*z + y*s; + + e = t*x*y + z*s; + f = t*y*y + c1; + g = t*y*z - x*s; + + i = t*x*z - y*s; + j = t*y*z + x*s; + k = t*z*z + c1; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix3 { + return new Matrix3(a, b, c, e, f, g, i, j, k); + } + + public function toString():String { + return "[Matrix3 (" + a + ", " + b + ", " + c + "), (" + e + ", " + f + ", " + g + "), (" + i + ", " + j + ", " + k + ")]"; + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/types/Matrix4.as b/0.0.2.0/src/alternativa/physics/types/Matrix4.as new file mode 100644 index 0000000..8cc23b2 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/types/Matrix4.as @@ -0,0 +1,402 @@ +package alternativa.physics.types { + import __AS3__.vec.Vector; + + /** + * + */ + public class Matrix4 { + + public var a:Number; + public var b:Number; + public var c:Number; + public var d:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + public var h:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + public var l:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix4(a:Number = 1, b:Number = 0, c:Number = 0, d:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, h:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1, l:Number = 0) { + this.a = a; + this.b = b; + this.c = c; + this.d = h; + + this.e = e; + this.f = f; + this.g = g; + this.h = h; + + this.i = i; + this.j = j; + this.k = k; + this.l = l; + } + + /** + * + * @return + */ + public function toIdentity():Matrix4 { + a = f = k = 1; + b = c = e = g = i = j = d = h = l = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + var det:Number = -cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk; + + a = (-gg*jj + ff*kk)/det; + b = (cc*jj - bb*kk)/det; + c = (-cc*ff + bb*gg)/det; + d = (dd*gg*jj - cc*hh*jj - dd*ff*kk + bb*hh*kk + cc*ff*ll - bb*gg*ll)/det; + e = (gg*ii - ee*kk)/det; + f = (-cc*ii + aa*kk)/det; + g = (cc*ee - aa*gg)/det; + h = (cc*hh*ii - dd*gg*ii + dd*ee*kk - aa*hh*kk - cc*ee*ll + aa*gg*ll)/det; + i = (-ff*ii + ee*jj)/det; + j = (bb*ii - aa*jj)/det; + k = (-bb*ee + aa*ff)/det; + l = (dd*ff*ii - bb*hh*ii - dd*ee*jj + aa*hh*jj + bb*ee*ll - aa*ff*ll)/det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + d = m.a*dd + m.b*hh + m.c*ll + m.d; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + h = m.e*dd + m.f*hh + m.g*ll + m.h; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + l = m.i*dd + m.j*hh + m.k*ll + m.l; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + d = aa*m.d + bb*m.h + cc*m.l + dd; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + h = ee*m.d + ff*m.h + gg*m.l + hh; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + l = ii*m.d + jj*m.h + kk*m.l + ll; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix4):Matrix4 { + a += m.a; + b += m.b; + c += m.c; + d += m.d; + e += m.e; + f += m.f; + g += m.g; + h += m.h; + i += m.i; + j += m.j; + k += m.k; + l += m.l; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix4):Matrix4 { + a -= m.a; + b -= m.b; + c -= m.c; + d -= m.d; + e -= m.e; + f -= m.f; + g -= m.g; + h -= m.h; + i -= m.i; + j -= m.j; + k -= m.k; + l -= m.l; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectors(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverse(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(idx:int, axis:Vector3):void { + switch (idx) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + case 3: + axis.x = d; + axis.y = h; + axis.z = l; + return; + } + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix4):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = m.d; + e = m.e; + f = m.f; + g = m.g; + h = m.h; + i = m.i; + j = m.j; + k = m.k; + l = m.l; + + return this; + } + + public function setFromMatrix3(m:Matrix3, offset:Vector3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = offset.x; + e = m.e; + f = m.f; + g = m.g; + h = offset.y; + i = m.i; + j = m.j; + k = m.k; + l = offset.z; + + return this; + } + + /** + * @param rotations + */ + public function getEulerAngles(rotations:Vector3):void { + if (-1 < i && i < 1) { + rotations.x = Math.atan2(j, k); + rotations.y = -Math.asin(i); + rotations.z = Math.atan2(e, a); + } else { + rotations.x = 0; + rotations.y = 0.5*((i <= -1) ? Math.PI : -Math.PI); + rotations.z = Math.atan2(-b, f); + } + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix4 { + return new Matrix4(a, b, c, d, e, f, g, h, i, j, k, l); + } + + public function toString():String { + return "[Matrix4 [" + a.toFixed(3) + " " + b.toFixed(3) + " " + c.toFixed(3) + " " + d.toFixed(3) + "] [" + e.toFixed(3) + " " + f.toFixed(3) + " " + g.toFixed(3) + " " + h.toFixed(3) + "] [" + i.toFixed(3) + " " + j.toFixed(3) + " " + k.toFixed(3) + " " + l.toFixed(3) + "]]"; + } + + } +} \ No newline at end of file diff --git a/0.0.2.0/src/alternativa/physics/types/Quaternion.as b/0.0.2.0/src/alternativa/physics/types/Quaternion.as new file mode 100644 index 0000000..7ad2af0 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/types/Quaternion.as @@ -0,0 +1,396 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + public class Quaternion { + + public var w:Number; + public var x:Number; + public var y:Number; + public var z:Number; + + public static function multiply(q1:Quaternion, q2:Quaternion, result:Quaternion):void { + result.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z; + result.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y; + result.y = q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z; + result.z = q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x; + } + + public static function createFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngle(axis, angle); + return q; + } + + public static function createFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngleComponents(x, y, z, angle); + return q; + } + + public function Quaternion(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0) { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + } + + public function reset(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0):Quaternion { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + return this; + } + + public function normalize():Quaternion { + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * Умножает на указанный кватернион слева: this * q + * + * @param q множитель + */ + public function prepend(q:Quaternion):Quaternion { + var ww:Number = w*q.w - x*q.x - y*q.y - z*q.z; + var xx:Number = w*q.x + x*q.w + y*q.z - z*q.y; + var yy:Number = w*q.y + y*q.w + z*q.x - x*q.z; + var zz:Number = w*q.z + z*q.w + x*q.y - y*q.x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Умножает на указанный кватернион справа: q * this + * + * @param q множитель + */ + public function append(q:Quaternion):Quaternion { + var ww:Number = q.w*w - q.x*x - q.y*y - q.z*z; + var xx:Number = q.w*x + q.x*w + q.y*z - q.z*y; + var yy:Number = q.w*y + q.y*w + q.z*x - q.x*z; + var zz:Number = q.w*z + q.z*w + q.x*y - q.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * + * @param vector + */ + public function rotateByVector(v:Vector3):Quaternion { + var ww:Number = -v.x*x - v.y*y - v.z*z; + var xx:Number = v.x*w + v.y*z - v.z*y; + var yy:Number = v.y*w + v.z*x - v.x*z; + var zz:Number = v.z*w + v.x*y - v.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Добавляет вращение, приданное вектором угловой скорости за указанное время. + * + * @param v + * @param scale + */ + public function addScaledVector(v:Vector3, scale:Number):Quaternion { + var vx:Number = v.x*scale; + var vy:Number = v.y*scale; + var vz:Number = v.z*scale; + var ww:Number = -x*vx - y*vy - z*vz; + var xx:Number = vx*w + vy*z - vz*y; + var yy:Number = vy*w + vz*x - vx*z; + var zz:Number = vz*w + vx*y - vy*x; + w += 0.5*ww; + x += 0.5*xx; + y += 0.5*yy; + z += 0.5*zz; + // inlined normalize + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param m + * @return + */ + public function toMatrix3(m:Matrix3):Quaternion { + var xx2:Number = 2*x*x; + var yy2:Number = 2*y*y; + var zz2:Number = 2*z*z; + var xy2:Number = 2*x*y; + var yz2:Number = 2*y*z; + var zx2:Number = 2*z*x; + var wx2:Number = 2*w*x; + var wy2:Number = 2*w*y; + var wz2:Number = 2*w*z; + + m.a = 1 - yy2 - zz2; + m.b = xy2 - wz2; + m.c = zx2 + wy2; + + m.e = xy2 + wz2; + m.f = 1 - xx2 - zz2; + m.g = yz2 - wx2; + + m.i = zx2 - wy2; + m.j = yz2 + wx2; + m.k = 1 - xx2 - yy2; + return this; + } + + /** + * + * @return + */ + public function length():Number { + return Math.sqrt(w*w + x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function lengthSqr():Number { + return w*w + x*x + y*y + z*z; + } + + /** + * + * @param axis + * @param angle + * @return + */ + public function setFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z); + x = axis.x*k; + y = axis.y*k; + z = axis.z*k; + return this; + } + + /** + * + * @param x + * @param y + * @param z + * @param angle + */ + public function setFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(x*x + y*y + z*z); + this.x = x*k; + this.y = y*k; + this.z = z*k; + return this; + } + + /** + * + * @param vector + */ + public function toAxisVector(v:Vector3 = null):Vector3 { + if (w < -1 || w > 1) { + normalize(); + } + if (v == null) { + v = new Vector3(); + } + if (w > -1 && w < 1) { + if (w == 0) { + v.x = x; + v.y = y; + v.z = z; + } else { + var angle:Number = 2*Math.acos(w); + var coeff:Number = 1/Math.sqrt(1 - w*w); + v.x = x*coeff*angle; + v.y = y*coeff*angle; + v.z = z*coeff*angle; + } + } else { + v.x = 0; + v.y = 0; + v.z = 0; + } + return v; + } + + /** + * + * @param rotations + */ + public function getEulerAngles(angles:Vector3):Vector3 { + var qi2:Number = 2*x*x; + var qj2:Number = 2*y*y; + var qk2:Number = 2*z*z; + var qij:Number = 2*x*y; + var qjk:Number = 2*y*z; + var qki:Number = 2*z*x; + var qri:Number = 2*w*x; + var qrj:Number = 2*w*y; + var qrk:Number = 2*w*z; + + var aa:Number = 1 - qj2 - qk2; + var bb:Number = qij - qrk; + var ee:Number = qij + qrk; + var ff:Number = 1 - qi2 - qk2; + var ii:Number = qki - qrj; + var jj:Number = qjk + qri; + var kk:Number = 1 - qi2 - qj2; + + if (-1 < ii && ii < 1) { + if (angles == null) { + angles = new Vector3(Math.atan2(jj, kk), -Math.asin(ii), Math.atan2(ee, aa)); + } else { + angles.x = Math.atan2(jj, kk); + angles.y = -Math.asin(ii); + angles.z = Math.atan2(ee, aa); + } + } else { + if (angles == null) { + angles = new Vector3(0, 0.5*((ii <= -1) ? Math.PI : -Math.PI), Math.atan2(-bb, ff)); + } else { + angles.x = 0; + angles.y = 0.5*((ii <= -1) ? Math.PI : -Math.PI); + angles.z = Math.atan2(-bb, ff); + } + } + return angles; + } + + /** + * + */ + public function conjugate():void { + x = -x; + y = -y; + z = -z; + } + + /** + * Выполняет линейную интерполяцию. + * + * @param q1 начало отрезка + * @param q2 конец отрезка + * @param t время, обычно задаётся в интервале [0, 1] + */ + public function nlerp(q1:Quaternion, q2:Quaternion, t:Number):Quaternion { + var d:Number = 1 - t; + w = q1.w*d + q2.w*t; + x = q1.x*d + q2.x*t; + y = q1.y*d + q2.y*t; + z = q1.z*d + q2.z*t; + // inlined normalize + d = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param q + * @return + */ + public function subtract(q:Quaternion):Quaternion { + w -= q.w; + x -= q.x; + y -= q.y; + z -= q.z; + return this; + } + + /** + * + * @param q1 + * @param q2 + * @return + */ + public function diff(q1:Quaternion, q2:Quaternion):Quaternion { + w = q2.w - q1.w; + x = q2.x - q1.x; + y = q2.y - q1.y; + z = q2.z - q1.z; + return this; + } + + /** + * + * @param q + * @return + */ + public function copy(q:Quaternion):Quaternion { + w = q.w; + x = q.x; + y = q.y; + z = q.z; + return this; + } + + /** + * @param reslult + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + result.w = w; + return result; + } + + /** + * + * @return + */ + public function clone():Quaternion { + return new Quaternion(w, x, y, z); + } + + /** + * + * @return + */ + public function toString():String { + return "[" + w + ", " + x + ", " + y + ", " + z + "]"; + } + } +} diff --git a/0.0.2.0/src/alternativa/physics/types/Vector3.as b/0.0.2.0/src/alternativa/physics/types/Vector3.as new file mode 100644 index 0000000..13ffda0 --- /dev/null +++ b/0.0.2.0/src/alternativa/physics/types/Vector3.as @@ -0,0 +1,331 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + /** + * + */ + public class Vector3 extends Vector3D { + + public static const ZERO:Vector3 = new Vector3(0, 0, 0); + public static const X_AXIS:Vector3 = new Vector3(1, 0, 0); + public static const Y_AXIS:Vector3 = new Vector3(0, 1, 0); + public static const Z_AXIS:Vector3 = new Vector3(0, 0, 1); + + /** + * + * @param x + * @param y + * @param z + */ + public function Vector3(x:Number = 0, y:Number = 0, z:Number = 0) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * + * @return + */ + public function vLength():Number { + return Math.sqrt(x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function vLengthSqr():Number { + return x*x + y*y + z*z; + } + + /** + * + * @param length + * @return + */ + public function vSetLength(length:Number):Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = length; + } else { + var k:Number = length/Math.sqrt(x*x + y*y + z*z); + x *= k; + y *= k; + z *= k; + } + return this; + } + + /** + * + * @return + */ + public function vNormalize():Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = 1; + } else { + d = Math.sqrt(d); + x /= d; + y /= d; + z /= d; + } + return this; + } + + /** + * Сложение координат. + * + * @param v точка, координаты которой прибавляются к собственным + */ + public function vAdd(v:Vector3):Vector3 { + x += v.x; + y += v.y; + z += v.z; + return this; + } + + /** + * + * @param k + * @param v + * @return + */ + public function vAddScaled(k:Number, v:Vector3):Vector3 { + x += k*v.x; + y += k*v.y; + z += k*v.z; + return this; + } + + /** + * Вычитание координат. + * + * @param v точка, координаты которой вычитаются из собственных + */ + public function vSubtract(v:Vector3):Vector3 { + x -= v.x; + y -= v.y; + z -= v.z; + return this; + } + + /** + * Вычисляет сумму векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + * @return this + */ + public function vSum(a:Vector3, b:Vector3):Vector3 { + x = a.x + b.x; + y = a.y + b.y; + z = a.z + b.z; + return this; + } + + /** + * Вычисление разности векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + */ + public function vDiff(a:Vector3, b:Vector3):Vector3 { + x = a.x - b.x; + y = a.y - b.y; + z = a.z - b.z; + return this; + } + + /** + * Умножение на скаляр. + * + * @param k число, на которое умножаются координаты + */ + public function vScale(k:Number):Vector3 { + x *= k; + y *= k; + z *= k; + return this; + } + + /** + * Инвертирование вектора. + */ + public function vReverse():Vector3 { + x = -x; + y = -y; + z = -z; + return this; + } + + /** + * + * @param v + * @return + */ + public function vDot(v:Vector3):Number { + return x*v.x + y*v.y + z*v.z; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross(v:Vector3):Vector3 { + var xx:Number = y*v.z - z*v.y; + var yy:Number = z*v.x - x*v.z; + var zz:Number = x*v.y - y*v.x; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross2(a:Vector3, b:Vector3):Vector3 { + x = a.y*b.z - a.z*b.y; + y = a.z*b.x - a.x*b.z; + z = a.x*b.y - a.y*b.x; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy3(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * + * @param matrix + */ + public function vTransformBy3Tr(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz + m.d; + y = m.e*xx + m.f*yy + m.g*zz + m.h; + z = m.i*xx + m.j*yy + m.k*zz + m.l; + return this; + } + + /** + * + * @param m + * @return + */ + public function vTransformInverseBy4(m:Matrix4):Vector3 { + var xx:Number = x - m.d; + var yy:Number = y - m.h; + var zz:Number = z - m.l; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * + * @param m + */ + public function vDeltaTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * Установка координат. + */ + public function vReset(x:Number = 0, y:Number = 0, z:Number = 0):Vector3 { + this.x = x; + this.y = y; + this.z = z; + return this; + } + + /** + * Копирование координат точки. + * + * @param v точка, координаты которой копируются + */ + public function vCopy(v:Vector3):Vector3 { + x = v.x; + y = v.y; + z = v.z; + return this; + } + + /** + * Клонирование точки. + * + * @return клонированная точка + */ + public function vClone():Vector3 { + return new Vector3(x, y, z); + } + + /** + * Копирует координаты вектора в экземпляр класса Vector3D. + * + * @param result экземпляр, в который копируются координаты + * @return переданный параметр + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + return result; + } + + /** + * @param source + * @return + */ + public function copyFromVector3D(source:Vector3D):Vector3 { + x = source.x; + y = source.y; + z = source.z; + return this; + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/.actionScriptProperties b/0.0.3.0/.actionScriptProperties new file mode 100644 index 0000000..f73adc8 --- /dev/null +++ b/0.0.3.0/.actionScriptProperties @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/0.0.3.0/.flexLibProperties b/0.0.3.0/.flexLibProperties new file mode 100644 index 0000000..9198733 --- /dev/null +++ b/0.0.3.0/.flexLibProperties @@ -0,0 +1,50 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/0.0.3.0/.project b/0.0.3.0/.project new file mode 100644 index 0000000..1f1425f --- /dev/null +++ b/0.0.3.0/.project @@ -0,0 +1,19 @@ + + + AlternativaPhysics + + + + + + com.adobe.flexbuilder.project.flexbuilder + + + + + + org.maven.ide.eclipse.maven2Nature + com.adobe.flexbuilder.project.flexlibnature + com.adobe.flexbuilder.project.actionscriptnature + + diff --git a/0.0.3.0/.settings/.svn/all-wcprops b/0.0.3.0/.settings/.svn/all-wcprops new file mode 100644 index 0000000..d0e9a8b --- /dev/null +++ b/0.0.3.0/.settings/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 89 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/.settings +END +org.eclipse.core.resources.prefs +K 25 +svn:wc:ra_dav:version-url +V 122 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/.settings/org.eclipse.core.resources.prefs +END diff --git a/0.0.3.0/.settings/.svn/entries b/0.0.3.0/.settings/.svn/entries new file mode 100644 index 0000000..ca29c24 --- /dev/null +++ b/0.0.3.0/.settings/.svn/entries @@ -0,0 +1,40 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/.settings +http://svndev.alternativaplatform.com + + + +2009-04-01T12:25:29.638016Z +10301 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +org.eclipse.core.resources.prefs +file + + + + +2010-10-28T04:32:41.000000Z +4f70b476f3e5075e399505021df2f89b +2009-04-01T12:25:29.638016Z +10301 +mike + diff --git a/0.0.3.0/.settings/.svn/format b/0.0.3.0/.settings/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.3.0/.settings/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.3.0/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base b/0.0.3.0/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base new file mode 100644 index 0000000..b3d0d49 --- /dev/null +++ b/0.0.3.0/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base @@ -0,0 +1,3 @@ +#Fri Mar 20 00:19:33 YEKT 2009 +eclipse.preferences.version=1 +encoding/=UTF-8 diff --git a/0.0.3.0/.settings/org.eclipse.core.resources.prefs b/0.0.3.0/.settings/org.eclipse.core.resources.prefs new file mode 100644 index 0000000..b3d0d49 --- /dev/null +++ b/0.0.3.0/.settings/org.eclipse.core.resources.prefs @@ -0,0 +1,3 @@ +#Fri Mar 20 00:19:33 YEKT 2009 +eclipse.preferences.version=1 +encoding/=UTF-8 diff --git a/0.0.3.0/.svn/all-wcprops b/0.0.3.0/.svn/all-wcprops new file mode 100644 index 0000000..2560bfb --- /dev/null +++ b/0.0.3.0/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 79 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0 +END +.flexLibProperties +K 25 +svn:wc:ra_dav:version-url +V 98 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/.flexLibProperties +END +.project +K 25 +svn:wc:ra_dav:version-url +V 88 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/.project +END +pom.xml +K 25 +svn:wc:ra_dav:version-url +V 87 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/pom.xml +END +.actionScriptProperties +K 25 +svn:wc:ra_dav:version-url +V 103 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/.actionScriptProperties +END diff --git a/0.0.3.0/.svn/dir-prop-base b/0.0.3.0/.svn/dir-prop-base new file mode 100644 index 0000000..8eda4b3 --- /dev/null +++ b/0.0.3.0/.svn/dir-prop-base @@ -0,0 +1,5 @@ +K 13 +svn:mergeinfo +V 85 +/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D:13892-14098 +END diff --git a/0.0.3.0/.svn/entries b/0.0.3.0/.svn/entries new file mode 100644 index 0000000..d79e2bf --- /dev/null +++ b/0.0.3.0/.svn/entries @@ -0,0 +1,85 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0 +http://svndev.alternativaplatform.com + + + +2009-07-06T03:57:24.281994Z +15761 +mike +has-props + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +META-INF +dir + +.flexLibProperties +file + + + + +2010-10-28T04:32:41.000000Z +ffb275c001e332742e82186f68792814 +2009-06-29T14:07:24.833358Z +15390 +mike + +.project +file + + + + +2010-10-28T04:32:41.000000Z +1e90cc68b52b93173b2b5de88eb5c3a3 +2009-04-20T17:15:55.260110Z +11541 +mike + +src +dir + +pom.xml +file + + + + +2010-10-28T04:32:41.000000Z +60079373bbcf24832b8c754aa2e9d1ac +2009-07-06T03:56:56.113843Z +15760 +mike + +.actionScriptProperties +file + + + + +2010-10-28T04:32:41.000000Z +b8656f882fff2baedd57a92db86fcf8c +2009-06-29T14:07:24.833358Z +15390 +mike + +.settings +dir + diff --git a/0.0.3.0/.svn/format b/0.0.3.0/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.3.0/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.3.0/.svn/text-base/.actionScriptProperties.svn-base b/0.0.3.0/.svn/text-base/.actionScriptProperties.svn-base new file mode 100644 index 0000000..f73adc8 --- /dev/null +++ b/0.0.3.0/.svn/text-base/.actionScriptProperties.svn-base @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/0.0.3.0/.svn/text-base/.flexLibProperties.svn-base b/0.0.3.0/.svn/text-base/.flexLibProperties.svn-base new file mode 100644 index 0000000..9198733 --- /dev/null +++ b/0.0.3.0/.svn/text-base/.flexLibProperties.svn-base @@ -0,0 +1,50 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/0.0.3.0/.svn/text-base/.project.svn-base b/0.0.3.0/.svn/text-base/.project.svn-base new file mode 100644 index 0000000..1f1425f --- /dev/null +++ b/0.0.3.0/.svn/text-base/.project.svn-base @@ -0,0 +1,19 @@ + + + AlternativaPhysics + + + + + + com.adobe.flexbuilder.project.flexbuilder + + + + + + org.maven.ide.eclipse.maven2Nature + com.adobe.flexbuilder.project.flexlibnature + com.adobe.flexbuilder.project.actionscriptnature + + diff --git a/0.0.3.0/.svn/text-base/pom.xml.svn-base b/0.0.3.0/.svn/text-base/pom.xml.svn-base new file mode 100644 index 0000000..54edae3 --- /dev/null +++ b/0.0.3.0/.svn/text-base/pom.xml.svn-base @@ -0,0 +1,17 @@ + + 4.0.0 + platform.clients.fp10.libraries + AlternativaPhysics + swc + 0.0.3.0 + + platform.tools.maven + FlashBasePom + 1.0 + + + scm:svn:http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0 + + + + \ No newline at end of file diff --git a/0.0.3.0/META-INF/.svn/all-wcprops b/0.0.3.0/META-INF/.svn/all-wcprops new file mode 100644 index 0000000..67de375 --- /dev/null +++ b/0.0.3.0/META-INF/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 88 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/META-INF +END +MANIFEST.MF +K 25 +svn:wc:ra_dav:version-url +V 100 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/META-INF/MANIFEST.MF +END diff --git a/0.0.3.0/META-INF/.svn/entries b/0.0.3.0/META-INF/.svn/entries new file mode 100644 index 0000000..8d56614 --- /dev/null +++ b/0.0.3.0/META-INF/.svn/entries @@ -0,0 +1,40 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/META-INF +http://svndev.alternativaplatform.com + + + +2009-04-20T17:15:55.260110Z +11541 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +MANIFEST.MF +file + + + + +2010-10-28T04:32:40.000000Z +7b64dceb50a6905850ff00a0bfbe77eb +2009-04-20T17:15:55.260110Z +11541 +mike + diff --git a/0.0.3.0/META-INF/.svn/format b/0.0.3.0/META-INF/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.3.0/META-INF/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.3.0/META-INF/.svn/text-base/MANIFEST.MF.svn-base b/0.0.3.0/META-INF/.svn/text-base/MANIFEST.MF.svn-base new file mode 100644 index 0000000..5066a55 --- /dev/null +++ b/0.0.3.0/META-INF/.svn/text-base/MANIFEST.MF.svn-base @@ -0,0 +1 @@ +Bundle-Name: platform.clients.fp10.libraries.AlternativaPhysics diff --git a/0.0.3.0/META-INF/MANIFEST.MF b/0.0.3.0/META-INF/MANIFEST.MF new file mode 100644 index 0000000..5066a55 --- /dev/null +++ b/0.0.3.0/META-INF/MANIFEST.MF @@ -0,0 +1 @@ +Bundle-Name: platform.clients.fp10.libraries.AlternativaPhysics diff --git a/0.0.3.0/pom.xml b/0.0.3.0/pom.xml new file mode 100644 index 0000000..54edae3 --- /dev/null +++ b/0.0.3.0/pom.xml @@ -0,0 +1,17 @@ + + 4.0.0 + platform.clients.fp10.libraries + AlternativaPhysics + swc + 0.0.3.0 + + platform.tools.maven + FlashBasePom + 1.0 + + + scm:svn:http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0 + + + + \ No newline at end of file diff --git a/0.0.3.0/src/.svn/all-wcprops b/0.0.3.0/src/.svn/all-wcprops new file mode 100644 index 0000000..66a9eae --- /dev/null +++ b/0.0.3.0/src/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 83 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src +END diff --git a/0.0.3.0/src/.svn/entries b/0.0.3.0/src/.svn/entries new file mode 100644 index 0000000..55ba86c --- /dev/null +++ b/0.0.3.0/src/.svn/entries @@ -0,0 +1,31 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src +http://svndev.alternativaplatform.com + + + +2009-07-03T12:54:42.818772Z +15744 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +alternativa +dir + diff --git a/0.0.3.0/src/.svn/format b/0.0.3.0/src/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.3.0/src/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.3.0/src/alternativa/.svn/all-wcprops b/0.0.3.0/src/alternativa/.svn/all-wcprops new file mode 100644 index 0000000..d4e3630 --- /dev/null +++ b/0.0.3.0/src/alternativa/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 95 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa +END diff --git a/0.0.3.0/src/alternativa/.svn/entries b/0.0.3.0/src/alternativa/.svn/entries new file mode 100644 index 0000000..72bdbfc --- /dev/null +++ b/0.0.3.0/src/alternativa/.svn/entries @@ -0,0 +1,31 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa +http://svndev.alternativaplatform.com + + + +2009-07-03T12:54:42.818772Z +15744 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +physics +dir + diff --git a/0.0.3.0/src/alternativa/.svn/format b/0.0.3.0/src/alternativa/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.3.0/src/alternativa/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.3.0/src/alternativa/physics/.svn/all-wcprops b/0.0.3.0/src/alternativa/physics/.svn/all-wcprops new file mode 100644 index 0000000..ca0304c --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 103 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics +END +altphysics.as +K 25 +svn:wc:ra_dav:version-url +V 117 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/altphysics.as +END diff --git a/0.0.3.0/src/alternativa/physics/.svn/entries b/0.0.3.0/src/alternativa/physics/.svn/entries new file mode 100644 index 0000000..73d7b5c --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/.svn/entries @@ -0,0 +1,49 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics +http://svndev.alternativaplatform.com + + + +2009-07-03T12:54:42.818772Z +15744 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +types +dir + +altphysics.as +file + + + + +2010-10-28T04:32:41.000000Z +04fbe40a27502dbbd0dba02a76b2df50 +2009-04-20T20:09:14.715403Z +11579 +mike + +collision +dir + +rigid +dir + diff --git a/0.0.3.0/src/alternativa/physics/.svn/format b/0.0.3.0/src/alternativa/physics/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.3.0/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base b/0.0.3.0/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base new file mode 100644 index 0000000..2715307 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base @@ -0,0 +1,3 @@ +package alternativa.physics { + public namespace altphysics = "http://alternativaplatform.com/en/altphysics"; +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/altphysics.as b/0.0.3.0/src/alternativa/physics/altphysics.as new file mode 100644 index 0000000..2715307 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/altphysics.as @@ -0,0 +1,3 @@ +package alternativa.physics { + public namespace altphysics = "http://alternativaplatform.com/en/altphysics"; +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/.svn/all-wcprops b/0.0.3.0/src/alternativa/physics/collision/.svn/all-wcprops new file mode 100644 index 0000000..898d034 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/.svn/all-wcprops @@ -0,0 +1,101 @@ +K 25 +svn:wc:ra_dav:version-url +V 113 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/collision +END +BoxSphereCollider.as +K 25 +svn:wc:ra_dav:version-url +V 134 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/collision/BoxSphereCollider.as +END +BoxBoxCollider.as +K 25 +svn:wc:ra_dav:version-url +V 131 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/collision/BoxBoxCollider.as +END +CollisionKdNode.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/collision/CollisionKdNode.as +END +SpherePlaneCollider.as +K 25 +svn:wc:ra_dav:version-url +V 136 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/collision/SpherePlaneCollider.as +END +KdTreeCollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/collision/KdTreeCollisionDetector.as +END +BoxRectCollider.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/collision/BoxRectCollider.as +END +ICollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 136 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/collision/ICollisionPredicate.as +END +BoxPlaneCollider.as +K 25 +svn:wc:ra_dav:version-url +V 133 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/collision/BoxPlaneCollider.as +END +CollisionKdTree.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/collision/CollisionKdTree.as +END +BoxTriangleCollider.as +K 25 +svn:wc:ra_dav:version-url +V 136 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/collision/BoxTriangleCollider.as +END +ICollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 135 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/collision/ICollisionDetector.as +END +IRayCollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 139 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/collision/IRayCollisionPredicate.as +END +IBodyCollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/collision/IBodyCollisionPredicate.as +END +BruteForceCollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 144 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/collision/BruteForceCollisionDetector.as +END +ICollider.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/collision/ICollider.as +END +SphereSphereCollider.as +K 25 +svn:wc:ra_dav:version-url +V 137 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/collision/SphereSphereCollider.as +END diff --git a/0.0.3.0/src/alternativa/physics/collision/.svn/entries b/0.0.3.0/src/alternativa/physics/collision/.svn/entries new file mode 100644 index 0000000..0d144c8 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/.svn/entries @@ -0,0 +1,226 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/collision +http://svndev.alternativaplatform.com + + + +2009-06-29T14:07:24.833358Z +15390 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +BoxSphereCollider.as +file + + + + +2010-10-28T04:32:40.000000Z +ad1dc1849019ac159d22cd93b1ba38d3 +2009-06-08T06:46:18.310732Z +14099 +mike + +BoxBoxCollider.as +file + + + + +2010-10-28T04:32:40.000000Z +e4eb46370e3c7098fa35d0da4e346d09 +2009-06-08T06:46:18.310732Z +14099 +mike + +CollisionKdNode.as +file + + + + +2010-10-28T04:32:40.000000Z +7952384e785be1fca93657d94121b246 +2009-04-17T17:39:21.367569Z +11341 +mike + +BoxRectCollider.as +file + + + + +2010-10-28T04:32:40.000000Z +8554254b88a8ecc31bf005fdee676a67 +2009-06-08T06:46:18.310732Z +14099 +mike + +KdTreeCollisionDetector.as +file + + + + +2010-10-28T04:32:40.000000Z +afefefe1aacd027b490adf1eb6a6f148 +2009-06-29T03:52:30.472906Z +15194 +mike + +SpherePlaneCollider.as +file + + + + +2010-10-28T04:32:40.000000Z +7fbbf70eef5caa9a09ce679819429874 +2009-04-13T03:07:43.422558Z +11019 +mike + +ICollisionPredicate.as +file + + + + +2010-10-28T04:32:40.000000Z +866471ea582e95e46e94eb7ae437c827 +2009-04-26T15:39:06.673206Z +11995 +mike + +BoxPlaneCollider.as +file + + + + +2010-10-28T04:32:40.000000Z +4344df47c2b9c1873d2dc711815b4737 +2009-04-13T03:07:43.422558Z +11019 +mike + +primitives +dir + +CollisionKdTree.as +file + + + + +2010-10-28T04:32:40.000000Z +92511d7533028a942e299ca3a4251da8 +2009-05-18T16:37:02.465501Z +13087 +mike + +BoxTriangleCollider.as +file + + + + +2010-10-28T04:32:40.000000Z +f95da4166903174a2c92c1ab262e719c +2009-05-26T08:41:33.471001Z +13389 +mike + +ICollisionDetector.as +file + + + + +2010-10-28T04:32:40.000000Z +c98a3e33a4a54e2e85da149e25b583fe +2009-06-08T11:22:42.165256Z +14112 +mike + +IRayCollisionPredicate.as +file + + + + +2010-10-28T04:32:40.000000Z +f7443fb728cf4e6157dcfb4f39665889 +2009-04-26T15:39:06.673206Z +11995 +mike + +types +dir + +IBodyCollisionPredicate.as +file + + + + +2010-10-28T04:32:40.000000Z +fce7e70f95b8ef96709157dcdc8433dd +2009-06-29T14:07:24.833358Z +15390 +mike + +ICollider.as +file + + + + +2010-10-28T04:32:40.000000Z +bc3785279b9b7810a0c84cb4fe6084fc +2009-04-13T03:07:43.422558Z +11019 +mike + +BruteForceCollisionDetector.as +file + + + + +2010-10-28T04:32:40.000000Z +c166c05be4909e221c2f129c9e7cac32 +2009-06-08T11:22:42.165256Z +14112 +mike + +SphereSphereCollider.as +file + + + + +2010-10-28T04:32:40.000000Z +fd6e8cf39dcf007125384bc91b347a00 +2009-06-08T06:46:18.310732Z +14099 +mike + diff --git a/0.0.3.0/src/alternativa/physics/collision/.svn/format b/0.0.3.0/src/alternativa/physics/collision/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/BoxBoxCollider.as.svn-base b/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/BoxBoxCollider.as.svn-base new file mode 100644 index 0000000..2893b49 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/BoxBoxCollider.as.svn-base @@ -0,0 +1,791 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Расчитывает точки контакта двух боксов. Нормаль контакта направляется в сторону бокса с меньшим ID. + */ + public class BoxBoxCollider implements ICollider { + + private var tolerance:Number = 0.001; + + private var pos1:Vector3 = new Vector3(); + private var pos2:Vector3 = new Vector3(); + private var vectorToBox1:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var colAxis:Vector3 = new Vector3(); + private var tmpAxis:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var tmpPoints:Vector. = new Vector.(8, true); + private var pcount:int; + + /** + * + */ + public function BoxBoxCollider() { + for (var i:int = 0; i < 8; i++) { + tmpPoints[i] = new ContactPoint(); + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + /** + * + * @param body1 + * @param body2 + * @param contactInfo + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + minOverlap = 1e10; + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + // Вектор из центра второго бокса в центр первого + pos1.x = transform1.d; pos1.y = transform1.h; pos1.z = transform1.l; + pos2.x = transform2.d; pos2.y = transform2.h; pos2.z = transform2.l; + vectorToBox1.x = pos1.x - pos2.x; + vectorToBox1.y = pos1.y - pos2.y; + vectorToBox1.z = pos1.z - pos2.z; + + // Проверка пересечения по основным осям +// box1.transform.getAxis(0, axis10); + axis10.x = transform1.a; axis10.y = transform1.e; axis10.z = transform1.i; + if (!testAxis(box1, box2, axis10, 0, vectorToBox1)) return false; +// box1.transform.getAxis(1, axis11); + axis11.x = transform1.b; axis11.y = transform1.f; axis11.z = transform1.j; + if (!testAxis(box1, box2, axis11, 1, vectorToBox1)) return false; +// box1.transform.getAxis(2, axis12); + axis12.x = transform1.c; axis12.y = transform1.g; axis12.z = transform1.k; + if (!testAxis(box1, box2, axis12, 2, vectorToBox1)) return false; + +// box2.transform.getAxis(0, axis20); + axis20.x = transform2.a; axis20.y = transform2.e; axis20.z = transform2.i; + if (!testAxis(box1, box2, axis20, 3, vectorToBox1)) return false; +// box2.transform.getAxis(1, axis21); + axis21.x = transform2.b; axis21.y = transform2.f; axis21.z = transform2.j; + if (!testAxis(box1, box2, axis21, 4, vectorToBox1)) return false; +// box2.transform.getAxis(2, axis22); + axis22.x = transform2.c; axis22.y = transform2.g; axis22.z = transform2.k; + if (!testAxis(box1, box2, axis22, 5, vectorToBox1)) return false; + + // Проверка производных осей + if (!testAxis(box1, box2, axis.vCross2(axis10, axis20), 6, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis10, axis21), 7, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis10, axis22), 8, vectorToBox1)) return false; + + if (!testAxis(box1, box2, axis.vCross2(axis11, axis20), 9, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis11, axis21), 10, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis11, axis22), 11, vectorToBox1)) return false; + + if (!testAxis(box1, box2, axis.vCross2(axis12, axis20), 12, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis12, axis21), 13, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis12, axis22), 14, vectorToBox1)) return false; + + if (bestAxisIndex < 6) { + // Контакт грань-(грань|ребро|вершина) + findFaceContactPoints(box1, box2, vectorToBox1, bestAxisIndex, contact); + } else { + // Контакт ребро-ребро + bestAxisIndex -= 6; + findEdgesIntersection(box1, box2, vectorToBox1, int(bestAxisIndex/3), bestAxisIndex%3, contact); + } + contact.body1 = box1.body; + contact.body2 = box2.body; + + return true; + } + + /** + * Выполняет поиск точек контакта грани одного бокса с гранью/ребром/вершиной другого. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param faceAxisIdx индекс оси первого бокса, перпендикулярной грани, с которой произошло столкновение + * @param contactInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, faceAxisIdx:int, contactInfo:Contact):void { + var swapNormal:Boolean = false; + if (faceAxisIdx > 2) { + // Столкновение с гранью второго бокса. Для дальнейших расчётов боксы меняются местами, + // но нормаль контакта всё равно должна быть направлена в сторону первоначального box1 + var tmpBox:CollisionBox = box1; + box1 = box2; + box2 = tmpBox; + vectorToBox1.vReverse(); + faceAxisIdx -= 3; + swapNormal = true; + } + box1.transform.getAxis(faceAxisIdx, colAxis); + var faceReversed:Boolean = colAxis.vDot(vectorToBox1) > 0; + if (!faceReversed) colAxis.vReverse(); + // Ищем ось второго бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + box2.transform.getAxis(axisIdx, axis); + var dot:Number = axis.vDot(colAxis); + if (dot < 0) dot = -dot; + if (dot > maxDot) { + maxDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Получаем список вершин грани второго бокса, переводим их в систему координат первого бокса и выполняем обрезку + // по грани первого бокса. Таким образом получается список потенциальных точек контакта. + box2.transform.getAxis(incFaceAxisIdx, axis); + getFaceVertsByAxis(box2, incFaceAxisIdx, axis.vDot(colAxis) < 0, verts1); + box2.transform.transformVectors(verts1, verts2); + box1.transform.transformVectorsInverse(verts2, verts1); + var pnum:int = clip(box1.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + pcount = 0; + for (var i:int = 0; i < pnum; i++) { + if ((pen = getPointBoxPenetration(box1.hs, verts1[i], faceAxisIdx, faceReversed)) > -tolerance) { + var cp:ContactPoint = tmpPoints[pcount++]; + box1.transform.transformVector(verts1[i], cp.pos); + cp.r1.vDiff(cp.pos, pos1); + cp.r2.vDiff(cp.pos, pos2); + cp.penetration = pen; + } + } + contactInfo.normal.vCopy(colAxis); + if (swapNormal) contactInfo.normal.vReverse(); + + if (pcount > 4) reducePoints(); + for (i = 0; i < pcount; i++) (contactInfo.points[i] as ContactPoint).copyFrom(tmpPoints[i]); + contactInfo.pcount = pcount; + } + + /** + * + * @param contactInfo + */ + private function reducePoints():void { + var i:int; + var minIdx:int; + var cp1:ContactPoint; + var cp2:ContactPoint; + while (pcount > 4) { + var minLen:Number = 1e10; + var p1:Vector3 = (tmpPoints[pcount - 1] as ContactPoint).pos; + var p2:Vector3; + for (i = 0; i < pcount; i++) { + p2 = (tmpPoints[i] as ContactPoint).pos; + var dx:Number = p2.x - p1.x; + var dy:Number = p2.y - p1.y; + var dz:Number = p2.z - p1.z; + var len:Number = dx*dx + dy*dy + dz*dz; + if (len < minLen) { + minLen = len; + minIdx = i; + } + p1 = p2; + } + cp1 = tmpPoints[minIdx == 0 ? (pcount - 1) : (minIdx - 1)]; + cp2 = tmpPoints[minIdx]; + p1 = cp1.pos; + p2 = cp2.pos; + p2.x = 0.5*(p1.x + p2.x); + p2.y = 0.5*(p1.y + p2.y); + p2.z = 0.5*(p1.z + p2.z); + cp2.penetration = 0.5*(cp1.penetration + cp2.penetration); + if (minIdx > 0) { + for (i = minIdx; i < pcount; i++) tmpPoints[i - 1] = tmpPoints[i]; + tmpPoints[pcount - 1] = cp1; + } + pcount--; + } + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + private function getFaceVertsByAxis(box:CollisionBox, axisIdx:int, reverse:Boolean, result:Vector.):void { + var hs:Vector3 = box.hs; + switch (axisIdx) { + case 0: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, -hs.y, hs.z); + } + break; + case 1: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, hs.y, hs.z); + } + break; + case 2: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + } else { + (result[0] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } + break; + } + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clip(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + return clipHighX(hs.x, pnum, verts2, verts1); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + } + return 0; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param contactInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):void { + box1.transform.getAxis(axisIdx1, axis10); + box2.transform.getAxis(axisIdx2, axis20); + colAxis.vCross2(axis10, axis20).vNormalize(); + // Разворот оси в сторону первого бокса + if (colAxis.vDot(vectorToBox1) < 0) colAxis.vReverse(); + + /* Кодирование рёбер + бит 0: 1 (тип контакта ребро-ребро) + биты 1-2: индекс направляющей оси ребра + бит 3: + бит 4: + бит 5: + */ + var edgeCode1:int = 1; + var edgeCode2:int = 1; + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + point1.vCopy(box1.hs); + point2.vCopy(box2.hs); + // x + if (axisIdx1 == 0) { + point1.x = 0; + halfLen1 = box1.hs.x; + } else { + box1.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.x = -point1.x; + edgeCode1 |= 8; // 1 << 3 + } + } + if (axisIdx2 == 0) { + point2.x = 0; + halfLen2 = box2.hs.x; + } else { + box2.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.x = -point2.x; + edgeCode2 |= 8; // 1 << 3 + } + } + // y + if (axisIdx1 == 1) { + point1.y = 0; + halfLen1 = box1.hs.y; + edgeCode1 |= 2; // 1 << 1 + } else { + box1.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.y = -point1.y; + edgeCode1 |= 16; // 1 << 4 + } + } + if (axisIdx2 == 1) { + point2.y = 0; + halfLen2 = box2.hs.y; + edgeCode2 |= 2; // 1 << 1 + } else { + box2.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.y = -point2.y; + edgeCode2 |= 16; // 1 << 4 + } + } + // z + if (axisIdx1 == 2) { + point1.z = 0; + halfLen1 = box1.hs.z; + edgeCode1 |= 4; // 2 << 1 + } else { + box1.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.z = -point1.z; + edgeCode1 |= 32; // 1 << 5 + } + } + if (axisIdx2 == 2) { + point2.z = 0; + halfLen2 = box2.hs.z; + edgeCode2 |= 4; // 2 << 1 + } else { + box2.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.z = -point2.z; + edgeCode2 |= 32; // 1 << 5 + } + } + // Получаем глобальные координаты средних точек рёбер + point1.vTransformBy4(box1.transform); + point2.vTransformBy4(box2.transform); + // Находим точку пересечения рёбер, решая систему уравнений + var k:Number = axis10.vDot(axis20); + var det:Number = k*k - 1; + vector.vDiff(point2, point1); + var c1:Number = axis10.vDot(vector); + var c2:Number = axis20.vDot(vector); + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.normal.vCopy(colAxis); + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(point1.x + axis10.x*t1 + point2.x + axis20.x*t2); + cp.pos.y = 0.5*(point1.y + axis10.y*t1 + point2.y + axis20.y*t2); + cp.pos.z = 0.5*(point1.z + axis10.z*t1 + point2.z + axis20.z*t2); + cp.r1.vDiff(cp.pos, pos1); + cp.r2.vDiff(cp.pos, pos2); + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение боксов вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box1 + * @param box2 + * @param axis + * @param axisIndex + * @param vectorToBox1 + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, axisIndex:int, vectorToBox1:Vector3):Boolean { + if (axis.vLengthSqr() < 0.0001) { + return true; + } + axis.vNormalize(); + + var overlap:Number = overlapOnAxis(box1, box2, axis, vectorToBox1); + if (overlap < -tolerance) return false; + if (overlap + tolerance < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия двух боксов вдоль заданной оси. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param axis ось + * @param vectorToBox1 вектор, соединяющий центр второго бокса с центром первого + * @return величина перекрытия боксов вдоль оси + */ + public function overlapOnAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, vectorToBox1:Vector3):Number { + var m:Matrix4 = box1.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box1.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box1.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box1.hs.z; + if (d < 0) d = -d; + projection += d; + + m = box2.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box2.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box2.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box2.hs.z; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox1.x*axis.x + vectorToBox1.y*axis.y + vectorToBox1.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} + import alternativa.physics.types.Vector3; + + class CollisionPointTmp { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + } diff --git a/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/BoxPlaneCollider.as.svn-base b/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/BoxPlaneCollider.as.svn-base new file mode 100644 index 0000000..8568dc7 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/BoxPlaneCollider.as.svn-base @@ -0,0 +1,78 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + public class BoxPlaneCollider implements ICollider { + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var normal:Vector3 = new Vector3(); + + public function BoxPlaneCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var box:RigidBox = body1 as RigidBox; +// var plane:RigidPlane; +// if (box == null) { +// box = body2 as RigidBox; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные координаты вершин бокса +// var sx:Number = box.halfSize.x; +// var sy:Number = box.halfSize.y; +// var sz:Number = box.halfSize.z; +// (verts1[0] as Vector3).reset(-sx, -sy, -sz); +// (verts1[1] as Vector3).reset(sx, -sy, -sz); +// (verts1[2] as Vector3).reset(sx, sy, -sz); +// (verts1[3] as Vector3).reset(-sx, sy, -sz); +// (verts1[4] as Vector3).reset(-sx, -sy, sz); +// (verts1[5] as Vector3).reset(sx, -sy, sz); +// (verts1[6] as Vector3).reset(sx, sy, sz); +// (verts1[7] as Vector3).reset(-sx, sy, sz); +// +// box.transform.transformVectors(verts1, verts2); +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// // Проверяем наличие столкновений с каждой вершиной +// collisionInfo.pcount = 0; +// for (var i:int = 0; i < 8; i++) { +// // Вершина добавляется в список точек столкновения, если лежит под плоскостью +// var dist:Number = (verts2[i] as Vector3).dot(normal); +// if (dist < offset) { +// var cp:ContactPoint; +// if (collisionInfo.pcount == collisionInfo.points.length) { +// cp = new ContactPoint(); +// collisionInfo.points[collisionInfo.pcount] = cp; +// } else { +// cp = collisionInfo.points[collisionInfo.pcount]; +// } +// cp.pos.copy(verts2[i]); +// cp.r1.diff(cp.pos, box.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); +// cp.penetration = offset - dist; +// collisionInfo.pcount++; +// } +// } +// if (collisionInfo.pcount > 0) { +// collisionInfo.body1 = box; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// return true; +// } + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/BoxRectCollider.as.svn-base b/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/BoxRectCollider.as.svn-base new file mode 100644 index 0000000..b5287f3 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/BoxRectCollider.as.svn-base @@ -0,0 +1,734 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + public class BoxRectCollider implements ICollider { + + private var tolerance:Number = 0.001; + + private var bPos:Vector3 = new Vector3(); + private var rPos:Vector3 = new Vector3(); + + private var vectorToBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var colAxis:Vector3 = new Vector3(); + private var tmpAxis:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxRectCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + /** + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + minOverlap = 1e10; + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + + // Вектор из центра прямоугольника в центр бокса + box.transform.getAxis(3, bPos); + rect.transform.getAxis(3, rPos); + vectorToBox.vDiff(bPos, rPos); + + // Проверка пересечения по нормали прямоугольника + rect.transform.getAxis(2, axis22); + if (!testAxis(box, rect, axis22, 0, vectorToBox)) return false; + + // Проверка пересечения по основным осям бокса + box.transform.getAxis(0, axis10); + if (!testAxis(box, rect, axis10, 1, vectorToBox)) return false; + box.transform.getAxis(1, axis11); + if (!testAxis(box, rect, axis11, 2, vectorToBox)) return false; + box.transform.getAxis(2, axis12); + if (!testAxis(box, rect, axis12, 3, vectorToBox)) return false; + + // Получаем направляющие рёбер прямоугольника + rect.transform.getAxis(0, axis20); + rect.transform.getAxis(1, axis21); + + // Проверка производных осей + if (!testAxis(box, rect, axis.vCross2(axis10, axis20), 4, vectorToBox)) return false; + if (!testAxis(box, rect, axis.vCross2(axis10, axis21), 5, vectorToBox)) return false; + + if (!testAxis(box, rect, axis.vCross2(axis11, axis20), 6, vectorToBox)) return false; + if (!testAxis(box, rect, axis.vCross2(axis11, axis21), 7, vectorToBox)) return false; + + if (!testAxis(box, rect, axis.vCross2(axis12, axis20), 8, vectorToBox)) return false; + if (!testAxis(box, rect, axis.vCross2(axis12, axis21), 9, vectorToBox)) return false; + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, rect, vectorToBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + findEdgesIntersection(box, rect, vectorToBox, int(bestAxisIndex/2), bestAxisIndex%2, contact); + } + contact.body1 = box.body; + contact.body2 = rect.body; + + return true; + } + + /** + * Выполняет поиск точек контакта бокса с прямоугольником. + * + * @param box бокс + * @param rect прямоугольник + * @param vectorToBox вектор, направленный из центра прямоугольника в центр бокса + * @param faceAxisIdx индекс оси, идентифицирующей полскость столкновения (грань бокса или полскость прямоугольника) + * @param colInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, faceAxisIdx:int, colInfo:Contact):Boolean { + var pnum:int, i:int, v:Vector3, cp:ContactPoint; + if (faceAxisIdx == 0) { + // Столкновение с плоскостью прямоугольника + + // Проверим положение бокса относительно плоскости прямоугольника + rect.transform.getAxis(2, colAxis); + var offset:Number = colAxis.vDot(rPos); + if (bPos.vDot(colAxis) < offset) return false; + + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + box.transform.getAxis(axisIdx, axis); + var dot:Number = axis.vDot(colAxis); + if (dot < 0) dot = -dot; + if (dot > maxDot) { + maxDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Получаем список вершин грани бокса, переводим их в систему координат прямоугольника и выполняем обрезку + // по прямоугольнику. Таким образом получается список потенциальных точек контакта. + box.transform.getAxis(incFaceAxisIdx, axis); + getFaceVertsByAxis(box.hs, incFaceAxisIdx, axis.vDot(colAxis) > 0, verts1); + box.transform.transformVectors(verts1, verts2); + rect.transform.transformVectorsInverse(verts2, verts1); + pnum = clipByRect(rect.hs); + // Проверяем каждую потенциальную точку на принадлежность нижней полуплоскости прямоугольника и добавляем такие точки в список контактов + colInfo.pcount = 0; + for (i = 0; i < pnum; i++) { + v = verts1[i]; + if (v.z < tolerance) { + cp = colInfo.points[colInfo.pcount++]; + rect.transform.transformVector(v, cp.pos); + cp.r1.vDiff(cp.pos, bPos); + cp.r2.vDiff(cp.pos, rPos); + cp.penetration = -v.z; + } + } + colInfo.normal.vCopy(colAxis); + } else { + // Столкновение с гранью бокса + faceAxisIdx--; + box.transform.getAxis(faceAxisIdx, colAxis); + var faceReversed:Boolean = colAxis.vDot(vectorToBox) > 0; + if (!faceReversed) colAxis.vReverse(); + + rect.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) return false; + + getFaceVertsByAxis(rect.hs, 2, false, verts1); + rect.transform.transformVectors(verts1, verts2); + box.transform.transformVectorsInverse(verts2, verts1); + pnum = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + colInfo.pcount = 0; + for (i = 0; i < pnum; i++) { + if ((pen = getPointBoxPenetration(box.hs, verts1[i], faceAxisIdx, faceReversed)) > -tolerance) { + cp = colInfo.points[colInfo.pcount++]; + box.transform.transformVector(verts1[i], cp.pos); + cp.r1.vDiff(cp.pos, bPos); + cp.r2.vDiff(cp.pos, rPos); + cp.penetration = pen; + } + } + colInfo.normal.vCopy(colAxis); + } + return true; + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + private function getFaceVertsByAxis(hs:Vector3, axisIdx:int, reverse:Boolean, result:Vector.):void { + switch (axisIdx) { + case 0: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, -hs.y, hs.z); + } + break; + case 1: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, hs.y, hs.z); + } + break; + case 2: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + } else { + (result[0] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } + break; + } + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + return clipHighX(hs.x, pnum, verts2, verts1); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + } + return 0; + } + + /** + * + * @param hs + * @return + */ + private function clipByRect(hs:Vector3):int { + var pnum:int = 4; + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box первый бокс + * @param rect второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param colInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, axisIdx1:int, axisIdx2:int, colInfo:Contact):void { + box.transform.getAxis(axisIdx1, axis10); + rect.transform.getAxis(axisIdx2, axis20); + colAxis.vCross2(axis10, axis20).vNormalize(); + // Разворот оси в сторону бокса + if (colAxis.vDot(vectorToBox) < 0) colAxis.vReverse(); + + /* Кодирование рёбер + бит 0: 1 (тип контакта ребро-ребро) + биты 1-2: индекс направляющей оси ребра + бит 3: + бит 4: + бит 5: + */ + var edgeCode1:int = 1; + var edgeCode2:int = 1; + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + point1.vCopy(box.hs); + point2.vCopy(rect.hs); + // x + if (axisIdx1 == 0) { + point1.x = 0; + halfLen1 = box.hs.x; + } else { + box.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.x = -point1.x; + edgeCode1 |= 8; // 1 << 3 + } + } + if (axisIdx2 == 0) { + point2.x = 0; + halfLen2 = rect.hs.x; + } else { + rect.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.x = -point2.x; + edgeCode2 |= 8; // 1 << 3 + } + } + // y + if (axisIdx1 == 1) { + point1.y = 0; + halfLen1 = box.hs.y; + edgeCode1 |= 2; // 1 << 1 + } else { + box.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.y = -point1.y; + edgeCode1 |= 16; // 1 << 4 + } + } + if (axisIdx2 == 1) { + point2.y = 0; + halfLen2 = rect.hs.y; + edgeCode2 |= 2; // 1 << 1 + } else { + rect.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.y = -point2.y; + edgeCode2 |= 16; // 1 << 4 + } + } + // z + if (axisIdx1 == 2) { + point1.z = 0; + halfLen1 = box.hs.z; + edgeCode1 |= 4; // 2 << 1 + } else { + box.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.z = -point1.z; + edgeCode1 |= 32; // 1 << 5 + } + } + // Получаем глобальные координаты средних точек рёбер + point1.vTransformBy4(box.transform); + point2.vTransformBy4(rect.transform); + // Находим точку пересечения рёбер, решая систему уравнений + var k:Number = axis10.vDot(axis20); + var det:Number = k*k - 1; + vector.vDiff(point2, point1); + var c1:Number = axis10.vDot(vector); + var c2:Number = axis20.vDot(vector); + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + colInfo.normal.vCopy(colAxis); + colInfo.pcount = 1; + var cp:ContactPoint = colInfo.points[0]; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(point1.x + axis10.x*t1 + point2.x + axis20.x*t2); + cp.pos.y = 0.5*(point1.y + axis10.y*t1 + point2.y + axis20.y*t2); + cp.pos.z = 0.5*(point1.z + axis10.z*t1 + point2.z + axis20.z*t2); + cp.r1.vDiff(cp.pos, bPos); + cp.r2.vDiff(cp.pos, rPos); + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box + * @param rect + * @param axis + * @param axisIndex + * @param vectorToBox + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + if (axis.vLengthSqr() < 0.0001) return true; + axis.vNormalize(); + + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -tolerance) return false; + if (overlap + tolerance < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия вдоль заданной оси. + * + * @param box бокс + * @param rect прямоугольник + * @param axis ось + * @param vectorToBox вектор, соединяющий центр прямоугольника с центром бокса + * @return величина перекрытия вдоль оси + */ + public function overlapOnAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, vectorToBox:Vector3):Number { + var m:Matrix4 = box.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box.hs.z; + if (d < 0) d = -d; + projection += d; + + m = rect.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*rect.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*rect.hs.y; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox.x*axis.x + vectorToBox.y*axis.y + vectorToBox.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/BoxSphereCollider.as.svn-base b/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/BoxSphereCollider.as.svn-base new file mode 100644 index 0000000..2b166f3 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/BoxSphereCollider.as.svn-base @@ -0,0 +1,104 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + use namespace altphysics; + /** + * + */ + public class BoxSphereCollider implements ICollider { + + private var center:Vector3 = new Vector3(); + private var closestPt:Vector3 = new Vector3(); + + private var bPos:Vector3 = new Vector3(); + private var sPos:Vector3 = new Vector3(); + + /** + * + */ + public function BoxSphereCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + // Зафиксированно столкновение + contact.body1 = sphere.body; + contact.body2 = box.body; + contact.normal.vCopy(closestPt).vTransformBy4(box.transform).vSubtract(sPos).vNormalize().vReverse(); + contact.pcount = 1; + + var cp:ContactPoint = contact.points[0]; + cp.penetration = sphere.r - Math.sqrt(distSqr); + cp.pos.vCopy(contact.normal).vScale(-sphere.r).vAdd(sPos); + cp.r1.vDiff(cp.pos, sPos); + cp.r2.vDiff(cp.pos, bPos); + + return true; + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/BoxTriangleCollider.as.svn-base b/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/BoxTriangleCollider.as.svn-base new file mode 100644 index 0000000..63c6efd --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/BoxTriangleCollider.as.svn-base @@ -0,0 +1,14 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Contact; + import alternativa.physics.collision.primitives.CollisionPrimitive; + + public class BoxTriangleCollider implements ICollider { + public function BoxTriangleCollider() { + } + + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/BruteForceCollisionDetector.as.svn-base b/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/BruteForceCollisionDetector.as.svn-base new file mode 100644 index 0000000..d2c2e2a --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/BruteForceCollisionDetector.as.svn-base @@ -0,0 +1,37 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + public class BruteForceCollisionDetector implements ICollisionDetector { + public function BruteForceCollisionDetector() { + } + + public function addPrimitive(primitive:CollisionPrimitive, isStatic:Boolean=true):Boolean { + return false; + } + + public function removePrimitive(primitive:CollisionPrimitive, isStatic:Boolean=true):Boolean { + return false; + } + + public function init():void { + } + + public function getAllCollisions(contacts:Vector.):int { + return 0; + } + + public function intersectRay(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, intersection:RayIntersection):Boolean { + return false; + } + + public function intersectRayWithStatic(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base b/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base new file mode 100644 index 0000000..e1a1657 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base @@ -0,0 +1,16 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.types.BoundBox; + + public class CollisionKdNode { + public var objects:Vector.; + public var boundBox:BoundBox; + public var parent:CollisionKdNode; + + public var axis:int = -1; // 0 - x, 1 - y, 2 - z + public var coord:Number; + public var positiveNode:CollisionKdNode; + public var negativeNode:CollisionKdNode; + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base b/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base new file mode 100644 index 0000000..6c62b79 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base @@ -0,0 +1,259 @@ +package alternativa.physics.collision { + + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + + /** + * @author mike + */ + public class CollisionKdTree { + + public var threshold:Number = 0.01; + public var minPrimitivesPerNode:int = 1; + + public var rootNode:CollisionKdNode; + public var staticChildren:Vector. = new Vector.(); + public var numStaticChildren:int; + private var staticBoundBoxes:Vector. = new Vector.(); + + /** + * @param primitive + */ + public function addStaticPrimitive(primitive:CollisionPrimitive):void { + staticChildren[numStaticChildren++] = primitive; + } + + /** + * @param primitive + * @return + */ + public function removeStaticPrimitive(primitive:CollisionPrimitive):Boolean { + var idx:int = staticChildren.indexOf(primitive); + if (idx < 0) return false; + staticChildren.splice(idx, 1); + numStaticChildren--; + return true; + } + + /** + * @param boundBox + */ + public function createTree(boundBox:BoundBox = null):void { + if (numStaticChildren == 0) return; + // Создаём корневую ноду + rootNode = new CollisionKdNode(); + rootNode.objects = new Vector.(); + // Расчитываем баунды объектов и рутовой ноды + var rootNodeBoundBox:BoundBox = rootNode.boundBox = (boundBox != null) ? boundBox : new BoundBox(); + for (var i:int = 0; i < numStaticChildren; i++) { + var child:CollisionPrimitive = staticChildren[i]; + var childBoundBox:BoundBox = staticBoundBoxes[i] = child.calculateAABB(); + rootNodeBoundBox.addBoundBox(childBoundBox); + rootNode.objects[i] = i; + } + staticBoundBoxes.length = numStaticChildren; + // Разделяем рутовую ноду + splitNode(rootNode); + } + + private var splitAxis:int; + private var splitCoord:Number; + private var splitCost:Number; + static private const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + static private const splitCoordsX:Vector. = new Vector.(); + static private const splitCoordsY:Vector. = new Vector.(); + static private const splitCoordsZ:Vector. = new Vector.(); + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + if (node.objects.length <= minPrimitivesPerNode) return; + + var objects:Vector. = node.objects; + var i:int, j:int, k:int, length:int = objects.length, c1:Number, c2:Number, ct1:Number, ct2:Number, area:Number, areaNegative:Number, areaPositive:Number, numNegative:int, numPositive:int, conflict:Boolean, cost:Number; + + var nodeBoundBox:BoundBox = node.boundBox; + + // Подготовка баунда с погрешностями + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + // Собираем опорные координаты + var numSplitCoordsX:int = 0, numSplitCoordsY:int = 0, numSplitCoordsZ:int = 0; + for (i = 0; i < length; i++) { + var boundBox:BoundBox = staticBoundBoxes[objects[i]]; + if (boundBox.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = boundBox.minX; + if (boundBox.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = boundBox.maxX; + + if (boundBox.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = boundBox.minY; + if (boundBox.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = boundBox.maxY; + + if (boundBox.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.minZ; + if (boundBox.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.maxZ; + } + + // Убираем дубликаты координат, ищем наилучший сплит + splitAxis = -1; splitCost = 1e308; + i = 0; area = (nodeBoundBox.maxY - nodeBoundBox.minY)*(nodeBoundBox.maxZ - nodeBoundBox.minZ); + while (i < numSplitCoordsX) { + if (isNaN(c1 = splitCoordsX[i++])) continue; + ct1 = c1 - threshold; + ct2 = c1 + threshold; + areaNegative = area*(c1 - nodeBoundBox.minX); + areaPositive = area*(nodeBoundBox.maxX - c1); + numNegative = numPositive = 0; + conflict = false; + // Проверяем объекты + for (j = 0; j < length; j++) { + boundBox = staticBoundBoxes[objects[j]]; + if (boundBox.maxX <= ct2) { + if (boundBox.minX < ct1) numNegative++; + } else { + if (boundBox.minX >= ct1) numPositive++; else {conflict = true; break;} + } + } + // Если хороший сплит, сохраняем + if (!conflict && (cost = areaNegative*numNegative + areaPositive*numPositive) < splitCost) { + splitCost = cost; + splitAxis = 0; + splitCoord = c1; + } + j = i; + while (++j < numSplitCoordsX) if ((c2 = splitCoordsX[j]) >= c1 - threshold && c2 <= c1 + threshold) splitCoordsX[j] = NaN; + } + i = 0; area = (nodeBoundBox.maxX - nodeBoundBox.minX)*(nodeBoundBox.maxZ - nodeBoundBox.minZ); + while (i < numSplitCoordsY) { + if (isNaN(c1 = splitCoordsY[i++])) continue; + ct1 = c1 - threshold; + ct2 = c1 + threshold; + areaNegative = area*(c1 - nodeBoundBox.minY); + areaPositive = area*(nodeBoundBox.maxY - c1); + numNegative = numPositive = 0; + conflict = false; + // Проверяем объекты + for (j = 0; j < length; j++) { + boundBox = staticBoundBoxes[objects[j]]; + if (boundBox.maxY <= ct2) { + if (boundBox.minY < ct1) numNegative++; + } else { + if (boundBox.minY >= ct1) numPositive++; else {conflict = true; break;} + } + } + // Если хороший сплит, сохраняем + if (!conflict && (cost = areaNegative*numNegative + areaPositive*numPositive) < splitCost) { + splitCost = cost; + splitAxis = 1; + splitCoord = c1; + } + j = i; + while (++j < numSplitCoordsY) if ((c2 = splitCoordsY[j]) >= c1 - threshold && c2 <= c1 + threshold) splitCoordsY[j] = NaN; + } + i = 0; area = (nodeBoundBox.maxX - nodeBoundBox.minX)*(nodeBoundBox.maxY - nodeBoundBox.minY); + while (i < numSplitCoordsZ) { + if (isNaN(c1 = splitCoordsZ[i++])) continue; + ct1 = c1 - threshold; + ct2 = c1 + threshold; + areaNegative = area*(c1 - nodeBoundBox.minZ); + areaPositive = area*(nodeBoundBox.maxZ - c1); + numNegative = numPositive = 0; + conflict = false; + // Проверяем объекты + for (j = 0; j < length; j++) { + boundBox = staticBoundBoxes[objects[j]]; + if (boundBox.maxZ <= ct2) { + if (boundBox.minZ < ct1) numNegative++; + } else { + if (boundBox.minZ >= ct1) numPositive++; else {conflict = true; break;} + } + } + // Если хороший сплит, сохраняем + if (!conflict && (cost = areaNegative*numNegative + areaPositive*numPositive) < splitCost) { + splitCost = cost; + splitAxis = 2; + splitCoord = c1; + } + j = i; + while (++j < numSplitCoordsZ) if ((c2 = splitCoordsZ[j]) >= c1 - threshold && c2 <= c1 + threshold) splitCoordsZ[j] = NaN; + } + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Разделяем ноду + var axisX:Boolean = splitAxis == 0, axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.positiveNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.positiveNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + ct1 = splitCoord - threshold; ct2 = splitCoord + threshold; + for (i = 0; i < length; i++) { + boundBox = staticBoundBoxes[objects[i]]; + var min:Number = axisX ? boundBox.minX : (axisY ? boundBox.minY : boundBox.minZ); + var max:Number = axisX ? boundBox.maxX : (axisY ? boundBox.maxY : boundBox.maxZ); + if (max <= ct2) { + // Объект в негативной стороне + if (node.negativeNode.objects == null) node.negativeNode.objects = new Vector.(); + node.negativeNode.objects.push(objects[i]); + objects[i] = -1; + } else { + if (min >= ct1) { + // Объект в положительной стороне + if (node.positiveNode.objects == null) node.positiveNode.objects = new Vector.(); + node.positiveNode.objects.push(objects[i]); + objects[i] = -1; + } else { + // Распилился + } + } + } + + // Очистка списка объектов + j = 0; + for (i = 0; i < length; i++) { + if (objects[i] >= 0) objects[j++] = objects[i]; + } + if (j > 0) objects.length = j; else node.objects = null; + + // Разделение дочерних нод + if (node.negativeNode.objects != null) splitNode(node.negativeNode); + if (node.positiveNode.objects != null) splitNode(node.positiveNode); + } + + /** + * + */ + public function traceTree():void { + traceNode("", rootNode); + } + + /** + * @param str + * @param node + */ + private function traceNode(str:String, node:CollisionKdNode):void { + if (node == null) return; + trace(str, node.axis == -1 ? "end" : ((node.axis == 0) ? "X" : ((node.axis == 1) ? "Y" : "Z")), "splitCoord=" + splitCoord, "bound", node.boundBox, "objs:", node.objects); + traceNode(str + "-", node.negativeNode); + traceNode(str + "+", node.positiveNode); + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base b/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base new file mode 100644 index 0000000..9cfdca5 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Body; + + public interface IBodyCollisionPredicate { + function considerBodies(body1:Body, body2:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base b/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base new file mode 100644 index 0000000..733231b --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base @@ -0,0 +1,12 @@ +package alternativa.physics.collision { + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + + /** + * Интерфейс определителя столкновений между двумя примитивами. + */ + public interface ICollider { + function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base b/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base new file mode 100644 index 0000000..83e94d7 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base @@ -0,0 +1,73 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + /** + * Интерфейс детектора столкновений. + */ + public interface ICollisionDetector { + + /** + * Добавляет физический примитив в коллайдер. + * + * @param primitive добавляемый примитив + * @param isStatic указывает тип примитива: статический или динамический + * @return true если примитив был успешно добавлен, иначе false + */ + function addPrimitive(primitive:CollisionPrimitive, isStatic:Boolean = true):Boolean; + + /** + * Удаляет физический примитив из коллайдера. + * + * @param primitive удаляемый примитив + * @param isStatic указывает тип примитива: статический или динамический + * @return true если примитив был успшено удалён, иначе false + */ + function removePrimitive(primitive:CollisionPrimitive, isStatic:Boolean = true):Boolean; + + /** + * Выполняет инициализацию детектора после обновления списка примитивов. + */ + function init():void; + + /** + * Получает все столкновения в текущей конфигурации физической геометрии. + * + * @param contacts список контактов, в кторые будет записана информация о столкновении + * @return количество найденных столкновений + */ + function getAllCollisions(contacts:Vector.):int; + + /** + * Тестирует луч на пересечение с физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param dir направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRay(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * Тестирует луч на пересечение со статической физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param dir направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRayWithStatic(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base b/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base new file mode 100644 index 0000000..bb1b756 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base @@ -0,0 +1,9 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + + public interface ICollisionPredicate { + + function considerCollision(primitive:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base b/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base new file mode 100644 index 0000000..0f3d929 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Body; + + public interface IRayCollisionPredicate { + function considerBody(body:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base b/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base new file mode 100644 index 0000000..b12218e --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base @@ -0,0 +1,452 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Детектор, хранящий статическую геометрию в kD-дереве и использующий дерево для ускорения тестов на пересечения. + */ + public class KdTreeCollisionDetector implements ICollisionDetector { + + altphysics var tree:CollisionKdTree; + altphysics var dynamicPrimitives:Vector.; + 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.(); + + addCollider(CollisionPrimitive.BOX, CollisionPrimitive.BOX, new BoxBoxCollider()); +// addCollider(CollisionPrimitive.BOX, CollisionPrimitive.PLANE, new BoxPlaneCollider()); + addCollider(CollisionPrimitive.BOX, CollisionPrimitive.SPHERE, new BoxSphereCollider()); + addCollider(CollisionPrimitive.BOX, CollisionPrimitive.RECT, new BoxRectCollider()); + +// 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():void { + tree.createTree(); +// tree.traceTree(); + } + + /** + * + * @param contacts + * @return + */ + public function getAllCollisions(contacts:Vector.):int { + var colNum:int = 0; + for (var i:int = 0; i < dynamicPrimitivesNum; i++) { + var primitive:CollisionPrimitive = dynamicPrimitives[i]; + primitive.calculateAABB(); + if (primitive.body != null && primitive.body.frozen) continue; + colNum += getPrimitiveNodeCollisions(tree.rootNode, primitive, contacts, colNum); + + // Столкновения динамических примитивов между собой + // TODO: Попробовать различные оптимизации (сортировка по баундам, встраивание в дерево) + for (var j:int = i + 1; j < dynamicPrimitivesNum; j++) { + if (collide(primitive, dynamicPrimitives[j], contacts[colNum])) colNum++; + } + } + + return colNum; + } + + /** + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + public function collide(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.collide(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 = (contact.points[0] as ContactPoint).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 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 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:Vector., startIndex:int):int { + var colNum:int = 0; + if (node.objects != null) { + // Поиск столкновений со статическими примитивами узла + var primitives:Vector. = tree.staticChildren; + var indices:Vector. = node.objects; + for (var i:int = indices.length - 1; i >= 0; i--) + if (collide(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; + } + + 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.getSegmentIntersection(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.objects != null && getRayNodeIntersection(origin, dir, collisionGroup, tree.staticChildren, node.objects, 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., indices:Vector., 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.getSegmentIntersection(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; + } \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/SpherePlaneCollider.as.svn-base b/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/SpherePlaneCollider.as.svn-base new file mode 100644 index 0000000..9b527d8 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/SpherePlaneCollider.as.svn-base @@ -0,0 +1,58 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + /** + * + */ + public class SpherePlaneCollider implements ICollider { + + private var normal:Vector3 = new Vector3(); + + /** + * + */ + public function SpherePlaneCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var sphere:RigidSphere = body1 as RigidSphere; +// var plane:RigidPlane; +// if (sphere == null) { +// sphere = body2 as RigidSphere; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// +// var dist:Number = sphere.state.pos.dot(normal) - offset; +// if (dist > sphere.r) return false; +// +// collisionInfo.body1 = sphere; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// collisionInfo.pcount = 1; +// +// var cp:ContactPoint = collisionInfo.points[0]; +// cp.penetration = sphere.r - dist; +// cp.pos.copy(normal).reverse().scale(sphere.r).add(sphere.state.pos); +// cp.r1.diff(cp.pos, sphere.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); + + return true; + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/SphereSphereCollider.as.svn-base b/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/SphereSphereCollider.as.svn-base new file mode 100644 index 0000000..9111a84 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/.svn/text-base/SphereSphereCollider.as.svn-base @@ -0,0 +1,60 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + public class SphereSphereCollider implements ICollider { + + private var p1:Vector3 = new Vector3(); + private var p2:Vector3 = new Vector3(); + + public function SphereSphereCollider() { + } + + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var s1:CollisionSphere; + var s2:CollisionSphere; + if (prim1.body != null) { + s1 = prim1 as CollisionSphere; + s2 = prim2 as CollisionSphere; + } else { + s1 = prim2 as CollisionSphere; + s2 = prim1 as CollisionSphere; + } + + s1.transform.getAxis(3, p1); + s2.transform.getAxis(3, p2); + var dx:Number = p1.x - p2.x; + var dy:Number = p1.y - p2.y; + var dz:Number = p1.z - p2.z; + var len:Number = dx*dx + dy*dy + dz*dz; + var sum:Number = s1.r + s2.r; + if (len > sum*sum) return false; + len = Math.sqrt(len); + dx /= len; + dy /= len; + dz /= len; + + contact.body1 = s1.body; + contact.body2 = s2.body; + contact.normal.x = dx; + contact.normal.y = dy; + contact.normal.z = dz; + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = sum - len; + cp.pos.x = p1.x - dx*s1.r; + cp.pos.y = p1.y - dy*s1.r; + cp.pos.z = p1.z - dz*s1.r; + cp.r1.vDiff(cp.pos, p1); + cp.r2.vDiff(cp.pos, p2); + + return true; + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/BoxBoxCollider.as b/0.0.3.0/src/alternativa/physics/collision/BoxBoxCollider.as new file mode 100644 index 0000000..2893b49 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/BoxBoxCollider.as @@ -0,0 +1,791 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Расчитывает точки контакта двух боксов. Нормаль контакта направляется в сторону бокса с меньшим ID. + */ + public class BoxBoxCollider implements ICollider { + + private var tolerance:Number = 0.001; + + private var pos1:Vector3 = new Vector3(); + private var pos2:Vector3 = new Vector3(); + private var vectorToBox1:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var colAxis:Vector3 = new Vector3(); + private var tmpAxis:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var tmpPoints:Vector. = new Vector.(8, true); + private var pcount:int; + + /** + * + */ + public function BoxBoxCollider() { + for (var i:int = 0; i < 8; i++) { + tmpPoints[i] = new ContactPoint(); + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + /** + * + * @param body1 + * @param body2 + * @param contactInfo + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + minOverlap = 1e10; + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + // Вектор из центра второго бокса в центр первого + pos1.x = transform1.d; pos1.y = transform1.h; pos1.z = transform1.l; + pos2.x = transform2.d; pos2.y = transform2.h; pos2.z = transform2.l; + vectorToBox1.x = pos1.x - pos2.x; + vectorToBox1.y = pos1.y - pos2.y; + vectorToBox1.z = pos1.z - pos2.z; + + // Проверка пересечения по основным осям +// box1.transform.getAxis(0, axis10); + axis10.x = transform1.a; axis10.y = transform1.e; axis10.z = transform1.i; + if (!testAxis(box1, box2, axis10, 0, vectorToBox1)) return false; +// box1.transform.getAxis(1, axis11); + axis11.x = transform1.b; axis11.y = transform1.f; axis11.z = transform1.j; + if (!testAxis(box1, box2, axis11, 1, vectorToBox1)) return false; +// box1.transform.getAxis(2, axis12); + axis12.x = transform1.c; axis12.y = transform1.g; axis12.z = transform1.k; + if (!testAxis(box1, box2, axis12, 2, vectorToBox1)) return false; + +// box2.transform.getAxis(0, axis20); + axis20.x = transform2.a; axis20.y = transform2.e; axis20.z = transform2.i; + if (!testAxis(box1, box2, axis20, 3, vectorToBox1)) return false; +// box2.transform.getAxis(1, axis21); + axis21.x = transform2.b; axis21.y = transform2.f; axis21.z = transform2.j; + if (!testAxis(box1, box2, axis21, 4, vectorToBox1)) return false; +// box2.transform.getAxis(2, axis22); + axis22.x = transform2.c; axis22.y = transform2.g; axis22.z = transform2.k; + if (!testAxis(box1, box2, axis22, 5, vectorToBox1)) return false; + + // Проверка производных осей + if (!testAxis(box1, box2, axis.vCross2(axis10, axis20), 6, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis10, axis21), 7, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis10, axis22), 8, vectorToBox1)) return false; + + if (!testAxis(box1, box2, axis.vCross2(axis11, axis20), 9, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis11, axis21), 10, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis11, axis22), 11, vectorToBox1)) return false; + + if (!testAxis(box1, box2, axis.vCross2(axis12, axis20), 12, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis12, axis21), 13, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis12, axis22), 14, vectorToBox1)) return false; + + if (bestAxisIndex < 6) { + // Контакт грань-(грань|ребро|вершина) + findFaceContactPoints(box1, box2, vectorToBox1, bestAxisIndex, contact); + } else { + // Контакт ребро-ребро + bestAxisIndex -= 6; + findEdgesIntersection(box1, box2, vectorToBox1, int(bestAxisIndex/3), bestAxisIndex%3, contact); + } + contact.body1 = box1.body; + contact.body2 = box2.body; + + return true; + } + + /** + * Выполняет поиск точек контакта грани одного бокса с гранью/ребром/вершиной другого. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param faceAxisIdx индекс оси первого бокса, перпендикулярной грани, с которой произошло столкновение + * @param contactInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, faceAxisIdx:int, contactInfo:Contact):void { + var swapNormal:Boolean = false; + if (faceAxisIdx > 2) { + // Столкновение с гранью второго бокса. Для дальнейших расчётов боксы меняются местами, + // но нормаль контакта всё равно должна быть направлена в сторону первоначального box1 + var tmpBox:CollisionBox = box1; + box1 = box2; + box2 = tmpBox; + vectorToBox1.vReverse(); + faceAxisIdx -= 3; + swapNormal = true; + } + box1.transform.getAxis(faceAxisIdx, colAxis); + var faceReversed:Boolean = colAxis.vDot(vectorToBox1) > 0; + if (!faceReversed) colAxis.vReverse(); + // Ищем ось второго бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + box2.transform.getAxis(axisIdx, axis); + var dot:Number = axis.vDot(colAxis); + if (dot < 0) dot = -dot; + if (dot > maxDot) { + maxDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Получаем список вершин грани второго бокса, переводим их в систему координат первого бокса и выполняем обрезку + // по грани первого бокса. Таким образом получается список потенциальных точек контакта. + box2.transform.getAxis(incFaceAxisIdx, axis); + getFaceVertsByAxis(box2, incFaceAxisIdx, axis.vDot(colAxis) < 0, verts1); + box2.transform.transformVectors(verts1, verts2); + box1.transform.transformVectorsInverse(verts2, verts1); + var pnum:int = clip(box1.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + pcount = 0; + for (var i:int = 0; i < pnum; i++) { + if ((pen = getPointBoxPenetration(box1.hs, verts1[i], faceAxisIdx, faceReversed)) > -tolerance) { + var cp:ContactPoint = tmpPoints[pcount++]; + box1.transform.transformVector(verts1[i], cp.pos); + cp.r1.vDiff(cp.pos, pos1); + cp.r2.vDiff(cp.pos, pos2); + cp.penetration = pen; + } + } + contactInfo.normal.vCopy(colAxis); + if (swapNormal) contactInfo.normal.vReverse(); + + if (pcount > 4) reducePoints(); + for (i = 0; i < pcount; i++) (contactInfo.points[i] as ContactPoint).copyFrom(tmpPoints[i]); + contactInfo.pcount = pcount; + } + + /** + * + * @param contactInfo + */ + private function reducePoints():void { + var i:int; + var minIdx:int; + var cp1:ContactPoint; + var cp2:ContactPoint; + while (pcount > 4) { + var minLen:Number = 1e10; + var p1:Vector3 = (tmpPoints[pcount - 1] as ContactPoint).pos; + var p2:Vector3; + for (i = 0; i < pcount; i++) { + p2 = (tmpPoints[i] as ContactPoint).pos; + var dx:Number = p2.x - p1.x; + var dy:Number = p2.y - p1.y; + var dz:Number = p2.z - p1.z; + var len:Number = dx*dx + dy*dy + dz*dz; + if (len < minLen) { + minLen = len; + minIdx = i; + } + p1 = p2; + } + cp1 = tmpPoints[minIdx == 0 ? (pcount - 1) : (minIdx - 1)]; + cp2 = tmpPoints[minIdx]; + p1 = cp1.pos; + p2 = cp2.pos; + p2.x = 0.5*(p1.x + p2.x); + p2.y = 0.5*(p1.y + p2.y); + p2.z = 0.5*(p1.z + p2.z); + cp2.penetration = 0.5*(cp1.penetration + cp2.penetration); + if (minIdx > 0) { + for (i = minIdx; i < pcount; i++) tmpPoints[i - 1] = tmpPoints[i]; + tmpPoints[pcount - 1] = cp1; + } + pcount--; + } + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + private function getFaceVertsByAxis(box:CollisionBox, axisIdx:int, reverse:Boolean, result:Vector.):void { + var hs:Vector3 = box.hs; + switch (axisIdx) { + case 0: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, -hs.y, hs.z); + } + break; + case 1: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, hs.y, hs.z); + } + break; + case 2: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + } else { + (result[0] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } + break; + } + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clip(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + return clipHighX(hs.x, pnum, verts2, verts1); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + } + return 0; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param contactInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):void { + box1.transform.getAxis(axisIdx1, axis10); + box2.transform.getAxis(axisIdx2, axis20); + colAxis.vCross2(axis10, axis20).vNormalize(); + // Разворот оси в сторону первого бокса + if (colAxis.vDot(vectorToBox1) < 0) colAxis.vReverse(); + + /* Кодирование рёбер + бит 0: 1 (тип контакта ребро-ребро) + биты 1-2: индекс направляющей оси ребра + бит 3: + бит 4: + бит 5: + */ + var edgeCode1:int = 1; + var edgeCode2:int = 1; + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + point1.vCopy(box1.hs); + point2.vCopy(box2.hs); + // x + if (axisIdx1 == 0) { + point1.x = 0; + halfLen1 = box1.hs.x; + } else { + box1.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.x = -point1.x; + edgeCode1 |= 8; // 1 << 3 + } + } + if (axisIdx2 == 0) { + point2.x = 0; + halfLen2 = box2.hs.x; + } else { + box2.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.x = -point2.x; + edgeCode2 |= 8; // 1 << 3 + } + } + // y + if (axisIdx1 == 1) { + point1.y = 0; + halfLen1 = box1.hs.y; + edgeCode1 |= 2; // 1 << 1 + } else { + box1.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.y = -point1.y; + edgeCode1 |= 16; // 1 << 4 + } + } + if (axisIdx2 == 1) { + point2.y = 0; + halfLen2 = box2.hs.y; + edgeCode2 |= 2; // 1 << 1 + } else { + box2.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.y = -point2.y; + edgeCode2 |= 16; // 1 << 4 + } + } + // z + if (axisIdx1 == 2) { + point1.z = 0; + halfLen1 = box1.hs.z; + edgeCode1 |= 4; // 2 << 1 + } else { + box1.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.z = -point1.z; + edgeCode1 |= 32; // 1 << 5 + } + } + if (axisIdx2 == 2) { + point2.z = 0; + halfLen2 = box2.hs.z; + edgeCode2 |= 4; // 2 << 1 + } else { + box2.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.z = -point2.z; + edgeCode2 |= 32; // 1 << 5 + } + } + // Получаем глобальные координаты средних точек рёбер + point1.vTransformBy4(box1.transform); + point2.vTransformBy4(box2.transform); + // Находим точку пересечения рёбер, решая систему уравнений + var k:Number = axis10.vDot(axis20); + var det:Number = k*k - 1; + vector.vDiff(point2, point1); + var c1:Number = axis10.vDot(vector); + var c2:Number = axis20.vDot(vector); + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.normal.vCopy(colAxis); + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(point1.x + axis10.x*t1 + point2.x + axis20.x*t2); + cp.pos.y = 0.5*(point1.y + axis10.y*t1 + point2.y + axis20.y*t2); + cp.pos.z = 0.5*(point1.z + axis10.z*t1 + point2.z + axis20.z*t2); + cp.r1.vDiff(cp.pos, pos1); + cp.r2.vDiff(cp.pos, pos2); + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение боксов вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box1 + * @param box2 + * @param axis + * @param axisIndex + * @param vectorToBox1 + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, axisIndex:int, vectorToBox1:Vector3):Boolean { + if (axis.vLengthSqr() < 0.0001) { + return true; + } + axis.vNormalize(); + + var overlap:Number = overlapOnAxis(box1, box2, axis, vectorToBox1); + if (overlap < -tolerance) return false; + if (overlap + tolerance < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия двух боксов вдоль заданной оси. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param axis ось + * @param vectorToBox1 вектор, соединяющий центр второго бокса с центром первого + * @return величина перекрытия боксов вдоль оси + */ + public function overlapOnAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, vectorToBox1:Vector3):Number { + var m:Matrix4 = box1.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box1.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box1.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box1.hs.z; + if (d < 0) d = -d; + projection += d; + + m = box2.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box2.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box2.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box2.hs.z; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox1.x*axis.x + vectorToBox1.y*axis.y + vectorToBox1.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} + import alternativa.physics.types.Vector3; + + class CollisionPointTmp { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + } diff --git a/0.0.3.0/src/alternativa/physics/collision/BoxPlaneCollider.as b/0.0.3.0/src/alternativa/physics/collision/BoxPlaneCollider.as new file mode 100644 index 0000000..8568dc7 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/BoxPlaneCollider.as @@ -0,0 +1,78 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + public class BoxPlaneCollider implements ICollider { + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var normal:Vector3 = new Vector3(); + + public function BoxPlaneCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var box:RigidBox = body1 as RigidBox; +// var plane:RigidPlane; +// if (box == null) { +// box = body2 as RigidBox; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные координаты вершин бокса +// var sx:Number = box.halfSize.x; +// var sy:Number = box.halfSize.y; +// var sz:Number = box.halfSize.z; +// (verts1[0] as Vector3).reset(-sx, -sy, -sz); +// (verts1[1] as Vector3).reset(sx, -sy, -sz); +// (verts1[2] as Vector3).reset(sx, sy, -sz); +// (verts1[3] as Vector3).reset(-sx, sy, -sz); +// (verts1[4] as Vector3).reset(-sx, -sy, sz); +// (verts1[5] as Vector3).reset(sx, -sy, sz); +// (verts1[6] as Vector3).reset(sx, sy, sz); +// (verts1[7] as Vector3).reset(-sx, sy, sz); +// +// box.transform.transformVectors(verts1, verts2); +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// // Проверяем наличие столкновений с каждой вершиной +// collisionInfo.pcount = 0; +// for (var i:int = 0; i < 8; i++) { +// // Вершина добавляется в список точек столкновения, если лежит под плоскостью +// var dist:Number = (verts2[i] as Vector3).dot(normal); +// if (dist < offset) { +// var cp:ContactPoint; +// if (collisionInfo.pcount == collisionInfo.points.length) { +// cp = new ContactPoint(); +// collisionInfo.points[collisionInfo.pcount] = cp; +// } else { +// cp = collisionInfo.points[collisionInfo.pcount]; +// } +// cp.pos.copy(verts2[i]); +// cp.r1.diff(cp.pos, box.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); +// cp.penetration = offset - dist; +// collisionInfo.pcount++; +// } +// } +// if (collisionInfo.pcount > 0) { +// collisionInfo.body1 = box; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// return true; +// } + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/BoxRectCollider.as b/0.0.3.0/src/alternativa/physics/collision/BoxRectCollider.as new file mode 100644 index 0000000..b5287f3 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/BoxRectCollider.as @@ -0,0 +1,734 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + public class BoxRectCollider implements ICollider { + + private var tolerance:Number = 0.001; + + private var bPos:Vector3 = new Vector3(); + private var rPos:Vector3 = new Vector3(); + + private var vectorToBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var colAxis:Vector3 = new Vector3(); + private var tmpAxis:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxRectCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + /** + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + minOverlap = 1e10; + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + + // Вектор из центра прямоугольника в центр бокса + box.transform.getAxis(3, bPos); + rect.transform.getAxis(3, rPos); + vectorToBox.vDiff(bPos, rPos); + + // Проверка пересечения по нормали прямоугольника + rect.transform.getAxis(2, axis22); + if (!testAxis(box, rect, axis22, 0, vectorToBox)) return false; + + // Проверка пересечения по основным осям бокса + box.transform.getAxis(0, axis10); + if (!testAxis(box, rect, axis10, 1, vectorToBox)) return false; + box.transform.getAxis(1, axis11); + if (!testAxis(box, rect, axis11, 2, vectorToBox)) return false; + box.transform.getAxis(2, axis12); + if (!testAxis(box, rect, axis12, 3, vectorToBox)) return false; + + // Получаем направляющие рёбер прямоугольника + rect.transform.getAxis(0, axis20); + rect.transform.getAxis(1, axis21); + + // Проверка производных осей + if (!testAxis(box, rect, axis.vCross2(axis10, axis20), 4, vectorToBox)) return false; + if (!testAxis(box, rect, axis.vCross2(axis10, axis21), 5, vectorToBox)) return false; + + if (!testAxis(box, rect, axis.vCross2(axis11, axis20), 6, vectorToBox)) return false; + if (!testAxis(box, rect, axis.vCross2(axis11, axis21), 7, vectorToBox)) return false; + + if (!testAxis(box, rect, axis.vCross2(axis12, axis20), 8, vectorToBox)) return false; + if (!testAxis(box, rect, axis.vCross2(axis12, axis21), 9, vectorToBox)) return false; + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, rect, vectorToBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + findEdgesIntersection(box, rect, vectorToBox, int(bestAxisIndex/2), bestAxisIndex%2, contact); + } + contact.body1 = box.body; + contact.body2 = rect.body; + + return true; + } + + /** + * Выполняет поиск точек контакта бокса с прямоугольником. + * + * @param box бокс + * @param rect прямоугольник + * @param vectorToBox вектор, направленный из центра прямоугольника в центр бокса + * @param faceAxisIdx индекс оси, идентифицирующей полскость столкновения (грань бокса или полскость прямоугольника) + * @param colInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, faceAxisIdx:int, colInfo:Contact):Boolean { + var pnum:int, i:int, v:Vector3, cp:ContactPoint; + if (faceAxisIdx == 0) { + // Столкновение с плоскостью прямоугольника + + // Проверим положение бокса относительно плоскости прямоугольника + rect.transform.getAxis(2, colAxis); + var offset:Number = colAxis.vDot(rPos); + if (bPos.vDot(colAxis) < offset) return false; + + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + box.transform.getAxis(axisIdx, axis); + var dot:Number = axis.vDot(colAxis); + if (dot < 0) dot = -dot; + if (dot > maxDot) { + maxDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Получаем список вершин грани бокса, переводим их в систему координат прямоугольника и выполняем обрезку + // по прямоугольнику. Таким образом получается список потенциальных точек контакта. + box.transform.getAxis(incFaceAxisIdx, axis); + getFaceVertsByAxis(box.hs, incFaceAxisIdx, axis.vDot(colAxis) > 0, verts1); + box.transform.transformVectors(verts1, verts2); + rect.transform.transformVectorsInverse(verts2, verts1); + pnum = clipByRect(rect.hs); + // Проверяем каждую потенциальную точку на принадлежность нижней полуплоскости прямоугольника и добавляем такие точки в список контактов + colInfo.pcount = 0; + for (i = 0; i < pnum; i++) { + v = verts1[i]; + if (v.z < tolerance) { + cp = colInfo.points[colInfo.pcount++]; + rect.transform.transformVector(v, cp.pos); + cp.r1.vDiff(cp.pos, bPos); + cp.r2.vDiff(cp.pos, rPos); + cp.penetration = -v.z; + } + } + colInfo.normal.vCopy(colAxis); + } else { + // Столкновение с гранью бокса + faceAxisIdx--; + box.transform.getAxis(faceAxisIdx, colAxis); + var faceReversed:Boolean = colAxis.vDot(vectorToBox) > 0; + if (!faceReversed) colAxis.vReverse(); + + rect.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) return false; + + getFaceVertsByAxis(rect.hs, 2, false, verts1); + rect.transform.transformVectors(verts1, verts2); + box.transform.transformVectorsInverse(verts2, verts1); + pnum = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + colInfo.pcount = 0; + for (i = 0; i < pnum; i++) { + if ((pen = getPointBoxPenetration(box.hs, verts1[i], faceAxisIdx, faceReversed)) > -tolerance) { + cp = colInfo.points[colInfo.pcount++]; + box.transform.transformVector(verts1[i], cp.pos); + cp.r1.vDiff(cp.pos, bPos); + cp.r2.vDiff(cp.pos, rPos); + cp.penetration = pen; + } + } + colInfo.normal.vCopy(colAxis); + } + return true; + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + private function getFaceVertsByAxis(hs:Vector3, axisIdx:int, reverse:Boolean, result:Vector.):void { + switch (axisIdx) { + case 0: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, -hs.y, hs.z); + } + break; + case 1: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, hs.y, hs.z); + } + break; + case 2: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + } else { + (result[0] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } + break; + } + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + return clipHighX(hs.x, pnum, verts2, verts1); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + } + return 0; + } + + /** + * + * @param hs + * @return + */ + private function clipByRect(hs:Vector3):int { + var pnum:int = 4; + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box первый бокс + * @param rect второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param colInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, axisIdx1:int, axisIdx2:int, colInfo:Contact):void { + box.transform.getAxis(axisIdx1, axis10); + rect.transform.getAxis(axisIdx2, axis20); + colAxis.vCross2(axis10, axis20).vNormalize(); + // Разворот оси в сторону бокса + if (colAxis.vDot(vectorToBox) < 0) colAxis.vReverse(); + + /* Кодирование рёбер + бит 0: 1 (тип контакта ребро-ребро) + биты 1-2: индекс направляющей оси ребра + бит 3: + бит 4: + бит 5: + */ + var edgeCode1:int = 1; + var edgeCode2:int = 1; + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + point1.vCopy(box.hs); + point2.vCopy(rect.hs); + // x + if (axisIdx1 == 0) { + point1.x = 0; + halfLen1 = box.hs.x; + } else { + box.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.x = -point1.x; + edgeCode1 |= 8; // 1 << 3 + } + } + if (axisIdx2 == 0) { + point2.x = 0; + halfLen2 = rect.hs.x; + } else { + rect.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.x = -point2.x; + edgeCode2 |= 8; // 1 << 3 + } + } + // y + if (axisIdx1 == 1) { + point1.y = 0; + halfLen1 = box.hs.y; + edgeCode1 |= 2; // 1 << 1 + } else { + box.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.y = -point1.y; + edgeCode1 |= 16; // 1 << 4 + } + } + if (axisIdx2 == 1) { + point2.y = 0; + halfLen2 = rect.hs.y; + edgeCode2 |= 2; // 1 << 1 + } else { + rect.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.y = -point2.y; + edgeCode2 |= 16; // 1 << 4 + } + } + // z + if (axisIdx1 == 2) { + point1.z = 0; + halfLen1 = box.hs.z; + edgeCode1 |= 4; // 2 << 1 + } else { + box.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.z = -point1.z; + edgeCode1 |= 32; // 1 << 5 + } + } + // Получаем глобальные координаты средних точек рёбер + point1.vTransformBy4(box.transform); + point2.vTransformBy4(rect.transform); + // Находим точку пересечения рёбер, решая систему уравнений + var k:Number = axis10.vDot(axis20); + var det:Number = k*k - 1; + vector.vDiff(point2, point1); + var c1:Number = axis10.vDot(vector); + var c2:Number = axis20.vDot(vector); + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + colInfo.normal.vCopy(colAxis); + colInfo.pcount = 1; + var cp:ContactPoint = colInfo.points[0]; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(point1.x + axis10.x*t1 + point2.x + axis20.x*t2); + cp.pos.y = 0.5*(point1.y + axis10.y*t1 + point2.y + axis20.y*t2); + cp.pos.z = 0.5*(point1.z + axis10.z*t1 + point2.z + axis20.z*t2); + cp.r1.vDiff(cp.pos, bPos); + cp.r2.vDiff(cp.pos, rPos); + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box + * @param rect + * @param axis + * @param axisIndex + * @param vectorToBox + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + if (axis.vLengthSqr() < 0.0001) return true; + axis.vNormalize(); + + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -tolerance) return false; + if (overlap + tolerance < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия вдоль заданной оси. + * + * @param box бокс + * @param rect прямоугольник + * @param axis ось + * @param vectorToBox вектор, соединяющий центр прямоугольника с центром бокса + * @return величина перекрытия вдоль оси + */ + public function overlapOnAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, vectorToBox:Vector3):Number { + var m:Matrix4 = box.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box.hs.z; + if (d < 0) d = -d; + projection += d; + + m = rect.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*rect.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*rect.hs.y; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox.x*axis.x + vectorToBox.y*axis.y + vectorToBox.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/BoxSphereCollider.as b/0.0.3.0/src/alternativa/physics/collision/BoxSphereCollider.as new file mode 100644 index 0000000..2b166f3 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/BoxSphereCollider.as @@ -0,0 +1,104 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + use namespace altphysics; + /** + * + */ + public class BoxSphereCollider implements ICollider { + + private var center:Vector3 = new Vector3(); + private var closestPt:Vector3 = new Vector3(); + + private var bPos:Vector3 = new Vector3(); + private var sPos:Vector3 = new Vector3(); + + /** + * + */ + public function BoxSphereCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + // Зафиксированно столкновение + contact.body1 = sphere.body; + contact.body2 = box.body; + contact.normal.vCopy(closestPt).vTransformBy4(box.transform).vSubtract(sPos).vNormalize().vReverse(); + contact.pcount = 1; + + var cp:ContactPoint = contact.points[0]; + cp.penetration = sphere.r - Math.sqrt(distSqr); + cp.pos.vCopy(contact.normal).vScale(-sphere.r).vAdd(sPos); + cp.r1.vDiff(cp.pos, sPos); + cp.r2.vDiff(cp.pos, bPos); + + return true; + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/BoxTriangleCollider.as b/0.0.3.0/src/alternativa/physics/collision/BoxTriangleCollider.as new file mode 100644 index 0000000..63c6efd --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/BoxTriangleCollider.as @@ -0,0 +1,14 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Contact; + import alternativa.physics.collision.primitives.CollisionPrimitive; + + public class BoxTriangleCollider implements ICollider { + public function BoxTriangleCollider() { + } + + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/BruteForceCollisionDetector.as b/0.0.3.0/src/alternativa/physics/collision/BruteForceCollisionDetector.as new file mode 100644 index 0000000..d2c2e2a --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/BruteForceCollisionDetector.as @@ -0,0 +1,37 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + public class BruteForceCollisionDetector implements ICollisionDetector { + public function BruteForceCollisionDetector() { + } + + public function addPrimitive(primitive:CollisionPrimitive, isStatic:Boolean=true):Boolean { + return false; + } + + public function removePrimitive(primitive:CollisionPrimitive, isStatic:Boolean=true):Boolean { + return false; + } + + public function init():void { + } + + public function getAllCollisions(contacts:Vector.):int { + return 0; + } + + public function intersectRay(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, intersection:RayIntersection):Boolean { + return false; + } + + public function intersectRayWithStatic(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/CollisionKdNode.as b/0.0.3.0/src/alternativa/physics/collision/CollisionKdNode.as new file mode 100644 index 0000000..e1a1657 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/CollisionKdNode.as @@ -0,0 +1,16 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.types.BoundBox; + + public class CollisionKdNode { + public var objects:Vector.; + public var boundBox:BoundBox; + public var parent:CollisionKdNode; + + public var axis:int = -1; // 0 - x, 1 - y, 2 - z + public var coord:Number; + public var positiveNode:CollisionKdNode; + public var negativeNode:CollisionKdNode; + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/CollisionKdTree.as b/0.0.3.0/src/alternativa/physics/collision/CollisionKdTree.as new file mode 100644 index 0000000..6c62b79 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/CollisionKdTree.as @@ -0,0 +1,259 @@ +package alternativa.physics.collision { + + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + + /** + * @author mike + */ + public class CollisionKdTree { + + public var threshold:Number = 0.01; + public var minPrimitivesPerNode:int = 1; + + public var rootNode:CollisionKdNode; + public var staticChildren:Vector. = new Vector.(); + public var numStaticChildren:int; + private var staticBoundBoxes:Vector. = new Vector.(); + + /** + * @param primitive + */ + public function addStaticPrimitive(primitive:CollisionPrimitive):void { + staticChildren[numStaticChildren++] = primitive; + } + + /** + * @param primitive + * @return + */ + public function removeStaticPrimitive(primitive:CollisionPrimitive):Boolean { + var idx:int = staticChildren.indexOf(primitive); + if (idx < 0) return false; + staticChildren.splice(idx, 1); + numStaticChildren--; + return true; + } + + /** + * @param boundBox + */ + public function createTree(boundBox:BoundBox = null):void { + if (numStaticChildren == 0) return; + // Создаём корневую ноду + rootNode = new CollisionKdNode(); + rootNode.objects = new Vector.(); + // Расчитываем баунды объектов и рутовой ноды + var rootNodeBoundBox:BoundBox = rootNode.boundBox = (boundBox != null) ? boundBox : new BoundBox(); + for (var i:int = 0; i < numStaticChildren; i++) { + var child:CollisionPrimitive = staticChildren[i]; + var childBoundBox:BoundBox = staticBoundBoxes[i] = child.calculateAABB(); + rootNodeBoundBox.addBoundBox(childBoundBox); + rootNode.objects[i] = i; + } + staticBoundBoxes.length = numStaticChildren; + // Разделяем рутовую ноду + splitNode(rootNode); + } + + private var splitAxis:int; + private var splitCoord:Number; + private var splitCost:Number; + static private const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + static private const splitCoordsX:Vector. = new Vector.(); + static private const splitCoordsY:Vector. = new Vector.(); + static private const splitCoordsZ:Vector. = new Vector.(); + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + if (node.objects.length <= minPrimitivesPerNode) return; + + var objects:Vector. = node.objects; + var i:int, j:int, k:int, length:int = objects.length, c1:Number, c2:Number, ct1:Number, ct2:Number, area:Number, areaNegative:Number, areaPositive:Number, numNegative:int, numPositive:int, conflict:Boolean, cost:Number; + + var nodeBoundBox:BoundBox = node.boundBox; + + // Подготовка баунда с погрешностями + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + // Собираем опорные координаты + var numSplitCoordsX:int = 0, numSplitCoordsY:int = 0, numSplitCoordsZ:int = 0; + for (i = 0; i < length; i++) { + var boundBox:BoundBox = staticBoundBoxes[objects[i]]; + if (boundBox.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = boundBox.minX; + if (boundBox.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = boundBox.maxX; + + if (boundBox.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = boundBox.minY; + if (boundBox.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = boundBox.maxY; + + if (boundBox.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.minZ; + if (boundBox.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.maxZ; + } + + // Убираем дубликаты координат, ищем наилучший сплит + splitAxis = -1; splitCost = 1e308; + i = 0; area = (nodeBoundBox.maxY - nodeBoundBox.minY)*(nodeBoundBox.maxZ - nodeBoundBox.minZ); + while (i < numSplitCoordsX) { + if (isNaN(c1 = splitCoordsX[i++])) continue; + ct1 = c1 - threshold; + ct2 = c1 + threshold; + areaNegative = area*(c1 - nodeBoundBox.minX); + areaPositive = area*(nodeBoundBox.maxX - c1); + numNegative = numPositive = 0; + conflict = false; + // Проверяем объекты + for (j = 0; j < length; j++) { + boundBox = staticBoundBoxes[objects[j]]; + if (boundBox.maxX <= ct2) { + if (boundBox.minX < ct1) numNegative++; + } else { + if (boundBox.minX >= ct1) numPositive++; else {conflict = true; break;} + } + } + // Если хороший сплит, сохраняем + if (!conflict && (cost = areaNegative*numNegative + areaPositive*numPositive) < splitCost) { + splitCost = cost; + splitAxis = 0; + splitCoord = c1; + } + j = i; + while (++j < numSplitCoordsX) if ((c2 = splitCoordsX[j]) >= c1 - threshold && c2 <= c1 + threshold) splitCoordsX[j] = NaN; + } + i = 0; area = (nodeBoundBox.maxX - nodeBoundBox.minX)*(nodeBoundBox.maxZ - nodeBoundBox.minZ); + while (i < numSplitCoordsY) { + if (isNaN(c1 = splitCoordsY[i++])) continue; + ct1 = c1 - threshold; + ct2 = c1 + threshold; + areaNegative = area*(c1 - nodeBoundBox.minY); + areaPositive = area*(nodeBoundBox.maxY - c1); + numNegative = numPositive = 0; + conflict = false; + // Проверяем объекты + for (j = 0; j < length; j++) { + boundBox = staticBoundBoxes[objects[j]]; + if (boundBox.maxY <= ct2) { + if (boundBox.minY < ct1) numNegative++; + } else { + if (boundBox.minY >= ct1) numPositive++; else {conflict = true; break;} + } + } + // Если хороший сплит, сохраняем + if (!conflict && (cost = areaNegative*numNegative + areaPositive*numPositive) < splitCost) { + splitCost = cost; + splitAxis = 1; + splitCoord = c1; + } + j = i; + while (++j < numSplitCoordsY) if ((c2 = splitCoordsY[j]) >= c1 - threshold && c2 <= c1 + threshold) splitCoordsY[j] = NaN; + } + i = 0; area = (nodeBoundBox.maxX - nodeBoundBox.minX)*(nodeBoundBox.maxY - nodeBoundBox.minY); + while (i < numSplitCoordsZ) { + if (isNaN(c1 = splitCoordsZ[i++])) continue; + ct1 = c1 - threshold; + ct2 = c1 + threshold; + areaNegative = area*(c1 - nodeBoundBox.minZ); + areaPositive = area*(nodeBoundBox.maxZ - c1); + numNegative = numPositive = 0; + conflict = false; + // Проверяем объекты + for (j = 0; j < length; j++) { + boundBox = staticBoundBoxes[objects[j]]; + if (boundBox.maxZ <= ct2) { + if (boundBox.minZ < ct1) numNegative++; + } else { + if (boundBox.minZ >= ct1) numPositive++; else {conflict = true; break;} + } + } + // Если хороший сплит, сохраняем + if (!conflict && (cost = areaNegative*numNegative + areaPositive*numPositive) < splitCost) { + splitCost = cost; + splitAxis = 2; + splitCoord = c1; + } + j = i; + while (++j < numSplitCoordsZ) if ((c2 = splitCoordsZ[j]) >= c1 - threshold && c2 <= c1 + threshold) splitCoordsZ[j] = NaN; + } + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Разделяем ноду + var axisX:Boolean = splitAxis == 0, axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.positiveNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.positiveNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + ct1 = splitCoord - threshold; ct2 = splitCoord + threshold; + for (i = 0; i < length; i++) { + boundBox = staticBoundBoxes[objects[i]]; + var min:Number = axisX ? boundBox.minX : (axisY ? boundBox.minY : boundBox.minZ); + var max:Number = axisX ? boundBox.maxX : (axisY ? boundBox.maxY : boundBox.maxZ); + if (max <= ct2) { + // Объект в негативной стороне + if (node.negativeNode.objects == null) node.negativeNode.objects = new Vector.(); + node.negativeNode.objects.push(objects[i]); + objects[i] = -1; + } else { + if (min >= ct1) { + // Объект в положительной стороне + if (node.positiveNode.objects == null) node.positiveNode.objects = new Vector.(); + node.positiveNode.objects.push(objects[i]); + objects[i] = -1; + } else { + // Распилился + } + } + } + + // Очистка списка объектов + j = 0; + for (i = 0; i < length; i++) { + if (objects[i] >= 0) objects[j++] = objects[i]; + } + if (j > 0) objects.length = j; else node.objects = null; + + // Разделение дочерних нод + if (node.negativeNode.objects != null) splitNode(node.negativeNode); + if (node.positiveNode.objects != null) splitNode(node.positiveNode); + } + + /** + * + */ + public function traceTree():void { + traceNode("", rootNode); + } + + /** + * @param str + * @param node + */ + private function traceNode(str:String, node:CollisionKdNode):void { + if (node == null) return; + trace(str, node.axis == -1 ? "end" : ((node.axis == 0) ? "X" : ((node.axis == 1) ? "Y" : "Z")), "splitCoord=" + splitCoord, "bound", node.boundBox, "objs:", node.objects); + traceNode(str + "-", node.negativeNode); + traceNode(str + "+", node.positiveNode); + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/IBodyCollisionPredicate.as b/0.0.3.0/src/alternativa/physics/collision/IBodyCollisionPredicate.as new file mode 100644 index 0000000..9cfdca5 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/IBodyCollisionPredicate.as @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Body; + + public interface IBodyCollisionPredicate { + function considerBodies(body1:Body, body2:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/ICollider.as b/0.0.3.0/src/alternativa/physics/collision/ICollider.as new file mode 100644 index 0000000..733231b --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/ICollider.as @@ -0,0 +1,12 @@ +package alternativa.physics.collision { + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + + /** + * Интерфейс определителя столкновений между двумя примитивами. + */ + public interface ICollider { + function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/ICollisionDetector.as b/0.0.3.0/src/alternativa/physics/collision/ICollisionDetector.as new file mode 100644 index 0000000..83e94d7 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/ICollisionDetector.as @@ -0,0 +1,73 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + /** + * Интерфейс детектора столкновений. + */ + public interface ICollisionDetector { + + /** + * Добавляет физический примитив в коллайдер. + * + * @param primitive добавляемый примитив + * @param isStatic указывает тип примитива: статический или динамический + * @return true если примитив был успешно добавлен, иначе false + */ + function addPrimitive(primitive:CollisionPrimitive, isStatic:Boolean = true):Boolean; + + /** + * Удаляет физический примитив из коллайдера. + * + * @param primitive удаляемый примитив + * @param isStatic указывает тип примитива: статический или динамический + * @return true если примитив был успшено удалён, иначе false + */ + function removePrimitive(primitive:CollisionPrimitive, isStatic:Boolean = true):Boolean; + + /** + * Выполняет инициализацию детектора после обновления списка примитивов. + */ + function init():void; + + /** + * Получает все столкновения в текущей конфигурации физической геометрии. + * + * @param contacts список контактов, в кторые будет записана информация о столкновении + * @return количество найденных столкновений + */ + function getAllCollisions(contacts:Vector.):int; + + /** + * Тестирует луч на пересечение с физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param dir направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRay(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * Тестирует луч на пересечение со статической физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param dir направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRayWithStatic(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/ICollisionPredicate.as b/0.0.3.0/src/alternativa/physics/collision/ICollisionPredicate.as new file mode 100644 index 0000000..bb1b756 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/ICollisionPredicate.as @@ -0,0 +1,9 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + + public interface ICollisionPredicate { + + function considerCollision(primitive:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/IRayCollisionPredicate.as b/0.0.3.0/src/alternativa/physics/collision/IRayCollisionPredicate.as new file mode 100644 index 0000000..0f3d929 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/IRayCollisionPredicate.as @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Body; + + public interface IRayCollisionPredicate { + function considerBody(body:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/KdTreeCollisionDetector.as b/0.0.3.0/src/alternativa/physics/collision/KdTreeCollisionDetector.as new file mode 100644 index 0000000..b12218e --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/KdTreeCollisionDetector.as @@ -0,0 +1,452 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Детектор, хранящий статическую геометрию в kD-дереве и использующий дерево для ускорения тестов на пересечения. + */ + public class KdTreeCollisionDetector implements ICollisionDetector { + + altphysics var tree:CollisionKdTree; + altphysics var dynamicPrimitives:Vector.; + 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.(); + + addCollider(CollisionPrimitive.BOX, CollisionPrimitive.BOX, new BoxBoxCollider()); +// addCollider(CollisionPrimitive.BOX, CollisionPrimitive.PLANE, new BoxPlaneCollider()); + addCollider(CollisionPrimitive.BOX, CollisionPrimitive.SPHERE, new BoxSphereCollider()); + addCollider(CollisionPrimitive.BOX, CollisionPrimitive.RECT, new BoxRectCollider()); + +// 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():void { + tree.createTree(); +// tree.traceTree(); + } + + /** + * + * @param contacts + * @return + */ + public function getAllCollisions(contacts:Vector.):int { + var colNum:int = 0; + for (var i:int = 0; i < dynamicPrimitivesNum; i++) { + var primitive:CollisionPrimitive = dynamicPrimitives[i]; + primitive.calculateAABB(); + if (primitive.body != null && primitive.body.frozen) continue; + colNum += getPrimitiveNodeCollisions(tree.rootNode, primitive, contacts, colNum); + + // Столкновения динамических примитивов между собой + // TODO: Попробовать различные оптимизации (сортировка по баундам, встраивание в дерево) + for (var j:int = i + 1; j < dynamicPrimitivesNum; j++) { + if (collide(primitive, dynamicPrimitives[j], contacts[colNum])) colNum++; + } + } + + return colNum; + } + + /** + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + public function collide(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.collide(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 = (contact.points[0] as ContactPoint).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 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 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:Vector., startIndex:int):int { + var colNum:int = 0; + if (node.objects != null) { + // Поиск столкновений со статическими примитивами узла + var primitives:Vector. = tree.staticChildren; + var indices:Vector. = node.objects; + for (var i:int = indices.length - 1; i >= 0; i--) + if (collide(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; + } + + 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.getSegmentIntersection(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.objects != null && getRayNodeIntersection(origin, dir, collisionGroup, tree.staticChildren, node.objects, 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., indices:Vector., 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.getSegmentIntersection(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; + } \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/SpherePlaneCollider.as b/0.0.3.0/src/alternativa/physics/collision/SpherePlaneCollider.as new file mode 100644 index 0000000..9b527d8 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/SpherePlaneCollider.as @@ -0,0 +1,58 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + /** + * + */ + public class SpherePlaneCollider implements ICollider { + + private var normal:Vector3 = new Vector3(); + + /** + * + */ + public function SpherePlaneCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var sphere:RigidSphere = body1 as RigidSphere; +// var plane:RigidPlane; +// if (sphere == null) { +// sphere = body2 as RigidSphere; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// +// var dist:Number = sphere.state.pos.dot(normal) - offset; +// if (dist > sphere.r) return false; +// +// collisionInfo.body1 = sphere; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// collisionInfo.pcount = 1; +// +// var cp:ContactPoint = collisionInfo.points[0]; +// cp.penetration = sphere.r - dist; +// cp.pos.copy(normal).reverse().scale(sphere.r).add(sphere.state.pos); +// cp.r1.diff(cp.pos, sphere.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); + + return true; + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/SphereSphereCollider.as b/0.0.3.0/src/alternativa/physics/collision/SphereSphereCollider.as new file mode 100644 index 0000000..9111a84 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/SphereSphereCollider.as @@ -0,0 +1,60 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + public class SphereSphereCollider implements ICollider { + + private var p1:Vector3 = new Vector3(); + private var p2:Vector3 = new Vector3(); + + public function SphereSphereCollider() { + } + + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var s1:CollisionSphere; + var s2:CollisionSphere; + if (prim1.body != null) { + s1 = prim1 as CollisionSphere; + s2 = prim2 as CollisionSphere; + } else { + s1 = prim2 as CollisionSphere; + s2 = prim1 as CollisionSphere; + } + + s1.transform.getAxis(3, p1); + s2.transform.getAxis(3, p2); + var dx:Number = p1.x - p2.x; + var dy:Number = p1.y - p2.y; + var dz:Number = p1.z - p2.z; + var len:Number = dx*dx + dy*dy + dz*dz; + var sum:Number = s1.r + s2.r; + if (len > sum*sum) return false; + len = Math.sqrt(len); + dx /= len; + dy /= len; + dz /= len; + + contact.body1 = s1.body; + contact.body2 = s2.body; + contact.normal.x = dx; + contact.normal.y = dy; + contact.normal.z = dz; + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = sum - len; + cp.pos.x = p1.x - dx*s1.r; + cp.pos.y = p1.y - dy*s1.r; + cp.pos.z = p1.z - dz*s1.r; + cp.r1.vDiff(cp.pos, p1); + cp.r2.vDiff(cp.pos, p2); + + return true; + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/primitives/.svn/all-wcprops b/0.0.3.0/src/alternativa/physics/collision/primitives/.svn/all-wcprops new file mode 100644 index 0000000..4760588 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/primitives/.svn/all-wcprops @@ -0,0 +1,35 @@ +K 25 +svn:wc:ra_dav:version-url +V 124 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/collision/primitives +END +CollisionTriangle.as +K 25 +svn:wc:ra_dav:version-url +V 145 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/collision/primitives/CollisionTriangle.as +END +CollisionSphere.as +K 25 +svn:wc:ra_dav:version-url +V 143 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/collision/primitives/CollisionSphere.as +END +CollisionBox.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/collision/primitives/CollisionBox.as +END +CollisionPrimitive.as +K 25 +svn:wc:ra_dav:version-url +V 146 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/collision/primitives/CollisionPrimitive.as +END +CollisionRect.as +K 25 +svn:wc:ra_dav:version-url +V 141 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/collision/primitives/CollisionRect.as +END diff --git a/0.0.3.0/src/alternativa/physics/collision/primitives/.svn/entries b/0.0.3.0/src/alternativa/physics/collision/primitives/.svn/entries new file mode 100644 index 0000000..d22bff1 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/primitives/.svn/entries @@ -0,0 +1,88 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/collision/primitives +http://svndev.alternativaplatform.com + + + +2009-06-25T09:18:57.805913Z +15117 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +CollisionTriangle.as +file + + + + +2010-10-28T04:32:40.000000Z +a1ccd11eb25d172d2739fce7fef21ee2 +2009-06-08T06:46:18.310732Z +14099 +mike + +CollisionSphere.as +file + + + + +2010-10-28T04:32:40.000000Z +1ca67429cc8bf27435b6ec46f8d6f80a +2009-05-26T08:41:33.471001Z +13389 +mike + +CollisionBox.as +file + + + + +2010-10-28T04:32:40.000000Z +e6a441a4317416b0d13d365f5b26ace4 +2009-06-08T07:53:17.170410Z +14100 +mike + +CollisionPrimitive.as +file + + + + +2010-10-28T04:32:40.000000Z +ba2d42da95941781dfd027568c57888c +2009-06-25T09:18:57.805913Z +15117 +mike + +CollisionRect.as +file + + + + +2010-10-28T04:32:40.000000Z +c76d5cb0626964578f2df221c8072142 +2009-06-08T07:53:17.170410Z +14100 +mike + diff --git a/0.0.3.0/src/alternativa/physics/collision/primitives/.svn/format b/0.0.3.0/src/alternativa/physics/collision/primitives/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/primitives/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.3.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base b/0.0.3.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base new file mode 100644 index 0000000..e3d90cc --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base @@ -0,0 +1,178 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + /** + * Ориентированный бокс. + */ + public class CollisionBox extends CollisionPrimitive { + + // Половинные размеры вдоль каждой из осей + public var hs:Vector3 = new Vector3(); + + // Используются в определении пересечения с лучом + private static var _o:Vector3 = new Vector3(); + private static var _v:Vector3 = new Vector3(); + + /** + * @param hs + * @param collisionGroup + */ + public function CollisionBox(hs:Vector3, collisionGroup:int) { + super(BOX, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + var t:Matrix4 = transform; + aabb.maxX = hs.x*(t.a < 0 ? -t.a : t.a) + hs.y*(t.b < 0 ? -t.b : t.b) + hs.z*(t.c < 0 ? -t.c : t.c); + aabb.minX = -aabb.maxX; + + aabb.maxY = hs.x*(t.e < 0 ? -t.e : t.e) + hs.y*(t.f < 0 ? -t.f : t.f) + hs.z*(t.g < 0 ? -t.g : t.g); + aabb.minY = -aabb.maxY; + + aabb.maxZ = hs.x*(t.i < 0 ? -t.i : t.i) + hs.y*(t.j < 0 ? -t.j : t.j) + hs.z*(t.k < 0 ? -t.k : t.k); + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var box:CollisionBox = source as CollisionBox; + if (box == null) return this; + super.copyFrom(box); + hs.vCopy(box.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionBox(hs, collisionGroup); + } + + /** + * Вычисляет параметры пересечения заданного сегмента с примитивом. + * + * @param origin начальная точка сегмента в мировых координатах + * @param vector вектор сегмента в мировых координатах + * @param threshold погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную + * погрешность, считается равной нулю. + * @param normal возвращаемое значение. Нормаль к примитиву в точке пересечения с сегментом. + * @return в случае наличия пересечения возвращается время точки пересечения, в противном случае возвращается -1. + */ + override public function getSegmentIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var tMin:Number = -1, tMax:Number = 1e308, t1:Number, t2:Number; + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var xx:Number = origin.x - transform.d; + var yy:Number = origin.y - transform.h; + var zz:Number = origin.z - transform.l; + _o.x = transform.a*xx + transform.e*yy + transform.i*zz; + _o.y = transform.b*xx + transform.f*yy + transform.j*zz; + _o.z = transform.c*xx + transform.g*yy + transform.k*zz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + _v.x = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + _v.y = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + _v.z = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + // X + if (_v.x < threshold && _v.x > -threshold) { + if (_o.x < -hs.x || _o.x > hs.x) return -1; + } else { + t1 = (-hs.x - _o.x)/_v.x; + t2 = (hs.x - _o.x)/_v.x; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.x = -1; + normal.y = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.x = 1; + normal.y = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Y + if (_v.y < threshold && _v.y > -threshold) { + if (_o.y < -hs.y || _o.y > hs.y) return -1; + } else { + t1 = (-hs.y - _o.y)/_v.y; + t2 = (hs.y - _o.y)/_v.y; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.y = -1; + normal.x = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.y = 1; + normal.x = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Z + if (_v.z < threshold && _v.z > -threshold) { + if (_o.z < -hs.z || _o.z > hs.z) return -1; + } else { + t1 = (-hs.z - _o.z)/_v.z; + t2 = (hs.z - _o.z)/_v.z; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.z = -1; + normal.x = normal.y = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.z = 1; + normal.x = normal.y = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Перевод нормали в мировую систему координат + // Inlined normal.vDeltaTransformBy4(transform); + xx = normal.x, yy = normal.y, zz = normal.z; + normal.x = transform.a*xx + transform.b*yy + transform.c*zz; + normal.y = transform.e*xx + transform.f*yy + transform.g*zz; + normal.z = transform.i*xx + transform.j*yy + transform.k*zz; + + return tMin; + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionPrimitive.as.svn-base b/0.0.3.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionPrimitive.as.svn-base new file mode 100644 index 0000000..352a940 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionPrimitive.as.svn-base @@ -0,0 +1,130 @@ +package alternativa.physics.collision.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.ICollisionPredicate; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Базовый класс для примитивов, использующихся детектором столкновений. + */ + public class CollisionPrimitive { + // Константы типов примитива + public static const BOX:int = 1; + public static const PLANE:int = 2; + public static const SPHERE:int = 3; + public static const RECT:int = 4; + public static const TRIANGLE:int = 5; + + // Тип примитива + public var type:int; + // Группы примитива. Каждая группа определяется установленным битом. Столкновения проверяются только для примитивов, + // имеющих хотя бы одну общую группу. + public var collisionGroup:int; + // Предикат примитива, вызывающийся детектором при нахождении столкновения. В зависимости от возвращённого + // предикатом результата столкновение либо регистрируется, либо игнорируется. + // Nullable + public var postCollisionPredicate:ICollisionPredicate; + + // Тело, владеющее примитивом. + // Nullable + altphysics var body:Body; + // Трансформация примитива в системе координат тела, если оно указано. Не допускается масштабирование матрицы. + altphysics var localTransform:Matrix4 = new Matrix4(); + // Полная трансформация примитива. Не допускается масштабирование матрицы. + altphysics var transform:Matrix4 = new Matrix4(); + // AABB в мировой системе координат. Расчитывается системой вызовом функции calculateBoundBox(). + altphysics var aabb:BoundBox = new BoundBox(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param type тип примитива + * @param collisionGroup группа примитива + */ + public function CollisionPrimitive(type:int, collisionGroup:int) { + this.type = type; + this.collisionGroup = collisionGroup; + } + + /** + * Устанавливает тело, владеющее примитивом. + * + * @param body тело, к которому относится примитив. Если указано значение null, матрица локальной трансформации удаляется. + * @param localTransform трансформация примитива в системе координат тела. Указание значения null равносильно + * заданию единичной матрицы. + */ + public function setBody(body:Body, localTransform:Matrix4 = null):void { + if (this.body == body) return; + this.body = body; + if (body == null) this.localTransform = null; + else { + if (this.localTransform == null) this.localTransform = new Matrix4(); + if (localTransform != null) this.localTransform.copy(localTransform); + } + } + + /** + * Наследники должны переопределять этот метод, реализуя в нём корректный расчёт ограничивающего бокса, выравненного + * по осям мировой системы координат. + * + * @return ссылка на свой баунд бокс + */ + public function calculateAABB():BoundBox { + return aabb; + } + + /** + * Клонирует примитив. Переопределять не рекомендуется. Вместо этого переопределяются методы createPrimitive() и copyFrom(). + * + * @return клон примитива + */ + public function clone():CollisionPrimitive { + var p:CollisionPrimitive = createPrimitive(); + return p.copyFrom(this); + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + type = source.type; + transform.copy(source.transform); + collisionGroup = source.collisionGroup; + setBody(source.body, source.localTransform); + aabb.copyFrom(source.aabb); + return this; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + protected function createPrimitive():CollisionPrimitive { + return new CollisionPrimitive(type, collisionGroup); + } + + /** + * Метод должен вычислять параметры пересечения заданного сегмента с примитивом. + * + * @param origin начальная точка сегмента в мировых координатах + * @param vector вектор сегмента в мировых координатах + * @param threshold погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную + * погрешность, считается равной нулю. + * @param normal возвращаемое значение. Нормаль к примитиву в точке пересечения с сегментом. + * @return в случае наличия пересечения возвращается время точки пересечения, в противном случае возвращается -1. + */ + public function getSegmentIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + return -1; + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base b/0.0.3.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base new file mode 100644 index 0000000..7d6aa47 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base @@ -0,0 +1,126 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + /** + * Ориентированный прямоугольник. Задаётся половинами размеров вдоль осей X (ширина) и Y (длина) локальной системы + * координат примитива. Таким образом, прямоугольник лежит в плоскости XY, его стороны параллельны осям этой + * плоскости, а нормаль направлена вдоль локальной оси Z. + * + * Прямоугольник может быть одно- или двусторонним. В случае одностороннего прямоугольника, + * столкновения с внутренней стороны не регистрируются. + */ + public class CollisionRect extends CollisionPrimitive { + // Половинные размеры прямоугольника вдоль осей X и Y. Нормаль направлена вдоль оси Z. + public var hs:Vector3 = new Vector3(); + // Флаг указывает, является примитив одно- или двусторонним + public var twoSided:Boolean = true; + + // Малое значение. Используется для указания фиктивной высоты примитива и в функции определения пересечения с сегментом. + private static const EPSILON:Number = 0.01; + // Переменные используются в функции поиска пересечения с лучом + private static var _o:Vector3 = new Vector3(); + private static var _v:Vector3 = new Vector3(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param hs половинные размерв прямоугольника вдоль осей X и Y. Значение z игнорируется. + * @param collisionGroup группа примитива + */ + public function CollisionRect(hs:Vector3, collisionGroup:int) { + super(RECT, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * Расчитывает ограничивающий бокс прямоугольника. Для избежания проблем высота примитива принимается равной + * не нулю, а малому значению. + * + * @return + */ + override public function calculateAABB():BoundBox { + // Баунд бокс прямоугольника имеет минимальную высоту, отличную от нуля во избежание проблем с построением kd-дерева + var t:Matrix4 = transform; + aabb.maxX = hs.x*(t.a < 0 ? -t.a : t.a) + hs.y*(t.b < 0 ? -t.b : t.b) + EPSILON*(t.c < 0 ? -t.c : t.c); + aabb.minX = -aabb.maxX; + + aabb.maxY = hs.x*(t.e < 0 ? -t.e : t.e) + hs.y*(t.f < 0 ? -t.f : t.f) + EPSILON*(t.g < 0 ? -t.g : t.g); + aabb.minY = -aabb.maxY; + + aabb.maxZ = hs.x*(t.i < 0 ? -t.i : t.i) + hs.y*(t.j < 0 ? -t.j : t.j) + EPSILON*(t.k < 0 ? -t.k : t.k); + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var rect:CollisionRect = source as CollisionRect; + if (rect == null) return this; + super.copyFrom(rect); + hs.vCopy(rect.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionRect(hs, collisionGroup); + } + + /** + * @param origin + * @param vector + * @param collisionGroup + * @param threshold + * @param normal + * @return + */ + override public function getSegmentIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var xx:Number = origin.x - transform.d; + var yy:Number = origin.y - transform.h; + var zz:Number = origin.z - transform.l; + _o.x = transform.a*xx + transform.e*yy + transform.i*zz; + _o.y = transform.b*xx + transform.f*yy + transform.j*zz; + _o.z = transform.c*xx + transform.g*yy + transform.k*zz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + _v.x = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + _v.y = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + _v.z = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + + // Проверка параллельности сегмента и плоскости примитива + if (_v.z > -threshold && _v.z < threshold) return -1; + var t:Number = -_o.z/_v.z; + if (t < 0) return -1; + // Проверка вхождения точки пересечения в прямоугольник + _o.x += _v.x*t; + _o.y += _v.y*t; + _o.z = 0; + if (_o.x < (-hs.x - EPSILON) || _o.x > (hs.x + EPSILON) || _o.y < (-hs.y - EPSILON) || _o.y > (hs.y + EPSILON)) return -1; + + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + return t; + } + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base b/0.0.3.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base new file mode 100644 index 0000000..1a9e6d1 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base @@ -0,0 +1,80 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + + /** + * Сфера. + */ + public class CollisionSphere extends CollisionPrimitive { + + // Радиус сферы + public var r:Number = 0; + + /** + * + * @param r + * @param collisionGroup + */ + public function CollisionSphere(r:Number, collisionGroup:int) { + super(SPHERE, collisionGroup); + this.r = r; + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + aabb.maxX = transform.d + r; + aabb.minX = transform.d - r; + + aabb.maxY = transform.h + r; + aabb.minY = transform.h - r; + + aabb.maxZ = transform.l + r; + aabb.minZ = transform.l - r; + + return aabb; + } + + /** + * @param origin + * @param vector + * @param threshold + * @param normal + * @return + */ + override public function getSegmentIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var px:Number = origin.x - transform.d; + var py:Number = origin.y - transform.h; + var pz:Number = origin.z - transform.l; + var k:Number = vector.x*px + vector.y*py + vector.z*pz; + if (k > 0) return -1; + var a:Number = vector.x*vector.x + vector.y*vector.y + vector.z*vector.z; + var D:Number = k*k - a*(px*px + py*py + pz*pz - r*r); + if (D < 0) return -1; + return -(k + Math.sqrt(D))/a; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var sphere:CollisionSphere = source as CollisionSphere; + if (sphere == null) return this; + super.copyFrom(sphere); + r = sphere.r; + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionSphere(r, collisionGroup); + } + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base b/0.0.3.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base new file mode 100644 index 0000000..a48803a --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base @@ -0,0 +1,39 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + public class CollisionTriangle extends CollisionPrimitive { + + public var v0:Vector3 = new Vector3(); + public var v1:Vector3 = new Vector3(); + public var v2:Vector3 = new Vector3(); + public var e0:Vector3 = new Vector3(); + public var e1:Vector3 = new Vector3(); + public var e2:Vector3 = new Vector3(); + public var len0:Number; + public var len1:Number; + public var len2:Number; + + public function CollisionTriangle(v0:Vector3, v1:Vector3, v2:Vector3, collisionGroup:int) { + super(TRIANGLE, collisionGroup); + + this.v0.vCopy(v0); + this.v1.vCopy(v1); + this.v2.vCopy(v2); + + e0.vDiff(v1, v0); + len0 = e0.vLength(); + e0.vNormalize(); + + e1.vDiff(v2, v1); + len1 = e1.vLength(); + e1.vNormalize(); + + e2.vDiff(v0, v2); + len2 = e2.vLength(); + e2.vNormalize(); + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/primitives/CollisionBox.as b/0.0.3.0/src/alternativa/physics/collision/primitives/CollisionBox.as new file mode 100644 index 0000000..e3d90cc --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/primitives/CollisionBox.as @@ -0,0 +1,178 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + /** + * Ориентированный бокс. + */ + public class CollisionBox extends CollisionPrimitive { + + // Половинные размеры вдоль каждой из осей + public var hs:Vector3 = new Vector3(); + + // Используются в определении пересечения с лучом + private static var _o:Vector3 = new Vector3(); + private static var _v:Vector3 = new Vector3(); + + /** + * @param hs + * @param collisionGroup + */ + public function CollisionBox(hs:Vector3, collisionGroup:int) { + super(BOX, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + var t:Matrix4 = transform; + aabb.maxX = hs.x*(t.a < 0 ? -t.a : t.a) + hs.y*(t.b < 0 ? -t.b : t.b) + hs.z*(t.c < 0 ? -t.c : t.c); + aabb.minX = -aabb.maxX; + + aabb.maxY = hs.x*(t.e < 0 ? -t.e : t.e) + hs.y*(t.f < 0 ? -t.f : t.f) + hs.z*(t.g < 0 ? -t.g : t.g); + aabb.minY = -aabb.maxY; + + aabb.maxZ = hs.x*(t.i < 0 ? -t.i : t.i) + hs.y*(t.j < 0 ? -t.j : t.j) + hs.z*(t.k < 0 ? -t.k : t.k); + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var box:CollisionBox = source as CollisionBox; + if (box == null) return this; + super.copyFrom(box); + hs.vCopy(box.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionBox(hs, collisionGroup); + } + + /** + * Вычисляет параметры пересечения заданного сегмента с примитивом. + * + * @param origin начальная точка сегмента в мировых координатах + * @param vector вектор сегмента в мировых координатах + * @param threshold погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную + * погрешность, считается равной нулю. + * @param normal возвращаемое значение. Нормаль к примитиву в точке пересечения с сегментом. + * @return в случае наличия пересечения возвращается время точки пересечения, в противном случае возвращается -1. + */ + override public function getSegmentIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var tMin:Number = -1, tMax:Number = 1e308, t1:Number, t2:Number; + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var xx:Number = origin.x - transform.d; + var yy:Number = origin.y - transform.h; + var zz:Number = origin.z - transform.l; + _o.x = transform.a*xx + transform.e*yy + transform.i*zz; + _o.y = transform.b*xx + transform.f*yy + transform.j*zz; + _o.z = transform.c*xx + transform.g*yy + transform.k*zz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + _v.x = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + _v.y = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + _v.z = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + // X + if (_v.x < threshold && _v.x > -threshold) { + if (_o.x < -hs.x || _o.x > hs.x) return -1; + } else { + t1 = (-hs.x - _o.x)/_v.x; + t2 = (hs.x - _o.x)/_v.x; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.x = -1; + normal.y = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.x = 1; + normal.y = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Y + if (_v.y < threshold && _v.y > -threshold) { + if (_o.y < -hs.y || _o.y > hs.y) return -1; + } else { + t1 = (-hs.y - _o.y)/_v.y; + t2 = (hs.y - _o.y)/_v.y; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.y = -1; + normal.x = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.y = 1; + normal.x = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Z + if (_v.z < threshold && _v.z > -threshold) { + if (_o.z < -hs.z || _o.z > hs.z) return -1; + } else { + t1 = (-hs.z - _o.z)/_v.z; + t2 = (hs.z - _o.z)/_v.z; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.z = -1; + normal.x = normal.y = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.z = 1; + normal.x = normal.y = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Перевод нормали в мировую систему координат + // Inlined normal.vDeltaTransformBy4(transform); + xx = normal.x, yy = normal.y, zz = normal.z; + normal.x = transform.a*xx + transform.b*yy + transform.c*zz; + normal.y = transform.e*xx + transform.f*yy + transform.g*zz; + normal.z = transform.i*xx + transform.j*yy + transform.k*zz; + + return tMin; + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/primitives/CollisionPrimitive.as b/0.0.3.0/src/alternativa/physics/collision/primitives/CollisionPrimitive.as new file mode 100644 index 0000000..352a940 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/primitives/CollisionPrimitive.as @@ -0,0 +1,130 @@ +package alternativa.physics.collision.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.ICollisionPredicate; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Базовый класс для примитивов, использующихся детектором столкновений. + */ + public class CollisionPrimitive { + // Константы типов примитива + public static const BOX:int = 1; + public static const PLANE:int = 2; + public static const SPHERE:int = 3; + public static const RECT:int = 4; + public static const TRIANGLE:int = 5; + + // Тип примитива + public var type:int; + // Группы примитива. Каждая группа определяется установленным битом. Столкновения проверяются только для примитивов, + // имеющих хотя бы одну общую группу. + public var collisionGroup:int; + // Предикат примитива, вызывающийся детектором при нахождении столкновения. В зависимости от возвращённого + // предикатом результата столкновение либо регистрируется, либо игнорируется. + // Nullable + public var postCollisionPredicate:ICollisionPredicate; + + // Тело, владеющее примитивом. + // Nullable + altphysics var body:Body; + // Трансформация примитива в системе координат тела, если оно указано. Не допускается масштабирование матрицы. + altphysics var localTransform:Matrix4 = new Matrix4(); + // Полная трансформация примитива. Не допускается масштабирование матрицы. + altphysics var transform:Matrix4 = new Matrix4(); + // AABB в мировой системе координат. Расчитывается системой вызовом функции calculateBoundBox(). + altphysics var aabb:BoundBox = new BoundBox(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param type тип примитива + * @param collisionGroup группа примитива + */ + public function CollisionPrimitive(type:int, collisionGroup:int) { + this.type = type; + this.collisionGroup = collisionGroup; + } + + /** + * Устанавливает тело, владеющее примитивом. + * + * @param body тело, к которому относится примитив. Если указано значение null, матрица локальной трансформации удаляется. + * @param localTransform трансформация примитива в системе координат тела. Указание значения null равносильно + * заданию единичной матрицы. + */ + public function setBody(body:Body, localTransform:Matrix4 = null):void { + if (this.body == body) return; + this.body = body; + if (body == null) this.localTransform = null; + else { + if (this.localTransform == null) this.localTransform = new Matrix4(); + if (localTransform != null) this.localTransform.copy(localTransform); + } + } + + /** + * Наследники должны переопределять этот метод, реализуя в нём корректный расчёт ограничивающего бокса, выравненного + * по осям мировой системы координат. + * + * @return ссылка на свой баунд бокс + */ + public function calculateAABB():BoundBox { + return aabb; + } + + /** + * Клонирует примитив. Переопределять не рекомендуется. Вместо этого переопределяются методы createPrimitive() и copyFrom(). + * + * @return клон примитива + */ + public function clone():CollisionPrimitive { + var p:CollisionPrimitive = createPrimitive(); + return p.copyFrom(this); + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + type = source.type; + transform.copy(source.transform); + collisionGroup = source.collisionGroup; + setBody(source.body, source.localTransform); + aabb.copyFrom(source.aabb); + return this; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + protected function createPrimitive():CollisionPrimitive { + return new CollisionPrimitive(type, collisionGroup); + } + + /** + * Метод должен вычислять параметры пересечения заданного сегмента с примитивом. + * + * @param origin начальная точка сегмента в мировых координатах + * @param vector вектор сегмента в мировых координатах + * @param threshold погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную + * погрешность, считается равной нулю. + * @param normal возвращаемое значение. Нормаль к примитиву в точке пересечения с сегментом. + * @return в случае наличия пересечения возвращается время точки пересечения, в противном случае возвращается -1. + */ + public function getSegmentIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + return -1; + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/primitives/CollisionRect.as b/0.0.3.0/src/alternativa/physics/collision/primitives/CollisionRect.as new file mode 100644 index 0000000..7d6aa47 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/primitives/CollisionRect.as @@ -0,0 +1,126 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + /** + * Ориентированный прямоугольник. Задаётся половинами размеров вдоль осей X (ширина) и Y (длина) локальной системы + * координат примитива. Таким образом, прямоугольник лежит в плоскости XY, его стороны параллельны осям этой + * плоскости, а нормаль направлена вдоль локальной оси Z. + * + * Прямоугольник может быть одно- или двусторонним. В случае одностороннего прямоугольника, + * столкновения с внутренней стороны не регистрируются. + */ + public class CollisionRect extends CollisionPrimitive { + // Половинные размеры прямоугольника вдоль осей X и Y. Нормаль направлена вдоль оси Z. + public var hs:Vector3 = new Vector3(); + // Флаг указывает, является примитив одно- или двусторонним + public var twoSided:Boolean = true; + + // Малое значение. Используется для указания фиктивной высоты примитива и в функции определения пересечения с сегментом. + private static const EPSILON:Number = 0.01; + // Переменные используются в функции поиска пересечения с лучом + private static var _o:Vector3 = new Vector3(); + private static var _v:Vector3 = new Vector3(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param hs половинные размерв прямоугольника вдоль осей X и Y. Значение z игнорируется. + * @param collisionGroup группа примитива + */ + public function CollisionRect(hs:Vector3, collisionGroup:int) { + super(RECT, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * Расчитывает ограничивающий бокс прямоугольника. Для избежания проблем высота примитива принимается равной + * не нулю, а малому значению. + * + * @return + */ + override public function calculateAABB():BoundBox { + // Баунд бокс прямоугольника имеет минимальную высоту, отличную от нуля во избежание проблем с построением kd-дерева + var t:Matrix4 = transform; + aabb.maxX = hs.x*(t.a < 0 ? -t.a : t.a) + hs.y*(t.b < 0 ? -t.b : t.b) + EPSILON*(t.c < 0 ? -t.c : t.c); + aabb.minX = -aabb.maxX; + + aabb.maxY = hs.x*(t.e < 0 ? -t.e : t.e) + hs.y*(t.f < 0 ? -t.f : t.f) + EPSILON*(t.g < 0 ? -t.g : t.g); + aabb.minY = -aabb.maxY; + + aabb.maxZ = hs.x*(t.i < 0 ? -t.i : t.i) + hs.y*(t.j < 0 ? -t.j : t.j) + EPSILON*(t.k < 0 ? -t.k : t.k); + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var rect:CollisionRect = source as CollisionRect; + if (rect == null) return this; + super.copyFrom(rect); + hs.vCopy(rect.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionRect(hs, collisionGroup); + } + + /** + * @param origin + * @param vector + * @param collisionGroup + * @param threshold + * @param normal + * @return + */ + override public function getSegmentIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var xx:Number = origin.x - transform.d; + var yy:Number = origin.y - transform.h; + var zz:Number = origin.z - transform.l; + _o.x = transform.a*xx + transform.e*yy + transform.i*zz; + _o.y = transform.b*xx + transform.f*yy + transform.j*zz; + _o.z = transform.c*xx + transform.g*yy + transform.k*zz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + _v.x = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + _v.y = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + _v.z = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + + // Проверка параллельности сегмента и плоскости примитива + if (_v.z > -threshold && _v.z < threshold) return -1; + var t:Number = -_o.z/_v.z; + if (t < 0) return -1; + // Проверка вхождения точки пересечения в прямоугольник + _o.x += _v.x*t; + _o.y += _v.y*t; + _o.z = 0; + if (_o.x < (-hs.x - EPSILON) || _o.x > (hs.x + EPSILON) || _o.y < (-hs.y - EPSILON) || _o.y > (hs.y + EPSILON)) return -1; + + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + return t; + } + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/primitives/CollisionSphere.as b/0.0.3.0/src/alternativa/physics/collision/primitives/CollisionSphere.as new file mode 100644 index 0000000..1a9e6d1 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/primitives/CollisionSphere.as @@ -0,0 +1,80 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + + /** + * Сфера. + */ + public class CollisionSphere extends CollisionPrimitive { + + // Радиус сферы + public var r:Number = 0; + + /** + * + * @param r + * @param collisionGroup + */ + public function CollisionSphere(r:Number, collisionGroup:int) { + super(SPHERE, collisionGroup); + this.r = r; + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + aabb.maxX = transform.d + r; + aabb.minX = transform.d - r; + + aabb.maxY = transform.h + r; + aabb.minY = transform.h - r; + + aabb.maxZ = transform.l + r; + aabb.minZ = transform.l - r; + + return aabb; + } + + /** + * @param origin + * @param vector + * @param threshold + * @param normal + * @return + */ + override public function getSegmentIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var px:Number = origin.x - transform.d; + var py:Number = origin.y - transform.h; + var pz:Number = origin.z - transform.l; + var k:Number = vector.x*px + vector.y*py + vector.z*pz; + if (k > 0) return -1; + var a:Number = vector.x*vector.x + vector.y*vector.y + vector.z*vector.z; + var D:Number = k*k - a*(px*px + py*py + pz*pz - r*r); + if (D < 0) return -1; + return -(k + Math.sqrt(D))/a; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var sphere:CollisionSphere = source as CollisionSphere; + if (sphere == null) return this; + super.copyFrom(sphere); + r = sphere.r; + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionSphere(r, collisionGroup); + } + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/primitives/CollisionTriangle.as b/0.0.3.0/src/alternativa/physics/collision/primitives/CollisionTriangle.as new file mode 100644 index 0000000..a48803a --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/primitives/CollisionTriangle.as @@ -0,0 +1,39 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + public class CollisionTriangle extends CollisionPrimitive { + + public var v0:Vector3 = new Vector3(); + public var v1:Vector3 = new Vector3(); + public var v2:Vector3 = new Vector3(); + public var e0:Vector3 = new Vector3(); + public var e1:Vector3 = new Vector3(); + public var e2:Vector3 = new Vector3(); + public var len0:Number; + public var len1:Number; + public var len2:Number; + + public function CollisionTriangle(v0:Vector3, v1:Vector3, v2:Vector3, collisionGroup:int) { + super(TRIANGLE, collisionGroup); + + this.v0.vCopy(v0); + this.v1.vCopy(v1); + this.v2.vCopy(v2); + + e0.vDiff(v1, v0); + len0 = e0.vLength(); + e0.vNormalize(); + + e1.vDiff(v2, v1); + len1 = e1.vLength(); + e1.vNormalize(); + + e2.vDiff(v0, v2); + len2 = e2.vLength(); + e2.vNormalize(); + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/types/.svn/all-wcprops b/0.0.3.0/src/alternativa/physics/collision/types/.svn/all-wcprops new file mode 100644 index 0000000..0a0dd6a --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/types/.svn/all-wcprops @@ -0,0 +1,23 @@ +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/collision/types +END +BoundBox.as +K 25 +svn:wc:ra_dav:version-url +V 131 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/collision/types/BoundBox.as +END +RayIntersection.as +K 25 +svn:wc:ra_dav:version-url +V 138 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/collision/types/RayIntersection.as +END +Ray.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/collision/types/Ray.as +END diff --git a/0.0.3.0/src/alternativa/physics/collision/types/.svn/entries b/0.0.3.0/src/alternativa/physics/collision/types/.svn/entries new file mode 100644 index 0000000..ac0505b --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/types/.svn/entries @@ -0,0 +1,64 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/collision/types +http://svndev.alternativaplatform.com + + + +2009-06-08T06:46:18.310732Z +14099 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +BoundBox.as +file + + + + +2010-10-28T04:32:40.000000Z +13aaaf1b1923de8a2579543651734ab7 +2009-05-18T16:37:02.465501Z +13087 +mike + +RayIntersection.as +file + + + + +2010-10-28T04:32:40.000000Z +a0a4cf3dd94468fdf44cd06734927c94 +2009-06-08T06:46:18.310732Z +14099 +mike + +Ray.as +file + + + + +2010-10-28T04:32:40.000000Z +55eeb1cf79a416bb863bee95f089dbcd +2009-04-17T17:39:21.367569Z +11341 +mike + diff --git a/0.0.3.0/src/alternativa/physics/collision/types/.svn/format b/0.0.3.0/src/alternativa/physics/collision/types/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/types/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.3.0/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base b/0.0.3.0/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base new file mode 100644 index 0000000..6efc097 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base @@ -0,0 +1,72 @@ +package alternativa.physics.collision.types { + + public class BoundBox { + + public var minX:Number = 1e308; + public var minY:Number = 1e308; + public var minZ:Number = 1e308; + public var maxX:Number = -1e308; + public var maxY:Number = -1e308; + public var maxZ:Number = -1e308; + + public function setSize(minX:Number, minY:Number, minZ:Number, maxX:Number, maxY:Number, maxZ:Number):void { + this.minX = minX; + this.minY = minY; + this.minZ = minZ; + this.maxX = maxX; + this.maxY = maxY; + this.maxZ = maxZ; + } + + public function addBoundBox(boundBox:BoundBox):void { + minX = (boundBox.minX < minX) ? boundBox.minX : minX; + minY = (boundBox.minY < minY) ? boundBox.minY : minY; + minZ = (boundBox.minZ < minZ) ? boundBox.minZ : minZ; + maxX = (boundBox.maxX > maxX) ? boundBox.maxX : maxX; + maxY = (boundBox.maxY > maxY) ? boundBox.maxY : maxY; + maxZ = (boundBox.maxZ > maxZ) ? boundBox.maxZ : maxZ; + } + + public function addPoint(x:Number, y:Number, z:Number):void { + if (x < minX) minX = x; + if (x > maxX) maxX = x; + if (y < minY) minY = y; + if (y > maxY) maxY = y; + if (z < minZ) minZ = z; + if (z > maxZ) maxZ = z; + } + + public function infinity():void { + minX = 1e308; + minY = 1e308; + minZ = 1e308; + maxX = -1e308; + maxY = -1e308; + maxZ = -1e308; + } + + public function intersects(bb:BoundBox, tolerance:Number):Boolean { + return !(minX > bb.maxX + tolerance || maxX < bb.minX - tolerance || minY > bb.maxY + tolerance || maxY < bb.minY - tolerance || minZ > bb.maxZ + tolerance || maxZ < bb.minZ - tolerance); + } + + public function copyFrom(boundBox:BoundBox):void { + minX = boundBox.minX; + minY = boundBox.minY; + minZ = boundBox.minZ; + maxX = boundBox.maxX; + maxY = boundBox.maxY; + maxZ = boundBox.maxZ; + } + + public function clone():BoundBox { + var clone:BoundBox = new BoundBox(); + clone.copyFrom(this); + return clone; + } + + public function toString():String { + return "BoundBox [" + minX + ", " + minY + ", " + minZ + " : " + maxX + ", " + maxY + ", " + maxZ + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base b/0.0.3.0/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base new file mode 100644 index 0000000..f80620a --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base @@ -0,0 +1,13 @@ +package alternativa.physics.collision.types { + import alternativa.physics.types.Vector3; + + /** + * Структура, описывающая луч и сегмент прямой в пространстве. + */ + public class Ray { + // Начало луча. Начальная точка сегмента на прямой луча. + public var origin:Vector3 = new Vector3(); + // Направляющий вектор луча. Длина вектора задаёт длину сегмента на луче. Конечная точка сегмента вычисляется как origin + dir. + public var dir:Vector3 = new Vector3(); + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base b/0.0.3.0/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base new file mode 100644 index 0000000..8a89f75 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base @@ -0,0 +1,25 @@ +package alternativa.physics.collision.types { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.types.Vector3; + + /** + * Структура описывает точку пересечения луча с физической геометрией. + */ + public class RayIntersection { + // Физический примитив, с которым пересекается луч + public var primitive:CollisionPrimitive; + // Положенеи точки пересечения + public var pos:Vector3 = new Vector3(); + // Нормаль поверхности физического примитива в точке пересечения + public var normal:Vector3 = new Vector3(); + // Время пересечения (|pos - ray.origin|/|ray.dir|) + public var t:Number = 0; + + public function copy(source:RayIntersection):void { + primitive = source.primitive; + pos.vCopy(source.pos); + normal.vCopy(source.normal); + t = source.t; + } + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/types/BoundBox.as b/0.0.3.0/src/alternativa/physics/collision/types/BoundBox.as new file mode 100644 index 0000000..6efc097 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/types/BoundBox.as @@ -0,0 +1,72 @@ +package alternativa.physics.collision.types { + + public class BoundBox { + + public var minX:Number = 1e308; + public var minY:Number = 1e308; + public var minZ:Number = 1e308; + public var maxX:Number = -1e308; + public var maxY:Number = -1e308; + public var maxZ:Number = -1e308; + + public function setSize(minX:Number, minY:Number, minZ:Number, maxX:Number, maxY:Number, maxZ:Number):void { + this.minX = minX; + this.minY = minY; + this.minZ = minZ; + this.maxX = maxX; + this.maxY = maxY; + this.maxZ = maxZ; + } + + public function addBoundBox(boundBox:BoundBox):void { + minX = (boundBox.minX < minX) ? boundBox.minX : minX; + minY = (boundBox.minY < minY) ? boundBox.minY : minY; + minZ = (boundBox.minZ < minZ) ? boundBox.minZ : minZ; + maxX = (boundBox.maxX > maxX) ? boundBox.maxX : maxX; + maxY = (boundBox.maxY > maxY) ? boundBox.maxY : maxY; + maxZ = (boundBox.maxZ > maxZ) ? boundBox.maxZ : maxZ; + } + + public function addPoint(x:Number, y:Number, z:Number):void { + if (x < minX) minX = x; + if (x > maxX) maxX = x; + if (y < minY) minY = y; + if (y > maxY) maxY = y; + if (z < minZ) minZ = z; + if (z > maxZ) maxZ = z; + } + + public function infinity():void { + minX = 1e308; + minY = 1e308; + minZ = 1e308; + maxX = -1e308; + maxY = -1e308; + maxZ = -1e308; + } + + public function intersects(bb:BoundBox, tolerance:Number):Boolean { + return !(minX > bb.maxX + tolerance || maxX < bb.minX - tolerance || minY > bb.maxY + tolerance || maxY < bb.minY - tolerance || minZ > bb.maxZ + tolerance || maxZ < bb.minZ - tolerance); + } + + public function copyFrom(boundBox:BoundBox):void { + minX = boundBox.minX; + minY = boundBox.minY; + minZ = boundBox.minZ; + maxX = boundBox.maxX; + maxY = boundBox.maxY; + maxZ = boundBox.maxZ; + } + + public function clone():BoundBox { + var clone:BoundBox = new BoundBox(); + clone.copyFrom(this); + return clone; + } + + public function toString():String { + return "BoundBox [" + minX + ", " + minY + ", " + minZ + " : " + maxX + ", " + maxY + ", " + maxZ + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/types/Ray.as b/0.0.3.0/src/alternativa/physics/collision/types/Ray.as new file mode 100644 index 0000000..f80620a --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/types/Ray.as @@ -0,0 +1,13 @@ +package alternativa.physics.collision.types { + import alternativa.physics.types.Vector3; + + /** + * Структура, описывающая луч и сегмент прямой в пространстве. + */ + public class Ray { + // Начало луча. Начальная точка сегмента на прямой луча. + public var origin:Vector3 = new Vector3(); + // Направляющий вектор луча. Длина вектора задаёт длину сегмента на луче. Конечная точка сегмента вычисляется как origin + dir. + public var dir:Vector3 = new Vector3(); + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/collision/types/RayIntersection.as b/0.0.3.0/src/alternativa/physics/collision/types/RayIntersection.as new file mode 100644 index 0000000..8a89f75 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/collision/types/RayIntersection.as @@ -0,0 +1,25 @@ +package alternativa.physics.collision.types { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.types.Vector3; + + /** + * Структура описывает точку пересечения луча с физической геометрией. + */ + public class RayIntersection { + // Физический примитив, с которым пересекается луч + public var primitive:CollisionPrimitive; + // Положенеи точки пересечения + public var pos:Vector3 = new Vector3(); + // Нормаль поверхности физического примитива в точке пересечения + public var normal:Vector3 = new Vector3(); + // Время пересечения (|pos - ray.origin|/|ray.dir|) + public var t:Number = 0; + + public function copy(source:RayIntersection):void { + primitive = source.primitive; + pos.vCopy(source.pos); + normal.vCopy(source.normal); + t = source.t; + } + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/rigid/.svn/all-wcprops b/0.0.3.0/src/alternativa/physics/rigid/.svn/all-wcprops new file mode 100644 index 0000000..4df54d8 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/rigid/.svn/all-wcprops @@ -0,0 +1,47 @@ +K 25 +svn:wc:ra_dav:version-url +V 109 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/rigid +END +Body.as +K 25 +svn:wc:ra_dav:version-url +V 117 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/rigid/Body.as +END +BodyState.as +K 25 +svn:wc:ra_dav:version-url +V 122 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/rigid/BodyState.as +END +PhysicsUtils.as +K 25 +svn:wc:ra_dav:version-url +V 125 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/rigid/PhysicsUtils.as +END +ContactPoint.as +K 25 +svn:wc:ra_dav:version-url +V 125 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/rigid/ContactPoint.as +END +RigidWorld.as +K 25 +svn:wc:ra_dav:version-url +V 123 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/rigid/RigidWorld.as +END +Contact.as +K 25 +svn:wc:ra_dav:version-url +V 120 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/rigid/Contact.as +END +BodyMaterial.as +K 25 +svn:wc:ra_dav:version-url +V 125 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/rigid/BodyMaterial.as +END diff --git a/0.0.3.0/src/alternativa/physics/rigid/.svn/entries b/0.0.3.0/src/alternativa/physics/rigid/.svn/entries new file mode 100644 index 0000000..75b40de --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/rigid/.svn/entries @@ -0,0 +1,118 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/rigid +http://svndev.alternativaplatform.com + + + +2009-07-03T12:54:42.818772Z +15744 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Body.as +file + + + + +2010-10-28T04:32:41.000000Z +23bad2eff16ce73dccb56a7a601cc3c0 +2009-06-29T14:07:24.833358Z +15390 +mike + +BodyState.as +file + + + + +2010-10-28T04:32:41.000000Z +8a089205beccac13a05eccfb9cfc6387 +2009-06-08T06:46:18.310732Z +14099 +mike + +PhysicsUtils.as +file + + + + +2010-10-28T04:32:41.000000Z +6d88004ddab2ae19e0d15f5b1b7c016a +2009-04-26T21:40:17.813142Z +12023 +mike + +constraints +dir + +ContactPoint.as +file + + + + +2010-10-28T04:32:41.000000Z +a94e73e3bfc85be2a79d6cc939512d91 +2009-06-08T06:46:18.310732Z +14099 +mike + +RigidWorld.as +file + + + + +2010-10-28T04:32:41.000000Z +b0b528a79a6fce55e5d800a15dc99534 +2009-07-03T12:54:42.818772Z +15744 +mike + +primitives +dir + +Contact.as +file + + + + +2010-10-28T04:32:41.000000Z +d65ad3d488d4a06addcbfa4aaf958f10 +2009-04-13T03:04:08.898183Z +11018 +mike + +BodyMaterial.as +file + + + + +2010-10-28T04:32:41.000000Z +49c7a55b5b26fdb03a60f24bac460f53 +2009-04-07T08:34:06.342994Z +10587 +mike + diff --git a/0.0.3.0/src/alternativa/physics/rigid/.svn/format b/0.0.3.0/src/alternativa/physics/rigid/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/rigid/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.3.0/src/alternativa/physics/rigid/.svn/text-base/Body.as.svn-base b/0.0.3.0/src/alternativa/physics/rigid/.svn/text-base/Body.as.svn-base new file mode 100644 index 0000000..7c08ce9 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/rigid/.svn/text-base/Body.as.svn-base @@ -0,0 +1,340 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.IBodyCollisionPredicate; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Quaternion; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class Body { + + public static var linDamping:Number = 0.997; + public static var rotDamping:Number = 0.997; + + public var name:String; + public var movable:Boolean = true; + public var canFreeze:Boolean = false; + + public var freezeCounter:int; + public var frozen:Boolean = false; + public var aabb:BoundBox = new BoundBox(); + public var postCollisionPredicate:IBodyCollisionPredicate; + + altphysics var id:int; + // Мир, в котором находится тело + altphysics var world:RigidWorld; + // Текущее и предыдущее состояние тела. Промежуточное состояние вычисляется линейной интерполяцией. + altphysics var state:BodyState = new BodyState(); + altphysics var prevState:BodyState = new BodyState(); + // Линейное и угловое ускорение тела на текущем шаге симуляции + altphysics var accel:Vector3 = new Vector3(); + altphysics var angleAccel:Vector3 = new Vector3(); + // Материал тела + altphysics var material:BodyMaterial = new BodyMaterial(); + + altphysics var invMass:Number = 1; + altphysics var invInertia:Matrix3 = new Matrix3(); + altphysics var invInertiaWorld:Matrix3 = new Matrix3(); + altphysics var baseMatrix:Matrix3 = new Matrix3(); + + altphysics const MAX_CONTACTS:int = 20; + altphysics var contacts:Vector. = new Vector.(MAX_CONTACTS); + altphysics var contactsNum:int; + + altphysics var collisionPrimitives:Vector.; + altphysics var collisionPrimitivesNum:int; + + // Аккумулятор сил + altphysics var forceAccum:Vector3 = new Vector3(); + // Аккумулятор моментов + altphysics var torqueAccum:Vector3 = new Vector3(); + + // Внутренние переменные для избежания создания экземпляров + private static var _r:Vector3 = new Vector3(); + private static var _f:Vector3 = new Vector3(); + + /** + * + * @param invMass + * @param invInertia + */ + public function Body(invMass:Number, invInertia:Matrix3) { + this.invMass = invMass; + this.invInertia.copy(invInertia); + } + + /** + * @param primitive + * @param localTransform + */ + public function addCollisionPrimitive(primitive:CollisionPrimitive, localTransform:Matrix4 = null):void { + if (primitive == null) throw new ArgumentError("Primitive cannot be null"); + if (collisionPrimitives == null) collisionPrimitives = new Vector.(); + collisionPrimitives[collisionPrimitivesNum++] = primitive; + primitive.setBody(this, localTransform); + } + + /** + * @param t + * @param result + */ + public function interpolate(t:Number, pos:Vector3, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.w = prevState.orientation.w*t1 + state.orientation.w*t; + orientation.x = prevState.orientation.x*t1 + state.orientation.x*t; + orientation.y = prevState.orientation.y*t1 + state.orientation.y*t; + orientation.z = prevState.orientation.z*t1 + state.orientation.z*t; + } + + /** + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + state.pos.vCopy(pos); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setPositionXYZ(x:Number, y:Number, z:Number):void { + state.pos.vReset(x, y, z); + } + + /** + * + * @param vel + */ + public function setVelocity(vel:Vector3):void { + state.velocity.vCopy(vel); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setVelocityXYZ(x:Number, y:Number, z:Number):void { + state.velocity.vReset(x, y, z); + } + + /** + * + * @param rot + */ + public function setRotation(rot:Vector3):void { + state.rotation.vCopy(rot); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setRotationXYZ(x:Number, y:Number, z:Number):void { + state.rotation.vReset(x, y, z); + } + + /** + * + * @param q + */ + public function setOrientation(q:Quaternion):void { + state.orientation.copy(q); + } + + /** + * @param r + * @param dir + * @param magnitude + */ + public function applyRelPosWorldImpulse(r:Vector3, dir:Vector3, magnitude:Number):void { + var d:Number = magnitude*invMass; + // Линейная часть + state.velocity.x += d*dir.x; + state.velocity.y += d*dir.y; + state.velocity.z += d*dir.z; + + // Вращательная часть + var rx:Number = r.x; + var ry:Number = r.y; + var rz:Number = r.z; + _r.x = (ry*dir.z - rz*dir.y)*magnitude; + _r.y = (rz*dir.x - rx*dir.z)*magnitude; + _r.z = (rx*dir.y - ry*dir.x)*magnitude; + _r.vTransformBy3(invInertiaWorld); + state.rotation.x += _r.x; + state.rotation.y += _r.y; + state.rotation.z += _r.z; + } + + /** + * @param f + */ + public function addForce(f:Vector3):void { + forceAccum.vAdd(f); + } + + /** + * @param pos + * @param f + */ + public function addWorldForce(pos:Vector3, force:Vector3):void { + forceAccum.vAdd(force); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(force)); + } + + /** + * @param pos + * @param f + */ + public function addWorldForceScaled(pos:Vector3, force:Vector3, scale:Number):void { + _f.x = scale*force.x, _f.y = scale*force.y, _f.z = scale*force.z; + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(_f)); + } + + /** + * @param pos + * @param f + */ + public function addLocalForce(pos:Vector3, force:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(pos, _r); + // Трансформируем вектор силы в мировую систему + baseMatrix.transformVector(force, _f); + // Добавляем силу и момент + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vCross(_f)); + } + + /** + * + * @param localPos + * @param worldForce + */ + public function addWorldForceAtLocalPoint(localPos:Vector3, worldForce:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(localPos, _r); + // Добавляем силу и момент + forceAccum.vAdd(worldForce); + torqueAccum.vAdd(_r.vCross(worldForce)); + } + + /** + * @param dt + */ + public function addExternalForces(dt:Number):void { + } + + /** + * @param t + */ + public function addTorque(t:Vector3):void { + torqueAccum.vAdd(t); + } + + /** + * + */ + altphysics function clearAccumulators():void { + forceAccum.vReset(); + torqueAccum.vReset(); + } + + /** + * + */ + altphysics function calcAccelerations():void { + accel.x = forceAccum.x*invMass; + accel.y = forceAccum.y*invMass; + accel.z = forceAccum.z*invMass; + invInertiaWorld.transformVector(torqueAccum, angleAccel); + } + + /** + * Вычисляет производные данные. + */ + altphysics function calcDerivedData():void { + // Вычисление базисной матрицы и обратного тензора инерции в мировых координатах + state.orientation.toMatrix3(baseMatrix); + invInertiaWorld.copy(invInertia).append(baseMatrix).prependTransposed(baseMatrix); + if (collisionPrimitives != null) { + aabb.infinity(); + for (var i:int = 0; i < collisionPrimitivesNum; i++) { + var primitive:CollisionPrimitive = collisionPrimitives[i]; + primitive.transform.setFromMatrix3(baseMatrix, state.pos); + if (primitive.localTransform != null) primitive.transform.prepend(primitive.localTransform); + primitive.calculateAABB(); + aabb.addBoundBox(primitive.aabb); + } + } + } + + /** + * + */ + altphysics function saveState():void { + prevState.copy(state); + } + + /** + * + */ + altphysics function restoreState():void { + state.copy(prevState); + } + + /** + * @param dt + */ + altphysics function integrateVelocity(dt:Number):void { + // v = v + a*t + state.velocity.x += accel.x*dt; + state.velocity.y += accel.y*dt; + state.velocity.z += accel.z*dt; + // rot = rot + eps*t + state.rotation.x += angleAccel.x*dt; + state.rotation.y += angleAccel.y*dt; + state.rotation.z += angleAccel.z*dt; + + state.velocity.x *= linDamping; + state.velocity.y *= linDamping; + state.velocity.z *= linDamping; + + state.rotation.x *= rotDamping; + state.rotation.y *= rotDamping; + state.rotation.z *= rotDamping; + } + + /** + * + */ + altphysics function integratePosition(dt:Number):void { + // pos = pos + v*t + state.pos.x += state.velocity.x*dt; + state.pos.y += state.velocity.y*dt; + state.pos.z += state.velocity.z*dt; + // q = q + 0.5*rot*q + state.orientation.addScaledVector(state.rotation, dt); + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/rigid/.svn/text-base/BodyMaterial.as.svn-base b/0.0.3.0/src/alternativa/physics/rigid/.svn/text-base/BodyMaterial.as.svn-base new file mode 100644 index 0000000..61d4382 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/rigid/.svn/text-base/BodyMaterial.as.svn-base @@ -0,0 +1,11 @@ +package alternativa.physics.rigid { + + public class BodyMaterial { + + public var restitution:Number = 0; + public var friction:Number = 0.3; +// public var dynamicFriction:Number = 0.2; +// public var dynamicFriction:Number = 0.2; + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/rigid/.svn/text-base/BodyState.as.svn-base b/0.0.3.0/src/alternativa/physics/rigid/.svn/text-base/BodyState.as.svn-base new file mode 100644 index 0000000..408b0ea --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/rigid/.svn/text-base/BodyState.as.svn-base @@ -0,0 +1,39 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Quaternion; + import alternativa.physics.types.Vector3; + + /** + * Класс описывает состояние твёрдого тела. + */ + public class BodyState { + /** + * Положение тела. + */ + public var pos:Vector3 = new Vector3(); + /** + * Ориентация тела. + */ + public var orientation:Quaternion = new Quaternion(); + /** + * Скорость тела. + */ + public var velocity:Vector3 = new Vector3(); + /** + * Угловая скорость тела. + */ + public var rotation:Vector3 = new Vector3(); + + /** + * Копирует значение указанного объекта. + * + * @param state + */ + public function copy(state:BodyState):void { + pos.vCopy(state.pos); + orientation.copy(state.orientation); + velocity.vCopy(state.velocity); + rotation.vCopy(state.rotation); + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/rigid/.svn/text-base/Contact.as.svn-base b/0.0.3.0/src/alternativa/physics/rigid/.svn/text-base/Contact.as.svn-base new file mode 100644 index 0000000..b73fc55 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/rigid/.svn/text-base/Contact.as.svn-base @@ -0,0 +1,53 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.types.Vector3; + + /** + * Информация о контакте между двумя телами. Содержит нормаль, множество точек контакта и прочие величины. + */ + public class Contact { + +// private static var pool:Vector. = new Vector.(); +// +// public static function create():ContactInfo { +// if (pool.length > 0) { +// return pool.pop(); +// } +// return new ContactInfo(); +// } +// +// public static function destroy(collInfo:ContactInfo):void { +// collInfo.body1 = collInfo.body2 = null; +// pool.push(collInfo); +// } + + // Максимальное количество точек контакта + private const N:int = 8; + + public var body1:Body; + public var body2:Body; + // Взаимный коэффициент отскока + public var restitution:Number; + // Взаимный коэффициент трения + public var friction:Number; + // Нормаль контакта. Направлена от второго тела к первому. + public var normal:Vector3 = new Vector3(); + // Список точек контакта + public var points:Vector. = new Vector.(N, true); + // Количество точек контакта + public var pcount:int; + // Максимальная глубина пересечения тел + public var maxPenetration:Number = 0; + + public var satisfied:Boolean; + + /** + * + */ + public function Contact() { + for (var i:int = 0; i < N; i++) points[i] = new ContactPoint(); + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/rigid/.svn/text-base/ContactPoint.as.svn-base b/0.0.3.0/src/alternativa/physics/rigid/.svn/text-base/ContactPoint.as.svn-base new file mode 100644 index 0000000..c36f958 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/rigid/.svn/text-base/ContactPoint.as.svn-base @@ -0,0 +1,52 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Vector3; + + public class ContactPoint { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + + // Величины, расчитываемые перед началом фазы решения контактов + + // Требуемая проекция конечной скорости на нормаль для упругого контакта + public var normalVel:Number; + // Минимальная скорость разделения неупругого контакта + public var minSepVel:Number; + // Изменение проекции скорости на единицу нормального импульса + public var velByUnitImpulseN:Number; + + public var angularInertia1:Number; + public var angularInertia2:Number; + + // Радиус-вектор точки контакта относительно центра первого тела + public var r1:Vector3 = new Vector3(); + // Радиус-вектор точки контакта относительно центра второго тела + public var r2:Vector3 = new Vector3(); + + // Величины, накапливаемые во время фазы решения контактов + + // Накопленный импульс, применяемый для получения требуемой относительной скорости в точке контакта + public var accumImpulseN:Number; + // Накопленный импульс, применяемый для разделения тел в точке контакта. Не создаёт момента. +// public var accumSepImpulse:Number; + + public var satisfied:Boolean; + + /** + * + * @param cp + */ + public function copyFrom(cp:ContactPoint):void { + pos.vCopy(cp.pos); + penetration = cp.penetration; + feature1 = cp.feature1; + feature2 = cp.feature2; + r1.vCopy(cp.r1); + r2.vCopy(cp.r2); + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/rigid/.svn/text-base/PhysicsUtils.as.svn-base b/0.0.3.0/src/alternativa/physics/rigid/.svn/text-base/PhysicsUtils.as.svn-base new file mode 100644 index 0000000..a9aaa99 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/rigid/.svn/text-base/PhysicsUtils.as.svn-base @@ -0,0 +1,49 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + public class PhysicsUtils { + + /** + * @param mass + * @param halfSize + * @param result + */ + public static function getBoxInvInertia(mass:Number, halfSize:Vector3, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + result.a = 3/(mass*(yy + zz)); + result.f = 3/(mass*(zz + xx)); + result.k = 3/(mass*(xx + yy)); + } + + /** + * @param mass + * @param r + * @param h + * @param result + */ + public static function getCylinderInvInertia(mass:Number, r:Number, h:Number, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + + result.a = result.f = 1/(mass*(h*h/12 + r*r/4)); + result.k = 2/(mass*r*r); + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/rigid/.svn/text-base/RigidWorld.as.svn-base b/0.0.3.0/src/alternativa/physics/rigid/.svn/text-base/RigidWorld.as.svn-base new file mode 100644 index 0000000..696580b --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/rigid/.svn/text-base/RigidWorld.as.svn-base @@ -0,0 +1,653 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.ICollisionDetector; + import alternativa.physics.collision.KdTreeCollisionDetector; + import alternativa.physics.rigid.constraints.Constraint; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Класс реализует физическую симуляцию поведения твёрдых тел. + */ + public class RigidWorld { + + private static var lastBodyId:int; + + // Максимальное количество контактов + altphysics const MAX_CONTACTS:int = 1000; + + ////////////////////////////////////////////////////////////////// + // Настроечные параметры симуляции. Могут быть изменены в любой + // момент времени без нарушения корректной работы симуляции. + ////////////////////////////////////////////////////////////////// + // Количество шагов, за которое пересекающиеся тела должны разделиться + public var penResolutionSteps:int = 10; + // Величина допустимой глубины пересечения + public var allowedPenetration:Number = 0.1; + // Максимальная скорость, добавляемая с целью разделения тел + public var maxPenResolutionSpeed:Number = 0.5; + // Количество итераций для обработки упругих контактов + public var collisionIterations:int = 5; + // Количество итераций для обработки неупругих контактов + public var contactIterations:int = 5; + // Флаг использования предсказания состояний + public var usePrediction:Boolean = false; + + public var freezeSteps:int = 10; + public var linSpeedFreezeLimit:Number = 1; + public var angSpeedFreezeLimit:Number = 0.01; + + // Переменные для процедуры разделения тел путём непосредственного их перемещенеия. + // !!!!!!!!!!!!!!!!!!!!!!!! + // !!! Экспериментально !!! + // !!!!!!!!!!!!!!!!!!!!!!!! + public var staticSeparationIterations:int = 10; + public var staticSeparationSteps:int = 10; + public var maxAngleMove:Number = 10; + public var useStaticSeparation:Boolean = false; + + // Вектор гравитации + altphysics var _gravity:Vector3 = new Vector3(0, 0, -9.8); + // Использующийся детектор столкновений + altphysics var collisionDetector:ICollisionDetector; + // Список тел, участвующих в симуляции + altphysics var bodies:Vector. = new Vector.(); + // Количество тел, участвующих в симуляции + altphysics var bodiesNum:int; + // Список контактов на текущем шаге симуляции + altphysics var contacts:Vector. = new Vector.(MAX_CONTACTS, true); + // Количество контактов на текущем шаге симуляции + altphysics var contactsNum:int; + // Список ограничений + altphysics var constraints:Vector. = new Vector.(); + // Количество ограничений + altphysics var constraintsNum:int; + // Временная метка. Число прошедших шагов с начала симуляции. + altphysics var timeStamp:uint; + + // Временные переменные для избежания создания экземпляров + private var _r:Vector3 = new Vector3(); + private var _t:Vector3 = new Vector3(); + private var _v:Vector3 = new Vector3(); + private var _v1:Vector3 = new Vector3(); + private var _v2:Vector3 = new Vector3(); + + /** + * + */ + public function RigidWorld() { + for (var i:int = 0; i < MAX_CONTACTS; i++) contacts[i] = new Contact(); + collisionDetector = new KdTreeCollisionDetector(); + } + + /** + * Вектор гравитации. + */ + public function get gravity():Vector3 { + return _gravity.vClone(); + } + + /** + * @private + */ + public function set gravity(value:Vector3):void { + _gravity.vCopy(value); + } + + /** + * Добавляет тело в симуляцию. + * @param body + * @return + */ + public function addBody(body:Body, isStatic:Boolean):Boolean { + var i:int = bodies.indexOf(body); + if (i > -1) return false; + bodies[bodiesNum++] = body; + body.world = this; + body.id = lastBodyId++; +// if (body.collisionPrimitives != null) { +// for (i = 0; i < body.collisionPrimitivesNum; i++) collisionDetector.addPrimitive(body.collisionPrimitives[i], isStatic); +// } + return true; + } + + /** + * Удаляет тело из симуляции. + * @param body + * @return + */ + public function removeBody(body:Body, isStatic:Boolean):Boolean { + var i:int = bodies.indexOf(body); + if (i == -1) return false; + bodies.splice(i, 1); + body.world = null; +// if (body.collisionPrimitives != null) { +// for (i = 0; i < body.collisionPrimitivesNum; i++) collisionDetector.removePrimitive(body.collisionPrimitives[i], isStatic); +// } + bodiesNum--; + return true; + } + + /** + * Добавляет ограничение. + * @param c + */ + public function addConstraint(c:Constraint):void { + constraints[constraintsNum++] = c; + c.world = this; + } + + /** + * Удаляет ограничение. + * @param c + */ + public function removeConstraint(c:Constraint):Boolean { + var idx:int = constraints.indexOf(c); + if (idx < 0) return false; + constraints.splice(idx, 1) + constraintsNum--; + c.world = null; + return true; + } + + /** + * Применяет к телам действующте на них силы. Аккумуляторы сил и моментов тел очищаются после завершения шага + * симуляции, поэтому на момент вызова метода могут уже содержать некоторые значения. + * + * @param dt промежуток времени, в течении которого действуют силы + */ + private function applyForces(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i]; + body.addExternalForces(dt); + body.calcAccelerations(); + // Ускорение свободного падения применяется только к подвижным телам во избежание некорректного изменения + // фиктивной скорости неподвижных тел. + if (body.movable && !body.frozen) { + body.accel.x += _gravity.x; + body.accel.y += _gravity.y; + body.accel.z += _gravity.z; + } + } + } + + /** + * Определяет все столкновения на текущем шаге симуляции и заполняет список получившихся контактов. + * + * @param dt длительность шага симуляции + */ + private function detectCollisions(dt:Number):void { + var i:int; + var j:int; + var body:Body; + + for (i = 0; i < bodiesNum; i++) { + body = bodies[i]; + if (body.frozen) continue; + body.contactsNum = 0; + body.saveState(); + // При включённом режиме предсказания состояние тел интегрируется на один шаг вперёд + if (usePrediction) { + body.integrateVelocity(dt); + body.integratePosition(dt); + } + body.calcDerivedData(); + } + + contactsNum = collisionDetector.getAllCollisions(contacts); + + // Расчёт относительных векторов точки контакта вынесен сюда из-за необходимости учитывать + // положение тел в предсказанном состоянии + for (i = 0; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + var b1:Body = contact.body1; + var b2:Body = contact.body2; + for (j = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.r1.x = cp.pos.x - b1.state.pos.x; + cp.r1.y = cp.pos.y - b1.state.pos.y; + cp.r1.z = cp.pos.z - b1.state.pos.z; + if (b2 != null) { + cp.r2.x = cp.pos.x - b2.state.pos.x; + cp.r2.y = cp.pos.y - b2.state.pos.y; + cp.r2.z = cp.pos.z - b2.state.pos.z; + } + } + } + + // Восстановление состояния тел + if (usePrediction) { + for (i = 0; i < bodiesNum; i++) { + body = bodies[i]; + if (body.frozen) continue; + body.restoreState(); + body.calcDerivedData(); + } + } + } + + /** + * Подготваливает полученные из детектора столкновений контакты, расчитывая значения, не меняющиеся + * в ходе шага симуляции. + */ + private function preProcessContacts(dt:Number):void { + var i:int; + for (i = 0; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + var b1:Body = contact.body1; + var b2:Body = contact.body2; + // Столкнувшиеся тела размораживаются + if (b1.frozen) { + b1.frozen = false; + b1.freezeCounter = 0; + } + if (b2 != null && b2.frozen) { + b2.frozen = false; + b2.freezeCounter = 0; + } + contact.restitution = b1.material.restitution; + if (b2 != null && b2.material.restitution < contact.restitution) contact.restitution = b2.material.restitution; + contact.friction = b1.material.friction; + if (b2 != null && b2.material.friction < contact.friction) contact.friction = b2.material.friction; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.accumImpulseN = 0; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + cp.velByUnitImpulseN = 0; + if (b1.movable) { + cp.angularInertia1 = _v.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1).vDot(contact.normal); + cp.velByUnitImpulseN += b1.invMass + cp.angularInertia1; + } + if (b2 != null && b2.movable) { + cp.angularInertia2 = _v.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2).vDot(contact.normal); + cp.velByUnitImpulseN += b2.invMass + cp.angularInertia2; + } + // Расчёт требуемой конечной скорости для упругого контакта + calcSepVelocity(b1, b2, cp, _v); + cp.normalVel = _v.vDot(contact.normal); + if (cp.normalVel < 0) cp.normalVel = - contact.restitution*cp.normalVel; + // Скорость разделения неупругого контакта + cp.minSepVel = cp.penetration > allowedPenetration ? (cp.penetration - allowedPenetration)/(penResolutionSteps*dt) : 0; + if (cp.minSepVel > maxPenResolutionSpeed) cp.minSepVel = maxPenResolutionSpeed; + } + } + for (i = 0; i < constraintsNum; i++) Constraint(constraints[i]).preProcess(dt); + } + + /** + * + * @param dt + * @param forceInelastic + */ + private function processContacts(dt:Number, forceInelastic:Boolean):void { + var iterNum:int = forceInelastic ? contactIterations : collisionIterations; + var i:int; + var forwardLoop:Boolean = false; + for (var iter:int = 0; iter < iterNum; iter++) { + forwardLoop = !forwardLoop; + for (i = 0; i < contactsNum; i++) resolveContact(contacts[i], forceInelastic, forwardLoop); + // Ограничения + for (i = 0; i < constraintsNum; i++) Constraint(constraints[i]).apply(dt); + } + + // Разделение контактов путём непосредственного изменения координат и ориентации. Экспериментально. +// if (forceInelastic && useStaticSeparation) performStaticSeparation(); + } + + /** + * + */ + private function resolveContact(contactInfo:Contact, forceInelastic:Boolean, forwardLoop:Boolean):void { + var b1:Body = contactInfo.body1; + var b2:Body = contactInfo.body2; + var normal:Vector3 = contactInfo.normal; + var i:int; + if (forwardLoop) { + for (i = 0; i < contactInfo.pcount; i++) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } else { + for (i = contactInfo.pcount - 1; i >= 0; i--) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } + } + + /** + * + * @param idx + * @param b1 + * @param b2 + * @param colInfo + * @param normal + * @param forceInelastic + */ + private function resolveContactPoint(idx:int, b1:Body, b2:Body, contact:Contact, normal:Vector3, forceInelastic:Boolean):void { + var cp:ContactPoint = contact.points[idx]; + if (!forceInelastic) cp.satisfied = true; + + var newVel:Number = 0; + calcSepVelocity(b1, b2, cp, _v); + var sepVel:Number = _v.vDot(contact.normal); + if (forceInelastic) { + var minSpeVel:Number = useStaticSeparation ? 0 : cp.minSepVel; + if (sepVel < minSpeVel) cp.satisfied = false; + else if (cp.satisfied) return; + newVel = minSpeVel; + } else { + newVel = cp.normalVel; + } + var deltaVel:Number = newVel - sepVel; + var impulse:Number = deltaVel/cp.velByUnitImpulseN; + var accumImpulse:Number = cp.accumImpulseN + impulse; + if (accumImpulse < 0) accumImpulse = 0; + var deltaImpulse:Number = accumImpulse - cp.accumImpulseN; + cp.accumImpulseN = accumImpulse; + // Применяем импульс к телам + if (b1.movable) b1.applyRelPosWorldImpulse(cp.r1, normal, deltaImpulse); + if (b2 != null && b2.movable) b2.applyRelPosWorldImpulse(cp.r2, normal, -deltaImpulse); + + // Учёт силы трения + calcSepVelocity(b1, b2, cp, _v); + // Расчитываем изменение касательной скорости на единицу касательного импульса + var tanSpeedByUnitImpulse:Number = 0; + _v.vAddScaled(-_v.vDot(contact.normal), contact.normal); + var tanSpeed:Number = _v.vLength(); + if (tanSpeed < 0.001) return; + _t.vCopy(_v).vNormalize().vReverse(); + // dV = b.invMass + ((invI * (r % t)) % r) * t + if (b1.movable) { + _v.vCross2(cp.r1, _t).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1); + tanSpeedByUnitImpulse += b1.invMass + _v.vDot(_t); + } + if (b2 != null && b2.movable) { + _v.vCross2(cp.r2, _t).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2); + tanSpeedByUnitImpulse += b2.invMass + _v.vDot(_t); + } + + var tanImpulse:Number = tanSpeed/tanSpeedByUnitImpulse; + var max:Number = contact.friction*cp.accumImpulseN; + if (max < 0) { + if (tanImpulse < max) tanImpulse = max; + } else { + if (tanImpulse > max) tanImpulse = max; + } + + // Применяем импульс к телам + if (b1.movable) b1.applyRelPosWorldImpulse(cp.r1, _t, tanImpulse); + if (b2 != null && b2.movable) b2.applyRelPosWorldImpulse(cp.r2, _t, -tanImpulse); + } + + /** + * + * @param cp + * @param normal + * @return + */ + private function calcSepVelocity(body1:Body, body2:Body, cp:ContactPoint, result:Vector3):void { + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 +// result.vCopy(body1.state.velocity).vAdd(_v1.vCross2(body1.state.rotation, cp.r1)); + var rot:Vector3 = body1.state.rotation; + var v:Vector3 = cp.r1; + var x:Number = rot.y*v.z - rot.z*v.y; + var y:Number = rot.z*v.x - rot.x*v.z; + var z:Number = rot.x*v.y - rot.y*v.x; + v = body1.state.velocity; + result.x = v.x + x; + result.y = v.y + y; + result.z = v.z + z; + // V2 = V2_c + w2%r2 + if (body2 != null) { +// result.vSubtract(body2.state.velocity).vSubtract(_v2.vCross2(body2.state.rotation, cp.r2)); + rot = body2.state.rotation; + v = cp.r2; + x = rot.y*v.z - rot.z*v.y; + y = rot.z*v.x - rot.x*v.z; + z = rot.x*v.y - rot.y*v.x; + v = body2.state.velocity; + result.x -= v.x + x; + result.y -= v.y + y; + result.z -= v.z + z; + } + } + + /** + * + * @param dt + */ + private function intergateVelocities(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + (bodies[i] as Body).integrateVelocity(dt); + } + } + + /** + * + * @param dt + */ + private function integratePositions(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i] as Body; + if (body.movable && !body.frozen) body.integratePosition(dt); + } + } + + /** + * + */ + private function performStaticSeparation():void { + var iterNum:int = staticSeparationIterations; +// iterNum = 100; + + // 1. В начале каждой итерации для всех контактов сбрасывается флаг satisfied с одновременным поиском наихудшего контакта, т.е. имеющего наибольшую величину пересечения тел. + // 2. Если найденный контакт имеет величину пересечения меньше предельно допустимой процедура прерывается, т.к. разделение контактов не требуется. + // 3. Если процедура продолжается, то выполняется разделение найденного контакта и установка его флага satisfied в true, после чего запускается внутренний цикл, состоящий + // из contactsNum - 1 итераций. На каждой итерации ищется наихудший контакт среди оставшихся (satisfied == false). Если такой контакт найден, то выполняется его разделение + // и переход к следующей итерации внутреннего цикла, иначе внутренний цикл прерывается и выполняется переход к следующей итерации внешнего цикла (шаг 1.). + for (var iter:int = 0; iter < iterNum; iter++) { + // Ищем контакт с максимальной величиной пересечения тел, одновременно сбрасывая флаги satisfied в false + var worstContact:Contact = contacts[0]; + var i:int; + for (i = 1; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + contact.satisfied = false; + if (contact.maxPenetration > worstContact.maxPenetration) worstContact = contact; + } + if (worstContact.maxPenetration <= allowedPenetration) return; + + resolveInterpenetration(worstContact); + // Внутренний цикл по оставшимся контактам + for (i = 1; i < contactsNum; i++) { + worstContact = getWorstContact(); + if (worstContact == null) break; + resolveInterpenetration(worstContact); + } + } + } + + /** + * + */ + private function getWorstContact():Contact { + var maxPen:Number = 0; + var worst:Contact = null; + for (var i:int = 0; i < contactsNum; i++) { + var c:Contact = contacts[i]; + if (!c.satisfied && c.maxPenetration > maxPen) { + worst = c; + maxPen = c.maxPenetration; + } + } + return maxPen > allowedPenetration ? worst : null; + } + + /** + * Разделяет указанный контакт, выполняя staticSeparationSteps итераций по списку точек контакта. + * 1. В начале каждой итерации ищется точка с наибольшим пересечением, одновременно сбрасываются флаги satisfied у точек. + * 2. Для найденной точки выполняется процедура разделения. + * 3. После выполняется pcount - 1 итерация по списку точек, каждый раз ищется наихудшая среди имеющих satisfied == false, для которой выполняется процедура разделения. + * Если наихудшая точка не найдена, то выполнется переход к следующей итерации внешнего цикла (шаг 1). + * + * В конце процедуры разделения обновляются значения пересечений для остальных точек контакта, а также для всех прочих контактов, относящихся к телам текущего. + * + * @param contact контакт для разделения + */ + private function resolveInterpenetration(contact:Contact):void { + contact.satisfied = true; + + for (var step:int = 0; step < staticSeparationSteps; step++) { + var worstCp:ContactPoint = contact.points[0]; + var cp:ContactPoint; + var i:int; + for (i = 1; i < contact.pcount; i++) { + cp = contact.points[i]; + cp.satisfied = false; + if (cp.penetration > worstCp.penetration) worstCp = cp; + } + if (worstCp.penetration <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + // Разделяем оставшиеся точки + var maxPen:Number = 0; + for (i = 1; i < contact.pcount; i++) { + // Поиск наихудшей точки + for (var j:int = 0; j < contact.pcount; j++) { + cp = contact.points[j]; + if (cp.satisfied) continue; + if (cp.penetration > maxPen) { + maxPen = cp.penetration; + worstCp = cp; + } + } + if (maxPen <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + } + } + } + + /** + * + * @param cp + * @param contact + */ + private function separateContactPoint(cp:ContactPoint, contact:Contact):void { + cp.satisfied = true; + + var b1:Body = contact.body1; + var b2:Body = contact.body2; + var totalMove:Number = cp.penetration - allowedPenetration; + var moveCoeff:Number = totalMove/cp.velByUnitImpulseN; + var linMove1:Number; + var angleMove1:Number; + if (b1.movable) { + linMove1 = b1.invMass*moveCoeff; + angleMove1 = cp.angularInertia1*moveCoeff; + if (angleMove1 > maxAngleMove) { + linMove1 += angleMove1 - maxAngleMove; + angleMove1 -= maxAngleMove; + } + b1.state.pos.vAddScaled(linMove1, contact.normal); + _v1.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vScale(angleMove1); + b1.state.orientation.addScaledVector(_v1, 1); + } + var linMove2:Number; + var angleMove2:Number; + if (b2 != null && b2.movable) { + linMove2 = b2.invMass*moveCoeff; + angleMove2 = cp.angularInertia2*moveCoeff; + if (angleMove2 > maxAngleMove) { + linMove2 += angleMove2 - maxAngleMove; + angleMove2 -= maxAngleMove; + } + b2.state.pos.vAddScaled(-linMove2, contact.normal); + _v2.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vScale(angleMove2); + _v2.vReverse(); + b2.state.orientation.addScaledVector(_v2, 1); + } + cp.penetration = allowedPenetration; + // Обновляем пересечения в других точках + var i:int; + for (i = 0; i < contact.pcount; i++) { + var cp1:ContactPoint = contact.points[i]; + if (cp1 == cp) continue; + var angularMove:Number; + if (b1.movable) { + angularMove = _v.vCross2(_v1, cp1.r1).vDot(contact.normal); + cp1.penetration -= linMove1 + angularMove; + } + if (b2 != null && b2.movable) { + angularMove = _v.vCross2(_v2, cp1.r2).vDot(contact.normal); + cp1.penetration -= linMove2 - angularMove; + } + // Обновление максимального значения для контакта + if (cp1.penetration > contact.maxPenetration) contact.maxPenetration = cp1.penetration; + } + // Обновляем пересечения для других контактов + var c:Contact; + var j:int; + if (b1.movable) { + for (i = 0; i < b1.contactsNum; i++) { + c = b1.contacts[i]; + if (c == contact) continue; + for (j = 0; j < c.pcount; j++) { + cp1 = c.points[j]; + if (b1 == c.body1) cp1.penetration -= linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r1).vDot(c.normal); + else cp1.penetration += linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r2).vDot(c.normal); + if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; + } + } + } + if (b2 != null && b2.movable) { + for (i = 0; i < b2.contactsNum; i++) { + c = b2.contacts[i]; + if (c == contact) continue; + for (j = 0; j < c.pcount; j++) { + cp1 = c.points[j]; + if (b2 == c.body1) cp1.penetration -= linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r1).vDot(c.normal); + else cp1.penetration += linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r2).vDot(c.normal); + if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; + } + } + } + } + + /** + * + */ + private function postPhysics():void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i]; + body.clearAccumulators(); + body.calcDerivedData(); + if (body.canFreeze) { + if (body.state.velocity.vLength() < linSpeedFreezeLimit && body.state.rotation.vLength() < angSpeedFreezeLimit) { + if (!body.frozen) { + body.freezeCounter++; + if (body.freezeCounter >= freezeSteps) body.frozen = true; + } + } else { + body.freezeCounter = 0; + body.frozen = false; + } + } + } + } + + /** + * + * @param dt + */ + public function runPhysics(dt:Number):void { + timeStamp++; + applyForces(dt); + detectCollisions(dt); + preProcessContacts(dt); + processContacts(dt, false); + intergateVelocities(dt); + processContacts(dt, true); + integratePositions(dt); + postPhysics(); + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/rigid/Body.as b/0.0.3.0/src/alternativa/physics/rigid/Body.as new file mode 100644 index 0000000..7c08ce9 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/rigid/Body.as @@ -0,0 +1,340 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.IBodyCollisionPredicate; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Quaternion; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class Body { + + public static var linDamping:Number = 0.997; + public static var rotDamping:Number = 0.997; + + public var name:String; + public var movable:Boolean = true; + public var canFreeze:Boolean = false; + + public var freezeCounter:int; + public var frozen:Boolean = false; + public var aabb:BoundBox = new BoundBox(); + public var postCollisionPredicate:IBodyCollisionPredicate; + + altphysics var id:int; + // Мир, в котором находится тело + altphysics var world:RigidWorld; + // Текущее и предыдущее состояние тела. Промежуточное состояние вычисляется линейной интерполяцией. + altphysics var state:BodyState = new BodyState(); + altphysics var prevState:BodyState = new BodyState(); + // Линейное и угловое ускорение тела на текущем шаге симуляции + altphysics var accel:Vector3 = new Vector3(); + altphysics var angleAccel:Vector3 = new Vector3(); + // Материал тела + altphysics var material:BodyMaterial = new BodyMaterial(); + + altphysics var invMass:Number = 1; + altphysics var invInertia:Matrix3 = new Matrix3(); + altphysics var invInertiaWorld:Matrix3 = new Matrix3(); + altphysics var baseMatrix:Matrix3 = new Matrix3(); + + altphysics const MAX_CONTACTS:int = 20; + altphysics var contacts:Vector. = new Vector.(MAX_CONTACTS); + altphysics var contactsNum:int; + + altphysics var collisionPrimitives:Vector.; + altphysics var collisionPrimitivesNum:int; + + // Аккумулятор сил + altphysics var forceAccum:Vector3 = new Vector3(); + // Аккумулятор моментов + altphysics var torqueAccum:Vector3 = new Vector3(); + + // Внутренние переменные для избежания создания экземпляров + private static var _r:Vector3 = new Vector3(); + private static var _f:Vector3 = new Vector3(); + + /** + * + * @param invMass + * @param invInertia + */ + public function Body(invMass:Number, invInertia:Matrix3) { + this.invMass = invMass; + this.invInertia.copy(invInertia); + } + + /** + * @param primitive + * @param localTransform + */ + public function addCollisionPrimitive(primitive:CollisionPrimitive, localTransform:Matrix4 = null):void { + if (primitive == null) throw new ArgumentError("Primitive cannot be null"); + if (collisionPrimitives == null) collisionPrimitives = new Vector.(); + collisionPrimitives[collisionPrimitivesNum++] = primitive; + primitive.setBody(this, localTransform); + } + + /** + * @param t + * @param result + */ + public function interpolate(t:Number, pos:Vector3, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.w = prevState.orientation.w*t1 + state.orientation.w*t; + orientation.x = prevState.orientation.x*t1 + state.orientation.x*t; + orientation.y = prevState.orientation.y*t1 + state.orientation.y*t; + orientation.z = prevState.orientation.z*t1 + state.orientation.z*t; + } + + /** + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + state.pos.vCopy(pos); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setPositionXYZ(x:Number, y:Number, z:Number):void { + state.pos.vReset(x, y, z); + } + + /** + * + * @param vel + */ + public function setVelocity(vel:Vector3):void { + state.velocity.vCopy(vel); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setVelocityXYZ(x:Number, y:Number, z:Number):void { + state.velocity.vReset(x, y, z); + } + + /** + * + * @param rot + */ + public function setRotation(rot:Vector3):void { + state.rotation.vCopy(rot); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setRotationXYZ(x:Number, y:Number, z:Number):void { + state.rotation.vReset(x, y, z); + } + + /** + * + * @param q + */ + public function setOrientation(q:Quaternion):void { + state.orientation.copy(q); + } + + /** + * @param r + * @param dir + * @param magnitude + */ + public function applyRelPosWorldImpulse(r:Vector3, dir:Vector3, magnitude:Number):void { + var d:Number = magnitude*invMass; + // Линейная часть + state.velocity.x += d*dir.x; + state.velocity.y += d*dir.y; + state.velocity.z += d*dir.z; + + // Вращательная часть + var rx:Number = r.x; + var ry:Number = r.y; + var rz:Number = r.z; + _r.x = (ry*dir.z - rz*dir.y)*magnitude; + _r.y = (rz*dir.x - rx*dir.z)*magnitude; + _r.z = (rx*dir.y - ry*dir.x)*magnitude; + _r.vTransformBy3(invInertiaWorld); + state.rotation.x += _r.x; + state.rotation.y += _r.y; + state.rotation.z += _r.z; + } + + /** + * @param f + */ + public function addForce(f:Vector3):void { + forceAccum.vAdd(f); + } + + /** + * @param pos + * @param f + */ + public function addWorldForce(pos:Vector3, force:Vector3):void { + forceAccum.vAdd(force); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(force)); + } + + /** + * @param pos + * @param f + */ + public function addWorldForceScaled(pos:Vector3, force:Vector3, scale:Number):void { + _f.x = scale*force.x, _f.y = scale*force.y, _f.z = scale*force.z; + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(_f)); + } + + /** + * @param pos + * @param f + */ + public function addLocalForce(pos:Vector3, force:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(pos, _r); + // Трансформируем вектор силы в мировую систему + baseMatrix.transformVector(force, _f); + // Добавляем силу и момент + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vCross(_f)); + } + + /** + * + * @param localPos + * @param worldForce + */ + public function addWorldForceAtLocalPoint(localPos:Vector3, worldForce:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(localPos, _r); + // Добавляем силу и момент + forceAccum.vAdd(worldForce); + torqueAccum.vAdd(_r.vCross(worldForce)); + } + + /** + * @param dt + */ + public function addExternalForces(dt:Number):void { + } + + /** + * @param t + */ + public function addTorque(t:Vector3):void { + torqueAccum.vAdd(t); + } + + /** + * + */ + altphysics function clearAccumulators():void { + forceAccum.vReset(); + torqueAccum.vReset(); + } + + /** + * + */ + altphysics function calcAccelerations():void { + accel.x = forceAccum.x*invMass; + accel.y = forceAccum.y*invMass; + accel.z = forceAccum.z*invMass; + invInertiaWorld.transformVector(torqueAccum, angleAccel); + } + + /** + * Вычисляет производные данные. + */ + altphysics function calcDerivedData():void { + // Вычисление базисной матрицы и обратного тензора инерции в мировых координатах + state.orientation.toMatrix3(baseMatrix); + invInertiaWorld.copy(invInertia).append(baseMatrix).prependTransposed(baseMatrix); + if (collisionPrimitives != null) { + aabb.infinity(); + for (var i:int = 0; i < collisionPrimitivesNum; i++) { + var primitive:CollisionPrimitive = collisionPrimitives[i]; + primitive.transform.setFromMatrix3(baseMatrix, state.pos); + if (primitive.localTransform != null) primitive.transform.prepend(primitive.localTransform); + primitive.calculateAABB(); + aabb.addBoundBox(primitive.aabb); + } + } + } + + /** + * + */ + altphysics function saveState():void { + prevState.copy(state); + } + + /** + * + */ + altphysics function restoreState():void { + state.copy(prevState); + } + + /** + * @param dt + */ + altphysics function integrateVelocity(dt:Number):void { + // v = v + a*t + state.velocity.x += accel.x*dt; + state.velocity.y += accel.y*dt; + state.velocity.z += accel.z*dt; + // rot = rot + eps*t + state.rotation.x += angleAccel.x*dt; + state.rotation.y += angleAccel.y*dt; + state.rotation.z += angleAccel.z*dt; + + state.velocity.x *= linDamping; + state.velocity.y *= linDamping; + state.velocity.z *= linDamping; + + state.rotation.x *= rotDamping; + state.rotation.y *= rotDamping; + state.rotation.z *= rotDamping; + } + + /** + * + */ + altphysics function integratePosition(dt:Number):void { + // pos = pos + v*t + state.pos.x += state.velocity.x*dt; + state.pos.y += state.velocity.y*dt; + state.pos.z += state.velocity.z*dt; + // q = q + 0.5*rot*q + state.orientation.addScaledVector(state.rotation, dt); + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/rigid/BodyMaterial.as b/0.0.3.0/src/alternativa/physics/rigid/BodyMaterial.as new file mode 100644 index 0000000..61d4382 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/rigid/BodyMaterial.as @@ -0,0 +1,11 @@ +package alternativa.physics.rigid { + + public class BodyMaterial { + + public var restitution:Number = 0; + public var friction:Number = 0.3; +// public var dynamicFriction:Number = 0.2; +// public var dynamicFriction:Number = 0.2; + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/rigid/BodyState.as b/0.0.3.0/src/alternativa/physics/rigid/BodyState.as new file mode 100644 index 0000000..408b0ea --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/rigid/BodyState.as @@ -0,0 +1,39 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Quaternion; + import alternativa.physics.types.Vector3; + + /** + * Класс описывает состояние твёрдого тела. + */ + public class BodyState { + /** + * Положение тела. + */ + public var pos:Vector3 = new Vector3(); + /** + * Ориентация тела. + */ + public var orientation:Quaternion = new Quaternion(); + /** + * Скорость тела. + */ + public var velocity:Vector3 = new Vector3(); + /** + * Угловая скорость тела. + */ + public var rotation:Vector3 = new Vector3(); + + /** + * Копирует значение указанного объекта. + * + * @param state + */ + public function copy(state:BodyState):void { + pos.vCopy(state.pos); + orientation.copy(state.orientation); + velocity.vCopy(state.velocity); + rotation.vCopy(state.rotation); + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/rigid/Contact.as b/0.0.3.0/src/alternativa/physics/rigid/Contact.as new file mode 100644 index 0000000..b73fc55 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/rigid/Contact.as @@ -0,0 +1,53 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.types.Vector3; + + /** + * Информация о контакте между двумя телами. Содержит нормаль, множество точек контакта и прочие величины. + */ + public class Contact { + +// private static var pool:Vector. = new Vector.(); +// +// public static function create():ContactInfo { +// if (pool.length > 0) { +// return pool.pop(); +// } +// return new ContactInfo(); +// } +// +// public static function destroy(collInfo:ContactInfo):void { +// collInfo.body1 = collInfo.body2 = null; +// pool.push(collInfo); +// } + + // Максимальное количество точек контакта + private const N:int = 8; + + public var body1:Body; + public var body2:Body; + // Взаимный коэффициент отскока + public var restitution:Number; + // Взаимный коэффициент трения + public var friction:Number; + // Нормаль контакта. Направлена от второго тела к первому. + public var normal:Vector3 = new Vector3(); + // Список точек контакта + public var points:Vector. = new Vector.(N, true); + // Количество точек контакта + public var pcount:int; + // Максимальная глубина пересечения тел + public var maxPenetration:Number = 0; + + public var satisfied:Boolean; + + /** + * + */ + public function Contact() { + for (var i:int = 0; i < N; i++) points[i] = new ContactPoint(); + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/rigid/ContactPoint.as b/0.0.3.0/src/alternativa/physics/rigid/ContactPoint.as new file mode 100644 index 0000000..c36f958 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/rigid/ContactPoint.as @@ -0,0 +1,52 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Vector3; + + public class ContactPoint { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + + // Величины, расчитываемые перед началом фазы решения контактов + + // Требуемая проекция конечной скорости на нормаль для упругого контакта + public var normalVel:Number; + // Минимальная скорость разделения неупругого контакта + public var minSepVel:Number; + // Изменение проекции скорости на единицу нормального импульса + public var velByUnitImpulseN:Number; + + public var angularInertia1:Number; + public var angularInertia2:Number; + + // Радиус-вектор точки контакта относительно центра первого тела + public var r1:Vector3 = new Vector3(); + // Радиус-вектор точки контакта относительно центра второго тела + public var r2:Vector3 = new Vector3(); + + // Величины, накапливаемые во время фазы решения контактов + + // Накопленный импульс, применяемый для получения требуемой относительной скорости в точке контакта + public var accumImpulseN:Number; + // Накопленный импульс, применяемый для разделения тел в точке контакта. Не создаёт момента. +// public var accumSepImpulse:Number; + + public var satisfied:Boolean; + + /** + * + * @param cp + */ + public function copyFrom(cp:ContactPoint):void { + pos.vCopy(cp.pos); + penetration = cp.penetration; + feature1 = cp.feature1; + feature2 = cp.feature2; + r1.vCopy(cp.r1); + r2.vCopy(cp.r2); + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/rigid/PhysicsUtils.as b/0.0.3.0/src/alternativa/physics/rigid/PhysicsUtils.as new file mode 100644 index 0000000..a9aaa99 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/rigid/PhysicsUtils.as @@ -0,0 +1,49 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + public class PhysicsUtils { + + /** + * @param mass + * @param halfSize + * @param result + */ + public static function getBoxInvInertia(mass:Number, halfSize:Vector3, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + result.a = 3/(mass*(yy + zz)); + result.f = 3/(mass*(zz + xx)); + result.k = 3/(mass*(xx + yy)); + } + + /** + * @param mass + * @param r + * @param h + * @param result + */ + public static function getCylinderInvInertia(mass:Number, r:Number, h:Number, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + + result.a = result.f = 1/(mass*(h*h/12 + r*r/4)); + result.k = 2/(mass*r*r); + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/rigid/RigidWorld.as b/0.0.3.0/src/alternativa/physics/rigid/RigidWorld.as new file mode 100644 index 0000000..696580b --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/rigid/RigidWorld.as @@ -0,0 +1,653 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.ICollisionDetector; + import alternativa.physics.collision.KdTreeCollisionDetector; + import alternativa.physics.rigid.constraints.Constraint; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Класс реализует физическую симуляцию поведения твёрдых тел. + */ + public class RigidWorld { + + private static var lastBodyId:int; + + // Максимальное количество контактов + altphysics const MAX_CONTACTS:int = 1000; + + ////////////////////////////////////////////////////////////////// + // Настроечные параметры симуляции. Могут быть изменены в любой + // момент времени без нарушения корректной работы симуляции. + ////////////////////////////////////////////////////////////////// + // Количество шагов, за которое пересекающиеся тела должны разделиться + public var penResolutionSteps:int = 10; + // Величина допустимой глубины пересечения + public var allowedPenetration:Number = 0.1; + // Максимальная скорость, добавляемая с целью разделения тел + public var maxPenResolutionSpeed:Number = 0.5; + // Количество итераций для обработки упругих контактов + public var collisionIterations:int = 5; + // Количество итераций для обработки неупругих контактов + public var contactIterations:int = 5; + // Флаг использования предсказания состояний + public var usePrediction:Boolean = false; + + public var freezeSteps:int = 10; + public var linSpeedFreezeLimit:Number = 1; + public var angSpeedFreezeLimit:Number = 0.01; + + // Переменные для процедуры разделения тел путём непосредственного их перемещенеия. + // !!!!!!!!!!!!!!!!!!!!!!!! + // !!! Экспериментально !!! + // !!!!!!!!!!!!!!!!!!!!!!!! + public var staticSeparationIterations:int = 10; + public var staticSeparationSteps:int = 10; + public var maxAngleMove:Number = 10; + public var useStaticSeparation:Boolean = false; + + // Вектор гравитации + altphysics var _gravity:Vector3 = new Vector3(0, 0, -9.8); + // Использующийся детектор столкновений + altphysics var collisionDetector:ICollisionDetector; + // Список тел, участвующих в симуляции + altphysics var bodies:Vector. = new Vector.(); + // Количество тел, участвующих в симуляции + altphysics var bodiesNum:int; + // Список контактов на текущем шаге симуляции + altphysics var contacts:Vector. = new Vector.(MAX_CONTACTS, true); + // Количество контактов на текущем шаге симуляции + altphysics var contactsNum:int; + // Список ограничений + altphysics var constraints:Vector. = new Vector.(); + // Количество ограничений + altphysics var constraintsNum:int; + // Временная метка. Число прошедших шагов с начала симуляции. + altphysics var timeStamp:uint; + + // Временные переменные для избежания создания экземпляров + private var _r:Vector3 = new Vector3(); + private var _t:Vector3 = new Vector3(); + private var _v:Vector3 = new Vector3(); + private var _v1:Vector3 = new Vector3(); + private var _v2:Vector3 = new Vector3(); + + /** + * + */ + public function RigidWorld() { + for (var i:int = 0; i < MAX_CONTACTS; i++) contacts[i] = new Contact(); + collisionDetector = new KdTreeCollisionDetector(); + } + + /** + * Вектор гравитации. + */ + public function get gravity():Vector3 { + return _gravity.vClone(); + } + + /** + * @private + */ + public function set gravity(value:Vector3):void { + _gravity.vCopy(value); + } + + /** + * Добавляет тело в симуляцию. + * @param body + * @return + */ + public function addBody(body:Body, isStatic:Boolean):Boolean { + var i:int = bodies.indexOf(body); + if (i > -1) return false; + bodies[bodiesNum++] = body; + body.world = this; + body.id = lastBodyId++; +// if (body.collisionPrimitives != null) { +// for (i = 0; i < body.collisionPrimitivesNum; i++) collisionDetector.addPrimitive(body.collisionPrimitives[i], isStatic); +// } + return true; + } + + /** + * Удаляет тело из симуляции. + * @param body + * @return + */ + public function removeBody(body:Body, isStatic:Boolean):Boolean { + var i:int = bodies.indexOf(body); + if (i == -1) return false; + bodies.splice(i, 1); + body.world = null; +// if (body.collisionPrimitives != null) { +// for (i = 0; i < body.collisionPrimitivesNum; i++) collisionDetector.removePrimitive(body.collisionPrimitives[i], isStatic); +// } + bodiesNum--; + return true; + } + + /** + * Добавляет ограничение. + * @param c + */ + public function addConstraint(c:Constraint):void { + constraints[constraintsNum++] = c; + c.world = this; + } + + /** + * Удаляет ограничение. + * @param c + */ + public function removeConstraint(c:Constraint):Boolean { + var idx:int = constraints.indexOf(c); + if (idx < 0) return false; + constraints.splice(idx, 1) + constraintsNum--; + c.world = null; + return true; + } + + /** + * Применяет к телам действующте на них силы. Аккумуляторы сил и моментов тел очищаются после завершения шага + * симуляции, поэтому на момент вызова метода могут уже содержать некоторые значения. + * + * @param dt промежуток времени, в течении которого действуют силы + */ + private function applyForces(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i]; + body.addExternalForces(dt); + body.calcAccelerations(); + // Ускорение свободного падения применяется только к подвижным телам во избежание некорректного изменения + // фиктивной скорости неподвижных тел. + if (body.movable && !body.frozen) { + body.accel.x += _gravity.x; + body.accel.y += _gravity.y; + body.accel.z += _gravity.z; + } + } + } + + /** + * Определяет все столкновения на текущем шаге симуляции и заполняет список получившихся контактов. + * + * @param dt длительность шага симуляции + */ + private function detectCollisions(dt:Number):void { + var i:int; + var j:int; + var body:Body; + + for (i = 0; i < bodiesNum; i++) { + body = bodies[i]; + if (body.frozen) continue; + body.contactsNum = 0; + body.saveState(); + // При включённом режиме предсказания состояние тел интегрируется на один шаг вперёд + if (usePrediction) { + body.integrateVelocity(dt); + body.integratePosition(dt); + } + body.calcDerivedData(); + } + + contactsNum = collisionDetector.getAllCollisions(contacts); + + // Расчёт относительных векторов точки контакта вынесен сюда из-за необходимости учитывать + // положение тел в предсказанном состоянии + for (i = 0; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + var b1:Body = contact.body1; + var b2:Body = contact.body2; + for (j = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.r1.x = cp.pos.x - b1.state.pos.x; + cp.r1.y = cp.pos.y - b1.state.pos.y; + cp.r1.z = cp.pos.z - b1.state.pos.z; + if (b2 != null) { + cp.r2.x = cp.pos.x - b2.state.pos.x; + cp.r2.y = cp.pos.y - b2.state.pos.y; + cp.r2.z = cp.pos.z - b2.state.pos.z; + } + } + } + + // Восстановление состояния тел + if (usePrediction) { + for (i = 0; i < bodiesNum; i++) { + body = bodies[i]; + if (body.frozen) continue; + body.restoreState(); + body.calcDerivedData(); + } + } + } + + /** + * Подготваливает полученные из детектора столкновений контакты, расчитывая значения, не меняющиеся + * в ходе шага симуляции. + */ + private function preProcessContacts(dt:Number):void { + var i:int; + for (i = 0; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + var b1:Body = contact.body1; + var b2:Body = contact.body2; + // Столкнувшиеся тела размораживаются + if (b1.frozen) { + b1.frozen = false; + b1.freezeCounter = 0; + } + if (b2 != null && b2.frozen) { + b2.frozen = false; + b2.freezeCounter = 0; + } + contact.restitution = b1.material.restitution; + if (b2 != null && b2.material.restitution < contact.restitution) contact.restitution = b2.material.restitution; + contact.friction = b1.material.friction; + if (b2 != null && b2.material.friction < contact.friction) contact.friction = b2.material.friction; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.accumImpulseN = 0; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + cp.velByUnitImpulseN = 0; + if (b1.movable) { + cp.angularInertia1 = _v.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1).vDot(contact.normal); + cp.velByUnitImpulseN += b1.invMass + cp.angularInertia1; + } + if (b2 != null && b2.movable) { + cp.angularInertia2 = _v.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2).vDot(contact.normal); + cp.velByUnitImpulseN += b2.invMass + cp.angularInertia2; + } + // Расчёт требуемой конечной скорости для упругого контакта + calcSepVelocity(b1, b2, cp, _v); + cp.normalVel = _v.vDot(contact.normal); + if (cp.normalVel < 0) cp.normalVel = - contact.restitution*cp.normalVel; + // Скорость разделения неупругого контакта + cp.minSepVel = cp.penetration > allowedPenetration ? (cp.penetration - allowedPenetration)/(penResolutionSteps*dt) : 0; + if (cp.minSepVel > maxPenResolutionSpeed) cp.minSepVel = maxPenResolutionSpeed; + } + } + for (i = 0; i < constraintsNum; i++) Constraint(constraints[i]).preProcess(dt); + } + + /** + * + * @param dt + * @param forceInelastic + */ + private function processContacts(dt:Number, forceInelastic:Boolean):void { + var iterNum:int = forceInelastic ? contactIterations : collisionIterations; + var i:int; + var forwardLoop:Boolean = false; + for (var iter:int = 0; iter < iterNum; iter++) { + forwardLoop = !forwardLoop; + for (i = 0; i < contactsNum; i++) resolveContact(contacts[i], forceInelastic, forwardLoop); + // Ограничения + for (i = 0; i < constraintsNum; i++) Constraint(constraints[i]).apply(dt); + } + + // Разделение контактов путём непосредственного изменения координат и ориентации. Экспериментально. +// if (forceInelastic && useStaticSeparation) performStaticSeparation(); + } + + /** + * + */ + private function resolveContact(contactInfo:Contact, forceInelastic:Boolean, forwardLoop:Boolean):void { + var b1:Body = contactInfo.body1; + var b2:Body = contactInfo.body2; + var normal:Vector3 = contactInfo.normal; + var i:int; + if (forwardLoop) { + for (i = 0; i < contactInfo.pcount; i++) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } else { + for (i = contactInfo.pcount - 1; i >= 0; i--) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } + } + + /** + * + * @param idx + * @param b1 + * @param b2 + * @param colInfo + * @param normal + * @param forceInelastic + */ + private function resolveContactPoint(idx:int, b1:Body, b2:Body, contact:Contact, normal:Vector3, forceInelastic:Boolean):void { + var cp:ContactPoint = contact.points[idx]; + if (!forceInelastic) cp.satisfied = true; + + var newVel:Number = 0; + calcSepVelocity(b1, b2, cp, _v); + var sepVel:Number = _v.vDot(contact.normal); + if (forceInelastic) { + var minSpeVel:Number = useStaticSeparation ? 0 : cp.minSepVel; + if (sepVel < minSpeVel) cp.satisfied = false; + else if (cp.satisfied) return; + newVel = minSpeVel; + } else { + newVel = cp.normalVel; + } + var deltaVel:Number = newVel - sepVel; + var impulse:Number = deltaVel/cp.velByUnitImpulseN; + var accumImpulse:Number = cp.accumImpulseN + impulse; + if (accumImpulse < 0) accumImpulse = 0; + var deltaImpulse:Number = accumImpulse - cp.accumImpulseN; + cp.accumImpulseN = accumImpulse; + // Применяем импульс к телам + if (b1.movable) b1.applyRelPosWorldImpulse(cp.r1, normal, deltaImpulse); + if (b2 != null && b2.movable) b2.applyRelPosWorldImpulse(cp.r2, normal, -deltaImpulse); + + // Учёт силы трения + calcSepVelocity(b1, b2, cp, _v); + // Расчитываем изменение касательной скорости на единицу касательного импульса + var tanSpeedByUnitImpulse:Number = 0; + _v.vAddScaled(-_v.vDot(contact.normal), contact.normal); + var tanSpeed:Number = _v.vLength(); + if (tanSpeed < 0.001) return; + _t.vCopy(_v).vNormalize().vReverse(); + // dV = b.invMass + ((invI * (r % t)) % r) * t + if (b1.movable) { + _v.vCross2(cp.r1, _t).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1); + tanSpeedByUnitImpulse += b1.invMass + _v.vDot(_t); + } + if (b2 != null && b2.movable) { + _v.vCross2(cp.r2, _t).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2); + tanSpeedByUnitImpulse += b2.invMass + _v.vDot(_t); + } + + var tanImpulse:Number = tanSpeed/tanSpeedByUnitImpulse; + var max:Number = contact.friction*cp.accumImpulseN; + if (max < 0) { + if (tanImpulse < max) tanImpulse = max; + } else { + if (tanImpulse > max) tanImpulse = max; + } + + // Применяем импульс к телам + if (b1.movable) b1.applyRelPosWorldImpulse(cp.r1, _t, tanImpulse); + if (b2 != null && b2.movable) b2.applyRelPosWorldImpulse(cp.r2, _t, -tanImpulse); + } + + /** + * + * @param cp + * @param normal + * @return + */ + private function calcSepVelocity(body1:Body, body2:Body, cp:ContactPoint, result:Vector3):void { + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 +// result.vCopy(body1.state.velocity).vAdd(_v1.vCross2(body1.state.rotation, cp.r1)); + var rot:Vector3 = body1.state.rotation; + var v:Vector3 = cp.r1; + var x:Number = rot.y*v.z - rot.z*v.y; + var y:Number = rot.z*v.x - rot.x*v.z; + var z:Number = rot.x*v.y - rot.y*v.x; + v = body1.state.velocity; + result.x = v.x + x; + result.y = v.y + y; + result.z = v.z + z; + // V2 = V2_c + w2%r2 + if (body2 != null) { +// result.vSubtract(body2.state.velocity).vSubtract(_v2.vCross2(body2.state.rotation, cp.r2)); + rot = body2.state.rotation; + v = cp.r2; + x = rot.y*v.z - rot.z*v.y; + y = rot.z*v.x - rot.x*v.z; + z = rot.x*v.y - rot.y*v.x; + v = body2.state.velocity; + result.x -= v.x + x; + result.y -= v.y + y; + result.z -= v.z + z; + } + } + + /** + * + * @param dt + */ + private function intergateVelocities(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + (bodies[i] as Body).integrateVelocity(dt); + } + } + + /** + * + * @param dt + */ + private function integratePositions(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i] as Body; + if (body.movable && !body.frozen) body.integratePosition(dt); + } + } + + /** + * + */ + private function performStaticSeparation():void { + var iterNum:int = staticSeparationIterations; +// iterNum = 100; + + // 1. В начале каждой итерации для всех контактов сбрасывается флаг satisfied с одновременным поиском наихудшего контакта, т.е. имеющего наибольшую величину пересечения тел. + // 2. Если найденный контакт имеет величину пересечения меньше предельно допустимой процедура прерывается, т.к. разделение контактов не требуется. + // 3. Если процедура продолжается, то выполняется разделение найденного контакта и установка его флага satisfied в true, после чего запускается внутренний цикл, состоящий + // из contactsNum - 1 итераций. На каждой итерации ищется наихудший контакт среди оставшихся (satisfied == false). Если такой контакт найден, то выполняется его разделение + // и переход к следующей итерации внутреннего цикла, иначе внутренний цикл прерывается и выполняется переход к следующей итерации внешнего цикла (шаг 1.). + for (var iter:int = 0; iter < iterNum; iter++) { + // Ищем контакт с максимальной величиной пересечения тел, одновременно сбрасывая флаги satisfied в false + var worstContact:Contact = contacts[0]; + var i:int; + for (i = 1; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + contact.satisfied = false; + if (contact.maxPenetration > worstContact.maxPenetration) worstContact = contact; + } + if (worstContact.maxPenetration <= allowedPenetration) return; + + resolveInterpenetration(worstContact); + // Внутренний цикл по оставшимся контактам + for (i = 1; i < contactsNum; i++) { + worstContact = getWorstContact(); + if (worstContact == null) break; + resolveInterpenetration(worstContact); + } + } + } + + /** + * + */ + private function getWorstContact():Contact { + var maxPen:Number = 0; + var worst:Contact = null; + for (var i:int = 0; i < contactsNum; i++) { + var c:Contact = contacts[i]; + if (!c.satisfied && c.maxPenetration > maxPen) { + worst = c; + maxPen = c.maxPenetration; + } + } + return maxPen > allowedPenetration ? worst : null; + } + + /** + * Разделяет указанный контакт, выполняя staticSeparationSteps итераций по списку точек контакта. + * 1. В начале каждой итерации ищется точка с наибольшим пересечением, одновременно сбрасываются флаги satisfied у точек. + * 2. Для найденной точки выполняется процедура разделения. + * 3. После выполняется pcount - 1 итерация по списку точек, каждый раз ищется наихудшая среди имеющих satisfied == false, для которой выполняется процедура разделения. + * Если наихудшая точка не найдена, то выполнется переход к следующей итерации внешнего цикла (шаг 1). + * + * В конце процедуры разделения обновляются значения пересечений для остальных точек контакта, а также для всех прочих контактов, относящихся к телам текущего. + * + * @param contact контакт для разделения + */ + private function resolveInterpenetration(contact:Contact):void { + contact.satisfied = true; + + for (var step:int = 0; step < staticSeparationSteps; step++) { + var worstCp:ContactPoint = contact.points[0]; + var cp:ContactPoint; + var i:int; + for (i = 1; i < contact.pcount; i++) { + cp = contact.points[i]; + cp.satisfied = false; + if (cp.penetration > worstCp.penetration) worstCp = cp; + } + if (worstCp.penetration <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + // Разделяем оставшиеся точки + var maxPen:Number = 0; + for (i = 1; i < contact.pcount; i++) { + // Поиск наихудшей точки + for (var j:int = 0; j < contact.pcount; j++) { + cp = contact.points[j]; + if (cp.satisfied) continue; + if (cp.penetration > maxPen) { + maxPen = cp.penetration; + worstCp = cp; + } + } + if (maxPen <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + } + } + } + + /** + * + * @param cp + * @param contact + */ + private function separateContactPoint(cp:ContactPoint, contact:Contact):void { + cp.satisfied = true; + + var b1:Body = contact.body1; + var b2:Body = contact.body2; + var totalMove:Number = cp.penetration - allowedPenetration; + var moveCoeff:Number = totalMove/cp.velByUnitImpulseN; + var linMove1:Number; + var angleMove1:Number; + if (b1.movable) { + linMove1 = b1.invMass*moveCoeff; + angleMove1 = cp.angularInertia1*moveCoeff; + if (angleMove1 > maxAngleMove) { + linMove1 += angleMove1 - maxAngleMove; + angleMove1 -= maxAngleMove; + } + b1.state.pos.vAddScaled(linMove1, contact.normal); + _v1.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vScale(angleMove1); + b1.state.orientation.addScaledVector(_v1, 1); + } + var linMove2:Number; + var angleMove2:Number; + if (b2 != null && b2.movable) { + linMove2 = b2.invMass*moveCoeff; + angleMove2 = cp.angularInertia2*moveCoeff; + if (angleMove2 > maxAngleMove) { + linMove2 += angleMove2 - maxAngleMove; + angleMove2 -= maxAngleMove; + } + b2.state.pos.vAddScaled(-linMove2, contact.normal); + _v2.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vScale(angleMove2); + _v2.vReverse(); + b2.state.orientation.addScaledVector(_v2, 1); + } + cp.penetration = allowedPenetration; + // Обновляем пересечения в других точках + var i:int; + for (i = 0; i < contact.pcount; i++) { + var cp1:ContactPoint = contact.points[i]; + if (cp1 == cp) continue; + var angularMove:Number; + if (b1.movable) { + angularMove = _v.vCross2(_v1, cp1.r1).vDot(contact.normal); + cp1.penetration -= linMove1 + angularMove; + } + if (b2 != null && b2.movable) { + angularMove = _v.vCross2(_v2, cp1.r2).vDot(contact.normal); + cp1.penetration -= linMove2 - angularMove; + } + // Обновление максимального значения для контакта + if (cp1.penetration > contact.maxPenetration) contact.maxPenetration = cp1.penetration; + } + // Обновляем пересечения для других контактов + var c:Contact; + var j:int; + if (b1.movable) { + for (i = 0; i < b1.contactsNum; i++) { + c = b1.contacts[i]; + if (c == contact) continue; + for (j = 0; j < c.pcount; j++) { + cp1 = c.points[j]; + if (b1 == c.body1) cp1.penetration -= linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r1).vDot(c.normal); + else cp1.penetration += linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r2).vDot(c.normal); + if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; + } + } + } + if (b2 != null && b2.movable) { + for (i = 0; i < b2.contactsNum; i++) { + c = b2.contacts[i]; + if (c == contact) continue; + for (j = 0; j < c.pcount; j++) { + cp1 = c.points[j]; + if (b2 == c.body1) cp1.penetration -= linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r1).vDot(c.normal); + else cp1.penetration += linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r2).vDot(c.normal); + if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; + } + } + } + } + + /** + * + */ + private function postPhysics():void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i]; + body.clearAccumulators(); + body.calcDerivedData(); + if (body.canFreeze) { + if (body.state.velocity.vLength() < linSpeedFreezeLimit && body.state.rotation.vLength() < angSpeedFreezeLimit) { + if (!body.frozen) { + body.freezeCounter++; + if (body.freezeCounter >= freezeSteps) body.frozen = true; + } + } else { + body.freezeCounter = 0; + body.frozen = false; + } + } + } + } + + /** + * + * @param dt + */ + public function runPhysics(dt:Number):void { + timeStamp++; + applyForces(dt); + detectCollisions(dt); + preProcessContacts(dt); + processContacts(dt, false); + intergateVelocities(dt); + processContacts(dt, true); + integratePositions(dt); + postPhysics(); + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/rigid/constraints/.svn/all-wcprops b/0.0.3.0/src/alternativa/physics/rigid/constraints/.svn/all-wcprops new file mode 100644 index 0000000..a394d1f --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/rigid/constraints/.svn/all-wcprops @@ -0,0 +1,17 @@ +K 25 +svn:wc:ra_dav:version-url +V 121 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/rigid/constraints +END +Constraint.as +K 25 +svn:wc:ra_dav:version-url +V 135 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/rigid/constraints/Constraint.as +END +MaxDistanceConstraint.as +K 25 +svn:wc:ra_dav:version-url +V 146 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/rigid/constraints/MaxDistanceConstraint.as +END diff --git a/0.0.3.0/src/alternativa/physics/rigid/constraints/.svn/entries b/0.0.3.0/src/alternativa/physics/rigid/constraints/.svn/entries new file mode 100644 index 0000000..bbdaff2 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/rigid/constraints/.svn/entries @@ -0,0 +1,52 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/rigid/constraints +http://svndev.alternativaplatform.com + + + +2009-06-29T03:58:47.301371Z +15198 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Constraint.as +file + + + + +2010-10-28T04:32:41.000000Z +757824f9eeb3c89243d104910e0d0529 +2009-04-23T13:43:38.903278Z +11779 +mike + +MaxDistanceConstraint.as +file + + + + +2010-10-28T04:32:41.000000Z +025a27c81aeaf18c860f3a4c4c63cf96 +2009-06-29T03:58:47.301371Z +15198 +mike + diff --git a/0.0.3.0/src/alternativa/physics/rigid/constraints/.svn/format b/0.0.3.0/src/alternativa/physics/rigid/constraints/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/rigid/constraints/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.3.0/src/alternativa/physics/rigid/constraints/.svn/text-base/Constraint.as.svn-base b/0.0.3.0/src/alternativa/physics/rigid/constraints/.svn/text-base/Constraint.as.svn-base new file mode 100644 index 0000000..c2cd912 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/rigid/constraints/.svn/text-base/Constraint.as.svn-base @@ -0,0 +1,35 @@ +package alternativa.physics.rigid.constraints { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.RigidWorld; + + use namespace altphysics; + + /** + * + */ + public class Constraint { + + altphysics var satisfied:Boolean; + altphysics var world:RigidWorld; + + /** + * + */ + public function Constraint() { + } + + /** + * @param dt + */ + public function preProcess(dt:Number):void { + } + + /** + * @param dt + */ + public function apply(dt:Number):void { + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/rigid/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base b/0.0.3.0/src/alternativa/physics/rigid/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base new file mode 100644 index 0000000..008d4a4 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/rigid/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base @@ -0,0 +1,155 @@ +package alternativa.physics.rigid.constraints { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.BodyState; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class MaxDistanceConstraint extends Constraint { + // Первое тело ограничения + altphysics var body1:Body; + // Второе тело ограничения. Может быть null. + altphysics var body2:Body; + // Первая точка крепления в системе координат первого тела + altphysics var r1:Vector3; + // Вторая точка привязки в системе координат второго тела. В случае отсутствия второго тела, координаты задаются в мировой системе. + altphysics var r2:Vector3; + // Максимально допустимое расстояние между точками крепления + altphysics var maxDistance:Number; + // Радиус-вектор первой точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr1:Vector3 = new Vector3(); + // Радиус-вектор второй точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr2:Vector3 = new Vector3(); + // Минимальная скорость сближения для удовлетворения ограничения + private var minClosingVel:Number; + // Изменение относительной скорости на единицу импульса + private var velByUnitImpulseN:Number; + // Направление корректирующего импульса + private var impulseDirection:Vector3 = new Vector3(); + + // Вспомогательные переменные + private static var _v:Vector3 = new Vector3(); + private static var _v1:Vector3 = new Vector3(); + private static var _v2:Vector3 = new Vector3(); + + /** + * @param body1 + * @param body2 + * @param r1 + * @param r2 + * @param maxDistance + */ + public function MaxDistanceConstraint(body1:Body, body2:Body, r1:Vector3, r2:Vector3, maxDistance:Number) { + super(); + this.body1 = body1; + this.body2 = body2; + this.r1 = r1.vClone(); + this.r2 = r2.vClone(); + this.maxDistance = maxDistance; + } + + /** + * @param dt + */ + override public function preProcess(dt:Number):void { + // Вычислим расстояние между точками + body1.baseMatrix.transformVector(r1, wr1); + if (body2 != null) body2.baseMatrix.transformVector(r2, wr2); + else { + wr2.x = r2.x; + wr2.y = r2.y; + wr2.z = r2.z; + } + var p1:Vector3 = body1.state.pos; + impulseDirection.x = wr2.x - wr1.x - p1.x; + impulseDirection.y = wr2.y - wr1.y - p1.y; + impulseDirection.z = wr2.z - wr1.z - p1.z; + if (body2 != null) { + var p2:Vector3 = body2.state.pos; + impulseDirection.x += p2.x; + impulseDirection.y += p2.y; + impulseDirection.z += p2.z; + } + + var len:Number = Math.sqrt(impulseDirection.x*impulseDirection.x + impulseDirection.y*impulseDirection.y + impulseDirection.z*impulseDirection.z); + var delta:Number = len - maxDistance; + if (delta > 0) { + satisfied = false; + if (len == 0) impulseDirection.x = 1; + else { + impulseDirection.x /= len; + impulseDirection.y /= len; + impulseDirection.z /= len; + } + minClosingVel = delta/(world.penResolutionSteps*dt); + if (minClosingVel > world.maxPenResolutionSpeed) minClosingVel = world.maxPenResolutionSpeed; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + velByUnitImpulseN = 0; + if (body1.movable) { +// velByUnitImpulseN += body1.invMass + _v.vCross2(wr1, impulseDirection).vTransformBy3(body1.invInertiaWorld).vCross(wr1).vDot(impulseDirection); + _v.x = wr1.y*impulseDirection.z - wr1.z*impulseDirection.y; + _v.y = wr1.z*impulseDirection.x - wr1.x*impulseDirection.z; + _v.z = wr1.x*impulseDirection.y - wr1.y*impulseDirection.x; + _v.vTransformBy3(body1.invInertiaWorld); + _v1.x = _v.y*wr1.z - _v.z*wr1.y; + _v1.y = _v.z*wr1.x - _v.x*wr1.z; + _v1.z = _v.x*wr1.y - _v.y*wr1.x; + velByUnitImpulseN += body1.invMass + _v1.x*impulseDirection.x + _v1.y*impulseDirection.y + _v1.z*impulseDirection.z; + } + if (body2 != null && body2.movable) { +// velByUnitImpulseN += body2.invMass + _v.vCross2(wr2, impulseDirection).vTransformBy3(body2.invInertiaWorld).vCross(wr2).vDot(impulseDirection); + _v.x = wr2.y*impulseDirection.z - wr2.z*impulseDirection.y; + _v.y = wr2.z*impulseDirection.x - wr2.x*impulseDirection.z; + _v.z = wr2.x*impulseDirection.y - wr2.y*impulseDirection.x; + _v.vTransformBy3(body2.invInertiaWorld); + _v1.x = _v.y*wr2.z - _v.z*wr2.y; + _v1.y = _v.z*wr2.x - _v.x*wr2.z; + _v1.z = _v.x*wr2.y - _v.y*wr2.x; + velByUnitImpulseN += body2.invMass + _v1.x*impulseDirection.x + _v1.y*impulseDirection.y + _v1.z*impulseDirection.z; + } + } else satisfied = true; + } + + /** + * @param dt + */ + override public function apply(dt:Number):void { + if (satisfied) return; + // Расчитываем проекцию скорости относительной скорости + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 + var state:BodyState = body1.state; + var rot:Vector3 = state.rotation; +// _v1.vCross2(state.rotation, wr1); + _v1.x = rot.y*wr1.z - rot.z*wr1.y; + _v1.y = rot.z*wr1.x - rot.x*wr1.z; + _v1.z = rot.x*wr1.y - rot.y*wr1.x; + _v.x = state.velocity.x + _v1.x; + _v.y = state.velocity.y + _v1.y; + _v.z = state.velocity.z + _v1.z; + // V2 = V2_c + w2%r2 + if (body2 != null) { + state = body2.state; +// _v2.vCross2(state.rotation, wr2); + _v2.x = rot.y*wr2.z - rot.z*wr2.y; + _v2.y = rot.z*wr2.x - rot.x*wr2.z; + _v2.z = rot.x*wr2.y - rot.y*wr2.x; + _v.x -= state.velocity.x - _v2.x; + _v.y -= state.velocity.y - _v2.y; + _v.z -= state.velocity.z - _v2.z; + } + var closingVel:Number = _v.x*impulseDirection.x + _v.y*impulseDirection.y + _v.z*impulseDirection.z; + if (closingVel > minClosingVel) return; + var impulse:Number = (minClosingVel - closingVel)/velByUnitImpulseN; + if (body1.movable) body1.applyRelPosWorldImpulse(wr1, impulseDirection, impulse); + if (body2 != null && body2.movable) body2.applyRelPosWorldImpulse(wr2, impulseDirection, -impulse); + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/rigid/constraints/Constraint.as b/0.0.3.0/src/alternativa/physics/rigid/constraints/Constraint.as new file mode 100644 index 0000000..c2cd912 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/rigid/constraints/Constraint.as @@ -0,0 +1,35 @@ +package alternativa.physics.rigid.constraints { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.RigidWorld; + + use namespace altphysics; + + /** + * + */ + public class Constraint { + + altphysics var satisfied:Boolean; + altphysics var world:RigidWorld; + + /** + * + */ + public function Constraint() { + } + + /** + * @param dt + */ + public function preProcess(dt:Number):void { + } + + /** + * @param dt + */ + public function apply(dt:Number):void { + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/rigid/constraints/MaxDistanceConstraint.as b/0.0.3.0/src/alternativa/physics/rigid/constraints/MaxDistanceConstraint.as new file mode 100644 index 0000000..008d4a4 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/rigid/constraints/MaxDistanceConstraint.as @@ -0,0 +1,155 @@ +package alternativa.physics.rigid.constraints { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.BodyState; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class MaxDistanceConstraint extends Constraint { + // Первое тело ограничения + altphysics var body1:Body; + // Второе тело ограничения. Может быть null. + altphysics var body2:Body; + // Первая точка крепления в системе координат первого тела + altphysics var r1:Vector3; + // Вторая точка привязки в системе координат второго тела. В случае отсутствия второго тела, координаты задаются в мировой системе. + altphysics var r2:Vector3; + // Максимально допустимое расстояние между точками крепления + altphysics var maxDistance:Number; + // Радиус-вектор первой точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr1:Vector3 = new Vector3(); + // Радиус-вектор второй точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr2:Vector3 = new Vector3(); + // Минимальная скорость сближения для удовлетворения ограничения + private var minClosingVel:Number; + // Изменение относительной скорости на единицу импульса + private var velByUnitImpulseN:Number; + // Направление корректирующего импульса + private var impulseDirection:Vector3 = new Vector3(); + + // Вспомогательные переменные + private static var _v:Vector3 = new Vector3(); + private static var _v1:Vector3 = new Vector3(); + private static var _v2:Vector3 = new Vector3(); + + /** + * @param body1 + * @param body2 + * @param r1 + * @param r2 + * @param maxDistance + */ + public function MaxDistanceConstraint(body1:Body, body2:Body, r1:Vector3, r2:Vector3, maxDistance:Number) { + super(); + this.body1 = body1; + this.body2 = body2; + this.r1 = r1.vClone(); + this.r2 = r2.vClone(); + this.maxDistance = maxDistance; + } + + /** + * @param dt + */ + override public function preProcess(dt:Number):void { + // Вычислим расстояние между точками + body1.baseMatrix.transformVector(r1, wr1); + if (body2 != null) body2.baseMatrix.transformVector(r2, wr2); + else { + wr2.x = r2.x; + wr2.y = r2.y; + wr2.z = r2.z; + } + var p1:Vector3 = body1.state.pos; + impulseDirection.x = wr2.x - wr1.x - p1.x; + impulseDirection.y = wr2.y - wr1.y - p1.y; + impulseDirection.z = wr2.z - wr1.z - p1.z; + if (body2 != null) { + var p2:Vector3 = body2.state.pos; + impulseDirection.x += p2.x; + impulseDirection.y += p2.y; + impulseDirection.z += p2.z; + } + + var len:Number = Math.sqrt(impulseDirection.x*impulseDirection.x + impulseDirection.y*impulseDirection.y + impulseDirection.z*impulseDirection.z); + var delta:Number = len - maxDistance; + if (delta > 0) { + satisfied = false; + if (len == 0) impulseDirection.x = 1; + else { + impulseDirection.x /= len; + impulseDirection.y /= len; + impulseDirection.z /= len; + } + minClosingVel = delta/(world.penResolutionSteps*dt); + if (minClosingVel > world.maxPenResolutionSpeed) minClosingVel = world.maxPenResolutionSpeed; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + velByUnitImpulseN = 0; + if (body1.movable) { +// velByUnitImpulseN += body1.invMass + _v.vCross2(wr1, impulseDirection).vTransformBy3(body1.invInertiaWorld).vCross(wr1).vDot(impulseDirection); + _v.x = wr1.y*impulseDirection.z - wr1.z*impulseDirection.y; + _v.y = wr1.z*impulseDirection.x - wr1.x*impulseDirection.z; + _v.z = wr1.x*impulseDirection.y - wr1.y*impulseDirection.x; + _v.vTransformBy3(body1.invInertiaWorld); + _v1.x = _v.y*wr1.z - _v.z*wr1.y; + _v1.y = _v.z*wr1.x - _v.x*wr1.z; + _v1.z = _v.x*wr1.y - _v.y*wr1.x; + velByUnitImpulseN += body1.invMass + _v1.x*impulseDirection.x + _v1.y*impulseDirection.y + _v1.z*impulseDirection.z; + } + if (body2 != null && body2.movable) { +// velByUnitImpulseN += body2.invMass + _v.vCross2(wr2, impulseDirection).vTransformBy3(body2.invInertiaWorld).vCross(wr2).vDot(impulseDirection); + _v.x = wr2.y*impulseDirection.z - wr2.z*impulseDirection.y; + _v.y = wr2.z*impulseDirection.x - wr2.x*impulseDirection.z; + _v.z = wr2.x*impulseDirection.y - wr2.y*impulseDirection.x; + _v.vTransformBy3(body2.invInertiaWorld); + _v1.x = _v.y*wr2.z - _v.z*wr2.y; + _v1.y = _v.z*wr2.x - _v.x*wr2.z; + _v1.z = _v.x*wr2.y - _v.y*wr2.x; + velByUnitImpulseN += body2.invMass + _v1.x*impulseDirection.x + _v1.y*impulseDirection.y + _v1.z*impulseDirection.z; + } + } else satisfied = true; + } + + /** + * @param dt + */ + override public function apply(dt:Number):void { + if (satisfied) return; + // Расчитываем проекцию скорости относительной скорости + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 + var state:BodyState = body1.state; + var rot:Vector3 = state.rotation; +// _v1.vCross2(state.rotation, wr1); + _v1.x = rot.y*wr1.z - rot.z*wr1.y; + _v1.y = rot.z*wr1.x - rot.x*wr1.z; + _v1.z = rot.x*wr1.y - rot.y*wr1.x; + _v.x = state.velocity.x + _v1.x; + _v.y = state.velocity.y + _v1.y; + _v.z = state.velocity.z + _v1.z; + // V2 = V2_c + w2%r2 + if (body2 != null) { + state = body2.state; +// _v2.vCross2(state.rotation, wr2); + _v2.x = rot.y*wr2.z - rot.z*wr2.y; + _v2.y = rot.z*wr2.x - rot.x*wr2.z; + _v2.z = rot.x*wr2.y - rot.y*wr2.x; + _v.x -= state.velocity.x - _v2.x; + _v.y -= state.velocity.y - _v2.y; + _v.z -= state.velocity.z - _v2.z; + } + var closingVel:Number = _v.x*impulseDirection.x + _v.y*impulseDirection.y + _v.z*impulseDirection.z; + if (closingVel > minClosingVel) return; + var impulse:Number = (minClosingVel - closingVel)/velByUnitImpulseN; + if (body1.movable) body1.applyRelPosWorldImpulse(wr1, impulseDirection, impulse); + if (body2 != null && body2.movable) body2.applyRelPosWorldImpulse(wr2, impulseDirection, -impulse); + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/rigid/primitives/.svn/all-wcprops b/0.0.3.0/src/alternativa/physics/rigid/primitives/.svn/all-wcprops new file mode 100644 index 0000000..1af7f1d --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/rigid/primitives/.svn/all-wcprops @@ -0,0 +1,35 @@ +K 25 +svn:wc:ra_dav:version-url +V 120 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/rigid/primitives +END +RigidPlane.as +K 25 +svn:wc:ra_dav:version-url +V 134 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/rigid/primitives/RigidPlane.as +END +RigidSphere.as +K 25 +svn:wc:ra_dav:version-url +V 135 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/rigid/primitives/RigidSphere.as +END +RigidBox.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/rigid/primitives/RigidBox.as +END +RigidCylinder.as +K 25 +svn:wc:ra_dav:version-url +V 137 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/rigid/primitives/RigidCylinder.as +END +RigidRect.as +K 25 +svn:wc:ra_dav:version-url +V 133 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/rigid/primitives/RigidRect.as +END diff --git a/0.0.3.0/src/alternativa/physics/rigid/primitives/.svn/entries b/0.0.3.0/src/alternativa/physics/rigid/primitives/.svn/entries new file mode 100644 index 0000000..660340e --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/rigid/primitives/.svn/entries @@ -0,0 +1,88 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/rigid/primitives +http://svndev.alternativaplatform.com + + + +2009-06-08T06:46:18.310732Z +14099 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +RigidPlane.as +file + + + + +2010-10-28T04:32:41.000000Z +c4713421e357ee1967dca9ab3ffa7290 +2009-06-08T06:46:18.310732Z +14099 +mike + +RigidSphere.as +file + + + + +2010-10-28T04:32:41.000000Z +79ef38eab05657ad9627bdac4ade7271 +2009-04-23T13:43:38.903278Z +11779 +mike + +RigidBox.as +file + + + + +2010-10-28T04:32:41.000000Z +da834a5d208ff30f3a74321f75301cd7 +2009-06-08T06:46:18.310732Z +14099 +mike + +RigidCylinder.as +file + + + + +2010-10-28T04:32:41.000000Z +61512d31f9e211dcb0864164aa6f5210 +2009-04-23T13:43:38.903278Z +11779 +mike + +RigidRect.as +file + + + + +2010-10-28T04:32:41.000000Z +4cff4e313a1f41d4ccfa6480ea78173e +2009-04-23T13:43:38.903278Z +11779 +mike + diff --git a/0.0.3.0/src/alternativa/physics/rigid/primitives/.svn/format b/0.0.3.0/src/alternativa/physics/rigid/primitives/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/rigid/primitives/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.3.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidBox.as.svn-base b/0.0.3.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidBox.as.svn-base new file mode 100644 index 0000000..c01ff61 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidBox.as.svn-base @@ -0,0 +1,56 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class RigidBox extends Body { + + /** + * + * @param halfSize + * @param mass + */ + public function RigidBox(halfSize:Vector3, mass:Number) { + super(0, Matrix3.ZERO); + addCollisionPrimitive(new CollisionBox(halfSize, 1)); + setParams(halfSize, mass); + } + + /** + * @param halfSize + * @param mass + */ + public function setParams(halfSize:Vector3, mass:Number):void { + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + invInertia.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + invInertia.a = 3*invMass/(yy + zz); + invInertia.f = 3*invMass/(zz + xx); + invInertia.k = 3*invMass/(xx + yy); + } + var prim:CollisionBox = collisionPrimitives[0] as CollisionBox; + prim.hs.vCopy(halfSize); + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidCylinder.as.svn-base b/0.0.3.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidCylinder.as.svn-base new file mode 100644 index 0000000..0f9f2b3 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidCylinder.as.svn-base @@ -0,0 +1,32 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + + use namespace altphysics; + + public class RigidCylinder extends Body { + + altphysics var r:Number; + altphysics var h:Number; + + public function RigidCylinder(mass:Number, radius:Number, height:Number) { + super(0, Matrix3.ZERO); + setParams(mass, radius, height); + } + + public function setParams(mass:Number, radius:Number, height:Number):void { + r = radius; + h = height; + invInertiaWorld.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + invInertia.a = invInertia.f = mass*(h*h/12 + r*r/4); + invInertia.k = 0.5*mass*r*r; + invInertia.invert(); + } + } + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidPlane.as.svn-base b/0.0.3.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidPlane.as.svn-base new file mode 100644 index 0000000..26f74b2 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidPlane.as.svn-base @@ -0,0 +1,23 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + public class RigidPlane extends Body { + + public var normal:Vector3 = new Vector3(); + public var offset:Number; + + public function RigidPlane() { + super(0, Matrix3.ZERO); + this.normal.vCopy(Vector3.Z_AXIS); + this.offset = 0; + movable = false; + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidRect.as.svn-base b/0.0.3.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidRect.as.svn-base new file mode 100644 index 0000000..3c3913d --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidRect.as.svn-base @@ -0,0 +1,20 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + public class RigidRect extends Body { + + public function RigidRect(halfWidth:Number, halfLength:Number) { + super(0, Matrix3.ZERO); + movable = false; + addCollisionPrimitive(new CollisionRect(new Vector3(halfWidth, halfLength, 0), 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidSphere.as.svn-base b/0.0.3.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidSphere.as.svn-base new file mode 100644 index 0000000..9668716 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidSphere.as.svn-base @@ -0,0 +1,29 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + + use namespace altphysics; + + public class RigidSphere extends Body { + + public function RigidSphere(radius:Number, mass:Number) { + var m:Matrix3; + if (mass == Infinity) { + mass = 0; + m = Matrix3.ZERO; + } else { + mass = 1/mass; + var r2:Number = radius*radius; + m = new Matrix3(); + m.a = m.f = m.k = 2.5*mass/r2; + } + super(mass, m); + if (mass == 0) movable = false; + addCollisionPrimitive(new CollisionSphere(radius, 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/rigid/primitives/RigidBox.as b/0.0.3.0/src/alternativa/physics/rigid/primitives/RigidBox.as new file mode 100644 index 0000000..c01ff61 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/rigid/primitives/RigidBox.as @@ -0,0 +1,56 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class RigidBox extends Body { + + /** + * + * @param halfSize + * @param mass + */ + public function RigidBox(halfSize:Vector3, mass:Number) { + super(0, Matrix3.ZERO); + addCollisionPrimitive(new CollisionBox(halfSize, 1)); + setParams(halfSize, mass); + } + + /** + * @param halfSize + * @param mass + */ + public function setParams(halfSize:Vector3, mass:Number):void { + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + invInertia.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + invInertia.a = 3*invMass/(yy + zz); + invInertia.f = 3*invMass/(zz + xx); + invInertia.k = 3*invMass/(xx + yy); + } + var prim:CollisionBox = collisionPrimitives[0] as CollisionBox; + prim.hs.vCopy(halfSize); + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/rigid/primitives/RigidCylinder.as b/0.0.3.0/src/alternativa/physics/rigid/primitives/RigidCylinder.as new file mode 100644 index 0000000..0f9f2b3 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/rigid/primitives/RigidCylinder.as @@ -0,0 +1,32 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + + use namespace altphysics; + + public class RigidCylinder extends Body { + + altphysics var r:Number; + altphysics var h:Number; + + public function RigidCylinder(mass:Number, radius:Number, height:Number) { + super(0, Matrix3.ZERO); + setParams(mass, radius, height); + } + + public function setParams(mass:Number, radius:Number, height:Number):void { + r = radius; + h = height; + invInertiaWorld.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + invInertia.a = invInertia.f = mass*(h*h/12 + r*r/4); + invInertia.k = 0.5*mass*r*r; + invInertia.invert(); + } + } + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/rigid/primitives/RigidPlane.as b/0.0.3.0/src/alternativa/physics/rigid/primitives/RigidPlane.as new file mode 100644 index 0000000..26f74b2 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/rigid/primitives/RigidPlane.as @@ -0,0 +1,23 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + public class RigidPlane extends Body { + + public var normal:Vector3 = new Vector3(); + public var offset:Number; + + public function RigidPlane() { + super(0, Matrix3.ZERO); + this.normal.vCopy(Vector3.Z_AXIS); + this.offset = 0; + movable = false; + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/rigid/primitives/RigidRect.as b/0.0.3.0/src/alternativa/physics/rigid/primitives/RigidRect.as new file mode 100644 index 0000000..3c3913d --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/rigid/primitives/RigidRect.as @@ -0,0 +1,20 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + public class RigidRect extends Body { + + public function RigidRect(halfWidth:Number, halfLength:Number) { + super(0, Matrix3.ZERO); + movable = false; + addCollisionPrimitive(new CollisionRect(new Vector3(halfWidth, halfLength, 0), 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/rigid/primitives/RigidSphere.as b/0.0.3.0/src/alternativa/physics/rigid/primitives/RigidSphere.as new file mode 100644 index 0000000..9668716 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/rigid/primitives/RigidSphere.as @@ -0,0 +1,29 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + + use namespace altphysics; + + public class RigidSphere extends Body { + + public function RigidSphere(radius:Number, mass:Number) { + var m:Matrix3; + if (mass == Infinity) { + mass = 0; + m = Matrix3.ZERO; + } else { + mass = 1/mass; + var r2:Number = radius*radius; + m = new Matrix3(); + m.a = m.f = m.k = 2.5*mass/r2; + } + super(mass, m); + if (mass == 0) movable = false; + addCollisionPrimitive(new CollisionSphere(radius, 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/types/.svn/all-wcprops b/0.0.3.0/src/alternativa/physics/types/.svn/all-wcprops new file mode 100644 index 0000000..b384644 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/types/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 109 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/types +END +Quaternion.as +K 25 +svn:wc:ra_dav:version-url +V 123 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/types/Quaternion.as +END +Vector3.as +K 25 +svn:wc:ra_dav:version-url +V 120 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/types/Vector3.as +END +Matrix3.as +K 25 +svn:wc:ra_dav:version-url +V 120 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/types/Matrix3.as +END +Matrix4.as +K 25 +svn:wc:ra_dav:version-url +V 120 +/!svn/ver/15761/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/types/Matrix4.as +END diff --git a/0.0.3.0/src/alternativa/physics/types/.svn/entries b/0.0.3.0/src/alternativa/physics/types/.svn/entries new file mode 100644 index 0000000..66a0c4c --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/types/.svn/entries @@ -0,0 +1,76 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.3.0/src/alternativa/physics/types +http://svndev.alternativaplatform.com + + + +2009-06-25T09:18:57.805913Z +15117 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Quaternion.as +file + + + + +2010-10-28T04:32:40.000000Z +595f36fdfea8e82e752bb989e97e3a9a +2009-05-04T04:55:56.466789Z +12306 +mike + +Vector3.as +file + + + + +2010-10-28T04:32:40.000000Z +c8476935b1206abe041e8822204346cd +2009-06-25T09:18:57.805913Z +15117 +mike + +Matrix3.as +file + + + + +2010-10-28T04:32:40.000000Z +3fde68593cd3eb5a7046c441a76c9c5c +2009-06-25T09:18:57.805913Z +15117 +mike + +Matrix4.as +file + + + + +2010-10-28T04:32:40.000000Z +b2ed6277a2052c2568a1b4f19c469223 +2009-05-04T04:55:56.466789Z +12306 +mike + diff --git a/0.0.3.0/src/alternativa/physics/types/.svn/format b/0.0.3.0/src/alternativa/physics/types/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/types/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.3.0/src/alternativa/physics/types/.svn/text-base/Matrix3.as.svn-base b/0.0.3.0/src/alternativa/physics/types/.svn/text-base/Matrix3.as.svn-base new file mode 100644 index 0000000..e07d0d1 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/types/.svn/text-base/Matrix3.as.svn-base @@ -0,0 +1,396 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + /** + * + */ + public class Matrix3 { + + public static const ZERO:Matrix3 = new Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0); + public static const IDENTITY:Matrix3 = new Matrix3(); + + public var a:Number; + public var b:Number; + public var c:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix3(a:Number = 1, b:Number = 0, c:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1) { + this.a = a; + this.b = b; + this.c = c; + + this.e = e; + this.f = f; + this.g = g; + + this.i = i; + this.j = j; + this.k = k; + } + + /** + * + * @return + */ + public function toIdentity():Matrix3 { + a = f = k = 1; + b = c = e = g = i = j = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + var det:Number = 1/(-cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk); + + a = (ff*kk - gg*jj)*det; + b = (cc*jj - bb*kk)*det; + c = (bb*gg - cc*ff)*det; + e = (gg*ii - ee*kk)*det; + f = (aa*kk - cc*ii)*det; + g = (cc*ee - aa*gg)*det; + i = (ee*jj - ff*ii)*det; + j = (bb*ii - aa*jj)*det; + k = (aa*ff - bb*ee)*det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prependTransposed(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.b + cc*m.c; + b = aa*m.e + bb*m.f + cc*m.g; + c = aa*m.i + bb*m.j + cc*m.k; + e = ee*m.a + ff*m.b + gg*m.c; + f = ee*m.e + ff*m.f + gg*m.g; + g = ee*m.i + ff*m.j + gg*m.k; + i = ii*m.a + jj*m.b + kk*m.c; + j = ii*m.e + jj*m.f + kk*m.g; + k = ii*m.i + jj*m.j + kk*m.k; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix3):Matrix3 { + a += m.a; + b += m.b; + c += m.c; + e += m.e; + f += m.f; + g += m.g; + i += m.i; + j += m.j; + k += m.k; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix3):Matrix3 { + a -= m.a; + b -= m.b; + c -= m.c; + e -= m.e; + f -= m.f; + g -= m.g; + i -= m.i; + j -= m.j; + k -= m.k; + + return this; + } + + /** + * + * @return + */ + public function transpose():Matrix3 { + var tmp:Number = b; + b = e; + e = tmp; + tmp = c; + c = i; + i = tmp; + tmp = g; + g = j; + j = tmp; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector3To3D(vin:Vector3, vout:Vector3D):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * + * @param matrix + */ + public function createSkewSymmetric(v:Vector3):Matrix3 { + a = f = k = 0; + b = -v.z; + c = v.y; + e = v.z; + g = -v.x; + i = -v.y; + j = v.x; + return this; + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(i:int, axis:Vector3):void { + switch (i) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + } + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix3):Matrix3 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @return + */ + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):Matrix3 { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + + return this; + } + + /** + * Метод формирует матрицу поворота на заданный угол относительно заданной оси. + * + * @param axis нормализованный вектор, задающий ось, относительно которой выполняется поворот + * @param angle угол поворота в радианах + */ + public function fromAxisAngle(axis:Vector3, angle:Number):void { + var c1:Number = Math.cos(angle); + var s:Number = Math.sin(angle); + var t:Number = 1 - c1; + var x:Number = axis.x; + var y:Number = axis.y; + var z:Number = axis.z; + + a = t*x*x + c1; + b = t*x*y - z*s; + c = t*x*z + y*s; + + e = t*x*y + z*s; + f = t*y*y + c1; + g = t*y*z - x*s; + + i = t*x*z - y*s; + j = t*y*z + x*s; + k = t*z*z + c1; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix3 { + return new Matrix3(a, b, c, e, f, g, i, j, k); + } + + public function toString():String { + return "[Matrix3 (" + a + ", " + b + ", " + c + "), (" + e + ", " + f + ", " + g + "), (" + i + ", " + j + ", " + k + ")]"; + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/types/.svn/text-base/Matrix4.as.svn-base b/0.0.3.0/src/alternativa/physics/types/.svn/text-base/Matrix4.as.svn-base new file mode 100644 index 0000000..8cc23b2 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/types/.svn/text-base/Matrix4.as.svn-base @@ -0,0 +1,402 @@ +package alternativa.physics.types { + import __AS3__.vec.Vector; + + /** + * + */ + public class Matrix4 { + + public var a:Number; + public var b:Number; + public var c:Number; + public var d:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + public var h:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + public var l:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix4(a:Number = 1, b:Number = 0, c:Number = 0, d:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, h:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1, l:Number = 0) { + this.a = a; + this.b = b; + this.c = c; + this.d = h; + + this.e = e; + this.f = f; + this.g = g; + this.h = h; + + this.i = i; + this.j = j; + this.k = k; + this.l = l; + } + + /** + * + * @return + */ + public function toIdentity():Matrix4 { + a = f = k = 1; + b = c = e = g = i = j = d = h = l = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + var det:Number = -cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk; + + a = (-gg*jj + ff*kk)/det; + b = (cc*jj - bb*kk)/det; + c = (-cc*ff + bb*gg)/det; + d = (dd*gg*jj - cc*hh*jj - dd*ff*kk + bb*hh*kk + cc*ff*ll - bb*gg*ll)/det; + e = (gg*ii - ee*kk)/det; + f = (-cc*ii + aa*kk)/det; + g = (cc*ee - aa*gg)/det; + h = (cc*hh*ii - dd*gg*ii + dd*ee*kk - aa*hh*kk - cc*ee*ll + aa*gg*ll)/det; + i = (-ff*ii + ee*jj)/det; + j = (bb*ii - aa*jj)/det; + k = (-bb*ee + aa*ff)/det; + l = (dd*ff*ii - bb*hh*ii - dd*ee*jj + aa*hh*jj + bb*ee*ll - aa*ff*ll)/det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + d = m.a*dd + m.b*hh + m.c*ll + m.d; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + h = m.e*dd + m.f*hh + m.g*ll + m.h; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + l = m.i*dd + m.j*hh + m.k*ll + m.l; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + d = aa*m.d + bb*m.h + cc*m.l + dd; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + h = ee*m.d + ff*m.h + gg*m.l + hh; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + l = ii*m.d + jj*m.h + kk*m.l + ll; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix4):Matrix4 { + a += m.a; + b += m.b; + c += m.c; + d += m.d; + e += m.e; + f += m.f; + g += m.g; + h += m.h; + i += m.i; + j += m.j; + k += m.k; + l += m.l; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix4):Matrix4 { + a -= m.a; + b -= m.b; + c -= m.c; + d -= m.d; + e -= m.e; + f -= m.f; + g -= m.g; + h -= m.h; + i -= m.i; + j -= m.j; + k -= m.k; + l -= m.l; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectors(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverse(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(idx:int, axis:Vector3):void { + switch (idx) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + case 3: + axis.x = d; + axis.y = h; + axis.z = l; + return; + } + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix4):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = m.d; + e = m.e; + f = m.f; + g = m.g; + h = m.h; + i = m.i; + j = m.j; + k = m.k; + l = m.l; + + return this; + } + + public function setFromMatrix3(m:Matrix3, offset:Vector3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = offset.x; + e = m.e; + f = m.f; + g = m.g; + h = offset.y; + i = m.i; + j = m.j; + k = m.k; + l = offset.z; + + return this; + } + + /** + * @param rotations + */ + public function getEulerAngles(rotations:Vector3):void { + if (-1 < i && i < 1) { + rotations.x = Math.atan2(j, k); + rotations.y = -Math.asin(i); + rotations.z = Math.atan2(e, a); + } else { + rotations.x = 0; + rotations.y = 0.5*((i <= -1) ? Math.PI : -Math.PI); + rotations.z = Math.atan2(-b, f); + } + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix4 { + return new Matrix4(a, b, c, d, e, f, g, h, i, j, k, l); + } + + public function toString():String { + return "[Matrix4 [" + a.toFixed(3) + " " + b.toFixed(3) + " " + c.toFixed(3) + " " + d.toFixed(3) + "] [" + e.toFixed(3) + " " + f.toFixed(3) + " " + g.toFixed(3) + " " + h.toFixed(3) + "] [" + i.toFixed(3) + " " + j.toFixed(3) + " " + k.toFixed(3) + " " + l.toFixed(3) + "]]"; + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/types/.svn/text-base/Quaternion.as.svn-base b/0.0.3.0/src/alternativa/physics/types/.svn/text-base/Quaternion.as.svn-base new file mode 100644 index 0000000..7ad2af0 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/types/.svn/text-base/Quaternion.as.svn-base @@ -0,0 +1,396 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + public class Quaternion { + + public var w:Number; + public var x:Number; + public var y:Number; + public var z:Number; + + public static function multiply(q1:Quaternion, q2:Quaternion, result:Quaternion):void { + result.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z; + result.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y; + result.y = q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z; + result.z = q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x; + } + + public static function createFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngle(axis, angle); + return q; + } + + public static function createFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngleComponents(x, y, z, angle); + return q; + } + + public function Quaternion(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0) { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + } + + public function reset(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0):Quaternion { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + return this; + } + + public function normalize():Quaternion { + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * Умножает на указанный кватернион слева: this * q + * + * @param q множитель + */ + public function prepend(q:Quaternion):Quaternion { + var ww:Number = w*q.w - x*q.x - y*q.y - z*q.z; + var xx:Number = w*q.x + x*q.w + y*q.z - z*q.y; + var yy:Number = w*q.y + y*q.w + z*q.x - x*q.z; + var zz:Number = w*q.z + z*q.w + x*q.y - y*q.x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Умножает на указанный кватернион справа: q * this + * + * @param q множитель + */ + public function append(q:Quaternion):Quaternion { + var ww:Number = q.w*w - q.x*x - q.y*y - q.z*z; + var xx:Number = q.w*x + q.x*w + q.y*z - q.z*y; + var yy:Number = q.w*y + q.y*w + q.z*x - q.x*z; + var zz:Number = q.w*z + q.z*w + q.x*y - q.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * + * @param vector + */ + public function rotateByVector(v:Vector3):Quaternion { + var ww:Number = -v.x*x - v.y*y - v.z*z; + var xx:Number = v.x*w + v.y*z - v.z*y; + var yy:Number = v.y*w + v.z*x - v.x*z; + var zz:Number = v.z*w + v.x*y - v.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Добавляет вращение, приданное вектором угловой скорости за указанное время. + * + * @param v + * @param scale + */ + public function addScaledVector(v:Vector3, scale:Number):Quaternion { + var vx:Number = v.x*scale; + var vy:Number = v.y*scale; + var vz:Number = v.z*scale; + var ww:Number = -x*vx - y*vy - z*vz; + var xx:Number = vx*w + vy*z - vz*y; + var yy:Number = vy*w + vz*x - vx*z; + var zz:Number = vz*w + vx*y - vy*x; + w += 0.5*ww; + x += 0.5*xx; + y += 0.5*yy; + z += 0.5*zz; + // inlined normalize + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param m + * @return + */ + public function toMatrix3(m:Matrix3):Quaternion { + var xx2:Number = 2*x*x; + var yy2:Number = 2*y*y; + var zz2:Number = 2*z*z; + var xy2:Number = 2*x*y; + var yz2:Number = 2*y*z; + var zx2:Number = 2*z*x; + var wx2:Number = 2*w*x; + var wy2:Number = 2*w*y; + var wz2:Number = 2*w*z; + + m.a = 1 - yy2 - zz2; + m.b = xy2 - wz2; + m.c = zx2 + wy2; + + m.e = xy2 + wz2; + m.f = 1 - xx2 - zz2; + m.g = yz2 - wx2; + + m.i = zx2 - wy2; + m.j = yz2 + wx2; + m.k = 1 - xx2 - yy2; + return this; + } + + /** + * + * @return + */ + public function length():Number { + return Math.sqrt(w*w + x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function lengthSqr():Number { + return w*w + x*x + y*y + z*z; + } + + /** + * + * @param axis + * @param angle + * @return + */ + public function setFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z); + x = axis.x*k; + y = axis.y*k; + z = axis.z*k; + return this; + } + + /** + * + * @param x + * @param y + * @param z + * @param angle + */ + public function setFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(x*x + y*y + z*z); + this.x = x*k; + this.y = y*k; + this.z = z*k; + return this; + } + + /** + * + * @param vector + */ + public function toAxisVector(v:Vector3 = null):Vector3 { + if (w < -1 || w > 1) { + normalize(); + } + if (v == null) { + v = new Vector3(); + } + if (w > -1 && w < 1) { + if (w == 0) { + v.x = x; + v.y = y; + v.z = z; + } else { + var angle:Number = 2*Math.acos(w); + var coeff:Number = 1/Math.sqrt(1 - w*w); + v.x = x*coeff*angle; + v.y = y*coeff*angle; + v.z = z*coeff*angle; + } + } else { + v.x = 0; + v.y = 0; + v.z = 0; + } + return v; + } + + /** + * + * @param rotations + */ + public function getEulerAngles(angles:Vector3):Vector3 { + var qi2:Number = 2*x*x; + var qj2:Number = 2*y*y; + var qk2:Number = 2*z*z; + var qij:Number = 2*x*y; + var qjk:Number = 2*y*z; + var qki:Number = 2*z*x; + var qri:Number = 2*w*x; + var qrj:Number = 2*w*y; + var qrk:Number = 2*w*z; + + var aa:Number = 1 - qj2 - qk2; + var bb:Number = qij - qrk; + var ee:Number = qij + qrk; + var ff:Number = 1 - qi2 - qk2; + var ii:Number = qki - qrj; + var jj:Number = qjk + qri; + var kk:Number = 1 - qi2 - qj2; + + if (-1 < ii && ii < 1) { + if (angles == null) { + angles = new Vector3(Math.atan2(jj, kk), -Math.asin(ii), Math.atan2(ee, aa)); + } else { + angles.x = Math.atan2(jj, kk); + angles.y = -Math.asin(ii); + angles.z = Math.atan2(ee, aa); + } + } else { + if (angles == null) { + angles = new Vector3(0, 0.5*((ii <= -1) ? Math.PI : -Math.PI), Math.atan2(-bb, ff)); + } else { + angles.x = 0; + angles.y = 0.5*((ii <= -1) ? Math.PI : -Math.PI); + angles.z = Math.atan2(-bb, ff); + } + } + return angles; + } + + /** + * + */ + public function conjugate():void { + x = -x; + y = -y; + z = -z; + } + + /** + * Выполняет линейную интерполяцию. + * + * @param q1 начало отрезка + * @param q2 конец отрезка + * @param t время, обычно задаётся в интервале [0, 1] + */ + public function nlerp(q1:Quaternion, q2:Quaternion, t:Number):Quaternion { + var d:Number = 1 - t; + w = q1.w*d + q2.w*t; + x = q1.x*d + q2.x*t; + y = q1.y*d + q2.y*t; + z = q1.z*d + q2.z*t; + // inlined normalize + d = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param q + * @return + */ + public function subtract(q:Quaternion):Quaternion { + w -= q.w; + x -= q.x; + y -= q.y; + z -= q.z; + return this; + } + + /** + * + * @param q1 + * @param q2 + * @return + */ + public function diff(q1:Quaternion, q2:Quaternion):Quaternion { + w = q2.w - q1.w; + x = q2.x - q1.x; + y = q2.y - q1.y; + z = q2.z - q1.z; + return this; + } + + /** + * + * @param q + * @return + */ + public function copy(q:Quaternion):Quaternion { + w = q.w; + x = q.x; + y = q.y; + z = q.z; + return this; + } + + /** + * @param reslult + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + result.w = w; + return result; + } + + /** + * + * @return + */ + public function clone():Quaternion { + return new Quaternion(w, x, y, z); + } + + /** + * + * @return + */ + public function toString():String { + return "[" + w + ", " + x + ", " + y + ", " + z + "]"; + } + } +} diff --git a/0.0.3.0/src/alternativa/physics/types/.svn/text-base/Vector3.as.svn-base b/0.0.3.0/src/alternativa/physics/types/.svn/text-base/Vector3.as.svn-base new file mode 100644 index 0000000..13ffda0 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/types/.svn/text-base/Vector3.as.svn-base @@ -0,0 +1,331 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + /** + * + */ + public class Vector3 extends Vector3D { + + public static const ZERO:Vector3 = new Vector3(0, 0, 0); + public static const X_AXIS:Vector3 = new Vector3(1, 0, 0); + public static const Y_AXIS:Vector3 = new Vector3(0, 1, 0); + public static const Z_AXIS:Vector3 = new Vector3(0, 0, 1); + + /** + * + * @param x + * @param y + * @param z + */ + public function Vector3(x:Number = 0, y:Number = 0, z:Number = 0) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * + * @return + */ + public function vLength():Number { + return Math.sqrt(x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function vLengthSqr():Number { + return x*x + y*y + z*z; + } + + /** + * + * @param length + * @return + */ + public function vSetLength(length:Number):Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = length; + } else { + var k:Number = length/Math.sqrt(x*x + y*y + z*z); + x *= k; + y *= k; + z *= k; + } + return this; + } + + /** + * + * @return + */ + public function vNormalize():Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = 1; + } else { + d = Math.sqrt(d); + x /= d; + y /= d; + z /= d; + } + return this; + } + + /** + * Сложение координат. + * + * @param v точка, координаты которой прибавляются к собственным + */ + public function vAdd(v:Vector3):Vector3 { + x += v.x; + y += v.y; + z += v.z; + return this; + } + + /** + * + * @param k + * @param v + * @return + */ + public function vAddScaled(k:Number, v:Vector3):Vector3 { + x += k*v.x; + y += k*v.y; + z += k*v.z; + return this; + } + + /** + * Вычитание координат. + * + * @param v точка, координаты которой вычитаются из собственных + */ + public function vSubtract(v:Vector3):Vector3 { + x -= v.x; + y -= v.y; + z -= v.z; + return this; + } + + /** + * Вычисляет сумму векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + * @return this + */ + public function vSum(a:Vector3, b:Vector3):Vector3 { + x = a.x + b.x; + y = a.y + b.y; + z = a.z + b.z; + return this; + } + + /** + * Вычисление разности векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + */ + public function vDiff(a:Vector3, b:Vector3):Vector3 { + x = a.x - b.x; + y = a.y - b.y; + z = a.z - b.z; + return this; + } + + /** + * Умножение на скаляр. + * + * @param k число, на которое умножаются координаты + */ + public function vScale(k:Number):Vector3 { + x *= k; + y *= k; + z *= k; + return this; + } + + /** + * Инвертирование вектора. + */ + public function vReverse():Vector3 { + x = -x; + y = -y; + z = -z; + return this; + } + + /** + * + * @param v + * @return + */ + public function vDot(v:Vector3):Number { + return x*v.x + y*v.y + z*v.z; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross(v:Vector3):Vector3 { + var xx:Number = y*v.z - z*v.y; + var yy:Number = z*v.x - x*v.z; + var zz:Number = x*v.y - y*v.x; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross2(a:Vector3, b:Vector3):Vector3 { + x = a.y*b.z - a.z*b.y; + y = a.z*b.x - a.x*b.z; + z = a.x*b.y - a.y*b.x; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy3(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * + * @param matrix + */ + public function vTransformBy3Tr(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz + m.d; + y = m.e*xx + m.f*yy + m.g*zz + m.h; + z = m.i*xx + m.j*yy + m.k*zz + m.l; + return this; + } + + /** + * + * @param m + * @return + */ + public function vTransformInverseBy4(m:Matrix4):Vector3 { + var xx:Number = x - m.d; + var yy:Number = y - m.h; + var zz:Number = z - m.l; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * + * @param m + */ + public function vDeltaTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * Установка координат. + */ + public function vReset(x:Number = 0, y:Number = 0, z:Number = 0):Vector3 { + this.x = x; + this.y = y; + this.z = z; + return this; + } + + /** + * Копирование координат точки. + * + * @param v точка, координаты которой копируются + */ + public function vCopy(v:Vector3):Vector3 { + x = v.x; + y = v.y; + z = v.z; + return this; + } + + /** + * Клонирование точки. + * + * @return клонированная точка + */ + public function vClone():Vector3 { + return new Vector3(x, y, z); + } + + /** + * Копирует координаты вектора в экземпляр класса Vector3D. + * + * @param result экземпляр, в который копируются координаты + * @return переданный параметр + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + return result; + } + + /** + * @param source + * @return + */ + public function copyFromVector3D(source:Vector3D):Vector3 { + x = source.x; + y = source.y; + z = source.z; + return this; + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/types/Matrix3.as b/0.0.3.0/src/alternativa/physics/types/Matrix3.as new file mode 100644 index 0000000..e07d0d1 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/types/Matrix3.as @@ -0,0 +1,396 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + /** + * + */ + public class Matrix3 { + + public static const ZERO:Matrix3 = new Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0); + public static const IDENTITY:Matrix3 = new Matrix3(); + + public var a:Number; + public var b:Number; + public var c:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix3(a:Number = 1, b:Number = 0, c:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1) { + this.a = a; + this.b = b; + this.c = c; + + this.e = e; + this.f = f; + this.g = g; + + this.i = i; + this.j = j; + this.k = k; + } + + /** + * + * @return + */ + public function toIdentity():Matrix3 { + a = f = k = 1; + b = c = e = g = i = j = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + var det:Number = 1/(-cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk); + + a = (ff*kk - gg*jj)*det; + b = (cc*jj - bb*kk)*det; + c = (bb*gg - cc*ff)*det; + e = (gg*ii - ee*kk)*det; + f = (aa*kk - cc*ii)*det; + g = (cc*ee - aa*gg)*det; + i = (ee*jj - ff*ii)*det; + j = (bb*ii - aa*jj)*det; + k = (aa*ff - bb*ee)*det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prependTransposed(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.b + cc*m.c; + b = aa*m.e + bb*m.f + cc*m.g; + c = aa*m.i + bb*m.j + cc*m.k; + e = ee*m.a + ff*m.b + gg*m.c; + f = ee*m.e + ff*m.f + gg*m.g; + g = ee*m.i + ff*m.j + gg*m.k; + i = ii*m.a + jj*m.b + kk*m.c; + j = ii*m.e + jj*m.f + kk*m.g; + k = ii*m.i + jj*m.j + kk*m.k; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix3):Matrix3 { + a += m.a; + b += m.b; + c += m.c; + e += m.e; + f += m.f; + g += m.g; + i += m.i; + j += m.j; + k += m.k; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix3):Matrix3 { + a -= m.a; + b -= m.b; + c -= m.c; + e -= m.e; + f -= m.f; + g -= m.g; + i -= m.i; + j -= m.j; + k -= m.k; + + return this; + } + + /** + * + * @return + */ + public function transpose():Matrix3 { + var tmp:Number = b; + b = e; + e = tmp; + tmp = c; + c = i; + i = tmp; + tmp = g; + g = j; + j = tmp; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector3To3D(vin:Vector3, vout:Vector3D):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * + * @param matrix + */ + public function createSkewSymmetric(v:Vector3):Matrix3 { + a = f = k = 0; + b = -v.z; + c = v.y; + e = v.z; + g = -v.x; + i = -v.y; + j = v.x; + return this; + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(i:int, axis:Vector3):void { + switch (i) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + } + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix3):Matrix3 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @return + */ + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):Matrix3 { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + + return this; + } + + /** + * Метод формирует матрицу поворота на заданный угол относительно заданной оси. + * + * @param axis нормализованный вектор, задающий ось, относительно которой выполняется поворот + * @param angle угол поворота в радианах + */ + public function fromAxisAngle(axis:Vector3, angle:Number):void { + var c1:Number = Math.cos(angle); + var s:Number = Math.sin(angle); + var t:Number = 1 - c1; + var x:Number = axis.x; + var y:Number = axis.y; + var z:Number = axis.z; + + a = t*x*x + c1; + b = t*x*y - z*s; + c = t*x*z + y*s; + + e = t*x*y + z*s; + f = t*y*y + c1; + g = t*y*z - x*s; + + i = t*x*z - y*s; + j = t*y*z + x*s; + k = t*z*z + c1; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix3 { + return new Matrix3(a, b, c, e, f, g, i, j, k); + } + + public function toString():String { + return "[Matrix3 (" + a + ", " + b + ", " + c + "), (" + e + ", " + f + ", " + g + "), (" + i + ", " + j + ", " + k + ")]"; + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/types/Matrix4.as b/0.0.3.0/src/alternativa/physics/types/Matrix4.as new file mode 100644 index 0000000..8cc23b2 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/types/Matrix4.as @@ -0,0 +1,402 @@ +package alternativa.physics.types { + import __AS3__.vec.Vector; + + /** + * + */ + public class Matrix4 { + + public var a:Number; + public var b:Number; + public var c:Number; + public var d:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + public var h:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + public var l:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix4(a:Number = 1, b:Number = 0, c:Number = 0, d:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, h:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1, l:Number = 0) { + this.a = a; + this.b = b; + this.c = c; + this.d = h; + + this.e = e; + this.f = f; + this.g = g; + this.h = h; + + this.i = i; + this.j = j; + this.k = k; + this.l = l; + } + + /** + * + * @return + */ + public function toIdentity():Matrix4 { + a = f = k = 1; + b = c = e = g = i = j = d = h = l = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + var det:Number = -cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk; + + a = (-gg*jj + ff*kk)/det; + b = (cc*jj - bb*kk)/det; + c = (-cc*ff + bb*gg)/det; + d = (dd*gg*jj - cc*hh*jj - dd*ff*kk + bb*hh*kk + cc*ff*ll - bb*gg*ll)/det; + e = (gg*ii - ee*kk)/det; + f = (-cc*ii + aa*kk)/det; + g = (cc*ee - aa*gg)/det; + h = (cc*hh*ii - dd*gg*ii + dd*ee*kk - aa*hh*kk - cc*ee*ll + aa*gg*ll)/det; + i = (-ff*ii + ee*jj)/det; + j = (bb*ii - aa*jj)/det; + k = (-bb*ee + aa*ff)/det; + l = (dd*ff*ii - bb*hh*ii - dd*ee*jj + aa*hh*jj + bb*ee*ll - aa*ff*ll)/det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + d = m.a*dd + m.b*hh + m.c*ll + m.d; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + h = m.e*dd + m.f*hh + m.g*ll + m.h; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + l = m.i*dd + m.j*hh + m.k*ll + m.l; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + d = aa*m.d + bb*m.h + cc*m.l + dd; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + h = ee*m.d + ff*m.h + gg*m.l + hh; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + l = ii*m.d + jj*m.h + kk*m.l + ll; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix4):Matrix4 { + a += m.a; + b += m.b; + c += m.c; + d += m.d; + e += m.e; + f += m.f; + g += m.g; + h += m.h; + i += m.i; + j += m.j; + k += m.k; + l += m.l; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix4):Matrix4 { + a -= m.a; + b -= m.b; + c -= m.c; + d -= m.d; + e -= m.e; + f -= m.f; + g -= m.g; + h -= m.h; + i -= m.i; + j -= m.j; + k -= m.k; + l -= m.l; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectors(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverse(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(idx:int, axis:Vector3):void { + switch (idx) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + case 3: + axis.x = d; + axis.y = h; + axis.z = l; + return; + } + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix4):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = m.d; + e = m.e; + f = m.f; + g = m.g; + h = m.h; + i = m.i; + j = m.j; + k = m.k; + l = m.l; + + return this; + } + + public function setFromMatrix3(m:Matrix3, offset:Vector3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = offset.x; + e = m.e; + f = m.f; + g = m.g; + h = offset.y; + i = m.i; + j = m.j; + k = m.k; + l = offset.z; + + return this; + } + + /** + * @param rotations + */ + public function getEulerAngles(rotations:Vector3):void { + if (-1 < i && i < 1) { + rotations.x = Math.atan2(j, k); + rotations.y = -Math.asin(i); + rotations.z = Math.atan2(e, a); + } else { + rotations.x = 0; + rotations.y = 0.5*((i <= -1) ? Math.PI : -Math.PI); + rotations.z = Math.atan2(-b, f); + } + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix4 { + return new Matrix4(a, b, c, d, e, f, g, h, i, j, k, l); + } + + public function toString():String { + return "[Matrix4 [" + a.toFixed(3) + " " + b.toFixed(3) + " " + c.toFixed(3) + " " + d.toFixed(3) + "] [" + e.toFixed(3) + " " + f.toFixed(3) + " " + g.toFixed(3) + " " + h.toFixed(3) + "] [" + i.toFixed(3) + " " + j.toFixed(3) + " " + k.toFixed(3) + " " + l.toFixed(3) + "]]"; + } + + } +} \ No newline at end of file diff --git a/0.0.3.0/src/alternativa/physics/types/Quaternion.as b/0.0.3.0/src/alternativa/physics/types/Quaternion.as new file mode 100644 index 0000000..7ad2af0 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/types/Quaternion.as @@ -0,0 +1,396 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + public class Quaternion { + + public var w:Number; + public var x:Number; + public var y:Number; + public var z:Number; + + public static function multiply(q1:Quaternion, q2:Quaternion, result:Quaternion):void { + result.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z; + result.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y; + result.y = q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z; + result.z = q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x; + } + + public static function createFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngle(axis, angle); + return q; + } + + public static function createFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngleComponents(x, y, z, angle); + return q; + } + + public function Quaternion(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0) { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + } + + public function reset(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0):Quaternion { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + return this; + } + + public function normalize():Quaternion { + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * Умножает на указанный кватернион слева: this * q + * + * @param q множитель + */ + public function prepend(q:Quaternion):Quaternion { + var ww:Number = w*q.w - x*q.x - y*q.y - z*q.z; + var xx:Number = w*q.x + x*q.w + y*q.z - z*q.y; + var yy:Number = w*q.y + y*q.w + z*q.x - x*q.z; + var zz:Number = w*q.z + z*q.w + x*q.y - y*q.x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Умножает на указанный кватернион справа: q * this + * + * @param q множитель + */ + public function append(q:Quaternion):Quaternion { + var ww:Number = q.w*w - q.x*x - q.y*y - q.z*z; + var xx:Number = q.w*x + q.x*w + q.y*z - q.z*y; + var yy:Number = q.w*y + q.y*w + q.z*x - q.x*z; + var zz:Number = q.w*z + q.z*w + q.x*y - q.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * + * @param vector + */ + public function rotateByVector(v:Vector3):Quaternion { + var ww:Number = -v.x*x - v.y*y - v.z*z; + var xx:Number = v.x*w + v.y*z - v.z*y; + var yy:Number = v.y*w + v.z*x - v.x*z; + var zz:Number = v.z*w + v.x*y - v.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Добавляет вращение, приданное вектором угловой скорости за указанное время. + * + * @param v + * @param scale + */ + public function addScaledVector(v:Vector3, scale:Number):Quaternion { + var vx:Number = v.x*scale; + var vy:Number = v.y*scale; + var vz:Number = v.z*scale; + var ww:Number = -x*vx - y*vy - z*vz; + var xx:Number = vx*w + vy*z - vz*y; + var yy:Number = vy*w + vz*x - vx*z; + var zz:Number = vz*w + vx*y - vy*x; + w += 0.5*ww; + x += 0.5*xx; + y += 0.5*yy; + z += 0.5*zz; + // inlined normalize + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param m + * @return + */ + public function toMatrix3(m:Matrix3):Quaternion { + var xx2:Number = 2*x*x; + var yy2:Number = 2*y*y; + var zz2:Number = 2*z*z; + var xy2:Number = 2*x*y; + var yz2:Number = 2*y*z; + var zx2:Number = 2*z*x; + var wx2:Number = 2*w*x; + var wy2:Number = 2*w*y; + var wz2:Number = 2*w*z; + + m.a = 1 - yy2 - zz2; + m.b = xy2 - wz2; + m.c = zx2 + wy2; + + m.e = xy2 + wz2; + m.f = 1 - xx2 - zz2; + m.g = yz2 - wx2; + + m.i = zx2 - wy2; + m.j = yz2 + wx2; + m.k = 1 - xx2 - yy2; + return this; + } + + /** + * + * @return + */ + public function length():Number { + return Math.sqrt(w*w + x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function lengthSqr():Number { + return w*w + x*x + y*y + z*z; + } + + /** + * + * @param axis + * @param angle + * @return + */ + public function setFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z); + x = axis.x*k; + y = axis.y*k; + z = axis.z*k; + return this; + } + + /** + * + * @param x + * @param y + * @param z + * @param angle + */ + public function setFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(x*x + y*y + z*z); + this.x = x*k; + this.y = y*k; + this.z = z*k; + return this; + } + + /** + * + * @param vector + */ + public function toAxisVector(v:Vector3 = null):Vector3 { + if (w < -1 || w > 1) { + normalize(); + } + if (v == null) { + v = new Vector3(); + } + if (w > -1 && w < 1) { + if (w == 0) { + v.x = x; + v.y = y; + v.z = z; + } else { + var angle:Number = 2*Math.acos(w); + var coeff:Number = 1/Math.sqrt(1 - w*w); + v.x = x*coeff*angle; + v.y = y*coeff*angle; + v.z = z*coeff*angle; + } + } else { + v.x = 0; + v.y = 0; + v.z = 0; + } + return v; + } + + /** + * + * @param rotations + */ + public function getEulerAngles(angles:Vector3):Vector3 { + var qi2:Number = 2*x*x; + var qj2:Number = 2*y*y; + var qk2:Number = 2*z*z; + var qij:Number = 2*x*y; + var qjk:Number = 2*y*z; + var qki:Number = 2*z*x; + var qri:Number = 2*w*x; + var qrj:Number = 2*w*y; + var qrk:Number = 2*w*z; + + var aa:Number = 1 - qj2 - qk2; + var bb:Number = qij - qrk; + var ee:Number = qij + qrk; + var ff:Number = 1 - qi2 - qk2; + var ii:Number = qki - qrj; + var jj:Number = qjk + qri; + var kk:Number = 1 - qi2 - qj2; + + if (-1 < ii && ii < 1) { + if (angles == null) { + angles = new Vector3(Math.atan2(jj, kk), -Math.asin(ii), Math.atan2(ee, aa)); + } else { + angles.x = Math.atan2(jj, kk); + angles.y = -Math.asin(ii); + angles.z = Math.atan2(ee, aa); + } + } else { + if (angles == null) { + angles = new Vector3(0, 0.5*((ii <= -1) ? Math.PI : -Math.PI), Math.atan2(-bb, ff)); + } else { + angles.x = 0; + angles.y = 0.5*((ii <= -1) ? Math.PI : -Math.PI); + angles.z = Math.atan2(-bb, ff); + } + } + return angles; + } + + /** + * + */ + public function conjugate():void { + x = -x; + y = -y; + z = -z; + } + + /** + * Выполняет линейную интерполяцию. + * + * @param q1 начало отрезка + * @param q2 конец отрезка + * @param t время, обычно задаётся в интервале [0, 1] + */ + public function nlerp(q1:Quaternion, q2:Quaternion, t:Number):Quaternion { + var d:Number = 1 - t; + w = q1.w*d + q2.w*t; + x = q1.x*d + q2.x*t; + y = q1.y*d + q2.y*t; + z = q1.z*d + q2.z*t; + // inlined normalize + d = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param q + * @return + */ + public function subtract(q:Quaternion):Quaternion { + w -= q.w; + x -= q.x; + y -= q.y; + z -= q.z; + return this; + } + + /** + * + * @param q1 + * @param q2 + * @return + */ + public function diff(q1:Quaternion, q2:Quaternion):Quaternion { + w = q2.w - q1.w; + x = q2.x - q1.x; + y = q2.y - q1.y; + z = q2.z - q1.z; + return this; + } + + /** + * + * @param q + * @return + */ + public function copy(q:Quaternion):Quaternion { + w = q.w; + x = q.x; + y = q.y; + z = q.z; + return this; + } + + /** + * @param reslult + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + result.w = w; + return result; + } + + /** + * + * @return + */ + public function clone():Quaternion { + return new Quaternion(w, x, y, z); + } + + /** + * + * @return + */ + public function toString():String { + return "[" + w + ", " + x + ", " + y + ", " + z + "]"; + } + } +} diff --git a/0.0.3.0/src/alternativa/physics/types/Vector3.as b/0.0.3.0/src/alternativa/physics/types/Vector3.as new file mode 100644 index 0000000..13ffda0 --- /dev/null +++ b/0.0.3.0/src/alternativa/physics/types/Vector3.as @@ -0,0 +1,331 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + /** + * + */ + public class Vector3 extends Vector3D { + + public static const ZERO:Vector3 = new Vector3(0, 0, 0); + public static const X_AXIS:Vector3 = new Vector3(1, 0, 0); + public static const Y_AXIS:Vector3 = new Vector3(0, 1, 0); + public static const Z_AXIS:Vector3 = new Vector3(0, 0, 1); + + /** + * + * @param x + * @param y + * @param z + */ + public function Vector3(x:Number = 0, y:Number = 0, z:Number = 0) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * + * @return + */ + public function vLength():Number { + return Math.sqrt(x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function vLengthSqr():Number { + return x*x + y*y + z*z; + } + + /** + * + * @param length + * @return + */ + public function vSetLength(length:Number):Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = length; + } else { + var k:Number = length/Math.sqrt(x*x + y*y + z*z); + x *= k; + y *= k; + z *= k; + } + return this; + } + + /** + * + * @return + */ + public function vNormalize():Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = 1; + } else { + d = Math.sqrt(d); + x /= d; + y /= d; + z /= d; + } + return this; + } + + /** + * Сложение координат. + * + * @param v точка, координаты которой прибавляются к собственным + */ + public function vAdd(v:Vector3):Vector3 { + x += v.x; + y += v.y; + z += v.z; + return this; + } + + /** + * + * @param k + * @param v + * @return + */ + public function vAddScaled(k:Number, v:Vector3):Vector3 { + x += k*v.x; + y += k*v.y; + z += k*v.z; + return this; + } + + /** + * Вычитание координат. + * + * @param v точка, координаты которой вычитаются из собственных + */ + public function vSubtract(v:Vector3):Vector3 { + x -= v.x; + y -= v.y; + z -= v.z; + return this; + } + + /** + * Вычисляет сумму векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + * @return this + */ + public function vSum(a:Vector3, b:Vector3):Vector3 { + x = a.x + b.x; + y = a.y + b.y; + z = a.z + b.z; + return this; + } + + /** + * Вычисление разности векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + */ + public function vDiff(a:Vector3, b:Vector3):Vector3 { + x = a.x - b.x; + y = a.y - b.y; + z = a.z - b.z; + return this; + } + + /** + * Умножение на скаляр. + * + * @param k число, на которое умножаются координаты + */ + public function vScale(k:Number):Vector3 { + x *= k; + y *= k; + z *= k; + return this; + } + + /** + * Инвертирование вектора. + */ + public function vReverse():Vector3 { + x = -x; + y = -y; + z = -z; + return this; + } + + /** + * + * @param v + * @return + */ + public function vDot(v:Vector3):Number { + return x*v.x + y*v.y + z*v.z; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross(v:Vector3):Vector3 { + var xx:Number = y*v.z - z*v.y; + var yy:Number = z*v.x - x*v.z; + var zz:Number = x*v.y - y*v.x; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross2(a:Vector3, b:Vector3):Vector3 { + x = a.y*b.z - a.z*b.y; + y = a.z*b.x - a.x*b.z; + z = a.x*b.y - a.y*b.x; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy3(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * + * @param matrix + */ + public function vTransformBy3Tr(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz + m.d; + y = m.e*xx + m.f*yy + m.g*zz + m.h; + z = m.i*xx + m.j*yy + m.k*zz + m.l; + return this; + } + + /** + * + * @param m + * @return + */ + public function vTransformInverseBy4(m:Matrix4):Vector3 { + var xx:Number = x - m.d; + var yy:Number = y - m.h; + var zz:Number = z - m.l; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * + * @param m + */ + public function vDeltaTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * Установка координат. + */ + public function vReset(x:Number = 0, y:Number = 0, z:Number = 0):Vector3 { + this.x = x; + this.y = y; + this.z = z; + return this; + } + + /** + * Копирование координат точки. + * + * @param v точка, координаты которой копируются + */ + public function vCopy(v:Vector3):Vector3 { + x = v.x; + y = v.y; + z = v.z; + return this; + } + + /** + * Клонирование точки. + * + * @return клонированная точка + */ + public function vClone():Vector3 { + return new Vector3(x, y, z); + } + + /** + * Копирует координаты вектора в экземпляр класса Vector3D. + * + * @param result экземпляр, в который копируются координаты + * @return переданный параметр + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + return result; + } + + /** + * @param source + * @return + */ + public function copyFromVector3D(source:Vector3D):Vector3 { + x = source.x; + y = source.y; + z = source.z; + return this; + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/.actionScriptProperties b/0.0.4.0/.actionScriptProperties new file mode 100644 index 0000000..e5b1b89 --- /dev/null +++ b/0.0.4.0/.actionScriptProperties @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/0.0.4.0/.flexLibProperties b/0.0.4.0/.flexLibProperties new file mode 100644 index 0000000..f39915e --- /dev/null +++ b/0.0.4.0/.flexLibProperties @@ -0,0 +1,51 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/0.0.4.0/.project b/0.0.4.0/.project new file mode 100644 index 0000000..1f1425f --- /dev/null +++ b/0.0.4.0/.project @@ -0,0 +1,19 @@ + + + AlternativaPhysics + + + + + + com.adobe.flexbuilder.project.flexbuilder + + + + + + org.maven.ide.eclipse.maven2Nature + com.adobe.flexbuilder.project.flexlibnature + com.adobe.flexbuilder.project.actionscriptnature + + diff --git a/0.0.4.0/.settings/.svn/all-wcprops b/0.0.4.0/.settings/.svn/all-wcprops new file mode 100644 index 0000000..c71d186 --- /dev/null +++ b/0.0.4.0/.settings/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 89 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/.settings +END +org.eclipse.core.resources.prefs +K 25 +svn:wc:ra_dav:version-url +V 122 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/.settings/org.eclipse.core.resources.prefs +END diff --git a/0.0.4.0/.settings/.svn/entries b/0.0.4.0/.settings/.svn/entries new file mode 100644 index 0000000..c8cb549 --- /dev/null +++ b/0.0.4.0/.settings/.svn/entries @@ -0,0 +1,40 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/.settings +http://svndev.alternativaplatform.com + + + +2009-04-01T12:25:29.638016Z +10301 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +org.eclipse.core.resources.prefs +file + + + + +2010-10-28T04:32:41.000000Z +4f70b476f3e5075e399505021df2f89b +2009-04-01T12:25:29.638016Z +10301 +mike + diff --git a/0.0.4.0/.settings/.svn/format b/0.0.4.0/.settings/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.4.0/.settings/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.4.0/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base b/0.0.4.0/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base new file mode 100644 index 0000000..b3d0d49 --- /dev/null +++ b/0.0.4.0/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base @@ -0,0 +1,3 @@ +#Fri Mar 20 00:19:33 YEKT 2009 +eclipse.preferences.version=1 +encoding/=UTF-8 diff --git a/0.0.4.0/.settings/org.eclipse.core.resources.prefs b/0.0.4.0/.settings/org.eclipse.core.resources.prefs new file mode 100644 index 0000000..b3d0d49 --- /dev/null +++ b/0.0.4.0/.settings/org.eclipse.core.resources.prefs @@ -0,0 +1,3 @@ +#Fri Mar 20 00:19:33 YEKT 2009 +eclipse.preferences.version=1 +encoding/=UTF-8 diff --git a/0.0.4.0/.svn/all-wcprops b/0.0.4.0/.svn/all-wcprops new file mode 100644 index 0000000..d461577 --- /dev/null +++ b/0.0.4.0/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 79 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0 +END +.flexLibProperties +K 25 +svn:wc:ra_dav:version-url +V 98 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/.flexLibProperties +END +.project +K 25 +svn:wc:ra_dav:version-url +V 88 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/.project +END +pom.xml +K 25 +svn:wc:ra_dav:version-url +V 87 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/pom.xml +END +.actionScriptProperties +K 25 +svn:wc:ra_dav:version-url +V 103 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/.actionScriptProperties +END diff --git a/0.0.4.0/.svn/dir-prop-base b/0.0.4.0/.svn/dir-prop-base new file mode 100644 index 0000000..8eda4b3 --- /dev/null +++ b/0.0.4.0/.svn/dir-prop-base @@ -0,0 +1,5 @@ +K 13 +svn:mergeinfo +V 85 +/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D:13892-14098 +END diff --git a/0.0.4.0/.svn/entries b/0.0.4.0/.svn/entries new file mode 100644 index 0000000..e012047 --- /dev/null +++ b/0.0.4.0/.svn/entries @@ -0,0 +1,85 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0 +http://svndev.alternativaplatform.com + + + +2009-07-20T09:20:57.489740Z +16620 +mike +has-props + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +META-INF +dir + +.flexLibProperties +file + + + + +2010-10-28T04:32:41.000000Z +3ac589a87a5983147605d64c4c08e3c2 +2009-07-17T13:10:32.947052Z +16516 +mike + +.project +file + + + + +2010-10-28T04:32:41.000000Z +1e90cc68b52b93173b2b5de88eb5c3a3 +2009-04-20T17:15:55.260110Z +11541 +mike + +src +dir + +pom.xml +file + + + + +2010-10-28T04:32:41.000000Z +51454b2e01e00b66604bbd27e6c396f7 +2009-07-20T09:20:57.489740Z +16620 +mike + +.actionScriptProperties +file + + + + +2010-10-28T04:32:41.000000Z +918335cb945c3f8866aeb8e6cfd1b110 +2009-07-20T09:20:57.489740Z +16620 +mike + +.settings +dir + diff --git a/0.0.4.0/.svn/format b/0.0.4.0/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.4.0/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.4.0/.svn/text-base/.actionScriptProperties.svn-base b/0.0.4.0/.svn/text-base/.actionScriptProperties.svn-base new file mode 100644 index 0000000..e5b1b89 --- /dev/null +++ b/0.0.4.0/.svn/text-base/.actionScriptProperties.svn-base @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/0.0.4.0/.svn/text-base/.flexLibProperties.svn-base b/0.0.4.0/.svn/text-base/.flexLibProperties.svn-base new file mode 100644 index 0000000..f39915e --- /dev/null +++ b/0.0.4.0/.svn/text-base/.flexLibProperties.svn-base @@ -0,0 +1,51 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/0.0.4.0/.svn/text-base/.project.svn-base b/0.0.4.0/.svn/text-base/.project.svn-base new file mode 100644 index 0000000..1f1425f --- /dev/null +++ b/0.0.4.0/.svn/text-base/.project.svn-base @@ -0,0 +1,19 @@ + + + AlternativaPhysics + + + + + + com.adobe.flexbuilder.project.flexbuilder + + + + + + org.maven.ide.eclipse.maven2Nature + com.adobe.flexbuilder.project.flexlibnature + com.adobe.flexbuilder.project.actionscriptnature + + diff --git a/0.0.4.0/.svn/text-base/pom.xml.svn-base b/0.0.4.0/.svn/text-base/pom.xml.svn-base new file mode 100644 index 0000000..29dce06 --- /dev/null +++ b/0.0.4.0/.svn/text-base/pom.xml.svn-base @@ -0,0 +1,17 @@ + + 4.0.0 + platform.clients.fp10.libraries + AlternativaPhysics + swc + 0.0.4.0 + + platform.tools.maven + FlashBasePom + 1.0 + + + scm:svn:http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0 + + + + \ No newline at end of file diff --git a/0.0.4.0/META-INF/.svn/all-wcprops b/0.0.4.0/META-INF/.svn/all-wcprops new file mode 100644 index 0000000..65d995f --- /dev/null +++ b/0.0.4.0/META-INF/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 88 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/META-INF +END +MANIFEST.MF +K 25 +svn:wc:ra_dav:version-url +V 100 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/META-INF/MANIFEST.MF +END diff --git a/0.0.4.0/META-INF/.svn/entries b/0.0.4.0/META-INF/.svn/entries new file mode 100644 index 0000000..63c8ccb --- /dev/null +++ b/0.0.4.0/META-INF/.svn/entries @@ -0,0 +1,40 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/META-INF +http://svndev.alternativaplatform.com + + + +2009-04-20T17:15:55.260110Z +11541 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +MANIFEST.MF +file + + + + +2010-10-28T04:32:41.000000Z +7b64dceb50a6905850ff00a0bfbe77eb +2009-04-20T17:15:55.260110Z +11541 +mike + diff --git a/0.0.4.0/META-INF/.svn/format b/0.0.4.0/META-INF/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.4.0/META-INF/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.4.0/META-INF/.svn/text-base/MANIFEST.MF.svn-base b/0.0.4.0/META-INF/.svn/text-base/MANIFEST.MF.svn-base new file mode 100644 index 0000000..5066a55 --- /dev/null +++ b/0.0.4.0/META-INF/.svn/text-base/MANIFEST.MF.svn-base @@ -0,0 +1 @@ +Bundle-Name: platform.clients.fp10.libraries.AlternativaPhysics diff --git a/0.0.4.0/META-INF/MANIFEST.MF b/0.0.4.0/META-INF/MANIFEST.MF new file mode 100644 index 0000000..5066a55 --- /dev/null +++ b/0.0.4.0/META-INF/MANIFEST.MF @@ -0,0 +1 @@ +Bundle-Name: platform.clients.fp10.libraries.AlternativaPhysics diff --git a/0.0.4.0/pom.xml b/0.0.4.0/pom.xml new file mode 100644 index 0000000..29dce06 --- /dev/null +++ b/0.0.4.0/pom.xml @@ -0,0 +1,17 @@ + + 4.0.0 + platform.clients.fp10.libraries + AlternativaPhysics + swc + 0.0.4.0 + + platform.tools.maven + FlashBasePom + 1.0 + + + scm:svn:http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0 + + + + \ No newline at end of file diff --git a/0.0.4.0/src/.svn/all-wcprops b/0.0.4.0/src/.svn/all-wcprops new file mode 100644 index 0000000..de4746b --- /dev/null +++ b/0.0.4.0/src/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 83 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src +END diff --git a/0.0.4.0/src/.svn/entries b/0.0.4.0/src/.svn/entries new file mode 100644 index 0000000..62761dd --- /dev/null +++ b/0.0.4.0/src/.svn/entries @@ -0,0 +1,31 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src +http://svndev.alternativaplatform.com + + + +2009-07-20T03:48:26.517738Z +16522 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +alternativa +dir + diff --git a/0.0.4.0/src/.svn/format b/0.0.4.0/src/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.4.0/src/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.4.0/src/alternativa/.svn/all-wcprops b/0.0.4.0/src/alternativa/.svn/all-wcprops new file mode 100644 index 0000000..f33b652 --- /dev/null +++ b/0.0.4.0/src/alternativa/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 95 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa +END diff --git a/0.0.4.0/src/alternativa/.svn/entries b/0.0.4.0/src/alternativa/.svn/entries new file mode 100644 index 0000000..de6ac41 --- /dev/null +++ b/0.0.4.0/src/alternativa/.svn/entries @@ -0,0 +1,31 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa +http://svndev.alternativaplatform.com + + + +2009-07-20T03:48:26.517738Z +16522 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +physics +dir + diff --git a/0.0.4.0/src/alternativa/.svn/format b/0.0.4.0/src/alternativa/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.4.0/src/alternativa/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.4.0/src/alternativa/physics/.svn/all-wcprops b/0.0.4.0/src/alternativa/physics/.svn/all-wcprops new file mode 100644 index 0000000..b141b77 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 103 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics +END +altphysics.as +K 25 +svn:wc:ra_dav:version-url +V 117 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/altphysics.as +END diff --git a/0.0.4.0/src/alternativa/physics/.svn/entries b/0.0.4.0/src/alternativa/physics/.svn/entries new file mode 100644 index 0000000..728d41c --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/.svn/entries @@ -0,0 +1,49 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics +http://svndev.alternativaplatform.com + + + +2009-07-20T03:48:26.517738Z +16522 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +types +dir + +altphysics.as +file + + + + +2010-10-28T04:32:41.000000Z +04fbe40a27502dbbd0dba02a76b2df50 +2009-04-20T20:09:14.715403Z +11579 +mike + +collision +dir + +rigid +dir + diff --git a/0.0.4.0/src/alternativa/physics/.svn/format b/0.0.4.0/src/alternativa/physics/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.4.0/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base b/0.0.4.0/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base new file mode 100644 index 0000000..2715307 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base @@ -0,0 +1,3 @@ +package alternativa.physics { + public namespace altphysics = "http://alternativaplatform.com/en/altphysics"; +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/altphysics.as b/0.0.4.0/src/alternativa/physics/altphysics.as new file mode 100644 index 0000000..2715307 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/altphysics.as @@ -0,0 +1,3 @@ +package alternativa.physics { + public namespace altphysics = "http://alternativaplatform.com/en/altphysics"; +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/.svn/all-wcprops b/0.0.4.0/src/alternativa/physics/collision/.svn/all-wcprops new file mode 100644 index 0000000..05f503e --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/.svn/all-wcprops @@ -0,0 +1,107 @@ +K 25 +svn:wc:ra_dav:version-url +V 113 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/collision +END +BoxSphereCollider.as +K 25 +svn:wc:ra_dav:version-url +V 134 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/collision/BoxSphereCollider.as +END +BoxBoxCollider.as +K 25 +svn:wc:ra_dav:version-url +V 131 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/collision/BoxBoxCollider.as +END +CollisionKdTree2D.as +K 25 +svn:wc:ra_dav:version-url +V 134 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/collision/CollisionKdTree2D.as +END +CollisionKdNode.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/collision/CollisionKdNode.as +END +SpherePlaneCollider.as +K 25 +svn:wc:ra_dav:version-url +V 136 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/collision/SpherePlaneCollider.as +END +KdTreeCollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/collision/KdTreeCollisionDetector.as +END +BoxRectCollider.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/collision/BoxRectCollider.as +END +ICollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 136 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/collision/ICollisionPredicate.as +END +BoxPlaneCollider.as +K 25 +svn:wc:ra_dav:version-url +V 133 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/collision/BoxPlaneCollider.as +END +CollisionKdTree.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/collision/CollisionKdTree.as +END +BoxTriangleCollider.as +K 25 +svn:wc:ra_dav:version-url +V 136 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/collision/BoxTriangleCollider.as +END +ICollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 135 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/collision/ICollisionDetector.as +END +IRayCollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 139 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/collision/IRayCollisionPredicate.as +END +IBodyCollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/collision/IBodyCollisionPredicate.as +END +BruteForceCollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 144 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/collision/BruteForceCollisionDetector.as +END +ICollider.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/collision/ICollider.as +END +SphereSphereCollider.as +K 25 +svn:wc:ra_dav:version-url +V 137 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/collision/SphereSphereCollider.as +END diff --git a/0.0.4.0/src/alternativa/physics/collision/.svn/entries b/0.0.4.0/src/alternativa/physics/collision/.svn/entries new file mode 100644 index 0000000..c000c21 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/.svn/entries @@ -0,0 +1,238 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/collision +http://svndev.alternativaplatform.com + + + +2009-07-20T03:48:26.517738Z +16522 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +BoxSphereCollider.as +file + + + + +2010-10-28T04:32:41.000000Z +2e0b0a368934a7baa3b26e9bb2e8c869 +2009-07-09T09:32:37.355253Z +16196 +mike + +BoxBoxCollider.as +file + + + + +2010-10-28T04:32:41.000000Z +91ed8f11174473575bf09600fb81680e +2009-07-09T09:32:37.355253Z +16196 +mike + +CollisionKdNode.as +file + + + + +2010-10-28T04:32:41.000000Z +1d52aba28ce259ca1f9916d70619263d +2009-07-20T03:48:26.517738Z +16522 +mike + +CollisionKdTree2D.as +file + + + + +2010-10-28T04:32:41.000000Z +d8211949ef77817543bc2882480da12f +2009-07-20T03:48:26.517738Z +16522 +mike + +BoxRectCollider.as +file + + + + +2010-10-28T04:32:41.000000Z +416d5c66e4960ff8b462c5a7b3c7bda8 +2009-07-09T09:32:37.355253Z +16196 +mike + +KdTreeCollisionDetector.as +file + + + + +2010-10-28T04:32:41.000000Z +a45c3f3895c14e2ad1ce04ecf8224368 +2009-07-20T03:48:26.517738Z +16522 +mike + +SpherePlaneCollider.as +file + + + + +2010-10-28T04:32:41.000000Z +1301b459a639d743519c946921b10e5a +2009-07-09T09:32:37.355253Z +16196 +mike + +ICollisionPredicate.as +file + + + + +2010-10-28T04:32:41.000000Z +866471ea582e95e46e94eb7ae437c827 +2009-04-26T15:39:06.673206Z +11995 +mike + +BoxPlaneCollider.as +file + + + + +2010-10-28T04:32:41.000000Z +6ff3df4e7f06da38f13bb72aa6fe105c +2009-07-09T09:32:37.355253Z +16196 +mike + +primitives +dir + +CollisionKdTree.as +file + + + + +2010-10-28T04:32:41.000000Z +45bb93cc65be7d8a55015c4ac75b1242 +2009-07-20T03:48:26.517738Z +16522 +mike + +BoxTriangleCollider.as +file + + + + +2010-10-28T04:32:41.000000Z +2db711a6a311ac389396ecdf465a3dfb +2009-07-09T09:32:37.355253Z +16196 +mike + +ICollisionDetector.as +file + + + + +2010-10-28T04:32:41.000000Z +c415c22df9a1b1064c0501d5d525a5cb +2009-07-13T10:34:12.822563Z +16296 +mike + +IRayCollisionPredicate.as +file + + + + +2010-10-28T04:32:41.000000Z +f7443fb728cf4e6157dcfb4f39665889 +2009-04-26T15:39:06.673206Z +11995 +mike + +types +dir + +IBodyCollisionPredicate.as +file + + + + +2010-10-28T04:32:41.000000Z +fce7e70f95b8ef96709157dcdc8433dd +2009-06-29T14:07:24.833358Z +15390 +mike + +ICollider.as +file + + + + +2010-10-28T04:32:41.000000Z +9eb18f33ec6d3fb0f2b90b5f6f0e62b9 +2009-07-09T09:32:37.355253Z +16196 +mike + +BruteForceCollisionDetector.as +file + + + + +2010-10-28T04:32:41.000000Z +a313af2ee5d9c84ec2551058b1b4c763 +2009-07-13T10:34:12.822563Z +16296 +mike + +SphereSphereCollider.as +file + + + + +2010-10-28T04:32:41.000000Z +14fa0da16c71aadc9921694d8fdacf08 +2009-07-09T09:32:37.355253Z +16196 +mike + diff --git a/0.0.4.0/src/alternativa/physics/collision/.svn/format b/0.0.4.0/src/alternativa/physics/collision/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/BoxBoxCollider.as.svn-base b/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/BoxBoxCollider.as.svn-base new file mode 100644 index 0000000..927d4db --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/BoxBoxCollider.as.svn-base @@ -0,0 +1,848 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Расчитывает точки контакта двух боксов. Нормаль контакта направляется в сторону бокса с меньшим ID. + */ + public class BoxBoxCollider implements ICollider { + + private var tolerance:Number = 0.001; + + private var pos1:Vector3 = new Vector3(); + private var pos2:Vector3 = new Vector3(); + private var vectorToBox1:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var colAxis:Vector3 = new Vector3(); + private var tmpAxis:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var tmpPoints:Vector. = new Vector.(8, true); + private var pcount:int; + + /** + * + */ + public function BoxBoxCollider() { + for (var i:int = 0; i < 8; i++) { + tmpPoints[i] = new ContactPoint(); + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + /** + * + * @param body1 + * @param body2 + * @param contactInfo + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + minOverlap = 1e10; + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + // Вектор из центра второго бокса в центр первого + pos1.x = transform1.d; pos1.y = transform1.h; pos1.z = transform1.l; + pos2.x = transform2.d; pos2.y = transform2.h; pos2.z = transform2.l; + vectorToBox1.x = pos1.x - pos2.x; + vectorToBox1.y = pos1.y - pos2.y; + vectorToBox1.z = pos1.z - pos2.z; + + // Проверка пересечения по основным осям +// box1.transform.getAxis(0, axis10); + axis10.x = transform1.a; axis10.y = transform1.e; axis10.z = transform1.i; + if (!testAxis(box1, box2, axis10, 0, vectorToBox1)) return false; +// box1.transform.getAxis(1, axis11); + axis11.x = transform1.b; axis11.y = transform1.f; axis11.z = transform1.j; + if (!testAxis(box1, box2, axis11, 1, vectorToBox1)) return false; +// box1.transform.getAxis(2, axis12); + axis12.x = transform1.c; axis12.y = transform1.g; axis12.z = transform1.k; + if (!testAxis(box1, box2, axis12, 2, vectorToBox1)) return false; + +// box2.transform.getAxis(0, axis20); + axis20.x = transform2.a; axis20.y = transform2.e; axis20.z = transform2.i; + if (!testAxis(box1, box2, axis20, 3, vectorToBox1)) return false; +// box2.transform.getAxis(1, axis21); + axis21.x = transform2.b; axis21.y = transform2.f; axis21.z = transform2.j; + if (!testAxis(box1, box2, axis21, 4, vectorToBox1)) return false; +// box2.transform.getAxis(2, axis22); + axis22.x = transform2.c; axis22.y = transform2.g; axis22.z = transform2.k; + if (!testAxis(box1, box2, axis22, 5, vectorToBox1)) return false; + + // Проверка производных осей + if (!testAxis(box1, box2, axis.vCross2(axis10, axis20), 6, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis10, axis21), 7, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis10, axis22), 8, vectorToBox1)) return false; + + if (!testAxis(box1, box2, axis.vCross2(axis11, axis20), 9, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis11, axis21), 10, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis11, axis22), 11, vectorToBox1)) return false; + + if (!testAxis(box1, box2, axis.vCross2(axis12, axis20), 12, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis12, axis21), 13, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis12, axis22), 14, vectorToBox1)) return false; + + if (bestAxisIndex < 6) { + // Контакт грань-(грань|ребро|вершина) + findFaceContactPoints(box1, box2, vectorToBox1, bestAxisIndex, contact); + } else { + // Контакт ребро-ребро + bestAxisIndex -= 6; + findEdgesIntersection(box1, box2, vectorToBox1, int(bestAxisIndex/3), bestAxisIndex%3, contact); + } + contact.body1 = box1.body; + contact.body2 = box2.body; + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + // Вектор из центра второго бокса в центр первого + pos1.x = transform1.d; pos1.y = transform1.h; pos1.z = transform1.l; + pos2.x = transform2.d; pos2.y = transform2.h; pos2.z = transform2.l; + vectorToBox1.x = pos1.x - pos2.x; + vectorToBox1.y = pos1.y - pos2.y; + vectorToBox1.z = pos1.z - pos2.z; + + // Проверка пересечения по основным осям + axis10.x = transform1.a; axis10.y = transform1.e; axis10.z = transform1.i; + if (!testAxis(box1, box2, axis10, 0, vectorToBox1)) return false; + axis11.x = transform1.b; axis11.y = transform1.f; axis11.z = transform1.j; + if (!testAxis(box1, box2, axis11, 1, vectorToBox1)) return false; + axis12.x = transform1.c; axis12.y = transform1.g; axis12.z = transform1.k; + if (!testAxis(box1, box2, axis12, 2, vectorToBox1)) return false; + + axis20.x = transform2.a; axis20.y = transform2.e; axis20.z = transform2.i; + if (!testAxis(box1, box2, axis20, 3, vectorToBox1)) return false; + axis21.x = transform2.b; axis21.y = transform2.f; axis21.z = transform2.j; + if (!testAxis(box1, box2, axis21, 4, vectorToBox1)) return false; + axis22.x = transform2.c; axis22.y = transform2.g; axis22.z = transform2.k; + if (!testAxis(box1, box2, axis22, 5, vectorToBox1)) return false; + + // Проверка производных осей + if (!testAxis(box1, box2, axis.vCross2(axis10, axis20), 6, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis10, axis21), 7, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis10, axis22), 8, vectorToBox1)) return false; + + if (!testAxis(box1, box2, axis.vCross2(axis11, axis20), 9, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis11, axis21), 10, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis11, axis22), 11, vectorToBox1)) return false; + + if (!testAxis(box1, box2, axis.vCross2(axis12, axis20), 12, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis12, axis21), 13, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis12, axis22), 14, vectorToBox1)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта грани одного бокса с гранью/ребром/вершиной другого. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param faceAxisIdx индекс оси первого бокса, перпендикулярной грани, с которой произошло столкновение + * @param contactInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, faceAxisIdx:int, contactInfo:Contact):void { + var swapNormal:Boolean = false; + if (faceAxisIdx > 2) { + // Столкновение с гранью второго бокса. Для дальнейших расчётов боксы меняются местами, + // но нормаль контакта всё равно должна быть направлена в сторону первоначального box1 + var tmpBox:CollisionBox = box1; + box1 = box2; + box2 = tmpBox; + vectorToBox1.vReverse(); + faceAxisIdx -= 3; + swapNormal = true; + } + box1.transform.getAxis(faceAxisIdx, colAxis); + var faceReversed:Boolean = colAxis.vDot(vectorToBox1) > 0; + if (!faceReversed) colAxis.vReverse(); + // Ищем ось второго бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + box2.transform.getAxis(axisIdx, axis); + var dot:Number = axis.vDot(colAxis); + if (dot < 0) dot = -dot; + if (dot > maxDot) { + maxDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Получаем список вершин грани второго бокса, переводим их в систему координат первого бокса и выполняем обрезку + // по грани первого бокса. Таким образом получается список потенциальных точек контакта. + box2.transform.getAxis(incFaceAxisIdx, axis); + getFaceVertsByAxis(box2, incFaceAxisIdx, axis.vDot(colAxis) < 0, verts1); + box2.transform.transformVectors(verts1, verts2); + box1.transform.transformVectorsInverse(verts2, verts1); + var pnum:int = clip(box1.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + pcount = 0; + for (var i:int = 0; i < pnum; i++) { + if ((pen = getPointBoxPenetration(box1.hs, verts1[i], faceAxisIdx, faceReversed)) > -tolerance) { + var cp:ContactPoint = tmpPoints[pcount++]; + box1.transform.transformVector(verts1[i], cp.pos); + cp.r1.vDiff(cp.pos, pos1); + cp.r2.vDiff(cp.pos, pos2); + cp.penetration = pen; + } + } + contactInfo.normal.vCopy(colAxis); + if (swapNormal) contactInfo.normal.vReverse(); + + if (pcount > 4) reducePoints(); + for (i = 0; i < pcount; i++) (contactInfo.points[i] as ContactPoint).copyFrom(tmpPoints[i]); + contactInfo.pcount = pcount; + } + + /** + * + * @param contactInfo + */ + private function reducePoints():void { + var i:int; + var minIdx:int; + var cp1:ContactPoint; + var cp2:ContactPoint; + while (pcount > 4) { + var minLen:Number = 1e10; + var p1:Vector3 = (tmpPoints[pcount - 1] as ContactPoint).pos; + var p2:Vector3; + for (i = 0; i < pcount; i++) { + p2 = (tmpPoints[i] as ContactPoint).pos; + var dx:Number = p2.x - p1.x; + var dy:Number = p2.y - p1.y; + var dz:Number = p2.z - p1.z; + var len:Number = dx*dx + dy*dy + dz*dz; + if (len < minLen) { + minLen = len; + minIdx = i; + } + p1 = p2; + } + cp1 = tmpPoints[minIdx == 0 ? (pcount - 1) : (minIdx - 1)]; + cp2 = tmpPoints[minIdx]; + p1 = cp1.pos; + p2 = cp2.pos; + p2.x = 0.5*(p1.x + p2.x); + p2.y = 0.5*(p1.y + p2.y); + p2.z = 0.5*(p1.z + p2.z); + cp2.penetration = 0.5*(cp1.penetration + cp2.penetration); + if (minIdx > 0) { + for (i = minIdx; i < pcount; i++) tmpPoints[i - 1] = tmpPoints[i]; + tmpPoints[pcount - 1] = cp1; + } + pcount--; + } + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + private function getFaceVertsByAxis(box:CollisionBox, axisIdx:int, reverse:Boolean, result:Vector.):void { + var hs:Vector3 = box.hs; + switch (axisIdx) { + case 0: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, -hs.y, hs.z); + } + break; + case 1: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, hs.y, hs.z); + } + break; + case 2: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + } else { + (result[0] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } + break; + } + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clip(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + return clipHighX(hs.x, pnum, verts2, verts1); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + } + return 0; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param contactInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):void { + box1.transform.getAxis(axisIdx1, axis10); + box2.transform.getAxis(axisIdx2, axis20); + colAxis.vCross2(axis10, axis20).vNormalize(); + // Разворот оси в сторону первого бокса + if (colAxis.vDot(vectorToBox1) < 0) colAxis.vReverse(); + + /* Кодирование рёбер + бит 0: 1 (тип контакта ребро-ребро) + биты 1-2: индекс направляющей оси ребра + бит 3: + бит 4: + бит 5: + */ + var edgeCode1:int = 1; + var edgeCode2:int = 1; + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + point1.vCopy(box1.hs); + point2.vCopy(box2.hs); + // x + if (axisIdx1 == 0) { + point1.x = 0; + halfLen1 = box1.hs.x; + } else { + box1.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.x = -point1.x; + edgeCode1 |= 8; // 1 << 3 + } + } + if (axisIdx2 == 0) { + point2.x = 0; + halfLen2 = box2.hs.x; + } else { + box2.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.x = -point2.x; + edgeCode2 |= 8; // 1 << 3 + } + } + // y + if (axisIdx1 == 1) { + point1.y = 0; + halfLen1 = box1.hs.y; + edgeCode1 |= 2; // 1 << 1 + } else { + box1.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.y = -point1.y; + edgeCode1 |= 16; // 1 << 4 + } + } + if (axisIdx2 == 1) { + point2.y = 0; + halfLen2 = box2.hs.y; + edgeCode2 |= 2; // 1 << 1 + } else { + box2.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.y = -point2.y; + edgeCode2 |= 16; // 1 << 4 + } + } + // z + if (axisIdx1 == 2) { + point1.z = 0; + halfLen1 = box1.hs.z; + edgeCode1 |= 4; // 2 << 1 + } else { + box1.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.z = -point1.z; + edgeCode1 |= 32; // 1 << 5 + } + } + if (axisIdx2 == 2) { + point2.z = 0; + halfLen2 = box2.hs.z; + edgeCode2 |= 4; // 2 << 1 + } else { + box2.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.z = -point2.z; + edgeCode2 |= 32; // 1 << 5 + } + } + // Получаем глобальные координаты средних точек рёбер + point1.vTransformBy4(box1.transform); + point2.vTransformBy4(box2.transform); + // Находим точку пересечения рёбер, решая систему уравнений + var k:Number = axis10.vDot(axis20); + var det:Number = k*k - 1; + vector.vDiff(point2, point1); + var c1:Number = axis10.vDot(vector); + var c2:Number = axis20.vDot(vector); + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.normal.vCopy(colAxis); + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(point1.x + axis10.x*t1 + point2.x + axis20.x*t2); + cp.pos.y = 0.5*(point1.y + axis10.y*t1 + point2.y + axis20.y*t2); + cp.pos.z = 0.5*(point1.z + axis10.z*t1 + point2.z + axis20.z*t2); + cp.r1.vDiff(cp.pos, pos1); + cp.r2.vDiff(cp.pos, pos2); + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение боксов вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box1 + * @param box2 + * @param axis + * @param axisIndex + * @param vectorToBox1 + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, axisIndex:int, vectorToBox1:Vector3):Boolean { + if (axis.vLengthSqr() < 0.0001) { + return true; + } + axis.vNormalize(); + + var overlap:Number = overlapOnAxis(box1, box2, axis, vectorToBox1); + if (overlap < -tolerance) return false; + if (overlap + tolerance < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия двух боксов вдоль заданной оси. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param axis ось + * @param vectorToBox1 вектор, соединяющий центр второго бокса с центром первого + * @return величина перекрытия боксов вдоль оси + */ + public function overlapOnAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, vectorToBox1:Vector3):Number { + var m:Matrix4 = box1.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box1.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box1.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box1.hs.z; + if (d < 0) d = -d; + projection += d; + + m = box2.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box2.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box2.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box2.hs.z; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox1.x*axis.x + vectorToBox1.y*axis.y + vectorToBox1.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} + import alternativa.physics.types.Vector3; + + class CollisionPointTmp { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + } diff --git a/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/BoxPlaneCollider.as.svn-base b/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/BoxPlaneCollider.as.svn-base new file mode 100644 index 0000000..a5ec330 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/BoxPlaneCollider.as.svn-base @@ -0,0 +1,87 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + public class BoxPlaneCollider implements ICollider { + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var normal:Vector3 = new Vector3(); + + public function BoxPlaneCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var box:RigidBox = body1 as RigidBox; +// var plane:RigidPlane; +// if (box == null) { +// box = body2 as RigidBox; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные координаты вершин бокса +// var sx:Number = box.halfSize.x; +// var sy:Number = box.halfSize.y; +// var sz:Number = box.halfSize.z; +// (verts1[0] as Vector3).reset(-sx, -sy, -sz); +// (verts1[1] as Vector3).reset(sx, -sy, -sz); +// (verts1[2] as Vector3).reset(sx, sy, -sz); +// (verts1[3] as Vector3).reset(-sx, sy, -sz); +// (verts1[4] as Vector3).reset(-sx, -sy, sz); +// (verts1[5] as Vector3).reset(sx, -sy, sz); +// (verts1[6] as Vector3).reset(sx, sy, sz); +// (verts1[7] as Vector3).reset(-sx, sy, sz); +// +// box.transform.transformVectors(verts1, verts2); +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// // Проверяем наличие столкновений с каждой вершиной +// collisionInfo.pcount = 0; +// for (var i:int = 0; i < 8; i++) { +// // Вершина добавляется в список точек столкновения, если лежит под плоскостью +// var dist:Number = (verts2[i] as Vector3).dot(normal); +// if (dist < offset) { +// var cp:ContactPoint; +// if (collisionInfo.pcount == collisionInfo.points.length) { +// cp = new ContactPoint(); +// collisionInfo.points[collisionInfo.pcount] = cp; +// } else { +// cp = collisionInfo.points[collisionInfo.pcount]; +// } +// cp.pos.copy(verts2[i]); +// cp.r1.diff(cp.pos, box.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); +// cp.penetration = offset - dist; +// collisionInfo.pcount++; +// } +// } +// if (collisionInfo.pcount > 0) { +// collisionInfo.body1 = box; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// return true; +// } + return false; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/BoxRectCollider.as.svn-base b/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/BoxRectCollider.as.svn-base new file mode 100644 index 0000000..4321499 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/BoxRectCollider.as.svn-base @@ -0,0 +1,743 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + public class BoxRectCollider implements ICollider { + + private var tolerance:Number = 0.001; + + private var bPos:Vector3 = new Vector3(); + private var rPos:Vector3 = new Vector3(); + + private var vectorToBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var colAxis:Vector3 = new Vector3(); + private var tmpAxis:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxRectCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + /** + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + minOverlap = 1e10; + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + + // Вектор из центра прямоугольника в центр бокса + box.transform.getAxis(3, bPos); + rect.transform.getAxis(3, rPos); + vectorToBox.vDiff(bPos, rPos); + + // Проверка пересечения по нормали прямоугольника + rect.transform.getAxis(2, axis22); + if (!testAxis(box, rect, axis22, 0, vectorToBox)) return false; + + // Проверка пересечения по основным осям бокса + box.transform.getAxis(0, axis10); + if (!testAxis(box, rect, axis10, 1, vectorToBox)) return false; + box.transform.getAxis(1, axis11); + if (!testAxis(box, rect, axis11, 2, vectorToBox)) return false; + box.transform.getAxis(2, axis12); + if (!testAxis(box, rect, axis12, 3, vectorToBox)) return false; + + // Получаем направляющие рёбер прямоугольника + rect.transform.getAxis(0, axis20); + rect.transform.getAxis(1, axis21); + + // Проверка производных осей + if (!testAxis(box, rect, axis.vCross2(axis10, axis20), 4, vectorToBox)) return false; + if (!testAxis(box, rect, axis.vCross2(axis10, axis21), 5, vectorToBox)) return false; + + if (!testAxis(box, rect, axis.vCross2(axis11, axis20), 6, vectorToBox)) return false; + if (!testAxis(box, rect, axis.vCross2(axis11, axis21), 7, vectorToBox)) return false; + + if (!testAxis(box, rect, axis.vCross2(axis12, axis20), 8, vectorToBox)) return false; + if (!testAxis(box, rect, axis.vCross2(axis12, axis21), 9, vectorToBox)) return false; + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, rect, vectorToBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + findEdgesIntersection(box, rect, vectorToBox, int(bestAxisIndex/2), bestAxisIndex%2, contact); + } + contact.body1 = box.body; + contact.body2 = rect.body; + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + /** + * Выполняет поиск точек контакта бокса с прямоугольником. + * + * @param box бокс + * @param rect прямоугольник + * @param vectorToBox вектор, направленный из центра прямоугольника в центр бокса + * @param faceAxisIdx индекс оси, идентифицирующей полскость столкновения (грань бокса или полскость прямоугольника) + * @param colInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, faceAxisIdx:int, colInfo:Contact):Boolean { + var pnum:int, i:int, v:Vector3, cp:ContactPoint; + if (faceAxisIdx == 0) { + // Столкновение с плоскостью прямоугольника + + // Проверим положение бокса относительно плоскости прямоугольника + rect.transform.getAxis(2, colAxis); + var offset:Number = colAxis.vDot(rPos); + if (bPos.vDot(colAxis) < offset) return false; + + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + box.transform.getAxis(axisIdx, axis); + var dot:Number = axis.vDot(colAxis); + if (dot < 0) dot = -dot; + if (dot > maxDot) { + maxDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Получаем список вершин грани бокса, переводим их в систему координат прямоугольника и выполняем обрезку + // по прямоугольнику. Таким образом получается список потенциальных точек контакта. + box.transform.getAxis(incFaceAxisIdx, axis); + getFaceVertsByAxis(box.hs, incFaceAxisIdx, axis.vDot(colAxis) > 0, verts1); + box.transform.transformVectors(verts1, verts2); + rect.transform.transformVectorsInverse(verts2, verts1); + pnum = clipByRect(rect.hs); + // Проверяем каждую потенциальную точку на принадлежность нижней полуплоскости прямоугольника и добавляем такие точки в список контактов + colInfo.pcount = 0; + for (i = 0; i < pnum; i++) { + v = verts1[i]; + if (v.z < tolerance) { + cp = colInfo.points[colInfo.pcount++]; + rect.transform.transformVector(v, cp.pos); + cp.r1.vDiff(cp.pos, bPos); + cp.r2.vDiff(cp.pos, rPos); + cp.penetration = -v.z; + } + } + colInfo.normal.vCopy(colAxis); + } else { + // Столкновение с гранью бокса + faceAxisIdx--; + box.transform.getAxis(faceAxisIdx, colAxis); + var faceReversed:Boolean = colAxis.vDot(vectorToBox) > 0; + if (!faceReversed) colAxis.vReverse(); + + rect.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) return false; + + getFaceVertsByAxis(rect.hs, 2, false, verts1); + rect.transform.transformVectors(verts1, verts2); + box.transform.transformVectorsInverse(verts2, verts1); + pnum = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + colInfo.pcount = 0; + for (i = 0; i < pnum; i++) { + if ((pen = getPointBoxPenetration(box.hs, verts1[i], faceAxisIdx, faceReversed)) > -tolerance) { + cp = colInfo.points[colInfo.pcount++]; + box.transform.transformVector(verts1[i], cp.pos); + cp.r1.vDiff(cp.pos, bPos); + cp.r2.vDiff(cp.pos, rPos); + cp.penetration = pen; + } + } + colInfo.normal.vCopy(colAxis); + } + return true; + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + private function getFaceVertsByAxis(hs:Vector3, axisIdx:int, reverse:Boolean, result:Vector.):void { + switch (axisIdx) { + case 0: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, -hs.y, hs.z); + } + break; + case 1: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, hs.y, hs.z); + } + break; + case 2: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + } else { + (result[0] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } + break; + } + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + return clipHighX(hs.x, pnum, verts2, verts1); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + } + return 0; + } + + /** + * + * @param hs + * @return + */ + private function clipByRect(hs:Vector3):int { + var pnum:int = 4; + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box первый бокс + * @param rect второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param colInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, axisIdx1:int, axisIdx2:int, colInfo:Contact):void { + box.transform.getAxis(axisIdx1, axis10); + rect.transform.getAxis(axisIdx2, axis20); + colAxis.vCross2(axis10, axis20).vNormalize(); + // Разворот оси в сторону бокса + if (colAxis.vDot(vectorToBox) < 0) colAxis.vReverse(); + + /* Кодирование рёбер + бит 0: 1 (тип контакта ребро-ребро) + биты 1-2: индекс направляющей оси ребра + бит 3: + бит 4: + бит 5: + */ + var edgeCode1:int = 1; + var edgeCode2:int = 1; + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + point1.vCopy(box.hs); + point2.vCopy(rect.hs); + // x + if (axisIdx1 == 0) { + point1.x = 0; + halfLen1 = box.hs.x; + } else { + box.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.x = -point1.x; + edgeCode1 |= 8; // 1 << 3 + } + } + if (axisIdx2 == 0) { + point2.x = 0; + halfLen2 = rect.hs.x; + } else { + rect.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.x = -point2.x; + edgeCode2 |= 8; // 1 << 3 + } + } + // y + if (axisIdx1 == 1) { + point1.y = 0; + halfLen1 = box.hs.y; + edgeCode1 |= 2; // 1 << 1 + } else { + box.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.y = -point1.y; + edgeCode1 |= 16; // 1 << 4 + } + } + if (axisIdx2 == 1) { + point2.y = 0; + halfLen2 = rect.hs.y; + edgeCode2 |= 2; // 1 << 1 + } else { + rect.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.y = -point2.y; + edgeCode2 |= 16; // 1 << 4 + } + } + // z + if (axisIdx1 == 2) { + point1.z = 0; + halfLen1 = box.hs.z; + edgeCode1 |= 4; // 2 << 1 + } else { + box.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.z = -point1.z; + edgeCode1 |= 32; // 1 << 5 + } + } + // Получаем глобальные координаты средних точек рёбер + point1.vTransformBy4(box.transform); + point2.vTransformBy4(rect.transform); + // Находим точку пересечения рёбер, решая систему уравнений + var k:Number = axis10.vDot(axis20); + var det:Number = k*k - 1; + vector.vDiff(point2, point1); + var c1:Number = axis10.vDot(vector); + var c2:Number = axis20.vDot(vector); + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + colInfo.normal.vCopy(colAxis); + colInfo.pcount = 1; + var cp:ContactPoint = colInfo.points[0]; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(point1.x + axis10.x*t1 + point2.x + axis20.x*t2); + cp.pos.y = 0.5*(point1.y + axis10.y*t1 + point2.y + axis20.y*t2); + cp.pos.z = 0.5*(point1.z + axis10.z*t1 + point2.z + axis20.z*t2); + cp.r1.vDiff(cp.pos, bPos); + cp.r2.vDiff(cp.pos, rPos); + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box + * @param rect + * @param axis + * @param axisIndex + * @param vectorToBox + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + if (axis.vLengthSqr() < 0.0001) return true; + axis.vNormalize(); + + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -tolerance) return false; + if (overlap + tolerance < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия вдоль заданной оси. + * + * @param box бокс + * @param rect прямоугольник + * @param axis ось + * @param vectorToBox вектор, соединяющий центр прямоугольника с центром бокса + * @return величина перекрытия вдоль оси + */ + public function overlapOnAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, vectorToBox:Vector3):Number { + var m:Matrix4 = box.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box.hs.z; + if (d < 0) d = -d; + projection += d; + + m = rect.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*rect.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*rect.hs.y; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox.x*axis.x + vectorToBox.y*axis.y + vectorToBox.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/BoxSphereCollider.as.svn-base b/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/BoxSphereCollider.as.svn-base new file mode 100644 index 0000000..bd3e849 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/BoxSphereCollider.as.svn-base @@ -0,0 +1,113 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + use namespace altphysics; + /** + * + */ + public class BoxSphereCollider implements ICollider { + + private var center:Vector3 = new Vector3(); + private var closestPt:Vector3 = new Vector3(); + + private var bPos:Vector3 = new Vector3(); + private var sPos:Vector3 = new Vector3(); + + /** + * + */ + public function BoxSphereCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + // Зафиксированно столкновение + contact.body1 = sphere.body; + contact.body2 = box.body; + contact.normal.vCopy(closestPt).vTransformBy4(box.transform).vSubtract(sPos).vNormalize().vReverse(); + contact.pcount = 1; + + var cp:ContactPoint = contact.points[0]; + cp.penetration = sphere.r - Math.sqrt(distSqr); + cp.pos.vCopy(contact.normal).vScale(-sphere.r).vAdd(sPos); + cp.r1.vDiff(cp.pos, sPos); + cp.r2.vDiff(cp.pos, bPos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/BoxTriangleCollider.as.svn-base b/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/BoxTriangleCollider.as.svn-base new file mode 100644 index 0000000..652375a --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/BoxTriangleCollider.as.svn-base @@ -0,0 +1,23 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Contact; + import alternativa.physics.collision.primitives.CollisionPrimitive; + + public class BoxTriangleCollider implements ICollider { + public function BoxTriangleCollider() { + } + + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + return false; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/BruteForceCollisionDetector.as.svn-base b/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/BruteForceCollisionDetector.as.svn-base new file mode 100644 index 0000000..85d84fa --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/BruteForceCollisionDetector.as.svn-base @@ -0,0 +1,63 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + public class BruteForceCollisionDetector implements ICollisionDetector { + public function BruteForceCollisionDetector() { + } + + public function addPrimitive(primitive:CollisionPrimitive, isStatic:Boolean=true):Boolean { + return false; + } + + public function removePrimitive(primitive:CollisionPrimitive, isStatic:Boolean=true):Boolean { + return false; + } + + public function init():void { + } + + public function getAllCollisions(contacts:Vector.):int { + return 0; + } + + public function intersectRay(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, intersection:RayIntersection):Boolean { + return false; + } + + public function intersectRayWithStatic(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean { + return false; + } + + public function testBodyPrimitiveCollision(body:Body, primitive:CollisionPrimitive):Boolean { + return false; + } + + /** + * + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + return false; + } + + /** + * + * @param prim1 + * @param prim2 + * @return + */ + public function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base b/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base new file mode 100644 index 0000000..e24922c --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base @@ -0,0 +1,18 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.types.BoundBox; + + public class CollisionKdNode { + public var indices:Vector.; + public var splitIndices:Vector.; + public var boundBox:BoundBox; + public var parent:CollisionKdNode; + public var splitTree:CollisionKdTree2D; + + public var axis:int = -1; // 0 - x, 1 - y, 2 - z + public var coord:Number; + public var positiveNode:CollisionKdNode; + public var negativeNode:CollisionKdNode; + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base b/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base new file mode 100644 index 0000000..c046f17 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base @@ -0,0 +1,280 @@ +package alternativa.physics.collision { + + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + + /** + * @author mike + */ + public class CollisionKdTree { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + public var rootNode:CollisionKdNode; + public var staticChildren:Vector. = new Vector.(); + public var numStaticChildren:int; + public var staticBoundBoxes:Vector. = new Vector.(); + + private var splitAxis:int; + private var splitCoord:Number; + private var splitCost:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + + /** + * @param primitive + */ + public function addStaticPrimitive(primitive:CollisionPrimitive):void { + staticChildren[numStaticChildren++] = primitive; + } + + /** + * @param primitive + * @return + */ + public function removeStaticPrimitive(primitive:CollisionPrimitive):Boolean { + var idx:int = staticChildren.indexOf(primitive); + if (idx < 0) return false; + staticChildren.splice(idx, 1); + numStaticChildren--; + return true; + } + + /** + * @param boundBox + */ + public function createTree(boundBox:BoundBox = null):void { + if (numStaticChildren == 0) return; + // Создаём корневую ноду + rootNode = new CollisionKdNode(); + rootNode.indices = new Vector.(); + // Расчитываем баунды объектов и рутовой ноды + var rootNodeBoundBox:BoundBox = rootNode.boundBox = (boundBox != null) ? boundBox : new BoundBox(); + for (var i:int = 0; i < numStaticChildren; ++i) { + var child:CollisionPrimitive = staticChildren[i]; + var childBoundBox:BoundBox = staticBoundBoxes[i] = child.calculateAABB(); + rootNodeBoundBox.addBoundBox(childBoundBox); + rootNode.indices[i] = i; + } + staticBoundBoxes.length = numStaticChildren; + // Разделяем рутовую ноду + splitNode(rootNode); + + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + var indices:Vector. = node.indices; + var numPrimitives:int = indices.length; + if (numPrimitives <= minPrimitivesPerNode) return; + + // Подготовка баунда с погрешностями + var nodeBoundBox:BoundBox = node.boundBox; + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + // Собираем опорные координаты + var i:int; + var j:int; + var numSplitCoordsX:int = 0 + var numSplitCoordsY:int = 0; + var numSplitCoordsZ:int = 0; + for (i = 0; i < numPrimitives; ++i) { + var boundBox:BoundBox = staticBoundBoxes[indices[i]]; + + if (boundBox.maxX - boundBox.minX <= doubleThreshold) { + if (boundBox.minX <= nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.minX; + else if (boundBox.maxX >= nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.maxX; + else splitCoordsX[numSplitCoordsX++] = (boundBox.minX + boundBox.maxX)*0.5; + } else { + if (boundBox.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = boundBox.minX; + if (boundBox.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = boundBox.maxX; + } + + if (boundBox.maxY - boundBox.minY <= doubleThreshold) { + if (boundBox.minY <= nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.minY; + else if (boundBox.maxY >= nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.maxY; + else splitCoordsY[numSplitCoordsY++] = (boundBox.minY + boundBox.maxY)*0.5; + } else { + if (boundBox.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = boundBox.minY; + if (boundBox.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = boundBox.maxY; + } + + if (boundBox.maxZ - boundBox.minZ <= doubleThreshold) { + if (boundBox.minZ <= nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.minZ; + else if (boundBox.maxZ >= nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.maxZ; + else splitCoordsZ[numSplitCoordsZ++] = (boundBox.minZ + boundBox.maxZ)*0.5; + } else { + if (boundBox.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.minZ; + if (boundBox.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numPrimitives; ++i) { + boundBox = staticBoundBoxes[indices[i]]; + var min:Number = axisX ? boundBox.minX : (axisY ? boundBox.minY : boundBox.minZ); + var max:Number = axisX ? boundBox.maxX : (axisY ? boundBox.maxY : boundBox.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(indices[i]); + indices[i] = -1; + } else { + if (node.splitIndices == null) node.splitIndices = new Vector.(); + node.splitIndices.push(indices[i]); + indices[i] = -1; + } + } else { + if (min >= coordMin) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(indices[i]); + indices[i] = -1; + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numPrimitives; ++i) { + if (indices[i] >= 0) indices[j++] = indices[i]; + } + if (j > 0) indices.length = j; + else node.indices = null; + + if (node.splitIndices != null) { + node.splitTree = new CollisionKdTree2D(this, node); + node.splitTree.createTree(); + } + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + /** + * + */ + public function traceTree():void { + traceNode("", rootNode); + } + + /** + * @param str + * @param node + */ + private function traceNode(str:String, node:CollisionKdNode):void { + if (node == null) return; + trace(str, node.axis == -1 ? "end" : ((node.axis == 0) ? "X" : ((node.axis == 1) ? "Y" : "Z")), "splitCoord=" + splitCoord, "bound", node.boundBox, "objs:", node.indices); + traceNode(str + "-", node.negativeNode); + traceNode(str + "+", node.positiveNode); + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base b/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base new file mode 100644 index 0000000..78aaa95 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base @@ -0,0 +1,226 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.types.BoundBox; + + /** + * + */ + public class CollisionKdTree2D { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + + public var parentTree:CollisionKdTree; + public var parentNode:CollisionKdNode; + public var rootNode:CollisionKdNode; + + private var splitAxis:int; + private var splitCost:Number; + private var splitCoord:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + /** + * + * @param parentTree + * @param parentNode + */ + public function CollisionKdTree2D(parentTree:CollisionKdTree, parentNode:CollisionKdNode) { + this.parentTree = parentTree; + this.parentNode = parentNode; + } + + /** + * + */ + public function createTree():void { + rootNode = new CollisionKdNode(); + rootNode.boundBox = parentNode.boundBox.clone(); + rootNode.indices = new Vector.(); + var numObjects:int = parentNode.splitIndices.length; + for (var i:int = 0; i < numObjects; ++i) rootNode.indices[i] = parentNode.splitIndices[i]; + + splitNode(rootNode); + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + if (node.indices.length <= minPrimitivesPerNode) return; + + var objects:Vector. = node.indices; + var i:int; + var j:int; + + var nodeBoundBox:BoundBox = node.boundBox; + + // Подготовка баунда с погрешностями + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + // Собираем опорные координаты + var numSplitCoordsX:int; + var numSplitCoordsY:int; + var numSplitCoordsZ:int; + var numObjects:int = objects.length; + for (i = 0; i < numObjects; ++i) { + var bb:BoundBox = staticBoundBoxes[objects[i]]; + + if (parentNode.axis != 0) { + if (bb.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = bb.minX; + if (bb.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = bb.maxX; + } + + if (parentNode.axis != 1) { + if (bb.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = bb.minY; + if (bb.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = bb.maxY; + } + + if (parentNode.axis != 2) { + if (bb.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = bb.minZ; + if (bb.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = bb.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + if (parentNode.axis != 0) checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + if (parentNode.axis != 1) checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + if (parentNode.axis != 2) checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numObjects; ++i) { + bb = staticBoundBoxes[objects[i]]; + var min:Number = axisX ? bb.minX : (axisY ? bb.minY : bb.minZ); + var max:Number = axisX ? bb.maxX : (axisY ? bb.maxY : bb.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(objects[i]); + objects[i] = -1; + } + } else { + if (min >= coordMin) { + if (max > coordMax) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(objects[i]); + objects[i] = -1; + } + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numObjects; ++i) { + if (objects[i] >= 0) objects[j++] = objects[i]; + } + if (j > 0) objects.length = j; + else node.indices = null; + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base b/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base new file mode 100644 index 0000000..9cfdca5 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Body; + + public interface IBodyCollisionPredicate { + function considerBodies(body1:Body, body2:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base b/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base new file mode 100644 index 0000000..eef9cb9 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base @@ -0,0 +1,14 @@ +package alternativa.physics.collision { + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + + /** + * Интерфейс определителя столкновений между двумя примитивами. + */ + public interface ICollider { + function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base b/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base new file mode 100644 index 0000000..af43dfb --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base @@ -0,0 +1,92 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + /** + * Интерфейс детектора столкновений. + */ + public interface ICollisionDetector { + + /** + * Добавляет физический примитив в коллайдер. + * + * @param primitive добавляемый примитив + * @param isStatic указывает тип примитива: статический или динамический + * @return true если примитив был успешно добавлен, иначе false + */ + function addPrimitive(primitive:CollisionPrimitive, isStatic:Boolean = true):Boolean; + + /** + * Удаляет физический примитив из коллайдера. + * + * @param primitive удаляемый примитив + * @param isStatic указывает тип примитива: статический или динамический + * @return true если примитив был успшено удалён, иначе false + */ + function removePrimitive(primitive:CollisionPrimitive, isStatic:Boolean = true):Boolean; + + /** + * Выполняет инициализацию детектора после обновления списка примитивов. + */ + function init():void; + + /** + * Получает все столкновения в текущей конфигурации физической геометрии. + * + * @param contacts список контактов, в кторые будет записана информация о столкновении + * @return количество найденных столкновений + */ + function getAllCollisions(contacts:Vector.):int; + + /** + * Тестирует луч на пересечение с физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param dir направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRay(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * Тестирует луч на пересечение со статической физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param dir направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRayWithStatic(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @return + */ + function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base b/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base new file mode 100644 index 0000000..bb1b756 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base @@ -0,0 +1,9 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + + public interface ICollisionPredicate { + + function considerCollision(primitive:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base b/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base new file mode 100644 index 0000000..0f3d929 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Body; + + public interface IRayCollisionPredicate { + function considerBody(body:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base b/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base new file mode 100644 index 0000000..1284123 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base @@ -0,0 +1,483 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Детектор, хранящий статическую геометрию в kD-дереве и использующий дерево для ускорения тестов на пересечения. + */ + public class KdTreeCollisionDetector implements ICollisionDetector { + + altphysics var tree:CollisionKdTree; + altphysics var dynamicPrimitives:Vector.; + 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.(); + + addCollider(CollisionPrimitive.BOX, CollisionPrimitive.BOX, new BoxBoxCollider()); +// addCollider(CollisionPrimitive.BOX, CollisionPrimitive.PLANE, new BoxPlaneCollider()); + addCollider(CollisionPrimitive.BOX, CollisionPrimitive.SPHERE, new BoxSphereCollider()); + addCollider(CollisionPrimitive.BOX, CollisionPrimitive.RECT, new BoxRectCollider()); + +// 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():void { + tree.createTree(); +// tree.traceTree(); + } + + /** + * + * @param contacts + * @return + */ + public function getAllCollisions(contacts:Vector.):int { + var colNum:int = 0; + for (var i:int = 0; i < dynamicPrimitivesNum; i++) { + var primitive:CollisionPrimitive = dynamicPrimitives[i]; + primitive.calculateAABB(); + if (primitive.body != null && primitive.body.frozen) continue; + colNum += getPrimitiveNodeCollisions(tree.rootNode, primitive, contacts, colNum); + + // Столкновения динамических примитивов между собой + // TODO: Попробовать различные оптимизации (сортировка по баундам, встраивание в дерево) + for (var j:int = i + 1; j < dynamicPrimitivesNum; j++) { + if (collide(primitive, dynamicPrimitives[j], contacts[colNum])) colNum++; + } + } + + return colNum; + } + + /** + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + public function collide(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.collide(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 = (contact.points[0] as ContactPoint).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.testCollision(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:Vector., startIndex:int):int { + var colNum:int = 0; + if (node.indices != null) { + // Поиск столкновений со статическими примитивами узла + var primitives:Vector. = tree.staticChildren; + var indices:Vector. = node.indices; + for (var i:int = indices.length - 1; i >= 0; i--) + if (collide(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; + } + + 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.getSegmentIntersection(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., indices:Vector., 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.getSegmentIntersection(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; + } \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/SpherePlaneCollider.as.svn-base b/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/SpherePlaneCollider.as.svn-base new file mode 100644 index 0000000..612c0d7 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/SpherePlaneCollider.as.svn-base @@ -0,0 +1,67 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + /** + * + */ + public class SpherePlaneCollider implements ICollider { + + private var normal:Vector3 = new Vector3(); + + /** + * + */ + public function SpherePlaneCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var sphere:RigidSphere = body1 as RigidSphere; +// var plane:RigidPlane; +// if (sphere == null) { +// sphere = body2 as RigidSphere; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// +// var dist:Number = sphere.state.pos.dot(normal) - offset; +// if (dist > sphere.r) return false; +// +// collisionInfo.body1 = sphere; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// collisionInfo.pcount = 1; +// +// var cp:ContactPoint = collisionInfo.points[0]; +// cp.penetration = sphere.r - dist; +// cp.pos.copy(normal).reverse().scale(sphere.r).add(sphere.state.pos); +// cp.r1.diff(cp.pos, sphere.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/SphereSphereCollider.as.svn-base b/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/SphereSphereCollider.as.svn-base new file mode 100644 index 0000000..352f2f2 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/.svn/text-base/SphereSphereCollider.as.svn-base @@ -0,0 +1,69 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + public class SphereSphereCollider implements ICollider { + + private var p1:Vector3 = new Vector3(); + private var p2:Vector3 = new Vector3(); + + public function SphereSphereCollider() { + } + + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var s1:CollisionSphere; + var s2:CollisionSphere; + if (prim1.body != null) { + s1 = prim1 as CollisionSphere; + s2 = prim2 as CollisionSphere; + } else { + s1 = prim2 as CollisionSphere; + s2 = prim1 as CollisionSphere; + } + + s1.transform.getAxis(3, p1); + s2.transform.getAxis(3, p2); + var dx:Number = p1.x - p2.x; + var dy:Number = p1.y - p2.y; + var dz:Number = p1.z - p2.z; + var len:Number = dx*dx + dy*dy + dz*dz; + var sum:Number = s1.r + s2.r; + if (len > sum*sum) return false; + len = Math.sqrt(len); + dx /= len; + dy /= len; + dz /= len; + + contact.body1 = s1.body; + contact.body2 = s2.body; + contact.normal.x = dx; + contact.normal.y = dy; + contact.normal.z = dz; + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = sum - len; + cp.pos.x = p1.x - dx*s1.r; + cp.pos.y = p1.y - dy*s1.r; + cp.pos.z = p1.z - dz*s1.r; + cp.r1.vDiff(cp.pos, p1); + cp.r2.vDiff(cp.pos, p2); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/BoxBoxCollider.as b/0.0.4.0/src/alternativa/physics/collision/BoxBoxCollider.as new file mode 100644 index 0000000..927d4db --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/BoxBoxCollider.as @@ -0,0 +1,848 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Расчитывает точки контакта двух боксов. Нормаль контакта направляется в сторону бокса с меньшим ID. + */ + public class BoxBoxCollider implements ICollider { + + private var tolerance:Number = 0.001; + + private var pos1:Vector3 = new Vector3(); + private var pos2:Vector3 = new Vector3(); + private var vectorToBox1:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var colAxis:Vector3 = new Vector3(); + private var tmpAxis:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var tmpPoints:Vector. = new Vector.(8, true); + private var pcount:int; + + /** + * + */ + public function BoxBoxCollider() { + for (var i:int = 0; i < 8; i++) { + tmpPoints[i] = new ContactPoint(); + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + /** + * + * @param body1 + * @param body2 + * @param contactInfo + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + minOverlap = 1e10; + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + // Вектор из центра второго бокса в центр первого + pos1.x = transform1.d; pos1.y = transform1.h; pos1.z = transform1.l; + pos2.x = transform2.d; pos2.y = transform2.h; pos2.z = transform2.l; + vectorToBox1.x = pos1.x - pos2.x; + vectorToBox1.y = pos1.y - pos2.y; + vectorToBox1.z = pos1.z - pos2.z; + + // Проверка пересечения по основным осям +// box1.transform.getAxis(0, axis10); + axis10.x = transform1.a; axis10.y = transform1.e; axis10.z = transform1.i; + if (!testAxis(box1, box2, axis10, 0, vectorToBox1)) return false; +// box1.transform.getAxis(1, axis11); + axis11.x = transform1.b; axis11.y = transform1.f; axis11.z = transform1.j; + if (!testAxis(box1, box2, axis11, 1, vectorToBox1)) return false; +// box1.transform.getAxis(2, axis12); + axis12.x = transform1.c; axis12.y = transform1.g; axis12.z = transform1.k; + if (!testAxis(box1, box2, axis12, 2, vectorToBox1)) return false; + +// box2.transform.getAxis(0, axis20); + axis20.x = transform2.a; axis20.y = transform2.e; axis20.z = transform2.i; + if (!testAxis(box1, box2, axis20, 3, vectorToBox1)) return false; +// box2.transform.getAxis(1, axis21); + axis21.x = transform2.b; axis21.y = transform2.f; axis21.z = transform2.j; + if (!testAxis(box1, box2, axis21, 4, vectorToBox1)) return false; +// box2.transform.getAxis(2, axis22); + axis22.x = transform2.c; axis22.y = transform2.g; axis22.z = transform2.k; + if (!testAxis(box1, box2, axis22, 5, vectorToBox1)) return false; + + // Проверка производных осей + if (!testAxis(box1, box2, axis.vCross2(axis10, axis20), 6, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis10, axis21), 7, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis10, axis22), 8, vectorToBox1)) return false; + + if (!testAxis(box1, box2, axis.vCross2(axis11, axis20), 9, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis11, axis21), 10, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis11, axis22), 11, vectorToBox1)) return false; + + if (!testAxis(box1, box2, axis.vCross2(axis12, axis20), 12, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis12, axis21), 13, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis12, axis22), 14, vectorToBox1)) return false; + + if (bestAxisIndex < 6) { + // Контакт грань-(грань|ребро|вершина) + findFaceContactPoints(box1, box2, vectorToBox1, bestAxisIndex, contact); + } else { + // Контакт ребро-ребро + bestAxisIndex -= 6; + findEdgesIntersection(box1, box2, vectorToBox1, int(bestAxisIndex/3), bestAxisIndex%3, contact); + } + contact.body1 = box1.body; + contact.body2 = box2.body; + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + // Вектор из центра второго бокса в центр первого + pos1.x = transform1.d; pos1.y = transform1.h; pos1.z = transform1.l; + pos2.x = transform2.d; pos2.y = transform2.h; pos2.z = transform2.l; + vectorToBox1.x = pos1.x - pos2.x; + vectorToBox1.y = pos1.y - pos2.y; + vectorToBox1.z = pos1.z - pos2.z; + + // Проверка пересечения по основным осям + axis10.x = transform1.a; axis10.y = transform1.e; axis10.z = transform1.i; + if (!testAxis(box1, box2, axis10, 0, vectorToBox1)) return false; + axis11.x = transform1.b; axis11.y = transform1.f; axis11.z = transform1.j; + if (!testAxis(box1, box2, axis11, 1, vectorToBox1)) return false; + axis12.x = transform1.c; axis12.y = transform1.g; axis12.z = transform1.k; + if (!testAxis(box1, box2, axis12, 2, vectorToBox1)) return false; + + axis20.x = transform2.a; axis20.y = transform2.e; axis20.z = transform2.i; + if (!testAxis(box1, box2, axis20, 3, vectorToBox1)) return false; + axis21.x = transform2.b; axis21.y = transform2.f; axis21.z = transform2.j; + if (!testAxis(box1, box2, axis21, 4, vectorToBox1)) return false; + axis22.x = transform2.c; axis22.y = transform2.g; axis22.z = transform2.k; + if (!testAxis(box1, box2, axis22, 5, vectorToBox1)) return false; + + // Проверка производных осей + if (!testAxis(box1, box2, axis.vCross2(axis10, axis20), 6, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis10, axis21), 7, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis10, axis22), 8, vectorToBox1)) return false; + + if (!testAxis(box1, box2, axis.vCross2(axis11, axis20), 9, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis11, axis21), 10, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis11, axis22), 11, vectorToBox1)) return false; + + if (!testAxis(box1, box2, axis.vCross2(axis12, axis20), 12, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis12, axis21), 13, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis12, axis22), 14, vectorToBox1)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта грани одного бокса с гранью/ребром/вершиной другого. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param faceAxisIdx индекс оси первого бокса, перпендикулярной грани, с которой произошло столкновение + * @param contactInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, faceAxisIdx:int, contactInfo:Contact):void { + var swapNormal:Boolean = false; + if (faceAxisIdx > 2) { + // Столкновение с гранью второго бокса. Для дальнейших расчётов боксы меняются местами, + // но нормаль контакта всё равно должна быть направлена в сторону первоначального box1 + var tmpBox:CollisionBox = box1; + box1 = box2; + box2 = tmpBox; + vectorToBox1.vReverse(); + faceAxisIdx -= 3; + swapNormal = true; + } + box1.transform.getAxis(faceAxisIdx, colAxis); + var faceReversed:Boolean = colAxis.vDot(vectorToBox1) > 0; + if (!faceReversed) colAxis.vReverse(); + // Ищем ось второго бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + box2.transform.getAxis(axisIdx, axis); + var dot:Number = axis.vDot(colAxis); + if (dot < 0) dot = -dot; + if (dot > maxDot) { + maxDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Получаем список вершин грани второго бокса, переводим их в систему координат первого бокса и выполняем обрезку + // по грани первого бокса. Таким образом получается список потенциальных точек контакта. + box2.transform.getAxis(incFaceAxisIdx, axis); + getFaceVertsByAxis(box2, incFaceAxisIdx, axis.vDot(colAxis) < 0, verts1); + box2.transform.transformVectors(verts1, verts2); + box1.transform.transformVectorsInverse(verts2, verts1); + var pnum:int = clip(box1.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + pcount = 0; + for (var i:int = 0; i < pnum; i++) { + if ((pen = getPointBoxPenetration(box1.hs, verts1[i], faceAxisIdx, faceReversed)) > -tolerance) { + var cp:ContactPoint = tmpPoints[pcount++]; + box1.transform.transformVector(verts1[i], cp.pos); + cp.r1.vDiff(cp.pos, pos1); + cp.r2.vDiff(cp.pos, pos2); + cp.penetration = pen; + } + } + contactInfo.normal.vCopy(colAxis); + if (swapNormal) contactInfo.normal.vReverse(); + + if (pcount > 4) reducePoints(); + for (i = 0; i < pcount; i++) (contactInfo.points[i] as ContactPoint).copyFrom(tmpPoints[i]); + contactInfo.pcount = pcount; + } + + /** + * + * @param contactInfo + */ + private function reducePoints():void { + var i:int; + var minIdx:int; + var cp1:ContactPoint; + var cp2:ContactPoint; + while (pcount > 4) { + var minLen:Number = 1e10; + var p1:Vector3 = (tmpPoints[pcount - 1] as ContactPoint).pos; + var p2:Vector3; + for (i = 0; i < pcount; i++) { + p2 = (tmpPoints[i] as ContactPoint).pos; + var dx:Number = p2.x - p1.x; + var dy:Number = p2.y - p1.y; + var dz:Number = p2.z - p1.z; + var len:Number = dx*dx + dy*dy + dz*dz; + if (len < minLen) { + minLen = len; + minIdx = i; + } + p1 = p2; + } + cp1 = tmpPoints[minIdx == 0 ? (pcount - 1) : (minIdx - 1)]; + cp2 = tmpPoints[minIdx]; + p1 = cp1.pos; + p2 = cp2.pos; + p2.x = 0.5*(p1.x + p2.x); + p2.y = 0.5*(p1.y + p2.y); + p2.z = 0.5*(p1.z + p2.z); + cp2.penetration = 0.5*(cp1.penetration + cp2.penetration); + if (minIdx > 0) { + for (i = minIdx; i < pcount; i++) tmpPoints[i - 1] = tmpPoints[i]; + tmpPoints[pcount - 1] = cp1; + } + pcount--; + } + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + private function getFaceVertsByAxis(box:CollisionBox, axisIdx:int, reverse:Boolean, result:Vector.):void { + var hs:Vector3 = box.hs; + switch (axisIdx) { + case 0: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, -hs.y, hs.z); + } + break; + case 1: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, hs.y, hs.z); + } + break; + case 2: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + } else { + (result[0] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } + break; + } + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clip(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + return clipHighX(hs.x, pnum, verts2, verts1); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + } + return 0; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param contactInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):void { + box1.transform.getAxis(axisIdx1, axis10); + box2.transform.getAxis(axisIdx2, axis20); + colAxis.vCross2(axis10, axis20).vNormalize(); + // Разворот оси в сторону первого бокса + if (colAxis.vDot(vectorToBox1) < 0) colAxis.vReverse(); + + /* Кодирование рёбер + бит 0: 1 (тип контакта ребро-ребро) + биты 1-2: индекс направляющей оси ребра + бит 3: + бит 4: + бит 5: + */ + var edgeCode1:int = 1; + var edgeCode2:int = 1; + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + point1.vCopy(box1.hs); + point2.vCopy(box2.hs); + // x + if (axisIdx1 == 0) { + point1.x = 0; + halfLen1 = box1.hs.x; + } else { + box1.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.x = -point1.x; + edgeCode1 |= 8; // 1 << 3 + } + } + if (axisIdx2 == 0) { + point2.x = 0; + halfLen2 = box2.hs.x; + } else { + box2.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.x = -point2.x; + edgeCode2 |= 8; // 1 << 3 + } + } + // y + if (axisIdx1 == 1) { + point1.y = 0; + halfLen1 = box1.hs.y; + edgeCode1 |= 2; // 1 << 1 + } else { + box1.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.y = -point1.y; + edgeCode1 |= 16; // 1 << 4 + } + } + if (axisIdx2 == 1) { + point2.y = 0; + halfLen2 = box2.hs.y; + edgeCode2 |= 2; // 1 << 1 + } else { + box2.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.y = -point2.y; + edgeCode2 |= 16; // 1 << 4 + } + } + // z + if (axisIdx1 == 2) { + point1.z = 0; + halfLen1 = box1.hs.z; + edgeCode1 |= 4; // 2 << 1 + } else { + box1.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.z = -point1.z; + edgeCode1 |= 32; // 1 << 5 + } + } + if (axisIdx2 == 2) { + point2.z = 0; + halfLen2 = box2.hs.z; + edgeCode2 |= 4; // 2 << 1 + } else { + box2.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.z = -point2.z; + edgeCode2 |= 32; // 1 << 5 + } + } + // Получаем глобальные координаты средних точек рёбер + point1.vTransformBy4(box1.transform); + point2.vTransformBy4(box2.transform); + // Находим точку пересечения рёбер, решая систему уравнений + var k:Number = axis10.vDot(axis20); + var det:Number = k*k - 1; + vector.vDiff(point2, point1); + var c1:Number = axis10.vDot(vector); + var c2:Number = axis20.vDot(vector); + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.normal.vCopy(colAxis); + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(point1.x + axis10.x*t1 + point2.x + axis20.x*t2); + cp.pos.y = 0.5*(point1.y + axis10.y*t1 + point2.y + axis20.y*t2); + cp.pos.z = 0.5*(point1.z + axis10.z*t1 + point2.z + axis20.z*t2); + cp.r1.vDiff(cp.pos, pos1); + cp.r2.vDiff(cp.pos, pos2); + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение боксов вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box1 + * @param box2 + * @param axis + * @param axisIndex + * @param vectorToBox1 + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, axisIndex:int, vectorToBox1:Vector3):Boolean { + if (axis.vLengthSqr() < 0.0001) { + return true; + } + axis.vNormalize(); + + var overlap:Number = overlapOnAxis(box1, box2, axis, vectorToBox1); + if (overlap < -tolerance) return false; + if (overlap + tolerance < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия двух боксов вдоль заданной оси. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param axis ось + * @param vectorToBox1 вектор, соединяющий центр второго бокса с центром первого + * @return величина перекрытия боксов вдоль оси + */ + public function overlapOnAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, vectorToBox1:Vector3):Number { + var m:Matrix4 = box1.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box1.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box1.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box1.hs.z; + if (d < 0) d = -d; + projection += d; + + m = box2.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box2.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box2.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box2.hs.z; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox1.x*axis.x + vectorToBox1.y*axis.y + vectorToBox1.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} + import alternativa.physics.types.Vector3; + + class CollisionPointTmp { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + } diff --git a/0.0.4.0/src/alternativa/physics/collision/BoxPlaneCollider.as b/0.0.4.0/src/alternativa/physics/collision/BoxPlaneCollider.as new file mode 100644 index 0000000..a5ec330 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/BoxPlaneCollider.as @@ -0,0 +1,87 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + public class BoxPlaneCollider implements ICollider { + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var normal:Vector3 = new Vector3(); + + public function BoxPlaneCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var box:RigidBox = body1 as RigidBox; +// var plane:RigidPlane; +// if (box == null) { +// box = body2 as RigidBox; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные координаты вершин бокса +// var sx:Number = box.halfSize.x; +// var sy:Number = box.halfSize.y; +// var sz:Number = box.halfSize.z; +// (verts1[0] as Vector3).reset(-sx, -sy, -sz); +// (verts1[1] as Vector3).reset(sx, -sy, -sz); +// (verts1[2] as Vector3).reset(sx, sy, -sz); +// (verts1[3] as Vector3).reset(-sx, sy, -sz); +// (verts1[4] as Vector3).reset(-sx, -sy, sz); +// (verts1[5] as Vector3).reset(sx, -sy, sz); +// (verts1[6] as Vector3).reset(sx, sy, sz); +// (verts1[7] as Vector3).reset(-sx, sy, sz); +// +// box.transform.transformVectors(verts1, verts2); +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// // Проверяем наличие столкновений с каждой вершиной +// collisionInfo.pcount = 0; +// for (var i:int = 0; i < 8; i++) { +// // Вершина добавляется в список точек столкновения, если лежит под плоскостью +// var dist:Number = (verts2[i] as Vector3).dot(normal); +// if (dist < offset) { +// var cp:ContactPoint; +// if (collisionInfo.pcount == collisionInfo.points.length) { +// cp = new ContactPoint(); +// collisionInfo.points[collisionInfo.pcount] = cp; +// } else { +// cp = collisionInfo.points[collisionInfo.pcount]; +// } +// cp.pos.copy(verts2[i]); +// cp.r1.diff(cp.pos, box.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); +// cp.penetration = offset - dist; +// collisionInfo.pcount++; +// } +// } +// if (collisionInfo.pcount > 0) { +// collisionInfo.body1 = box; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// return true; +// } + return false; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/BoxRectCollider.as b/0.0.4.0/src/alternativa/physics/collision/BoxRectCollider.as new file mode 100644 index 0000000..4321499 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/BoxRectCollider.as @@ -0,0 +1,743 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + public class BoxRectCollider implements ICollider { + + private var tolerance:Number = 0.001; + + private var bPos:Vector3 = new Vector3(); + private var rPos:Vector3 = new Vector3(); + + private var vectorToBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var colAxis:Vector3 = new Vector3(); + private var tmpAxis:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxRectCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + /** + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + minOverlap = 1e10; + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + + // Вектор из центра прямоугольника в центр бокса + box.transform.getAxis(3, bPos); + rect.transform.getAxis(3, rPos); + vectorToBox.vDiff(bPos, rPos); + + // Проверка пересечения по нормали прямоугольника + rect.transform.getAxis(2, axis22); + if (!testAxis(box, rect, axis22, 0, vectorToBox)) return false; + + // Проверка пересечения по основным осям бокса + box.transform.getAxis(0, axis10); + if (!testAxis(box, rect, axis10, 1, vectorToBox)) return false; + box.transform.getAxis(1, axis11); + if (!testAxis(box, rect, axis11, 2, vectorToBox)) return false; + box.transform.getAxis(2, axis12); + if (!testAxis(box, rect, axis12, 3, vectorToBox)) return false; + + // Получаем направляющие рёбер прямоугольника + rect.transform.getAxis(0, axis20); + rect.transform.getAxis(1, axis21); + + // Проверка производных осей + if (!testAxis(box, rect, axis.vCross2(axis10, axis20), 4, vectorToBox)) return false; + if (!testAxis(box, rect, axis.vCross2(axis10, axis21), 5, vectorToBox)) return false; + + if (!testAxis(box, rect, axis.vCross2(axis11, axis20), 6, vectorToBox)) return false; + if (!testAxis(box, rect, axis.vCross2(axis11, axis21), 7, vectorToBox)) return false; + + if (!testAxis(box, rect, axis.vCross2(axis12, axis20), 8, vectorToBox)) return false; + if (!testAxis(box, rect, axis.vCross2(axis12, axis21), 9, vectorToBox)) return false; + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, rect, vectorToBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + findEdgesIntersection(box, rect, vectorToBox, int(bestAxisIndex/2), bestAxisIndex%2, contact); + } + contact.body1 = box.body; + contact.body2 = rect.body; + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + /** + * Выполняет поиск точек контакта бокса с прямоугольником. + * + * @param box бокс + * @param rect прямоугольник + * @param vectorToBox вектор, направленный из центра прямоугольника в центр бокса + * @param faceAxisIdx индекс оси, идентифицирующей полскость столкновения (грань бокса или полскость прямоугольника) + * @param colInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, faceAxisIdx:int, colInfo:Contact):Boolean { + var pnum:int, i:int, v:Vector3, cp:ContactPoint; + if (faceAxisIdx == 0) { + // Столкновение с плоскостью прямоугольника + + // Проверим положение бокса относительно плоскости прямоугольника + rect.transform.getAxis(2, colAxis); + var offset:Number = colAxis.vDot(rPos); + if (bPos.vDot(colAxis) < offset) return false; + + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + box.transform.getAxis(axisIdx, axis); + var dot:Number = axis.vDot(colAxis); + if (dot < 0) dot = -dot; + if (dot > maxDot) { + maxDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Получаем список вершин грани бокса, переводим их в систему координат прямоугольника и выполняем обрезку + // по прямоугольнику. Таким образом получается список потенциальных точек контакта. + box.transform.getAxis(incFaceAxisIdx, axis); + getFaceVertsByAxis(box.hs, incFaceAxisIdx, axis.vDot(colAxis) > 0, verts1); + box.transform.transformVectors(verts1, verts2); + rect.transform.transformVectorsInverse(verts2, verts1); + pnum = clipByRect(rect.hs); + // Проверяем каждую потенциальную точку на принадлежность нижней полуплоскости прямоугольника и добавляем такие точки в список контактов + colInfo.pcount = 0; + for (i = 0; i < pnum; i++) { + v = verts1[i]; + if (v.z < tolerance) { + cp = colInfo.points[colInfo.pcount++]; + rect.transform.transformVector(v, cp.pos); + cp.r1.vDiff(cp.pos, bPos); + cp.r2.vDiff(cp.pos, rPos); + cp.penetration = -v.z; + } + } + colInfo.normal.vCopy(colAxis); + } else { + // Столкновение с гранью бокса + faceAxisIdx--; + box.transform.getAxis(faceAxisIdx, colAxis); + var faceReversed:Boolean = colAxis.vDot(vectorToBox) > 0; + if (!faceReversed) colAxis.vReverse(); + + rect.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) return false; + + getFaceVertsByAxis(rect.hs, 2, false, verts1); + rect.transform.transformVectors(verts1, verts2); + box.transform.transformVectorsInverse(verts2, verts1); + pnum = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + colInfo.pcount = 0; + for (i = 0; i < pnum; i++) { + if ((pen = getPointBoxPenetration(box.hs, verts1[i], faceAxisIdx, faceReversed)) > -tolerance) { + cp = colInfo.points[colInfo.pcount++]; + box.transform.transformVector(verts1[i], cp.pos); + cp.r1.vDiff(cp.pos, bPos); + cp.r2.vDiff(cp.pos, rPos); + cp.penetration = pen; + } + } + colInfo.normal.vCopy(colAxis); + } + return true; + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + private function getFaceVertsByAxis(hs:Vector3, axisIdx:int, reverse:Boolean, result:Vector.):void { + switch (axisIdx) { + case 0: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, -hs.y, hs.z); + } + break; + case 1: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, hs.y, hs.z); + } + break; + case 2: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + } else { + (result[0] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } + break; + } + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + return clipHighX(hs.x, pnum, verts2, verts1); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + } + return 0; + } + + /** + * + * @param hs + * @return + */ + private function clipByRect(hs:Vector3):int { + var pnum:int = 4; + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box первый бокс + * @param rect второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param colInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, axisIdx1:int, axisIdx2:int, colInfo:Contact):void { + box.transform.getAxis(axisIdx1, axis10); + rect.transform.getAxis(axisIdx2, axis20); + colAxis.vCross2(axis10, axis20).vNormalize(); + // Разворот оси в сторону бокса + if (colAxis.vDot(vectorToBox) < 0) colAxis.vReverse(); + + /* Кодирование рёбер + бит 0: 1 (тип контакта ребро-ребро) + биты 1-2: индекс направляющей оси ребра + бит 3: + бит 4: + бит 5: + */ + var edgeCode1:int = 1; + var edgeCode2:int = 1; + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + point1.vCopy(box.hs); + point2.vCopy(rect.hs); + // x + if (axisIdx1 == 0) { + point1.x = 0; + halfLen1 = box.hs.x; + } else { + box.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.x = -point1.x; + edgeCode1 |= 8; // 1 << 3 + } + } + if (axisIdx2 == 0) { + point2.x = 0; + halfLen2 = rect.hs.x; + } else { + rect.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.x = -point2.x; + edgeCode2 |= 8; // 1 << 3 + } + } + // y + if (axisIdx1 == 1) { + point1.y = 0; + halfLen1 = box.hs.y; + edgeCode1 |= 2; // 1 << 1 + } else { + box.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.y = -point1.y; + edgeCode1 |= 16; // 1 << 4 + } + } + if (axisIdx2 == 1) { + point2.y = 0; + halfLen2 = rect.hs.y; + edgeCode2 |= 2; // 1 << 1 + } else { + rect.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.y = -point2.y; + edgeCode2 |= 16; // 1 << 4 + } + } + // z + if (axisIdx1 == 2) { + point1.z = 0; + halfLen1 = box.hs.z; + edgeCode1 |= 4; // 2 << 1 + } else { + box.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.z = -point1.z; + edgeCode1 |= 32; // 1 << 5 + } + } + // Получаем глобальные координаты средних точек рёбер + point1.vTransformBy4(box.transform); + point2.vTransformBy4(rect.transform); + // Находим точку пересечения рёбер, решая систему уравнений + var k:Number = axis10.vDot(axis20); + var det:Number = k*k - 1; + vector.vDiff(point2, point1); + var c1:Number = axis10.vDot(vector); + var c2:Number = axis20.vDot(vector); + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + colInfo.normal.vCopy(colAxis); + colInfo.pcount = 1; + var cp:ContactPoint = colInfo.points[0]; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(point1.x + axis10.x*t1 + point2.x + axis20.x*t2); + cp.pos.y = 0.5*(point1.y + axis10.y*t1 + point2.y + axis20.y*t2); + cp.pos.z = 0.5*(point1.z + axis10.z*t1 + point2.z + axis20.z*t2); + cp.r1.vDiff(cp.pos, bPos); + cp.r2.vDiff(cp.pos, rPos); + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box + * @param rect + * @param axis + * @param axisIndex + * @param vectorToBox + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + if (axis.vLengthSqr() < 0.0001) return true; + axis.vNormalize(); + + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -tolerance) return false; + if (overlap + tolerance < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия вдоль заданной оси. + * + * @param box бокс + * @param rect прямоугольник + * @param axis ось + * @param vectorToBox вектор, соединяющий центр прямоугольника с центром бокса + * @return величина перекрытия вдоль оси + */ + public function overlapOnAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, vectorToBox:Vector3):Number { + var m:Matrix4 = box.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box.hs.z; + if (d < 0) d = -d; + projection += d; + + m = rect.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*rect.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*rect.hs.y; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox.x*axis.x + vectorToBox.y*axis.y + vectorToBox.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/BoxSphereCollider.as b/0.0.4.0/src/alternativa/physics/collision/BoxSphereCollider.as new file mode 100644 index 0000000..bd3e849 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/BoxSphereCollider.as @@ -0,0 +1,113 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + use namespace altphysics; + /** + * + */ + public class BoxSphereCollider implements ICollider { + + private var center:Vector3 = new Vector3(); + private var closestPt:Vector3 = new Vector3(); + + private var bPos:Vector3 = new Vector3(); + private var sPos:Vector3 = new Vector3(); + + /** + * + */ + public function BoxSphereCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + // Зафиксированно столкновение + contact.body1 = sphere.body; + contact.body2 = box.body; + contact.normal.vCopy(closestPt).vTransformBy4(box.transform).vSubtract(sPos).vNormalize().vReverse(); + contact.pcount = 1; + + var cp:ContactPoint = contact.points[0]; + cp.penetration = sphere.r - Math.sqrt(distSqr); + cp.pos.vCopy(contact.normal).vScale(-sphere.r).vAdd(sPos); + cp.r1.vDiff(cp.pos, sPos); + cp.r2.vDiff(cp.pos, bPos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/BoxTriangleCollider.as b/0.0.4.0/src/alternativa/physics/collision/BoxTriangleCollider.as new file mode 100644 index 0000000..652375a --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/BoxTriangleCollider.as @@ -0,0 +1,23 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Contact; + import alternativa.physics.collision.primitives.CollisionPrimitive; + + public class BoxTriangleCollider implements ICollider { + public function BoxTriangleCollider() { + } + + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + return false; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/BruteForceCollisionDetector.as b/0.0.4.0/src/alternativa/physics/collision/BruteForceCollisionDetector.as new file mode 100644 index 0000000..85d84fa --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/BruteForceCollisionDetector.as @@ -0,0 +1,63 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + public class BruteForceCollisionDetector implements ICollisionDetector { + public function BruteForceCollisionDetector() { + } + + public function addPrimitive(primitive:CollisionPrimitive, isStatic:Boolean=true):Boolean { + return false; + } + + public function removePrimitive(primitive:CollisionPrimitive, isStatic:Boolean=true):Boolean { + return false; + } + + public function init():void { + } + + public function getAllCollisions(contacts:Vector.):int { + return 0; + } + + public function intersectRay(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, intersection:RayIntersection):Boolean { + return false; + } + + public function intersectRayWithStatic(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean { + return false; + } + + public function testBodyPrimitiveCollision(body:Body, primitive:CollisionPrimitive):Boolean { + return false; + } + + /** + * + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + return false; + } + + /** + * + * @param prim1 + * @param prim2 + * @return + */ + public function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/CollisionKdNode.as b/0.0.4.0/src/alternativa/physics/collision/CollisionKdNode.as new file mode 100644 index 0000000..e24922c --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/CollisionKdNode.as @@ -0,0 +1,18 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.types.BoundBox; + + public class CollisionKdNode { + public var indices:Vector.; + public var splitIndices:Vector.; + public var boundBox:BoundBox; + public var parent:CollisionKdNode; + public var splitTree:CollisionKdTree2D; + + public var axis:int = -1; // 0 - x, 1 - y, 2 - z + public var coord:Number; + public var positiveNode:CollisionKdNode; + public var negativeNode:CollisionKdNode; + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/CollisionKdTree.as b/0.0.4.0/src/alternativa/physics/collision/CollisionKdTree.as new file mode 100644 index 0000000..c046f17 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/CollisionKdTree.as @@ -0,0 +1,280 @@ +package alternativa.physics.collision { + + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + + /** + * @author mike + */ + public class CollisionKdTree { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + public var rootNode:CollisionKdNode; + public var staticChildren:Vector. = new Vector.(); + public var numStaticChildren:int; + public var staticBoundBoxes:Vector. = new Vector.(); + + private var splitAxis:int; + private var splitCoord:Number; + private var splitCost:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + + /** + * @param primitive + */ + public function addStaticPrimitive(primitive:CollisionPrimitive):void { + staticChildren[numStaticChildren++] = primitive; + } + + /** + * @param primitive + * @return + */ + public function removeStaticPrimitive(primitive:CollisionPrimitive):Boolean { + var idx:int = staticChildren.indexOf(primitive); + if (idx < 0) return false; + staticChildren.splice(idx, 1); + numStaticChildren--; + return true; + } + + /** + * @param boundBox + */ + public function createTree(boundBox:BoundBox = null):void { + if (numStaticChildren == 0) return; + // Создаём корневую ноду + rootNode = new CollisionKdNode(); + rootNode.indices = new Vector.(); + // Расчитываем баунды объектов и рутовой ноды + var rootNodeBoundBox:BoundBox = rootNode.boundBox = (boundBox != null) ? boundBox : new BoundBox(); + for (var i:int = 0; i < numStaticChildren; ++i) { + var child:CollisionPrimitive = staticChildren[i]; + var childBoundBox:BoundBox = staticBoundBoxes[i] = child.calculateAABB(); + rootNodeBoundBox.addBoundBox(childBoundBox); + rootNode.indices[i] = i; + } + staticBoundBoxes.length = numStaticChildren; + // Разделяем рутовую ноду + splitNode(rootNode); + + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + var indices:Vector. = node.indices; + var numPrimitives:int = indices.length; + if (numPrimitives <= minPrimitivesPerNode) return; + + // Подготовка баунда с погрешностями + var nodeBoundBox:BoundBox = node.boundBox; + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + // Собираем опорные координаты + var i:int; + var j:int; + var numSplitCoordsX:int = 0 + var numSplitCoordsY:int = 0; + var numSplitCoordsZ:int = 0; + for (i = 0; i < numPrimitives; ++i) { + var boundBox:BoundBox = staticBoundBoxes[indices[i]]; + + if (boundBox.maxX - boundBox.minX <= doubleThreshold) { + if (boundBox.minX <= nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.minX; + else if (boundBox.maxX >= nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.maxX; + else splitCoordsX[numSplitCoordsX++] = (boundBox.minX + boundBox.maxX)*0.5; + } else { + if (boundBox.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = boundBox.minX; + if (boundBox.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = boundBox.maxX; + } + + if (boundBox.maxY - boundBox.minY <= doubleThreshold) { + if (boundBox.minY <= nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.minY; + else if (boundBox.maxY >= nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.maxY; + else splitCoordsY[numSplitCoordsY++] = (boundBox.minY + boundBox.maxY)*0.5; + } else { + if (boundBox.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = boundBox.minY; + if (boundBox.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = boundBox.maxY; + } + + if (boundBox.maxZ - boundBox.minZ <= doubleThreshold) { + if (boundBox.minZ <= nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.minZ; + else if (boundBox.maxZ >= nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.maxZ; + else splitCoordsZ[numSplitCoordsZ++] = (boundBox.minZ + boundBox.maxZ)*0.5; + } else { + if (boundBox.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.minZ; + if (boundBox.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numPrimitives; ++i) { + boundBox = staticBoundBoxes[indices[i]]; + var min:Number = axisX ? boundBox.minX : (axisY ? boundBox.minY : boundBox.minZ); + var max:Number = axisX ? boundBox.maxX : (axisY ? boundBox.maxY : boundBox.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(indices[i]); + indices[i] = -1; + } else { + if (node.splitIndices == null) node.splitIndices = new Vector.(); + node.splitIndices.push(indices[i]); + indices[i] = -1; + } + } else { + if (min >= coordMin) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(indices[i]); + indices[i] = -1; + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numPrimitives; ++i) { + if (indices[i] >= 0) indices[j++] = indices[i]; + } + if (j > 0) indices.length = j; + else node.indices = null; + + if (node.splitIndices != null) { + node.splitTree = new CollisionKdTree2D(this, node); + node.splitTree.createTree(); + } + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + /** + * + */ + public function traceTree():void { + traceNode("", rootNode); + } + + /** + * @param str + * @param node + */ + private function traceNode(str:String, node:CollisionKdNode):void { + if (node == null) return; + trace(str, node.axis == -1 ? "end" : ((node.axis == 0) ? "X" : ((node.axis == 1) ? "Y" : "Z")), "splitCoord=" + splitCoord, "bound", node.boundBox, "objs:", node.indices); + traceNode(str + "-", node.negativeNode); + traceNode(str + "+", node.positiveNode); + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/CollisionKdTree2D.as b/0.0.4.0/src/alternativa/physics/collision/CollisionKdTree2D.as new file mode 100644 index 0000000..78aaa95 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/CollisionKdTree2D.as @@ -0,0 +1,226 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.types.BoundBox; + + /** + * + */ + public class CollisionKdTree2D { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + + public var parentTree:CollisionKdTree; + public var parentNode:CollisionKdNode; + public var rootNode:CollisionKdNode; + + private var splitAxis:int; + private var splitCost:Number; + private var splitCoord:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + /** + * + * @param parentTree + * @param parentNode + */ + public function CollisionKdTree2D(parentTree:CollisionKdTree, parentNode:CollisionKdNode) { + this.parentTree = parentTree; + this.parentNode = parentNode; + } + + /** + * + */ + public function createTree():void { + rootNode = new CollisionKdNode(); + rootNode.boundBox = parentNode.boundBox.clone(); + rootNode.indices = new Vector.(); + var numObjects:int = parentNode.splitIndices.length; + for (var i:int = 0; i < numObjects; ++i) rootNode.indices[i] = parentNode.splitIndices[i]; + + splitNode(rootNode); + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + if (node.indices.length <= minPrimitivesPerNode) return; + + var objects:Vector. = node.indices; + var i:int; + var j:int; + + var nodeBoundBox:BoundBox = node.boundBox; + + // Подготовка баунда с погрешностями + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + // Собираем опорные координаты + var numSplitCoordsX:int; + var numSplitCoordsY:int; + var numSplitCoordsZ:int; + var numObjects:int = objects.length; + for (i = 0; i < numObjects; ++i) { + var bb:BoundBox = staticBoundBoxes[objects[i]]; + + if (parentNode.axis != 0) { + if (bb.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = bb.minX; + if (bb.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = bb.maxX; + } + + if (parentNode.axis != 1) { + if (bb.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = bb.minY; + if (bb.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = bb.maxY; + } + + if (parentNode.axis != 2) { + if (bb.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = bb.minZ; + if (bb.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = bb.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + if (parentNode.axis != 0) checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + if (parentNode.axis != 1) checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + if (parentNode.axis != 2) checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numObjects; ++i) { + bb = staticBoundBoxes[objects[i]]; + var min:Number = axisX ? bb.minX : (axisY ? bb.minY : bb.minZ); + var max:Number = axisX ? bb.maxX : (axisY ? bb.maxY : bb.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(objects[i]); + objects[i] = -1; + } + } else { + if (min >= coordMin) { + if (max > coordMax) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(objects[i]); + objects[i] = -1; + } + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numObjects; ++i) { + if (objects[i] >= 0) objects[j++] = objects[i]; + } + if (j > 0) objects.length = j; + else node.indices = null; + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/IBodyCollisionPredicate.as b/0.0.4.0/src/alternativa/physics/collision/IBodyCollisionPredicate.as new file mode 100644 index 0000000..9cfdca5 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/IBodyCollisionPredicate.as @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Body; + + public interface IBodyCollisionPredicate { + function considerBodies(body1:Body, body2:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/ICollider.as b/0.0.4.0/src/alternativa/physics/collision/ICollider.as new file mode 100644 index 0000000..eef9cb9 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/ICollider.as @@ -0,0 +1,14 @@ +package alternativa.physics.collision { + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + + /** + * Интерфейс определителя столкновений между двумя примитивами. + */ + public interface ICollider { + function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/ICollisionDetector.as b/0.0.4.0/src/alternativa/physics/collision/ICollisionDetector.as new file mode 100644 index 0000000..af43dfb --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/ICollisionDetector.as @@ -0,0 +1,92 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + /** + * Интерфейс детектора столкновений. + */ + public interface ICollisionDetector { + + /** + * Добавляет физический примитив в коллайдер. + * + * @param primitive добавляемый примитив + * @param isStatic указывает тип примитива: статический или динамический + * @return true если примитив был успешно добавлен, иначе false + */ + function addPrimitive(primitive:CollisionPrimitive, isStatic:Boolean = true):Boolean; + + /** + * Удаляет физический примитив из коллайдера. + * + * @param primitive удаляемый примитив + * @param isStatic указывает тип примитива: статический или динамический + * @return true если примитив был успшено удалён, иначе false + */ + function removePrimitive(primitive:CollisionPrimitive, isStatic:Boolean = true):Boolean; + + /** + * Выполняет инициализацию детектора после обновления списка примитивов. + */ + function init():void; + + /** + * Получает все столкновения в текущей конфигурации физической геометрии. + * + * @param contacts список контактов, в кторые будет записана информация о столкновении + * @return количество найденных столкновений + */ + function getAllCollisions(contacts:Vector.):int; + + /** + * Тестирует луч на пересечение с физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param dir направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRay(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * Тестирует луч на пересечение со статической физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param dir направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRayWithStatic(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @return + */ + function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/ICollisionPredicate.as b/0.0.4.0/src/alternativa/physics/collision/ICollisionPredicate.as new file mode 100644 index 0000000..bb1b756 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/ICollisionPredicate.as @@ -0,0 +1,9 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + + public interface ICollisionPredicate { + + function considerCollision(primitive:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/IRayCollisionPredicate.as b/0.0.4.0/src/alternativa/physics/collision/IRayCollisionPredicate.as new file mode 100644 index 0000000..0f3d929 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/IRayCollisionPredicate.as @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Body; + + public interface IRayCollisionPredicate { + function considerBody(body:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/KdTreeCollisionDetector.as b/0.0.4.0/src/alternativa/physics/collision/KdTreeCollisionDetector.as new file mode 100644 index 0000000..1284123 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/KdTreeCollisionDetector.as @@ -0,0 +1,483 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Детектор, хранящий статическую геометрию в kD-дереве и использующий дерево для ускорения тестов на пересечения. + */ + public class KdTreeCollisionDetector implements ICollisionDetector { + + altphysics var tree:CollisionKdTree; + altphysics var dynamicPrimitives:Vector.; + 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.(); + + addCollider(CollisionPrimitive.BOX, CollisionPrimitive.BOX, new BoxBoxCollider()); +// addCollider(CollisionPrimitive.BOX, CollisionPrimitive.PLANE, new BoxPlaneCollider()); + addCollider(CollisionPrimitive.BOX, CollisionPrimitive.SPHERE, new BoxSphereCollider()); + addCollider(CollisionPrimitive.BOX, CollisionPrimitive.RECT, new BoxRectCollider()); + +// 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():void { + tree.createTree(); +// tree.traceTree(); + } + + /** + * + * @param contacts + * @return + */ + public function getAllCollisions(contacts:Vector.):int { + var colNum:int = 0; + for (var i:int = 0; i < dynamicPrimitivesNum; i++) { + var primitive:CollisionPrimitive = dynamicPrimitives[i]; + primitive.calculateAABB(); + if (primitive.body != null && primitive.body.frozen) continue; + colNum += getPrimitiveNodeCollisions(tree.rootNode, primitive, contacts, colNum); + + // Столкновения динамических примитивов между собой + // TODO: Попробовать различные оптимизации (сортировка по баундам, встраивание в дерево) + for (var j:int = i + 1; j < dynamicPrimitivesNum; j++) { + if (collide(primitive, dynamicPrimitives[j], contacts[colNum])) colNum++; + } + } + + return colNum; + } + + /** + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + public function collide(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.collide(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 = (contact.points[0] as ContactPoint).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.testCollision(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:Vector., startIndex:int):int { + var colNum:int = 0; + if (node.indices != null) { + // Поиск столкновений со статическими примитивами узла + var primitives:Vector. = tree.staticChildren; + var indices:Vector. = node.indices; + for (var i:int = indices.length - 1; i >= 0; i--) + if (collide(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; + } + + 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.getSegmentIntersection(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., indices:Vector., 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.getSegmentIntersection(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; + } \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/SpherePlaneCollider.as b/0.0.4.0/src/alternativa/physics/collision/SpherePlaneCollider.as new file mode 100644 index 0000000..612c0d7 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/SpherePlaneCollider.as @@ -0,0 +1,67 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + /** + * + */ + public class SpherePlaneCollider implements ICollider { + + private var normal:Vector3 = new Vector3(); + + /** + * + */ + public function SpherePlaneCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var sphere:RigidSphere = body1 as RigidSphere; +// var plane:RigidPlane; +// if (sphere == null) { +// sphere = body2 as RigidSphere; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// +// var dist:Number = sphere.state.pos.dot(normal) - offset; +// if (dist > sphere.r) return false; +// +// collisionInfo.body1 = sphere; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// collisionInfo.pcount = 1; +// +// var cp:ContactPoint = collisionInfo.points[0]; +// cp.penetration = sphere.r - dist; +// cp.pos.copy(normal).reverse().scale(sphere.r).add(sphere.state.pos); +// cp.r1.diff(cp.pos, sphere.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/SphereSphereCollider.as b/0.0.4.0/src/alternativa/physics/collision/SphereSphereCollider.as new file mode 100644 index 0000000..352f2f2 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/SphereSphereCollider.as @@ -0,0 +1,69 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + public class SphereSphereCollider implements ICollider { + + private var p1:Vector3 = new Vector3(); + private var p2:Vector3 = new Vector3(); + + public function SphereSphereCollider() { + } + + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var s1:CollisionSphere; + var s2:CollisionSphere; + if (prim1.body != null) { + s1 = prim1 as CollisionSphere; + s2 = prim2 as CollisionSphere; + } else { + s1 = prim2 as CollisionSphere; + s2 = prim1 as CollisionSphere; + } + + s1.transform.getAxis(3, p1); + s2.transform.getAxis(3, p2); + var dx:Number = p1.x - p2.x; + var dy:Number = p1.y - p2.y; + var dz:Number = p1.z - p2.z; + var len:Number = dx*dx + dy*dy + dz*dz; + var sum:Number = s1.r + s2.r; + if (len > sum*sum) return false; + len = Math.sqrt(len); + dx /= len; + dy /= len; + dz /= len; + + contact.body1 = s1.body; + contact.body2 = s2.body; + contact.normal.x = dx; + contact.normal.y = dy; + contact.normal.z = dz; + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = sum - len; + cp.pos.x = p1.x - dx*s1.r; + cp.pos.y = p1.y - dy*s1.r; + cp.pos.z = p1.z - dz*s1.r; + cp.r1.vDiff(cp.pos, p1); + cp.r2.vDiff(cp.pos, p2); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/primitives/.svn/all-wcprops b/0.0.4.0/src/alternativa/physics/collision/primitives/.svn/all-wcprops new file mode 100644 index 0000000..2301292 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/primitives/.svn/all-wcprops @@ -0,0 +1,35 @@ +K 25 +svn:wc:ra_dav:version-url +V 124 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/collision/primitives +END +CollisionTriangle.as +K 25 +svn:wc:ra_dav:version-url +V 145 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/collision/primitives/CollisionTriangle.as +END +CollisionSphere.as +K 25 +svn:wc:ra_dav:version-url +V 143 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/collision/primitives/CollisionSphere.as +END +CollisionBox.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/collision/primitives/CollisionBox.as +END +CollisionPrimitive.as +K 25 +svn:wc:ra_dav:version-url +V 146 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/collision/primitives/CollisionPrimitive.as +END +CollisionRect.as +K 25 +svn:wc:ra_dav:version-url +V 141 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/collision/primitives/CollisionRect.as +END diff --git a/0.0.4.0/src/alternativa/physics/collision/primitives/.svn/entries b/0.0.4.0/src/alternativa/physics/collision/primitives/.svn/entries new file mode 100644 index 0000000..66d99a3 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/primitives/.svn/entries @@ -0,0 +1,88 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/collision/primitives +http://svndev.alternativaplatform.com + + + +2009-07-20T03:48:26.517738Z +16522 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +CollisionTriangle.as +file + + + + +2010-10-28T04:32:41.000000Z +a1ccd11eb25d172d2739fce7fef21ee2 +2009-06-08T06:46:18.310732Z +14099 +mike + +CollisionSphere.as +file + + + + +2010-10-28T04:32:41.000000Z +1ca67429cc8bf27435b6ec46f8d6f80a +2009-05-26T08:41:33.471001Z +13389 +mike + +CollisionBox.as +file + + + + +2010-10-28T04:32:41.000000Z +e6a441a4317416b0d13d365f5b26ace4 +2009-06-08T07:53:17.170410Z +14100 +mike + +CollisionPrimitive.as +file + + + + +2010-10-28T04:32:41.000000Z +ba2d42da95941781dfd027568c57888c +2009-06-25T09:18:57.805913Z +15117 +mike + +CollisionRect.as +file + + + + +2010-10-28T04:32:41.000000Z +35b591916c051038ea65b31a1964b82a +2009-07-20T03:48:26.517738Z +16522 +mike + diff --git a/0.0.4.0/src/alternativa/physics/collision/primitives/.svn/format b/0.0.4.0/src/alternativa/physics/collision/primitives/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/primitives/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.4.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base b/0.0.4.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base new file mode 100644 index 0000000..e3d90cc --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base @@ -0,0 +1,178 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + /** + * Ориентированный бокс. + */ + public class CollisionBox extends CollisionPrimitive { + + // Половинные размеры вдоль каждой из осей + public var hs:Vector3 = new Vector3(); + + // Используются в определении пересечения с лучом + private static var _o:Vector3 = new Vector3(); + private static var _v:Vector3 = new Vector3(); + + /** + * @param hs + * @param collisionGroup + */ + public function CollisionBox(hs:Vector3, collisionGroup:int) { + super(BOX, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + var t:Matrix4 = transform; + aabb.maxX = hs.x*(t.a < 0 ? -t.a : t.a) + hs.y*(t.b < 0 ? -t.b : t.b) + hs.z*(t.c < 0 ? -t.c : t.c); + aabb.minX = -aabb.maxX; + + aabb.maxY = hs.x*(t.e < 0 ? -t.e : t.e) + hs.y*(t.f < 0 ? -t.f : t.f) + hs.z*(t.g < 0 ? -t.g : t.g); + aabb.minY = -aabb.maxY; + + aabb.maxZ = hs.x*(t.i < 0 ? -t.i : t.i) + hs.y*(t.j < 0 ? -t.j : t.j) + hs.z*(t.k < 0 ? -t.k : t.k); + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var box:CollisionBox = source as CollisionBox; + if (box == null) return this; + super.copyFrom(box); + hs.vCopy(box.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionBox(hs, collisionGroup); + } + + /** + * Вычисляет параметры пересечения заданного сегмента с примитивом. + * + * @param origin начальная точка сегмента в мировых координатах + * @param vector вектор сегмента в мировых координатах + * @param threshold погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную + * погрешность, считается равной нулю. + * @param normal возвращаемое значение. Нормаль к примитиву в точке пересечения с сегментом. + * @return в случае наличия пересечения возвращается время точки пересечения, в противном случае возвращается -1. + */ + override public function getSegmentIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var tMin:Number = -1, tMax:Number = 1e308, t1:Number, t2:Number; + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var xx:Number = origin.x - transform.d; + var yy:Number = origin.y - transform.h; + var zz:Number = origin.z - transform.l; + _o.x = transform.a*xx + transform.e*yy + transform.i*zz; + _o.y = transform.b*xx + transform.f*yy + transform.j*zz; + _o.z = transform.c*xx + transform.g*yy + transform.k*zz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + _v.x = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + _v.y = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + _v.z = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + // X + if (_v.x < threshold && _v.x > -threshold) { + if (_o.x < -hs.x || _o.x > hs.x) return -1; + } else { + t1 = (-hs.x - _o.x)/_v.x; + t2 = (hs.x - _o.x)/_v.x; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.x = -1; + normal.y = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.x = 1; + normal.y = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Y + if (_v.y < threshold && _v.y > -threshold) { + if (_o.y < -hs.y || _o.y > hs.y) return -1; + } else { + t1 = (-hs.y - _o.y)/_v.y; + t2 = (hs.y - _o.y)/_v.y; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.y = -1; + normal.x = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.y = 1; + normal.x = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Z + if (_v.z < threshold && _v.z > -threshold) { + if (_o.z < -hs.z || _o.z > hs.z) return -1; + } else { + t1 = (-hs.z - _o.z)/_v.z; + t2 = (hs.z - _o.z)/_v.z; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.z = -1; + normal.x = normal.y = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.z = 1; + normal.x = normal.y = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Перевод нормали в мировую систему координат + // Inlined normal.vDeltaTransformBy4(transform); + xx = normal.x, yy = normal.y, zz = normal.z; + normal.x = transform.a*xx + transform.b*yy + transform.c*zz; + normal.y = transform.e*xx + transform.f*yy + transform.g*zz; + normal.z = transform.i*xx + transform.j*yy + transform.k*zz; + + return tMin; + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionPrimitive.as.svn-base b/0.0.4.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionPrimitive.as.svn-base new file mode 100644 index 0000000..352a940 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionPrimitive.as.svn-base @@ -0,0 +1,130 @@ +package alternativa.physics.collision.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.ICollisionPredicate; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Базовый класс для примитивов, использующихся детектором столкновений. + */ + public class CollisionPrimitive { + // Константы типов примитива + public static const BOX:int = 1; + public static const PLANE:int = 2; + public static const SPHERE:int = 3; + public static const RECT:int = 4; + public static const TRIANGLE:int = 5; + + // Тип примитива + public var type:int; + // Группы примитива. Каждая группа определяется установленным битом. Столкновения проверяются только для примитивов, + // имеющих хотя бы одну общую группу. + public var collisionGroup:int; + // Предикат примитива, вызывающийся детектором при нахождении столкновения. В зависимости от возвращённого + // предикатом результата столкновение либо регистрируется, либо игнорируется. + // Nullable + public var postCollisionPredicate:ICollisionPredicate; + + // Тело, владеющее примитивом. + // Nullable + altphysics var body:Body; + // Трансформация примитива в системе координат тела, если оно указано. Не допускается масштабирование матрицы. + altphysics var localTransform:Matrix4 = new Matrix4(); + // Полная трансформация примитива. Не допускается масштабирование матрицы. + altphysics var transform:Matrix4 = new Matrix4(); + // AABB в мировой системе координат. Расчитывается системой вызовом функции calculateBoundBox(). + altphysics var aabb:BoundBox = new BoundBox(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param type тип примитива + * @param collisionGroup группа примитива + */ + public function CollisionPrimitive(type:int, collisionGroup:int) { + this.type = type; + this.collisionGroup = collisionGroup; + } + + /** + * Устанавливает тело, владеющее примитивом. + * + * @param body тело, к которому относится примитив. Если указано значение null, матрица локальной трансформации удаляется. + * @param localTransform трансформация примитива в системе координат тела. Указание значения null равносильно + * заданию единичной матрицы. + */ + public function setBody(body:Body, localTransform:Matrix4 = null):void { + if (this.body == body) return; + this.body = body; + if (body == null) this.localTransform = null; + else { + if (this.localTransform == null) this.localTransform = new Matrix4(); + if (localTransform != null) this.localTransform.copy(localTransform); + } + } + + /** + * Наследники должны переопределять этот метод, реализуя в нём корректный расчёт ограничивающего бокса, выравненного + * по осям мировой системы координат. + * + * @return ссылка на свой баунд бокс + */ + public function calculateAABB():BoundBox { + return aabb; + } + + /** + * Клонирует примитив. Переопределять не рекомендуется. Вместо этого переопределяются методы createPrimitive() и copyFrom(). + * + * @return клон примитива + */ + public function clone():CollisionPrimitive { + var p:CollisionPrimitive = createPrimitive(); + return p.copyFrom(this); + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + type = source.type; + transform.copy(source.transform); + collisionGroup = source.collisionGroup; + setBody(source.body, source.localTransform); + aabb.copyFrom(source.aabb); + return this; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + protected function createPrimitive():CollisionPrimitive { + return new CollisionPrimitive(type, collisionGroup); + } + + /** + * Метод должен вычислять параметры пересечения заданного сегмента с примитивом. + * + * @param origin начальная точка сегмента в мировых координатах + * @param vector вектор сегмента в мировых координатах + * @param threshold погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную + * погрешность, считается равной нулю. + * @param normal возвращаемое значение. Нормаль к примитиву в точке пересечения с сегментом. + * @return в случае наличия пересечения возвращается время точки пересечения, в противном случае возвращается -1. + */ + public function getSegmentIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + return -1; + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base b/0.0.4.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base new file mode 100644 index 0000000..e0ee128 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base @@ -0,0 +1,126 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + /** + * Ориентированный прямоугольник. Задаётся половинами размеров вдоль осей X (ширина) и Y (длина) локальной системы + * координат примитива. Таким образом, прямоугольник лежит в плоскости XY, его стороны параллельны осям этой + * плоскости, а нормаль направлена вдоль локальной оси Z. + * + * Прямоугольник может быть одно- или двусторонним. В случае одностороннего прямоугольника, + * столкновения с внутренней стороны не регистрируются. + */ + public class CollisionRect extends CollisionPrimitive { + // Половинные размеры прямоугольника вдоль осей X и Y. Нормаль направлена вдоль оси Z. + public var hs:Vector3 = new Vector3(); + // Флаг указывает, является примитив одно- или двусторонним + public var twoSided:Boolean = true; + + // Малое значение. Используется для указания фиктивной высоты примитива и в функции определения пересечения с сегментом. + private static const EPSILON:Number = 0.005; + // Переменные используются в функции поиска пересечения с лучом + private static var _o:Vector3 = new Vector3(); + private static var _v:Vector3 = new Vector3(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param hs половинные размерв прямоугольника вдоль осей X и Y. Значение z игнорируется. + * @param collisionGroup группа примитива + */ + public function CollisionRect(hs:Vector3, collisionGroup:int) { + super(RECT, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * Расчитывает ограничивающий бокс прямоугольника. Для избежания проблем высота примитива принимается равной + * не нулю, а малому значению. + * + * @return + */ + override public function calculateAABB():BoundBox { + // Баунд бокс прямоугольника имеет минимальную высоту, отличную от нуля во избежание проблем с построением kd-дерева + var t:Matrix4 = transform; + aabb.maxX = hs.x*(t.a < 0 ? -t.a : t.a) + hs.y*(t.b < 0 ? -t.b : t.b) + EPSILON*(t.c < 0 ? -t.c : t.c); + aabb.minX = -aabb.maxX; + + aabb.maxY = hs.x*(t.e < 0 ? -t.e : t.e) + hs.y*(t.f < 0 ? -t.f : t.f) + EPSILON*(t.g < 0 ? -t.g : t.g); + aabb.minY = -aabb.maxY; + + aabb.maxZ = hs.x*(t.i < 0 ? -t.i : t.i) + hs.y*(t.j < 0 ? -t.j : t.j) + EPSILON*(t.k < 0 ? -t.k : t.k); + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var rect:CollisionRect = source as CollisionRect; + if (rect == null) return this; + super.copyFrom(rect); + hs.vCopy(rect.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionRect(hs, collisionGroup); + } + + /** + * @param origin + * @param vector + * @param collisionGroup + * @param threshold + * @param normal + * @return + */ + override public function getSegmentIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var xx:Number = origin.x - transform.d; + var yy:Number = origin.y - transform.h; + var zz:Number = origin.z - transform.l; + _o.x = transform.a*xx + transform.e*yy + transform.i*zz; + _o.y = transform.b*xx + transform.f*yy + transform.j*zz; + _o.z = transform.c*xx + transform.g*yy + transform.k*zz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + _v.x = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + _v.y = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + _v.z = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + + // Проверка параллельности сегмента и плоскости примитива + if (_v.z > -threshold && _v.z < threshold) return -1; + var t:Number = -_o.z/_v.z; + if (t < 0) return -1; + // Проверка вхождения точки пересечения в прямоугольник + _o.x += _v.x*t; + _o.y += _v.y*t; + _o.z = 0; + if (_o.x < (-hs.x - EPSILON) || _o.x > (hs.x + EPSILON) || _o.y < (-hs.y - EPSILON) || _o.y > (hs.y + EPSILON)) return -1; + + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + return t; + } + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base b/0.0.4.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base new file mode 100644 index 0000000..1a9e6d1 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base @@ -0,0 +1,80 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + + /** + * Сфера. + */ + public class CollisionSphere extends CollisionPrimitive { + + // Радиус сферы + public var r:Number = 0; + + /** + * + * @param r + * @param collisionGroup + */ + public function CollisionSphere(r:Number, collisionGroup:int) { + super(SPHERE, collisionGroup); + this.r = r; + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + aabb.maxX = transform.d + r; + aabb.minX = transform.d - r; + + aabb.maxY = transform.h + r; + aabb.minY = transform.h - r; + + aabb.maxZ = transform.l + r; + aabb.minZ = transform.l - r; + + return aabb; + } + + /** + * @param origin + * @param vector + * @param threshold + * @param normal + * @return + */ + override public function getSegmentIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var px:Number = origin.x - transform.d; + var py:Number = origin.y - transform.h; + var pz:Number = origin.z - transform.l; + var k:Number = vector.x*px + vector.y*py + vector.z*pz; + if (k > 0) return -1; + var a:Number = vector.x*vector.x + vector.y*vector.y + vector.z*vector.z; + var D:Number = k*k - a*(px*px + py*py + pz*pz - r*r); + if (D < 0) return -1; + return -(k + Math.sqrt(D))/a; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var sphere:CollisionSphere = source as CollisionSphere; + if (sphere == null) return this; + super.copyFrom(sphere); + r = sphere.r; + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionSphere(r, collisionGroup); + } + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base b/0.0.4.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base new file mode 100644 index 0000000..a48803a --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base @@ -0,0 +1,39 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + public class CollisionTriangle extends CollisionPrimitive { + + public var v0:Vector3 = new Vector3(); + public var v1:Vector3 = new Vector3(); + public var v2:Vector3 = new Vector3(); + public var e0:Vector3 = new Vector3(); + public var e1:Vector3 = new Vector3(); + public var e2:Vector3 = new Vector3(); + public var len0:Number; + public var len1:Number; + public var len2:Number; + + public function CollisionTriangle(v0:Vector3, v1:Vector3, v2:Vector3, collisionGroup:int) { + super(TRIANGLE, collisionGroup); + + this.v0.vCopy(v0); + this.v1.vCopy(v1); + this.v2.vCopy(v2); + + e0.vDiff(v1, v0); + len0 = e0.vLength(); + e0.vNormalize(); + + e1.vDiff(v2, v1); + len1 = e1.vLength(); + e1.vNormalize(); + + e2.vDiff(v0, v2); + len2 = e2.vLength(); + e2.vNormalize(); + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/primitives/CollisionBox.as b/0.0.4.0/src/alternativa/physics/collision/primitives/CollisionBox.as new file mode 100644 index 0000000..e3d90cc --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/primitives/CollisionBox.as @@ -0,0 +1,178 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + /** + * Ориентированный бокс. + */ + public class CollisionBox extends CollisionPrimitive { + + // Половинные размеры вдоль каждой из осей + public var hs:Vector3 = new Vector3(); + + // Используются в определении пересечения с лучом + private static var _o:Vector3 = new Vector3(); + private static var _v:Vector3 = new Vector3(); + + /** + * @param hs + * @param collisionGroup + */ + public function CollisionBox(hs:Vector3, collisionGroup:int) { + super(BOX, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + var t:Matrix4 = transform; + aabb.maxX = hs.x*(t.a < 0 ? -t.a : t.a) + hs.y*(t.b < 0 ? -t.b : t.b) + hs.z*(t.c < 0 ? -t.c : t.c); + aabb.minX = -aabb.maxX; + + aabb.maxY = hs.x*(t.e < 0 ? -t.e : t.e) + hs.y*(t.f < 0 ? -t.f : t.f) + hs.z*(t.g < 0 ? -t.g : t.g); + aabb.minY = -aabb.maxY; + + aabb.maxZ = hs.x*(t.i < 0 ? -t.i : t.i) + hs.y*(t.j < 0 ? -t.j : t.j) + hs.z*(t.k < 0 ? -t.k : t.k); + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var box:CollisionBox = source as CollisionBox; + if (box == null) return this; + super.copyFrom(box); + hs.vCopy(box.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionBox(hs, collisionGroup); + } + + /** + * Вычисляет параметры пересечения заданного сегмента с примитивом. + * + * @param origin начальная точка сегмента в мировых координатах + * @param vector вектор сегмента в мировых координатах + * @param threshold погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную + * погрешность, считается равной нулю. + * @param normal возвращаемое значение. Нормаль к примитиву в точке пересечения с сегментом. + * @return в случае наличия пересечения возвращается время точки пересечения, в противном случае возвращается -1. + */ + override public function getSegmentIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var tMin:Number = -1, tMax:Number = 1e308, t1:Number, t2:Number; + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var xx:Number = origin.x - transform.d; + var yy:Number = origin.y - transform.h; + var zz:Number = origin.z - transform.l; + _o.x = transform.a*xx + transform.e*yy + transform.i*zz; + _o.y = transform.b*xx + transform.f*yy + transform.j*zz; + _o.z = transform.c*xx + transform.g*yy + transform.k*zz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + _v.x = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + _v.y = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + _v.z = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + // X + if (_v.x < threshold && _v.x > -threshold) { + if (_o.x < -hs.x || _o.x > hs.x) return -1; + } else { + t1 = (-hs.x - _o.x)/_v.x; + t2 = (hs.x - _o.x)/_v.x; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.x = -1; + normal.y = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.x = 1; + normal.y = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Y + if (_v.y < threshold && _v.y > -threshold) { + if (_o.y < -hs.y || _o.y > hs.y) return -1; + } else { + t1 = (-hs.y - _o.y)/_v.y; + t2 = (hs.y - _o.y)/_v.y; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.y = -1; + normal.x = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.y = 1; + normal.x = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Z + if (_v.z < threshold && _v.z > -threshold) { + if (_o.z < -hs.z || _o.z > hs.z) return -1; + } else { + t1 = (-hs.z - _o.z)/_v.z; + t2 = (hs.z - _o.z)/_v.z; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.z = -1; + normal.x = normal.y = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.z = 1; + normal.x = normal.y = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Перевод нормали в мировую систему координат + // Inlined normal.vDeltaTransformBy4(transform); + xx = normal.x, yy = normal.y, zz = normal.z; + normal.x = transform.a*xx + transform.b*yy + transform.c*zz; + normal.y = transform.e*xx + transform.f*yy + transform.g*zz; + normal.z = transform.i*xx + transform.j*yy + transform.k*zz; + + return tMin; + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/primitives/CollisionPrimitive.as b/0.0.4.0/src/alternativa/physics/collision/primitives/CollisionPrimitive.as new file mode 100644 index 0000000..352a940 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/primitives/CollisionPrimitive.as @@ -0,0 +1,130 @@ +package alternativa.physics.collision.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.ICollisionPredicate; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Базовый класс для примитивов, использующихся детектором столкновений. + */ + public class CollisionPrimitive { + // Константы типов примитива + public static const BOX:int = 1; + public static const PLANE:int = 2; + public static const SPHERE:int = 3; + public static const RECT:int = 4; + public static const TRIANGLE:int = 5; + + // Тип примитива + public var type:int; + // Группы примитива. Каждая группа определяется установленным битом. Столкновения проверяются только для примитивов, + // имеющих хотя бы одну общую группу. + public var collisionGroup:int; + // Предикат примитива, вызывающийся детектором при нахождении столкновения. В зависимости от возвращённого + // предикатом результата столкновение либо регистрируется, либо игнорируется. + // Nullable + public var postCollisionPredicate:ICollisionPredicate; + + // Тело, владеющее примитивом. + // Nullable + altphysics var body:Body; + // Трансформация примитива в системе координат тела, если оно указано. Не допускается масштабирование матрицы. + altphysics var localTransform:Matrix4 = new Matrix4(); + // Полная трансформация примитива. Не допускается масштабирование матрицы. + altphysics var transform:Matrix4 = new Matrix4(); + // AABB в мировой системе координат. Расчитывается системой вызовом функции calculateBoundBox(). + altphysics var aabb:BoundBox = new BoundBox(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param type тип примитива + * @param collisionGroup группа примитива + */ + public function CollisionPrimitive(type:int, collisionGroup:int) { + this.type = type; + this.collisionGroup = collisionGroup; + } + + /** + * Устанавливает тело, владеющее примитивом. + * + * @param body тело, к которому относится примитив. Если указано значение null, матрица локальной трансформации удаляется. + * @param localTransform трансформация примитива в системе координат тела. Указание значения null равносильно + * заданию единичной матрицы. + */ + public function setBody(body:Body, localTransform:Matrix4 = null):void { + if (this.body == body) return; + this.body = body; + if (body == null) this.localTransform = null; + else { + if (this.localTransform == null) this.localTransform = new Matrix4(); + if (localTransform != null) this.localTransform.copy(localTransform); + } + } + + /** + * Наследники должны переопределять этот метод, реализуя в нём корректный расчёт ограничивающего бокса, выравненного + * по осям мировой системы координат. + * + * @return ссылка на свой баунд бокс + */ + public function calculateAABB():BoundBox { + return aabb; + } + + /** + * Клонирует примитив. Переопределять не рекомендуется. Вместо этого переопределяются методы createPrimitive() и copyFrom(). + * + * @return клон примитива + */ + public function clone():CollisionPrimitive { + var p:CollisionPrimitive = createPrimitive(); + return p.copyFrom(this); + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + type = source.type; + transform.copy(source.transform); + collisionGroup = source.collisionGroup; + setBody(source.body, source.localTransform); + aabb.copyFrom(source.aabb); + return this; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + protected function createPrimitive():CollisionPrimitive { + return new CollisionPrimitive(type, collisionGroup); + } + + /** + * Метод должен вычислять параметры пересечения заданного сегмента с примитивом. + * + * @param origin начальная точка сегмента в мировых координатах + * @param vector вектор сегмента в мировых координатах + * @param threshold погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную + * погрешность, считается равной нулю. + * @param normal возвращаемое значение. Нормаль к примитиву в точке пересечения с сегментом. + * @return в случае наличия пересечения возвращается время точки пересечения, в противном случае возвращается -1. + */ + public function getSegmentIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + return -1; + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/primitives/CollisionRect.as b/0.0.4.0/src/alternativa/physics/collision/primitives/CollisionRect.as new file mode 100644 index 0000000..e0ee128 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/primitives/CollisionRect.as @@ -0,0 +1,126 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + /** + * Ориентированный прямоугольник. Задаётся половинами размеров вдоль осей X (ширина) и Y (длина) локальной системы + * координат примитива. Таким образом, прямоугольник лежит в плоскости XY, его стороны параллельны осям этой + * плоскости, а нормаль направлена вдоль локальной оси Z. + * + * Прямоугольник может быть одно- или двусторонним. В случае одностороннего прямоугольника, + * столкновения с внутренней стороны не регистрируются. + */ + public class CollisionRect extends CollisionPrimitive { + // Половинные размеры прямоугольника вдоль осей X и Y. Нормаль направлена вдоль оси Z. + public var hs:Vector3 = new Vector3(); + // Флаг указывает, является примитив одно- или двусторонним + public var twoSided:Boolean = true; + + // Малое значение. Используется для указания фиктивной высоты примитива и в функции определения пересечения с сегментом. + private static const EPSILON:Number = 0.005; + // Переменные используются в функции поиска пересечения с лучом + private static var _o:Vector3 = new Vector3(); + private static var _v:Vector3 = new Vector3(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param hs половинные размерв прямоугольника вдоль осей X и Y. Значение z игнорируется. + * @param collisionGroup группа примитива + */ + public function CollisionRect(hs:Vector3, collisionGroup:int) { + super(RECT, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * Расчитывает ограничивающий бокс прямоугольника. Для избежания проблем высота примитива принимается равной + * не нулю, а малому значению. + * + * @return + */ + override public function calculateAABB():BoundBox { + // Баунд бокс прямоугольника имеет минимальную высоту, отличную от нуля во избежание проблем с построением kd-дерева + var t:Matrix4 = transform; + aabb.maxX = hs.x*(t.a < 0 ? -t.a : t.a) + hs.y*(t.b < 0 ? -t.b : t.b) + EPSILON*(t.c < 0 ? -t.c : t.c); + aabb.minX = -aabb.maxX; + + aabb.maxY = hs.x*(t.e < 0 ? -t.e : t.e) + hs.y*(t.f < 0 ? -t.f : t.f) + EPSILON*(t.g < 0 ? -t.g : t.g); + aabb.minY = -aabb.maxY; + + aabb.maxZ = hs.x*(t.i < 0 ? -t.i : t.i) + hs.y*(t.j < 0 ? -t.j : t.j) + EPSILON*(t.k < 0 ? -t.k : t.k); + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var rect:CollisionRect = source as CollisionRect; + if (rect == null) return this; + super.copyFrom(rect); + hs.vCopy(rect.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionRect(hs, collisionGroup); + } + + /** + * @param origin + * @param vector + * @param collisionGroup + * @param threshold + * @param normal + * @return + */ + override public function getSegmentIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var xx:Number = origin.x - transform.d; + var yy:Number = origin.y - transform.h; + var zz:Number = origin.z - transform.l; + _o.x = transform.a*xx + transform.e*yy + transform.i*zz; + _o.y = transform.b*xx + transform.f*yy + transform.j*zz; + _o.z = transform.c*xx + transform.g*yy + transform.k*zz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + _v.x = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + _v.y = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + _v.z = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + + // Проверка параллельности сегмента и плоскости примитива + if (_v.z > -threshold && _v.z < threshold) return -1; + var t:Number = -_o.z/_v.z; + if (t < 0) return -1; + // Проверка вхождения точки пересечения в прямоугольник + _o.x += _v.x*t; + _o.y += _v.y*t; + _o.z = 0; + if (_o.x < (-hs.x - EPSILON) || _o.x > (hs.x + EPSILON) || _o.y < (-hs.y - EPSILON) || _o.y > (hs.y + EPSILON)) return -1; + + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + return t; + } + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/primitives/CollisionSphere.as b/0.0.4.0/src/alternativa/physics/collision/primitives/CollisionSphere.as new file mode 100644 index 0000000..1a9e6d1 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/primitives/CollisionSphere.as @@ -0,0 +1,80 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + + /** + * Сфера. + */ + public class CollisionSphere extends CollisionPrimitive { + + // Радиус сферы + public var r:Number = 0; + + /** + * + * @param r + * @param collisionGroup + */ + public function CollisionSphere(r:Number, collisionGroup:int) { + super(SPHERE, collisionGroup); + this.r = r; + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + aabb.maxX = transform.d + r; + aabb.minX = transform.d - r; + + aabb.maxY = transform.h + r; + aabb.minY = transform.h - r; + + aabb.maxZ = transform.l + r; + aabb.minZ = transform.l - r; + + return aabb; + } + + /** + * @param origin + * @param vector + * @param threshold + * @param normal + * @return + */ + override public function getSegmentIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var px:Number = origin.x - transform.d; + var py:Number = origin.y - transform.h; + var pz:Number = origin.z - transform.l; + var k:Number = vector.x*px + vector.y*py + vector.z*pz; + if (k > 0) return -1; + var a:Number = vector.x*vector.x + vector.y*vector.y + vector.z*vector.z; + var D:Number = k*k - a*(px*px + py*py + pz*pz - r*r); + if (D < 0) return -1; + return -(k + Math.sqrt(D))/a; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var sphere:CollisionSphere = source as CollisionSphere; + if (sphere == null) return this; + super.copyFrom(sphere); + r = sphere.r; + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionSphere(r, collisionGroup); + } + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/primitives/CollisionTriangle.as b/0.0.4.0/src/alternativa/physics/collision/primitives/CollisionTriangle.as new file mode 100644 index 0000000..a48803a --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/primitives/CollisionTriangle.as @@ -0,0 +1,39 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + public class CollisionTriangle extends CollisionPrimitive { + + public var v0:Vector3 = new Vector3(); + public var v1:Vector3 = new Vector3(); + public var v2:Vector3 = new Vector3(); + public var e0:Vector3 = new Vector3(); + public var e1:Vector3 = new Vector3(); + public var e2:Vector3 = new Vector3(); + public var len0:Number; + public var len1:Number; + public var len2:Number; + + public function CollisionTriangle(v0:Vector3, v1:Vector3, v2:Vector3, collisionGroup:int) { + super(TRIANGLE, collisionGroup); + + this.v0.vCopy(v0); + this.v1.vCopy(v1); + this.v2.vCopy(v2); + + e0.vDiff(v1, v0); + len0 = e0.vLength(); + e0.vNormalize(); + + e1.vDiff(v2, v1); + len1 = e1.vLength(); + e1.vNormalize(); + + e2.vDiff(v0, v2); + len2 = e2.vLength(); + e2.vNormalize(); + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/types/.svn/all-wcprops b/0.0.4.0/src/alternativa/physics/collision/types/.svn/all-wcprops new file mode 100644 index 0000000..dca7380 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/types/.svn/all-wcprops @@ -0,0 +1,23 @@ +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/collision/types +END +BoundBox.as +K 25 +svn:wc:ra_dav:version-url +V 131 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/collision/types/BoundBox.as +END +RayIntersection.as +K 25 +svn:wc:ra_dav:version-url +V 138 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/collision/types/RayIntersection.as +END +Ray.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/collision/types/Ray.as +END diff --git a/0.0.4.0/src/alternativa/physics/collision/types/.svn/entries b/0.0.4.0/src/alternativa/physics/collision/types/.svn/entries new file mode 100644 index 0000000..52a520c --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/types/.svn/entries @@ -0,0 +1,64 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/collision/types +http://svndev.alternativaplatform.com + + + +2009-06-08T06:46:18.310732Z +14099 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +BoundBox.as +file + + + + +2010-10-28T04:32:41.000000Z +13aaaf1b1923de8a2579543651734ab7 +2009-05-18T16:37:02.465501Z +13087 +mike + +RayIntersection.as +file + + + + +2010-10-28T04:32:41.000000Z +a0a4cf3dd94468fdf44cd06734927c94 +2009-06-08T06:46:18.310732Z +14099 +mike + +Ray.as +file + + + + +2010-10-28T04:32:41.000000Z +55eeb1cf79a416bb863bee95f089dbcd +2009-04-17T17:39:21.367569Z +11341 +mike + diff --git a/0.0.4.0/src/alternativa/physics/collision/types/.svn/format b/0.0.4.0/src/alternativa/physics/collision/types/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/types/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.4.0/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base b/0.0.4.0/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base new file mode 100644 index 0000000..6efc097 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base @@ -0,0 +1,72 @@ +package alternativa.physics.collision.types { + + public class BoundBox { + + public var minX:Number = 1e308; + public var minY:Number = 1e308; + public var minZ:Number = 1e308; + public var maxX:Number = -1e308; + public var maxY:Number = -1e308; + public var maxZ:Number = -1e308; + + public function setSize(minX:Number, minY:Number, minZ:Number, maxX:Number, maxY:Number, maxZ:Number):void { + this.minX = minX; + this.minY = minY; + this.minZ = minZ; + this.maxX = maxX; + this.maxY = maxY; + this.maxZ = maxZ; + } + + public function addBoundBox(boundBox:BoundBox):void { + minX = (boundBox.minX < minX) ? boundBox.minX : minX; + minY = (boundBox.minY < minY) ? boundBox.minY : minY; + minZ = (boundBox.minZ < minZ) ? boundBox.minZ : minZ; + maxX = (boundBox.maxX > maxX) ? boundBox.maxX : maxX; + maxY = (boundBox.maxY > maxY) ? boundBox.maxY : maxY; + maxZ = (boundBox.maxZ > maxZ) ? boundBox.maxZ : maxZ; + } + + public function addPoint(x:Number, y:Number, z:Number):void { + if (x < minX) minX = x; + if (x > maxX) maxX = x; + if (y < minY) minY = y; + if (y > maxY) maxY = y; + if (z < minZ) minZ = z; + if (z > maxZ) maxZ = z; + } + + public function infinity():void { + minX = 1e308; + minY = 1e308; + minZ = 1e308; + maxX = -1e308; + maxY = -1e308; + maxZ = -1e308; + } + + public function intersects(bb:BoundBox, tolerance:Number):Boolean { + return !(minX > bb.maxX + tolerance || maxX < bb.minX - tolerance || minY > bb.maxY + tolerance || maxY < bb.minY - tolerance || minZ > bb.maxZ + tolerance || maxZ < bb.minZ - tolerance); + } + + public function copyFrom(boundBox:BoundBox):void { + minX = boundBox.minX; + minY = boundBox.minY; + minZ = boundBox.minZ; + maxX = boundBox.maxX; + maxY = boundBox.maxY; + maxZ = boundBox.maxZ; + } + + public function clone():BoundBox { + var clone:BoundBox = new BoundBox(); + clone.copyFrom(this); + return clone; + } + + public function toString():String { + return "BoundBox [" + minX + ", " + minY + ", " + minZ + " : " + maxX + ", " + maxY + ", " + maxZ + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base b/0.0.4.0/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base new file mode 100644 index 0000000..f80620a --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base @@ -0,0 +1,13 @@ +package alternativa.physics.collision.types { + import alternativa.physics.types.Vector3; + + /** + * Структура, описывающая луч и сегмент прямой в пространстве. + */ + public class Ray { + // Начало луча. Начальная точка сегмента на прямой луча. + public var origin:Vector3 = new Vector3(); + // Направляющий вектор луча. Длина вектора задаёт длину сегмента на луче. Конечная точка сегмента вычисляется как origin + dir. + public var dir:Vector3 = new Vector3(); + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base b/0.0.4.0/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base new file mode 100644 index 0000000..8a89f75 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base @@ -0,0 +1,25 @@ +package alternativa.physics.collision.types { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.types.Vector3; + + /** + * Структура описывает точку пересечения луча с физической геометрией. + */ + public class RayIntersection { + // Физический примитив, с которым пересекается луч + public var primitive:CollisionPrimitive; + // Положенеи точки пересечения + public var pos:Vector3 = new Vector3(); + // Нормаль поверхности физического примитива в точке пересечения + public var normal:Vector3 = new Vector3(); + // Время пересечения (|pos - ray.origin|/|ray.dir|) + public var t:Number = 0; + + public function copy(source:RayIntersection):void { + primitive = source.primitive; + pos.vCopy(source.pos); + normal.vCopy(source.normal); + t = source.t; + } + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/types/BoundBox.as b/0.0.4.0/src/alternativa/physics/collision/types/BoundBox.as new file mode 100644 index 0000000..6efc097 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/types/BoundBox.as @@ -0,0 +1,72 @@ +package alternativa.physics.collision.types { + + public class BoundBox { + + public var minX:Number = 1e308; + public var minY:Number = 1e308; + public var minZ:Number = 1e308; + public var maxX:Number = -1e308; + public var maxY:Number = -1e308; + public var maxZ:Number = -1e308; + + public function setSize(minX:Number, minY:Number, minZ:Number, maxX:Number, maxY:Number, maxZ:Number):void { + this.minX = minX; + this.minY = minY; + this.minZ = minZ; + this.maxX = maxX; + this.maxY = maxY; + this.maxZ = maxZ; + } + + public function addBoundBox(boundBox:BoundBox):void { + minX = (boundBox.minX < minX) ? boundBox.minX : minX; + minY = (boundBox.minY < minY) ? boundBox.minY : minY; + minZ = (boundBox.minZ < minZ) ? boundBox.minZ : minZ; + maxX = (boundBox.maxX > maxX) ? boundBox.maxX : maxX; + maxY = (boundBox.maxY > maxY) ? boundBox.maxY : maxY; + maxZ = (boundBox.maxZ > maxZ) ? boundBox.maxZ : maxZ; + } + + public function addPoint(x:Number, y:Number, z:Number):void { + if (x < minX) minX = x; + if (x > maxX) maxX = x; + if (y < minY) minY = y; + if (y > maxY) maxY = y; + if (z < minZ) minZ = z; + if (z > maxZ) maxZ = z; + } + + public function infinity():void { + minX = 1e308; + minY = 1e308; + minZ = 1e308; + maxX = -1e308; + maxY = -1e308; + maxZ = -1e308; + } + + public function intersects(bb:BoundBox, tolerance:Number):Boolean { + return !(minX > bb.maxX + tolerance || maxX < bb.minX - tolerance || minY > bb.maxY + tolerance || maxY < bb.minY - tolerance || minZ > bb.maxZ + tolerance || maxZ < bb.minZ - tolerance); + } + + public function copyFrom(boundBox:BoundBox):void { + minX = boundBox.minX; + minY = boundBox.minY; + minZ = boundBox.minZ; + maxX = boundBox.maxX; + maxY = boundBox.maxY; + maxZ = boundBox.maxZ; + } + + public function clone():BoundBox { + var clone:BoundBox = new BoundBox(); + clone.copyFrom(this); + return clone; + } + + public function toString():String { + return "BoundBox [" + minX + ", " + minY + ", " + minZ + " : " + maxX + ", " + maxY + ", " + maxZ + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/types/Ray.as b/0.0.4.0/src/alternativa/physics/collision/types/Ray.as new file mode 100644 index 0000000..f80620a --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/types/Ray.as @@ -0,0 +1,13 @@ +package alternativa.physics.collision.types { + import alternativa.physics.types.Vector3; + + /** + * Структура, описывающая луч и сегмент прямой в пространстве. + */ + public class Ray { + // Начало луча. Начальная точка сегмента на прямой луча. + public var origin:Vector3 = new Vector3(); + // Направляющий вектор луча. Длина вектора задаёт длину сегмента на луче. Конечная точка сегмента вычисляется как origin + dir. + public var dir:Vector3 = new Vector3(); + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/collision/types/RayIntersection.as b/0.0.4.0/src/alternativa/physics/collision/types/RayIntersection.as new file mode 100644 index 0000000..8a89f75 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/collision/types/RayIntersection.as @@ -0,0 +1,25 @@ +package alternativa.physics.collision.types { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.types.Vector3; + + /** + * Структура описывает точку пересечения луча с физической геометрией. + */ + public class RayIntersection { + // Физический примитив, с которым пересекается луч + public var primitive:CollisionPrimitive; + // Положенеи точки пересечения + public var pos:Vector3 = new Vector3(); + // Нормаль поверхности физического примитива в точке пересечения + public var normal:Vector3 = new Vector3(); + // Время пересечения (|pos - ray.origin|/|ray.dir|) + public var t:Number = 0; + + public function copy(source:RayIntersection):void { + primitive = source.primitive; + pos.vCopy(source.pos); + normal.vCopy(source.normal); + t = source.t; + } + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/rigid/.svn/all-wcprops b/0.0.4.0/src/alternativa/physics/rigid/.svn/all-wcprops new file mode 100644 index 0000000..39932ee --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/rigid/.svn/all-wcprops @@ -0,0 +1,47 @@ +K 25 +svn:wc:ra_dav:version-url +V 109 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/rigid +END +Body.as +K 25 +svn:wc:ra_dav:version-url +V 117 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/rigid/Body.as +END +BodyState.as +K 25 +svn:wc:ra_dav:version-url +V 122 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/rigid/BodyState.as +END +PhysicsUtils.as +K 25 +svn:wc:ra_dav:version-url +V 125 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/rigid/PhysicsUtils.as +END +ContactPoint.as +K 25 +svn:wc:ra_dav:version-url +V 125 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/rigid/ContactPoint.as +END +RigidWorld.as +K 25 +svn:wc:ra_dav:version-url +V 123 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/rigid/RigidWorld.as +END +Contact.as +K 25 +svn:wc:ra_dav:version-url +V 120 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/rigid/Contact.as +END +BodyMaterial.as +K 25 +svn:wc:ra_dav:version-url +V 125 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/rigid/BodyMaterial.as +END diff --git a/0.0.4.0/src/alternativa/physics/rigid/.svn/entries b/0.0.4.0/src/alternativa/physics/rigid/.svn/entries new file mode 100644 index 0000000..4e5cca5 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/rigid/.svn/entries @@ -0,0 +1,118 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/rigid +http://svndev.alternativaplatform.com + + + +2009-07-03T12:54:42.818772Z +15744 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Body.as +file + + + + +2010-10-28T04:32:41.000000Z +23bad2eff16ce73dccb56a7a601cc3c0 +2009-06-29T14:07:24.833358Z +15390 +mike + +BodyState.as +file + + + + +2010-10-28T04:32:41.000000Z +8a089205beccac13a05eccfb9cfc6387 +2009-06-08T06:46:18.310732Z +14099 +mike + +PhysicsUtils.as +file + + + + +2010-10-28T04:32:41.000000Z +6d88004ddab2ae19e0d15f5b1b7c016a +2009-04-26T21:40:17.813142Z +12023 +mike + +constraints +dir + +ContactPoint.as +file + + + + +2010-10-28T04:32:41.000000Z +a94e73e3bfc85be2a79d6cc939512d91 +2009-06-08T06:46:18.310732Z +14099 +mike + +RigidWorld.as +file + + + + +2010-10-28T04:32:41.000000Z +b0b528a79a6fce55e5d800a15dc99534 +2009-07-03T12:54:42.818772Z +15744 +mike + +primitives +dir + +Contact.as +file + + + + +2010-10-28T04:32:41.000000Z +d65ad3d488d4a06addcbfa4aaf958f10 +2009-04-13T03:04:08.898183Z +11018 +mike + +BodyMaterial.as +file + + + + +2010-10-28T04:32:41.000000Z +49c7a55b5b26fdb03a60f24bac460f53 +2009-04-07T08:34:06.342994Z +10587 +mike + diff --git a/0.0.4.0/src/alternativa/physics/rigid/.svn/format b/0.0.4.0/src/alternativa/physics/rigid/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/rigid/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.4.0/src/alternativa/physics/rigid/.svn/text-base/Body.as.svn-base b/0.0.4.0/src/alternativa/physics/rigid/.svn/text-base/Body.as.svn-base new file mode 100644 index 0000000..7c08ce9 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/rigid/.svn/text-base/Body.as.svn-base @@ -0,0 +1,340 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.IBodyCollisionPredicate; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Quaternion; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class Body { + + public static var linDamping:Number = 0.997; + public static var rotDamping:Number = 0.997; + + public var name:String; + public var movable:Boolean = true; + public var canFreeze:Boolean = false; + + public var freezeCounter:int; + public var frozen:Boolean = false; + public var aabb:BoundBox = new BoundBox(); + public var postCollisionPredicate:IBodyCollisionPredicate; + + altphysics var id:int; + // Мир, в котором находится тело + altphysics var world:RigidWorld; + // Текущее и предыдущее состояние тела. Промежуточное состояние вычисляется линейной интерполяцией. + altphysics var state:BodyState = new BodyState(); + altphysics var prevState:BodyState = new BodyState(); + // Линейное и угловое ускорение тела на текущем шаге симуляции + altphysics var accel:Vector3 = new Vector3(); + altphysics var angleAccel:Vector3 = new Vector3(); + // Материал тела + altphysics var material:BodyMaterial = new BodyMaterial(); + + altphysics var invMass:Number = 1; + altphysics var invInertia:Matrix3 = new Matrix3(); + altphysics var invInertiaWorld:Matrix3 = new Matrix3(); + altphysics var baseMatrix:Matrix3 = new Matrix3(); + + altphysics const MAX_CONTACTS:int = 20; + altphysics var contacts:Vector. = new Vector.(MAX_CONTACTS); + altphysics var contactsNum:int; + + altphysics var collisionPrimitives:Vector.; + altphysics var collisionPrimitivesNum:int; + + // Аккумулятор сил + altphysics var forceAccum:Vector3 = new Vector3(); + // Аккумулятор моментов + altphysics var torqueAccum:Vector3 = new Vector3(); + + // Внутренние переменные для избежания создания экземпляров + private static var _r:Vector3 = new Vector3(); + private static var _f:Vector3 = new Vector3(); + + /** + * + * @param invMass + * @param invInertia + */ + public function Body(invMass:Number, invInertia:Matrix3) { + this.invMass = invMass; + this.invInertia.copy(invInertia); + } + + /** + * @param primitive + * @param localTransform + */ + public function addCollisionPrimitive(primitive:CollisionPrimitive, localTransform:Matrix4 = null):void { + if (primitive == null) throw new ArgumentError("Primitive cannot be null"); + if (collisionPrimitives == null) collisionPrimitives = new Vector.(); + collisionPrimitives[collisionPrimitivesNum++] = primitive; + primitive.setBody(this, localTransform); + } + + /** + * @param t + * @param result + */ + public function interpolate(t:Number, pos:Vector3, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.w = prevState.orientation.w*t1 + state.orientation.w*t; + orientation.x = prevState.orientation.x*t1 + state.orientation.x*t; + orientation.y = prevState.orientation.y*t1 + state.orientation.y*t; + orientation.z = prevState.orientation.z*t1 + state.orientation.z*t; + } + + /** + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + state.pos.vCopy(pos); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setPositionXYZ(x:Number, y:Number, z:Number):void { + state.pos.vReset(x, y, z); + } + + /** + * + * @param vel + */ + public function setVelocity(vel:Vector3):void { + state.velocity.vCopy(vel); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setVelocityXYZ(x:Number, y:Number, z:Number):void { + state.velocity.vReset(x, y, z); + } + + /** + * + * @param rot + */ + public function setRotation(rot:Vector3):void { + state.rotation.vCopy(rot); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setRotationXYZ(x:Number, y:Number, z:Number):void { + state.rotation.vReset(x, y, z); + } + + /** + * + * @param q + */ + public function setOrientation(q:Quaternion):void { + state.orientation.copy(q); + } + + /** + * @param r + * @param dir + * @param magnitude + */ + public function applyRelPosWorldImpulse(r:Vector3, dir:Vector3, magnitude:Number):void { + var d:Number = magnitude*invMass; + // Линейная часть + state.velocity.x += d*dir.x; + state.velocity.y += d*dir.y; + state.velocity.z += d*dir.z; + + // Вращательная часть + var rx:Number = r.x; + var ry:Number = r.y; + var rz:Number = r.z; + _r.x = (ry*dir.z - rz*dir.y)*magnitude; + _r.y = (rz*dir.x - rx*dir.z)*magnitude; + _r.z = (rx*dir.y - ry*dir.x)*magnitude; + _r.vTransformBy3(invInertiaWorld); + state.rotation.x += _r.x; + state.rotation.y += _r.y; + state.rotation.z += _r.z; + } + + /** + * @param f + */ + public function addForce(f:Vector3):void { + forceAccum.vAdd(f); + } + + /** + * @param pos + * @param f + */ + public function addWorldForce(pos:Vector3, force:Vector3):void { + forceAccum.vAdd(force); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(force)); + } + + /** + * @param pos + * @param f + */ + public function addWorldForceScaled(pos:Vector3, force:Vector3, scale:Number):void { + _f.x = scale*force.x, _f.y = scale*force.y, _f.z = scale*force.z; + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(_f)); + } + + /** + * @param pos + * @param f + */ + public function addLocalForce(pos:Vector3, force:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(pos, _r); + // Трансформируем вектор силы в мировую систему + baseMatrix.transformVector(force, _f); + // Добавляем силу и момент + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vCross(_f)); + } + + /** + * + * @param localPos + * @param worldForce + */ + public function addWorldForceAtLocalPoint(localPos:Vector3, worldForce:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(localPos, _r); + // Добавляем силу и момент + forceAccum.vAdd(worldForce); + torqueAccum.vAdd(_r.vCross(worldForce)); + } + + /** + * @param dt + */ + public function addExternalForces(dt:Number):void { + } + + /** + * @param t + */ + public function addTorque(t:Vector3):void { + torqueAccum.vAdd(t); + } + + /** + * + */ + altphysics function clearAccumulators():void { + forceAccum.vReset(); + torqueAccum.vReset(); + } + + /** + * + */ + altphysics function calcAccelerations():void { + accel.x = forceAccum.x*invMass; + accel.y = forceAccum.y*invMass; + accel.z = forceAccum.z*invMass; + invInertiaWorld.transformVector(torqueAccum, angleAccel); + } + + /** + * Вычисляет производные данные. + */ + altphysics function calcDerivedData():void { + // Вычисление базисной матрицы и обратного тензора инерции в мировых координатах + state.orientation.toMatrix3(baseMatrix); + invInertiaWorld.copy(invInertia).append(baseMatrix).prependTransposed(baseMatrix); + if (collisionPrimitives != null) { + aabb.infinity(); + for (var i:int = 0; i < collisionPrimitivesNum; i++) { + var primitive:CollisionPrimitive = collisionPrimitives[i]; + primitive.transform.setFromMatrix3(baseMatrix, state.pos); + if (primitive.localTransform != null) primitive.transform.prepend(primitive.localTransform); + primitive.calculateAABB(); + aabb.addBoundBox(primitive.aabb); + } + } + } + + /** + * + */ + altphysics function saveState():void { + prevState.copy(state); + } + + /** + * + */ + altphysics function restoreState():void { + state.copy(prevState); + } + + /** + * @param dt + */ + altphysics function integrateVelocity(dt:Number):void { + // v = v + a*t + state.velocity.x += accel.x*dt; + state.velocity.y += accel.y*dt; + state.velocity.z += accel.z*dt; + // rot = rot + eps*t + state.rotation.x += angleAccel.x*dt; + state.rotation.y += angleAccel.y*dt; + state.rotation.z += angleAccel.z*dt; + + state.velocity.x *= linDamping; + state.velocity.y *= linDamping; + state.velocity.z *= linDamping; + + state.rotation.x *= rotDamping; + state.rotation.y *= rotDamping; + state.rotation.z *= rotDamping; + } + + /** + * + */ + altphysics function integratePosition(dt:Number):void { + // pos = pos + v*t + state.pos.x += state.velocity.x*dt; + state.pos.y += state.velocity.y*dt; + state.pos.z += state.velocity.z*dt; + // q = q + 0.5*rot*q + state.orientation.addScaledVector(state.rotation, dt); + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/rigid/.svn/text-base/BodyMaterial.as.svn-base b/0.0.4.0/src/alternativa/physics/rigid/.svn/text-base/BodyMaterial.as.svn-base new file mode 100644 index 0000000..61d4382 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/rigid/.svn/text-base/BodyMaterial.as.svn-base @@ -0,0 +1,11 @@ +package alternativa.physics.rigid { + + public class BodyMaterial { + + public var restitution:Number = 0; + public var friction:Number = 0.3; +// public var dynamicFriction:Number = 0.2; +// public var dynamicFriction:Number = 0.2; + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/rigid/.svn/text-base/BodyState.as.svn-base b/0.0.4.0/src/alternativa/physics/rigid/.svn/text-base/BodyState.as.svn-base new file mode 100644 index 0000000..408b0ea --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/rigid/.svn/text-base/BodyState.as.svn-base @@ -0,0 +1,39 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Quaternion; + import alternativa.physics.types.Vector3; + + /** + * Класс описывает состояние твёрдого тела. + */ + public class BodyState { + /** + * Положение тела. + */ + public var pos:Vector3 = new Vector3(); + /** + * Ориентация тела. + */ + public var orientation:Quaternion = new Quaternion(); + /** + * Скорость тела. + */ + public var velocity:Vector3 = new Vector3(); + /** + * Угловая скорость тела. + */ + public var rotation:Vector3 = new Vector3(); + + /** + * Копирует значение указанного объекта. + * + * @param state + */ + public function copy(state:BodyState):void { + pos.vCopy(state.pos); + orientation.copy(state.orientation); + velocity.vCopy(state.velocity); + rotation.vCopy(state.rotation); + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/rigid/.svn/text-base/Contact.as.svn-base b/0.0.4.0/src/alternativa/physics/rigid/.svn/text-base/Contact.as.svn-base new file mode 100644 index 0000000..b73fc55 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/rigid/.svn/text-base/Contact.as.svn-base @@ -0,0 +1,53 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.types.Vector3; + + /** + * Информация о контакте между двумя телами. Содержит нормаль, множество точек контакта и прочие величины. + */ + public class Contact { + +// private static var pool:Vector. = new Vector.(); +// +// public static function create():ContactInfo { +// if (pool.length > 0) { +// return pool.pop(); +// } +// return new ContactInfo(); +// } +// +// public static function destroy(collInfo:ContactInfo):void { +// collInfo.body1 = collInfo.body2 = null; +// pool.push(collInfo); +// } + + // Максимальное количество точек контакта + private const N:int = 8; + + public var body1:Body; + public var body2:Body; + // Взаимный коэффициент отскока + public var restitution:Number; + // Взаимный коэффициент трения + public var friction:Number; + // Нормаль контакта. Направлена от второго тела к первому. + public var normal:Vector3 = new Vector3(); + // Список точек контакта + public var points:Vector. = new Vector.(N, true); + // Количество точек контакта + public var pcount:int; + // Максимальная глубина пересечения тел + public var maxPenetration:Number = 0; + + public var satisfied:Boolean; + + /** + * + */ + public function Contact() { + for (var i:int = 0; i < N; i++) points[i] = new ContactPoint(); + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/rigid/.svn/text-base/ContactPoint.as.svn-base b/0.0.4.0/src/alternativa/physics/rigid/.svn/text-base/ContactPoint.as.svn-base new file mode 100644 index 0000000..c36f958 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/rigid/.svn/text-base/ContactPoint.as.svn-base @@ -0,0 +1,52 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Vector3; + + public class ContactPoint { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + + // Величины, расчитываемые перед началом фазы решения контактов + + // Требуемая проекция конечной скорости на нормаль для упругого контакта + public var normalVel:Number; + // Минимальная скорость разделения неупругого контакта + public var minSepVel:Number; + // Изменение проекции скорости на единицу нормального импульса + public var velByUnitImpulseN:Number; + + public var angularInertia1:Number; + public var angularInertia2:Number; + + // Радиус-вектор точки контакта относительно центра первого тела + public var r1:Vector3 = new Vector3(); + // Радиус-вектор точки контакта относительно центра второго тела + public var r2:Vector3 = new Vector3(); + + // Величины, накапливаемые во время фазы решения контактов + + // Накопленный импульс, применяемый для получения требуемой относительной скорости в точке контакта + public var accumImpulseN:Number; + // Накопленный импульс, применяемый для разделения тел в точке контакта. Не создаёт момента. +// public var accumSepImpulse:Number; + + public var satisfied:Boolean; + + /** + * + * @param cp + */ + public function copyFrom(cp:ContactPoint):void { + pos.vCopy(cp.pos); + penetration = cp.penetration; + feature1 = cp.feature1; + feature2 = cp.feature2; + r1.vCopy(cp.r1); + r2.vCopy(cp.r2); + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/rigid/.svn/text-base/PhysicsUtils.as.svn-base b/0.0.4.0/src/alternativa/physics/rigid/.svn/text-base/PhysicsUtils.as.svn-base new file mode 100644 index 0000000..a9aaa99 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/rigid/.svn/text-base/PhysicsUtils.as.svn-base @@ -0,0 +1,49 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + public class PhysicsUtils { + + /** + * @param mass + * @param halfSize + * @param result + */ + public static function getBoxInvInertia(mass:Number, halfSize:Vector3, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + result.a = 3/(mass*(yy + zz)); + result.f = 3/(mass*(zz + xx)); + result.k = 3/(mass*(xx + yy)); + } + + /** + * @param mass + * @param r + * @param h + * @param result + */ + public static function getCylinderInvInertia(mass:Number, r:Number, h:Number, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + + result.a = result.f = 1/(mass*(h*h/12 + r*r/4)); + result.k = 2/(mass*r*r); + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/rigid/.svn/text-base/RigidWorld.as.svn-base b/0.0.4.0/src/alternativa/physics/rigid/.svn/text-base/RigidWorld.as.svn-base new file mode 100644 index 0000000..696580b --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/rigid/.svn/text-base/RigidWorld.as.svn-base @@ -0,0 +1,653 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.ICollisionDetector; + import alternativa.physics.collision.KdTreeCollisionDetector; + import alternativa.physics.rigid.constraints.Constraint; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Класс реализует физическую симуляцию поведения твёрдых тел. + */ + public class RigidWorld { + + private static var lastBodyId:int; + + // Максимальное количество контактов + altphysics const MAX_CONTACTS:int = 1000; + + ////////////////////////////////////////////////////////////////// + // Настроечные параметры симуляции. Могут быть изменены в любой + // момент времени без нарушения корректной работы симуляции. + ////////////////////////////////////////////////////////////////// + // Количество шагов, за которое пересекающиеся тела должны разделиться + public var penResolutionSteps:int = 10; + // Величина допустимой глубины пересечения + public var allowedPenetration:Number = 0.1; + // Максимальная скорость, добавляемая с целью разделения тел + public var maxPenResolutionSpeed:Number = 0.5; + // Количество итераций для обработки упругих контактов + public var collisionIterations:int = 5; + // Количество итераций для обработки неупругих контактов + public var contactIterations:int = 5; + // Флаг использования предсказания состояний + public var usePrediction:Boolean = false; + + public var freezeSteps:int = 10; + public var linSpeedFreezeLimit:Number = 1; + public var angSpeedFreezeLimit:Number = 0.01; + + // Переменные для процедуры разделения тел путём непосредственного их перемещенеия. + // !!!!!!!!!!!!!!!!!!!!!!!! + // !!! Экспериментально !!! + // !!!!!!!!!!!!!!!!!!!!!!!! + public var staticSeparationIterations:int = 10; + public var staticSeparationSteps:int = 10; + public var maxAngleMove:Number = 10; + public var useStaticSeparation:Boolean = false; + + // Вектор гравитации + altphysics var _gravity:Vector3 = new Vector3(0, 0, -9.8); + // Использующийся детектор столкновений + altphysics var collisionDetector:ICollisionDetector; + // Список тел, участвующих в симуляции + altphysics var bodies:Vector. = new Vector.(); + // Количество тел, участвующих в симуляции + altphysics var bodiesNum:int; + // Список контактов на текущем шаге симуляции + altphysics var contacts:Vector. = new Vector.(MAX_CONTACTS, true); + // Количество контактов на текущем шаге симуляции + altphysics var contactsNum:int; + // Список ограничений + altphysics var constraints:Vector. = new Vector.(); + // Количество ограничений + altphysics var constraintsNum:int; + // Временная метка. Число прошедших шагов с начала симуляции. + altphysics var timeStamp:uint; + + // Временные переменные для избежания создания экземпляров + private var _r:Vector3 = new Vector3(); + private var _t:Vector3 = new Vector3(); + private var _v:Vector3 = new Vector3(); + private var _v1:Vector3 = new Vector3(); + private var _v2:Vector3 = new Vector3(); + + /** + * + */ + public function RigidWorld() { + for (var i:int = 0; i < MAX_CONTACTS; i++) contacts[i] = new Contact(); + collisionDetector = new KdTreeCollisionDetector(); + } + + /** + * Вектор гравитации. + */ + public function get gravity():Vector3 { + return _gravity.vClone(); + } + + /** + * @private + */ + public function set gravity(value:Vector3):void { + _gravity.vCopy(value); + } + + /** + * Добавляет тело в симуляцию. + * @param body + * @return + */ + public function addBody(body:Body, isStatic:Boolean):Boolean { + var i:int = bodies.indexOf(body); + if (i > -1) return false; + bodies[bodiesNum++] = body; + body.world = this; + body.id = lastBodyId++; +// if (body.collisionPrimitives != null) { +// for (i = 0; i < body.collisionPrimitivesNum; i++) collisionDetector.addPrimitive(body.collisionPrimitives[i], isStatic); +// } + return true; + } + + /** + * Удаляет тело из симуляции. + * @param body + * @return + */ + public function removeBody(body:Body, isStatic:Boolean):Boolean { + var i:int = bodies.indexOf(body); + if (i == -1) return false; + bodies.splice(i, 1); + body.world = null; +// if (body.collisionPrimitives != null) { +// for (i = 0; i < body.collisionPrimitivesNum; i++) collisionDetector.removePrimitive(body.collisionPrimitives[i], isStatic); +// } + bodiesNum--; + return true; + } + + /** + * Добавляет ограничение. + * @param c + */ + public function addConstraint(c:Constraint):void { + constraints[constraintsNum++] = c; + c.world = this; + } + + /** + * Удаляет ограничение. + * @param c + */ + public function removeConstraint(c:Constraint):Boolean { + var idx:int = constraints.indexOf(c); + if (idx < 0) return false; + constraints.splice(idx, 1) + constraintsNum--; + c.world = null; + return true; + } + + /** + * Применяет к телам действующте на них силы. Аккумуляторы сил и моментов тел очищаются после завершения шага + * симуляции, поэтому на момент вызова метода могут уже содержать некоторые значения. + * + * @param dt промежуток времени, в течении которого действуют силы + */ + private function applyForces(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i]; + body.addExternalForces(dt); + body.calcAccelerations(); + // Ускорение свободного падения применяется только к подвижным телам во избежание некорректного изменения + // фиктивной скорости неподвижных тел. + if (body.movable && !body.frozen) { + body.accel.x += _gravity.x; + body.accel.y += _gravity.y; + body.accel.z += _gravity.z; + } + } + } + + /** + * Определяет все столкновения на текущем шаге симуляции и заполняет список получившихся контактов. + * + * @param dt длительность шага симуляции + */ + private function detectCollisions(dt:Number):void { + var i:int; + var j:int; + var body:Body; + + for (i = 0; i < bodiesNum; i++) { + body = bodies[i]; + if (body.frozen) continue; + body.contactsNum = 0; + body.saveState(); + // При включённом режиме предсказания состояние тел интегрируется на один шаг вперёд + if (usePrediction) { + body.integrateVelocity(dt); + body.integratePosition(dt); + } + body.calcDerivedData(); + } + + contactsNum = collisionDetector.getAllCollisions(contacts); + + // Расчёт относительных векторов точки контакта вынесен сюда из-за необходимости учитывать + // положение тел в предсказанном состоянии + for (i = 0; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + var b1:Body = contact.body1; + var b2:Body = contact.body2; + for (j = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.r1.x = cp.pos.x - b1.state.pos.x; + cp.r1.y = cp.pos.y - b1.state.pos.y; + cp.r1.z = cp.pos.z - b1.state.pos.z; + if (b2 != null) { + cp.r2.x = cp.pos.x - b2.state.pos.x; + cp.r2.y = cp.pos.y - b2.state.pos.y; + cp.r2.z = cp.pos.z - b2.state.pos.z; + } + } + } + + // Восстановление состояния тел + if (usePrediction) { + for (i = 0; i < bodiesNum; i++) { + body = bodies[i]; + if (body.frozen) continue; + body.restoreState(); + body.calcDerivedData(); + } + } + } + + /** + * Подготваливает полученные из детектора столкновений контакты, расчитывая значения, не меняющиеся + * в ходе шага симуляции. + */ + private function preProcessContacts(dt:Number):void { + var i:int; + for (i = 0; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + var b1:Body = contact.body1; + var b2:Body = contact.body2; + // Столкнувшиеся тела размораживаются + if (b1.frozen) { + b1.frozen = false; + b1.freezeCounter = 0; + } + if (b2 != null && b2.frozen) { + b2.frozen = false; + b2.freezeCounter = 0; + } + contact.restitution = b1.material.restitution; + if (b2 != null && b2.material.restitution < contact.restitution) contact.restitution = b2.material.restitution; + contact.friction = b1.material.friction; + if (b2 != null && b2.material.friction < contact.friction) contact.friction = b2.material.friction; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.accumImpulseN = 0; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + cp.velByUnitImpulseN = 0; + if (b1.movable) { + cp.angularInertia1 = _v.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1).vDot(contact.normal); + cp.velByUnitImpulseN += b1.invMass + cp.angularInertia1; + } + if (b2 != null && b2.movable) { + cp.angularInertia2 = _v.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2).vDot(contact.normal); + cp.velByUnitImpulseN += b2.invMass + cp.angularInertia2; + } + // Расчёт требуемой конечной скорости для упругого контакта + calcSepVelocity(b1, b2, cp, _v); + cp.normalVel = _v.vDot(contact.normal); + if (cp.normalVel < 0) cp.normalVel = - contact.restitution*cp.normalVel; + // Скорость разделения неупругого контакта + cp.minSepVel = cp.penetration > allowedPenetration ? (cp.penetration - allowedPenetration)/(penResolutionSteps*dt) : 0; + if (cp.minSepVel > maxPenResolutionSpeed) cp.minSepVel = maxPenResolutionSpeed; + } + } + for (i = 0; i < constraintsNum; i++) Constraint(constraints[i]).preProcess(dt); + } + + /** + * + * @param dt + * @param forceInelastic + */ + private function processContacts(dt:Number, forceInelastic:Boolean):void { + var iterNum:int = forceInelastic ? contactIterations : collisionIterations; + var i:int; + var forwardLoop:Boolean = false; + for (var iter:int = 0; iter < iterNum; iter++) { + forwardLoop = !forwardLoop; + for (i = 0; i < contactsNum; i++) resolveContact(contacts[i], forceInelastic, forwardLoop); + // Ограничения + for (i = 0; i < constraintsNum; i++) Constraint(constraints[i]).apply(dt); + } + + // Разделение контактов путём непосредственного изменения координат и ориентации. Экспериментально. +// if (forceInelastic && useStaticSeparation) performStaticSeparation(); + } + + /** + * + */ + private function resolveContact(contactInfo:Contact, forceInelastic:Boolean, forwardLoop:Boolean):void { + var b1:Body = contactInfo.body1; + var b2:Body = contactInfo.body2; + var normal:Vector3 = contactInfo.normal; + var i:int; + if (forwardLoop) { + for (i = 0; i < contactInfo.pcount; i++) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } else { + for (i = contactInfo.pcount - 1; i >= 0; i--) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } + } + + /** + * + * @param idx + * @param b1 + * @param b2 + * @param colInfo + * @param normal + * @param forceInelastic + */ + private function resolveContactPoint(idx:int, b1:Body, b2:Body, contact:Contact, normal:Vector3, forceInelastic:Boolean):void { + var cp:ContactPoint = contact.points[idx]; + if (!forceInelastic) cp.satisfied = true; + + var newVel:Number = 0; + calcSepVelocity(b1, b2, cp, _v); + var sepVel:Number = _v.vDot(contact.normal); + if (forceInelastic) { + var minSpeVel:Number = useStaticSeparation ? 0 : cp.minSepVel; + if (sepVel < minSpeVel) cp.satisfied = false; + else if (cp.satisfied) return; + newVel = minSpeVel; + } else { + newVel = cp.normalVel; + } + var deltaVel:Number = newVel - sepVel; + var impulse:Number = deltaVel/cp.velByUnitImpulseN; + var accumImpulse:Number = cp.accumImpulseN + impulse; + if (accumImpulse < 0) accumImpulse = 0; + var deltaImpulse:Number = accumImpulse - cp.accumImpulseN; + cp.accumImpulseN = accumImpulse; + // Применяем импульс к телам + if (b1.movable) b1.applyRelPosWorldImpulse(cp.r1, normal, deltaImpulse); + if (b2 != null && b2.movable) b2.applyRelPosWorldImpulse(cp.r2, normal, -deltaImpulse); + + // Учёт силы трения + calcSepVelocity(b1, b2, cp, _v); + // Расчитываем изменение касательной скорости на единицу касательного импульса + var tanSpeedByUnitImpulse:Number = 0; + _v.vAddScaled(-_v.vDot(contact.normal), contact.normal); + var tanSpeed:Number = _v.vLength(); + if (tanSpeed < 0.001) return; + _t.vCopy(_v).vNormalize().vReverse(); + // dV = b.invMass + ((invI * (r % t)) % r) * t + if (b1.movable) { + _v.vCross2(cp.r1, _t).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1); + tanSpeedByUnitImpulse += b1.invMass + _v.vDot(_t); + } + if (b2 != null && b2.movable) { + _v.vCross2(cp.r2, _t).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2); + tanSpeedByUnitImpulse += b2.invMass + _v.vDot(_t); + } + + var tanImpulse:Number = tanSpeed/tanSpeedByUnitImpulse; + var max:Number = contact.friction*cp.accumImpulseN; + if (max < 0) { + if (tanImpulse < max) tanImpulse = max; + } else { + if (tanImpulse > max) tanImpulse = max; + } + + // Применяем импульс к телам + if (b1.movable) b1.applyRelPosWorldImpulse(cp.r1, _t, tanImpulse); + if (b2 != null && b2.movable) b2.applyRelPosWorldImpulse(cp.r2, _t, -tanImpulse); + } + + /** + * + * @param cp + * @param normal + * @return + */ + private function calcSepVelocity(body1:Body, body2:Body, cp:ContactPoint, result:Vector3):void { + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 +// result.vCopy(body1.state.velocity).vAdd(_v1.vCross2(body1.state.rotation, cp.r1)); + var rot:Vector3 = body1.state.rotation; + var v:Vector3 = cp.r1; + var x:Number = rot.y*v.z - rot.z*v.y; + var y:Number = rot.z*v.x - rot.x*v.z; + var z:Number = rot.x*v.y - rot.y*v.x; + v = body1.state.velocity; + result.x = v.x + x; + result.y = v.y + y; + result.z = v.z + z; + // V2 = V2_c + w2%r2 + if (body2 != null) { +// result.vSubtract(body2.state.velocity).vSubtract(_v2.vCross2(body2.state.rotation, cp.r2)); + rot = body2.state.rotation; + v = cp.r2; + x = rot.y*v.z - rot.z*v.y; + y = rot.z*v.x - rot.x*v.z; + z = rot.x*v.y - rot.y*v.x; + v = body2.state.velocity; + result.x -= v.x + x; + result.y -= v.y + y; + result.z -= v.z + z; + } + } + + /** + * + * @param dt + */ + private function intergateVelocities(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + (bodies[i] as Body).integrateVelocity(dt); + } + } + + /** + * + * @param dt + */ + private function integratePositions(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i] as Body; + if (body.movable && !body.frozen) body.integratePosition(dt); + } + } + + /** + * + */ + private function performStaticSeparation():void { + var iterNum:int = staticSeparationIterations; +// iterNum = 100; + + // 1. В начале каждой итерации для всех контактов сбрасывается флаг satisfied с одновременным поиском наихудшего контакта, т.е. имеющего наибольшую величину пересечения тел. + // 2. Если найденный контакт имеет величину пересечения меньше предельно допустимой процедура прерывается, т.к. разделение контактов не требуется. + // 3. Если процедура продолжается, то выполняется разделение найденного контакта и установка его флага satisfied в true, после чего запускается внутренний цикл, состоящий + // из contactsNum - 1 итераций. На каждой итерации ищется наихудший контакт среди оставшихся (satisfied == false). Если такой контакт найден, то выполняется его разделение + // и переход к следующей итерации внутреннего цикла, иначе внутренний цикл прерывается и выполняется переход к следующей итерации внешнего цикла (шаг 1.). + for (var iter:int = 0; iter < iterNum; iter++) { + // Ищем контакт с максимальной величиной пересечения тел, одновременно сбрасывая флаги satisfied в false + var worstContact:Contact = contacts[0]; + var i:int; + for (i = 1; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + contact.satisfied = false; + if (contact.maxPenetration > worstContact.maxPenetration) worstContact = contact; + } + if (worstContact.maxPenetration <= allowedPenetration) return; + + resolveInterpenetration(worstContact); + // Внутренний цикл по оставшимся контактам + for (i = 1; i < contactsNum; i++) { + worstContact = getWorstContact(); + if (worstContact == null) break; + resolveInterpenetration(worstContact); + } + } + } + + /** + * + */ + private function getWorstContact():Contact { + var maxPen:Number = 0; + var worst:Contact = null; + for (var i:int = 0; i < contactsNum; i++) { + var c:Contact = contacts[i]; + if (!c.satisfied && c.maxPenetration > maxPen) { + worst = c; + maxPen = c.maxPenetration; + } + } + return maxPen > allowedPenetration ? worst : null; + } + + /** + * Разделяет указанный контакт, выполняя staticSeparationSteps итераций по списку точек контакта. + * 1. В начале каждой итерации ищется точка с наибольшим пересечением, одновременно сбрасываются флаги satisfied у точек. + * 2. Для найденной точки выполняется процедура разделения. + * 3. После выполняется pcount - 1 итерация по списку точек, каждый раз ищется наихудшая среди имеющих satisfied == false, для которой выполняется процедура разделения. + * Если наихудшая точка не найдена, то выполнется переход к следующей итерации внешнего цикла (шаг 1). + * + * В конце процедуры разделения обновляются значения пересечений для остальных точек контакта, а также для всех прочих контактов, относящихся к телам текущего. + * + * @param contact контакт для разделения + */ + private function resolveInterpenetration(contact:Contact):void { + contact.satisfied = true; + + for (var step:int = 0; step < staticSeparationSteps; step++) { + var worstCp:ContactPoint = contact.points[0]; + var cp:ContactPoint; + var i:int; + for (i = 1; i < contact.pcount; i++) { + cp = contact.points[i]; + cp.satisfied = false; + if (cp.penetration > worstCp.penetration) worstCp = cp; + } + if (worstCp.penetration <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + // Разделяем оставшиеся точки + var maxPen:Number = 0; + for (i = 1; i < contact.pcount; i++) { + // Поиск наихудшей точки + for (var j:int = 0; j < contact.pcount; j++) { + cp = contact.points[j]; + if (cp.satisfied) continue; + if (cp.penetration > maxPen) { + maxPen = cp.penetration; + worstCp = cp; + } + } + if (maxPen <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + } + } + } + + /** + * + * @param cp + * @param contact + */ + private function separateContactPoint(cp:ContactPoint, contact:Contact):void { + cp.satisfied = true; + + var b1:Body = contact.body1; + var b2:Body = contact.body2; + var totalMove:Number = cp.penetration - allowedPenetration; + var moveCoeff:Number = totalMove/cp.velByUnitImpulseN; + var linMove1:Number; + var angleMove1:Number; + if (b1.movable) { + linMove1 = b1.invMass*moveCoeff; + angleMove1 = cp.angularInertia1*moveCoeff; + if (angleMove1 > maxAngleMove) { + linMove1 += angleMove1 - maxAngleMove; + angleMove1 -= maxAngleMove; + } + b1.state.pos.vAddScaled(linMove1, contact.normal); + _v1.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vScale(angleMove1); + b1.state.orientation.addScaledVector(_v1, 1); + } + var linMove2:Number; + var angleMove2:Number; + if (b2 != null && b2.movable) { + linMove2 = b2.invMass*moveCoeff; + angleMove2 = cp.angularInertia2*moveCoeff; + if (angleMove2 > maxAngleMove) { + linMove2 += angleMove2 - maxAngleMove; + angleMove2 -= maxAngleMove; + } + b2.state.pos.vAddScaled(-linMove2, contact.normal); + _v2.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vScale(angleMove2); + _v2.vReverse(); + b2.state.orientation.addScaledVector(_v2, 1); + } + cp.penetration = allowedPenetration; + // Обновляем пересечения в других точках + var i:int; + for (i = 0; i < contact.pcount; i++) { + var cp1:ContactPoint = contact.points[i]; + if (cp1 == cp) continue; + var angularMove:Number; + if (b1.movable) { + angularMove = _v.vCross2(_v1, cp1.r1).vDot(contact.normal); + cp1.penetration -= linMove1 + angularMove; + } + if (b2 != null && b2.movable) { + angularMove = _v.vCross2(_v2, cp1.r2).vDot(contact.normal); + cp1.penetration -= linMove2 - angularMove; + } + // Обновление максимального значения для контакта + if (cp1.penetration > contact.maxPenetration) contact.maxPenetration = cp1.penetration; + } + // Обновляем пересечения для других контактов + var c:Contact; + var j:int; + if (b1.movable) { + for (i = 0; i < b1.contactsNum; i++) { + c = b1.contacts[i]; + if (c == contact) continue; + for (j = 0; j < c.pcount; j++) { + cp1 = c.points[j]; + if (b1 == c.body1) cp1.penetration -= linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r1).vDot(c.normal); + else cp1.penetration += linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r2).vDot(c.normal); + if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; + } + } + } + if (b2 != null && b2.movable) { + for (i = 0; i < b2.contactsNum; i++) { + c = b2.contacts[i]; + if (c == contact) continue; + for (j = 0; j < c.pcount; j++) { + cp1 = c.points[j]; + if (b2 == c.body1) cp1.penetration -= linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r1).vDot(c.normal); + else cp1.penetration += linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r2).vDot(c.normal); + if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; + } + } + } + } + + /** + * + */ + private function postPhysics():void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i]; + body.clearAccumulators(); + body.calcDerivedData(); + if (body.canFreeze) { + if (body.state.velocity.vLength() < linSpeedFreezeLimit && body.state.rotation.vLength() < angSpeedFreezeLimit) { + if (!body.frozen) { + body.freezeCounter++; + if (body.freezeCounter >= freezeSteps) body.frozen = true; + } + } else { + body.freezeCounter = 0; + body.frozen = false; + } + } + } + } + + /** + * + * @param dt + */ + public function runPhysics(dt:Number):void { + timeStamp++; + applyForces(dt); + detectCollisions(dt); + preProcessContacts(dt); + processContacts(dt, false); + intergateVelocities(dt); + processContacts(dt, true); + integratePositions(dt); + postPhysics(); + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/rigid/Body.as b/0.0.4.0/src/alternativa/physics/rigid/Body.as new file mode 100644 index 0000000..7c08ce9 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/rigid/Body.as @@ -0,0 +1,340 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.IBodyCollisionPredicate; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Quaternion; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class Body { + + public static var linDamping:Number = 0.997; + public static var rotDamping:Number = 0.997; + + public var name:String; + public var movable:Boolean = true; + public var canFreeze:Boolean = false; + + public var freezeCounter:int; + public var frozen:Boolean = false; + public var aabb:BoundBox = new BoundBox(); + public var postCollisionPredicate:IBodyCollisionPredicate; + + altphysics var id:int; + // Мир, в котором находится тело + altphysics var world:RigidWorld; + // Текущее и предыдущее состояние тела. Промежуточное состояние вычисляется линейной интерполяцией. + altphysics var state:BodyState = new BodyState(); + altphysics var prevState:BodyState = new BodyState(); + // Линейное и угловое ускорение тела на текущем шаге симуляции + altphysics var accel:Vector3 = new Vector3(); + altphysics var angleAccel:Vector3 = new Vector3(); + // Материал тела + altphysics var material:BodyMaterial = new BodyMaterial(); + + altphysics var invMass:Number = 1; + altphysics var invInertia:Matrix3 = new Matrix3(); + altphysics var invInertiaWorld:Matrix3 = new Matrix3(); + altphysics var baseMatrix:Matrix3 = new Matrix3(); + + altphysics const MAX_CONTACTS:int = 20; + altphysics var contacts:Vector. = new Vector.(MAX_CONTACTS); + altphysics var contactsNum:int; + + altphysics var collisionPrimitives:Vector.; + altphysics var collisionPrimitivesNum:int; + + // Аккумулятор сил + altphysics var forceAccum:Vector3 = new Vector3(); + // Аккумулятор моментов + altphysics var torqueAccum:Vector3 = new Vector3(); + + // Внутренние переменные для избежания создания экземпляров + private static var _r:Vector3 = new Vector3(); + private static var _f:Vector3 = new Vector3(); + + /** + * + * @param invMass + * @param invInertia + */ + public function Body(invMass:Number, invInertia:Matrix3) { + this.invMass = invMass; + this.invInertia.copy(invInertia); + } + + /** + * @param primitive + * @param localTransform + */ + public function addCollisionPrimitive(primitive:CollisionPrimitive, localTransform:Matrix4 = null):void { + if (primitive == null) throw new ArgumentError("Primitive cannot be null"); + if (collisionPrimitives == null) collisionPrimitives = new Vector.(); + collisionPrimitives[collisionPrimitivesNum++] = primitive; + primitive.setBody(this, localTransform); + } + + /** + * @param t + * @param result + */ + public function interpolate(t:Number, pos:Vector3, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.w = prevState.orientation.w*t1 + state.orientation.w*t; + orientation.x = prevState.orientation.x*t1 + state.orientation.x*t; + orientation.y = prevState.orientation.y*t1 + state.orientation.y*t; + orientation.z = prevState.orientation.z*t1 + state.orientation.z*t; + } + + /** + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + state.pos.vCopy(pos); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setPositionXYZ(x:Number, y:Number, z:Number):void { + state.pos.vReset(x, y, z); + } + + /** + * + * @param vel + */ + public function setVelocity(vel:Vector3):void { + state.velocity.vCopy(vel); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setVelocityXYZ(x:Number, y:Number, z:Number):void { + state.velocity.vReset(x, y, z); + } + + /** + * + * @param rot + */ + public function setRotation(rot:Vector3):void { + state.rotation.vCopy(rot); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setRotationXYZ(x:Number, y:Number, z:Number):void { + state.rotation.vReset(x, y, z); + } + + /** + * + * @param q + */ + public function setOrientation(q:Quaternion):void { + state.orientation.copy(q); + } + + /** + * @param r + * @param dir + * @param magnitude + */ + public function applyRelPosWorldImpulse(r:Vector3, dir:Vector3, magnitude:Number):void { + var d:Number = magnitude*invMass; + // Линейная часть + state.velocity.x += d*dir.x; + state.velocity.y += d*dir.y; + state.velocity.z += d*dir.z; + + // Вращательная часть + var rx:Number = r.x; + var ry:Number = r.y; + var rz:Number = r.z; + _r.x = (ry*dir.z - rz*dir.y)*magnitude; + _r.y = (rz*dir.x - rx*dir.z)*magnitude; + _r.z = (rx*dir.y - ry*dir.x)*magnitude; + _r.vTransformBy3(invInertiaWorld); + state.rotation.x += _r.x; + state.rotation.y += _r.y; + state.rotation.z += _r.z; + } + + /** + * @param f + */ + public function addForce(f:Vector3):void { + forceAccum.vAdd(f); + } + + /** + * @param pos + * @param f + */ + public function addWorldForce(pos:Vector3, force:Vector3):void { + forceAccum.vAdd(force); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(force)); + } + + /** + * @param pos + * @param f + */ + public function addWorldForceScaled(pos:Vector3, force:Vector3, scale:Number):void { + _f.x = scale*force.x, _f.y = scale*force.y, _f.z = scale*force.z; + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(_f)); + } + + /** + * @param pos + * @param f + */ + public function addLocalForce(pos:Vector3, force:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(pos, _r); + // Трансформируем вектор силы в мировую систему + baseMatrix.transformVector(force, _f); + // Добавляем силу и момент + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vCross(_f)); + } + + /** + * + * @param localPos + * @param worldForce + */ + public function addWorldForceAtLocalPoint(localPos:Vector3, worldForce:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(localPos, _r); + // Добавляем силу и момент + forceAccum.vAdd(worldForce); + torqueAccum.vAdd(_r.vCross(worldForce)); + } + + /** + * @param dt + */ + public function addExternalForces(dt:Number):void { + } + + /** + * @param t + */ + public function addTorque(t:Vector3):void { + torqueAccum.vAdd(t); + } + + /** + * + */ + altphysics function clearAccumulators():void { + forceAccum.vReset(); + torqueAccum.vReset(); + } + + /** + * + */ + altphysics function calcAccelerations():void { + accel.x = forceAccum.x*invMass; + accel.y = forceAccum.y*invMass; + accel.z = forceAccum.z*invMass; + invInertiaWorld.transformVector(torqueAccum, angleAccel); + } + + /** + * Вычисляет производные данные. + */ + altphysics function calcDerivedData():void { + // Вычисление базисной матрицы и обратного тензора инерции в мировых координатах + state.orientation.toMatrix3(baseMatrix); + invInertiaWorld.copy(invInertia).append(baseMatrix).prependTransposed(baseMatrix); + if (collisionPrimitives != null) { + aabb.infinity(); + for (var i:int = 0; i < collisionPrimitivesNum; i++) { + var primitive:CollisionPrimitive = collisionPrimitives[i]; + primitive.transform.setFromMatrix3(baseMatrix, state.pos); + if (primitive.localTransform != null) primitive.transform.prepend(primitive.localTransform); + primitive.calculateAABB(); + aabb.addBoundBox(primitive.aabb); + } + } + } + + /** + * + */ + altphysics function saveState():void { + prevState.copy(state); + } + + /** + * + */ + altphysics function restoreState():void { + state.copy(prevState); + } + + /** + * @param dt + */ + altphysics function integrateVelocity(dt:Number):void { + // v = v + a*t + state.velocity.x += accel.x*dt; + state.velocity.y += accel.y*dt; + state.velocity.z += accel.z*dt; + // rot = rot + eps*t + state.rotation.x += angleAccel.x*dt; + state.rotation.y += angleAccel.y*dt; + state.rotation.z += angleAccel.z*dt; + + state.velocity.x *= linDamping; + state.velocity.y *= linDamping; + state.velocity.z *= linDamping; + + state.rotation.x *= rotDamping; + state.rotation.y *= rotDamping; + state.rotation.z *= rotDamping; + } + + /** + * + */ + altphysics function integratePosition(dt:Number):void { + // pos = pos + v*t + state.pos.x += state.velocity.x*dt; + state.pos.y += state.velocity.y*dt; + state.pos.z += state.velocity.z*dt; + // q = q + 0.5*rot*q + state.orientation.addScaledVector(state.rotation, dt); + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/rigid/BodyMaterial.as b/0.0.4.0/src/alternativa/physics/rigid/BodyMaterial.as new file mode 100644 index 0000000..61d4382 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/rigid/BodyMaterial.as @@ -0,0 +1,11 @@ +package alternativa.physics.rigid { + + public class BodyMaterial { + + public var restitution:Number = 0; + public var friction:Number = 0.3; +// public var dynamicFriction:Number = 0.2; +// public var dynamicFriction:Number = 0.2; + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/rigid/BodyState.as b/0.0.4.0/src/alternativa/physics/rigid/BodyState.as new file mode 100644 index 0000000..408b0ea --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/rigid/BodyState.as @@ -0,0 +1,39 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Quaternion; + import alternativa.physics.types.Vector3; + + /** + * Класс описывает состояние твёрдого тела. + */ + public class BodyState { + /** + * Положение тела. + */ + public var pos:Vector3 = new Vector3(); + /** + * Ориентация тела. + */ + public var orientation:Quaternion = new Quaternion(); + /** + * Скорость тела. + */ + public var velocity:Vector3 = new Vector3(); + /** + * Угловая скорость тела. + */ + public var rotation:Vector3 = new Vector3(); + + /** + * Копирует значение указанного объекта. + * + * @param state + */ + public function copy(state:BodyState):void { + pos.vCopy(state.pos); + orientation.copy(state.orientation); + velocity.vCopy(state.velocity); + rotation.vCopy(state.rotation); + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/rigid/Contact.as b/0.0.4.0/src/alternativa/physics/rigid/Contact.as new file mode 100644 index 0000000..b73fc55 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/rigid/Contact.as @@ -0,0 +1,53 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.types.Vector3; + + /** + * Информация о контакте между двумя телами. Содержит нормаль, множество точек контакта и прочие величины. + */ + public class Contact { + +// private static var pool:Vector. = new Vector.(); +// +// public static function create():ContactInfo { +// if (pool.length > 0) { +// return pool.pop(); +// } +// return new ContactInfo(); +// } +// +// public static function destroy(collInfo:ContactInfo):void { +// collInfo.body1 = collInfo.body2 = null; +// pool.push(collInfo); +// } + + // Максимальное количество точек контакта + private const N:int = 8; + + public var body1:Body; + public var body2:Body; + // Взаимный коэффициент отскока + public var restitution:Number; + // Взаимный коэффициент трения + public var friction:Number; + // Нормаль контакта. Направлена от второго тела к первому. + public var normal:Vector3 = new Vector3(); + // Список точек контакта + public var points:Vector. = new Vector.(N, true); + // Количество точек контакта + public var pcount:int; + // Максимальная глубина пересечения тел + public var maxPenetration:Number = 0; + + public var satisfied:Boolean; + + /** + * + */ + public function Contact() { + for (var i:int = 0; i < N; i++) points[i] = new ContactPoint(); + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/rigid/ContactPoint.as b/0.0.4.0/src/alternativa/physics/rigid/ContactPoint.as new file mode 100644 index 0000000..c36f958 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/rigid/ContactPoint.as @@ -0,0 +1,52 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Vector3; + + public class ContactPoint { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + + // Величины, расчитываемые перед началом фазы решения контактов + + // Требуемая проекция конечной скорости на нормаль для упругого контакта + public var normalVel:Number; + // Минимальная скорость разделения неупругого контакта + public var minSepVel:Number; + // Изменение проекции скорости на единицу нормального импульса + public var velByUnitImpulseN:Number; + + public var angularInertia1:Number; + public var angularInertia2:Number; + + // Радиус-вектор точки контакта относительно центра первого тела + public var r1:Vector3 = new Vector3(); + // Радиус-вектор точки контакта относительно центра второго тела + public var r2:Vector3 = new Vector3(); + + // Величины, накапливаемые во время фазы решения контактов + + // Накопленный импульс, применяемый для получения требуемой относительной скорости в точке контакта + public var accumImpulseN:Number; + // Накопленный импульс, применяемый для разделения тел в точке контакта. Не создаёт момента. +// public var accumSepImpulse:Number; + + public var satisfied:Boolean; + + /** + * + * @param cp + */ + public function copyFrom(cp:ContactPoint):void { + pos.vCopy(cp.pos); + penetration = cp.penetration; + feature1 = cp.feature1; + feature2 = cp.feature2; + r1.vCopy(cp.r1); + r2.vCopy(cp.r2); + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/rigid/PhysicsUtils.as b/0.0.4.0/src/alternativa/physics/rigid/PhysicsUtils.as new file mode 100644 index 0000000..a9aaa99 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/rigid/PhysicsUtils.as @@ -0,0 +1,49 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + public class PhysicsUtils { + + /** + * @param mass + * @param halfSize + * @param result + */ + public static function getBoxInvInertia(mass:Number, halfSize:Vector3, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + result.a = 3/(mass*(yy + zz)); + result.f = 3/(mass*(zz + xx)); + result.k = 3/(mass*(xx + yy)); + } + + /** + * @param mass + * @param r + * @param h + * @param result + */ + public static function getCylinderInvInertia(mass:Number, r:Number, h:Number, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + + result.a = result.f = 1/(mass*(h*h/12 + r*r/4)); + result.k = 2/(mass*r*r); + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/rigid/RigidWorld.as b/0.0.4.0/src/alternativa/physics/rigid/RigidWorld.as new file mode 100644 index 0000000..696580b --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/rigid/RigidWorld.as @@ -0,0 +1,653 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.ICollisionDetector; + import alternativa.physics.collision.KdTreeCollisionDetector; + import alternativa.physics.rigid.constraints.Constraint; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Класс реализует физическую симуляцию поведения твёрдых тел. + */ + public class RigidWorld { + + private static var lastBodyId:int; + + // Максимальное количество контактов + altphysics const MAX_CONTACTS:int = 1000; + + ////////////////////////////////////////////////////////////////// + // Настроечные параметры симуляции. Могут быть изменены в любой + // момент времени без нарушения корректной работы симуляции. + ////////////////////////////////////////////////////////////////// + // Количество шагов, за которое пересекающиеся тела должны разделиться + public var penResolutionSteps:int = 10; + // Величина допустимой глубины пересечения + public var allowedPenetration:Number = 0.1; + // Максимальная скорость, добавляемая с целью разделения тел + public var maxPenResolutionSpeed:Number = 0.5; + // Количество итераций для обработки упругих контактов + public var collisionIterations:int = 5; + // Количество итераций для обработки неупругих контактов + public var contactIterations:int = 5; + // Флаг использования предсказания состояний + public var usePrediction:Boolean = false; + + public var freezeSteps:int = 10; + public var linSpeedFreezeLimit:Number = 1; + public var angSpeedFreezeLimit:Number = 0.01; + + // Переменные для процедуры разделения тел путём непосредственного их перемещенеия. + // !!!!!!!!!!!!!!!!!!!!!!!! + // !!! Экспериментально !!! + // !!!!!!!!!!!!!!!!!!!!!!!! + public var staticSeparationIterations:int = 10; + public var staticSeparationSteps:int = 10; + public var maxAngleMove:Number = 10; + public var useStaticSeparation:Boolean = false; + + // Вектор гравитации + altphysics var _gravity:Vector3 = new Vector3(0, 0, -9.8); + // Использующийся детектор столкновений + altphysics var collisionDetector:ICollisionDetector; + // Список тел, участвующих в симуляции + altphysics var bodies:Vector. = new Vector.(); + // Количество тел, участвующих в симуляции + altphysics var bodiesNum:int; + // Список контактов на текущем шаге симуляции + altphysics var contacts:Vector. = new Vector.(MAX_CONTACTS, true); + // Количество контактов на текущем шаге симуляции + altphysics var contactsNum:int; + // Список ограничений + altphysics var constraints:Vector. = new Vector.(); + // Количество ограничений + altphysics var constraintsNum:int; + // Временная метка. Число прошедших шагов с начала симуляции. + altphysics var timeStamp:uint; + + // Временные переменные для избежания создания экземпляров + private var _r:Vector3 = new Vector3(); + private var _t:Vector3 = new Vector3(); + private var _v:Vector3 = new Vector3(); + private var _v1:Vector3 = new Vector3(); + private var _v2:Vector3 = new Vector3(); + + /** + * + */ + public function RigidWorld() { + for (var i:int = 0; i < MAX_CONTACTS; i++) contacts[i] = new Contact(); + collisionDetector = new KdTreeCollisionDetector(); + } + + /** + * Вектор гравитации. + */ + public function get gravity():Vector3 { + return _gravity.vClone(); + } + + /** + * @private + */ + public function set gravity(value:Vector3):void { + _gravity.vCopy(value); + } + + /** + * Добавляет тело в симуляцию. + * @param body + * @return + */ + public function addBody(body:Body, isStatic:Boolean):Boolean { + var i:int = bodies.indexOf(body); + if (i > -1) return false; + bodies[bodiesNum++] = body; + body.world = this; + body.id = lastBodyId++; +// if (body.collisionPrimitives != null) { +// for (i = 0; i < body.collisionPrimitivesNum; i++) collisionDetector.addPrimitive(body.collisionPrimitives[i], isStatic); +// } + return true; + } + + /** + * Удаляет тело из симуляции. + * @param body + * @return + */ + public function removeBody(body:Body, isStatic:Boolean):Boolean { + var i:int = bodies.indexOf(body); + if (i == -1) return false; + bodies.splice(i, 1); + body.world = null; +// if (body.collisionPrimitives != null) { +// for (i = 0; i < body.collisionPrimitivesNum; i++) collisionDetector.removePrimitive(body.collisionPrimitives[i], isStatic); +// } + bodiesNum--; + return true; + } + + /** + * Добавляет ограничение. + * @param c + */ + public function addConstraint(c:Constraint):void { + constraints[constraintsNum++] = c; + c.world = this; + } + + /** + * Удаляет ограничение. + * @param c + */ + public function removeConstraint(c:Constraint):Boolean { + var idx:int = constraints.indexOf(c); + if (idx < 0) return false; + constraints.splice(idx, 1) + constraintsNum--; + c.world = null; + return true; + } + + /** + * Применяет к телам действующте на них силы. Аккумуляторы сил и моментов тел очищаются после завершения шага + * симуляции, поэтому на момент вызова метода могут уже содержать некоторые значения. + * + * @param dt промежуток времени, в течении которого действуют силы + */ + private function applyForces(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i]; + body.addExternalForces(dt); + body.calcAccelerations(); + // Ускорение свободного падения применяется только к подвижным телам во избежание некорректного изменения + // фиктивной скорости неподвижных тел. + if (body.movable && !body.frozen) { + body.accel.x += _gravity.x; + body.accel.y += _gravity.y; + body.accel.z += _gravity.z; + } + } + } + + /** + * Определяет все столкновения на текущем шаге симуляции и заполняет список получившихся контактов. + * + * @param dt длительность шага симуляции + */ + private function detectCollisions(dt:Number):void { + var i:int; + var j:int; + var body:Body; + + for (i = 0; i < bodiesNum; i++) { + body = bodies[i]; + if (body.frozen) continue; + body.contactsNum = 0; + body.saveState(); + // При включённом режиме предсказания состояние тел интегрируется на один шаг вперёд + if (usePrediction) { + body.integrateVelocity(dt); + body.integratePosition(dt); + } + body.calcDerivedData(); + } + + contactsNum = collisionDetector.getAllCollisions(contacts); + + // Расчёт относительных векторов точки контакта вынесен сюда из-за необходимости учитывать + // положение тел в предсказанном состоянии + for (i = 0; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + var b1:Body = contact.body1; + var b2:Body = contact.body2; + for (j = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.r1.x = cp.pos.x - b1.state.pos.x; + cp.r1.y = cp.pos.y - b1.state.pos.y; + cp.r1.z = cp.pos.z - b1.state.pos.z; + if (b2 != null) { + cp.r2.x = cp.pos.x - b2.state.pos.x; + cp.r2.y = cp.pos.y - b2.state.pos.y; + cp.r2.z = cp.pos.z - b2.state.pos.z; + } + } + } + + // Восстановление состояния тел + if (usePrediction) { + for (i = 0; i < bodiesNum; i++) { + body = bodies[i]; + if (body.frozen) continue; + body.restoreState(); + body.calcDerivedData(); + } + } + } + + /** + * Подготваливает полученные из детектора столкновений контакты, расчитывая значения, не меняющиеся + * в ходе шага симуляции. + */ + private function preProcessContacts(dt:Number):void { + var i:int; + for (i = 0; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + var b1:Body = contact.body1; + var b2:Body = contact.body2; + // Столкнувшиеся тела размораживаются + if (b1.frozen) { + b1.frozen = false; + b1.freezeCounter = 0; + } + if (b2 != null && b2.frozen) { + b2.frozen = false; + b2.freezeCounter = 0; + } + contact.restitution = b1.material.restitution; + if (b2 != null && b2.material.restitution < contact.restitution) contact.restitution = b2.material.restitution; + contact.friction = b1.material.friction; + if (b2 != null && b2.material.friction < contact.friction) contact.friction = b2.material.friction; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.accumImpulseN = 0; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + cp.velByUnitImpulseN = 0; + if (b1.movable) { + cp.angularInertia1 = _v.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1).vDot(contact.normal); + cp.velByUnitImpulseN += b1.invMass + cp.angularInertia1; + } + if (b2 != null && b2.movable) { + cp.angularInertia2 = _v.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2).vDot(contact.normal); + cp.velByUnitImpulseN += b2.invMass + cp.angularInertia2; + } + // Расчёт требуемой конечной скорости для упругого контакта + calcSepVelocity(b1, b2, cp, _v); + cp.normalVel = _v.vDot(contact.normal); + if (cp.normalVel < 0) cp.normalVel = - contact.restitution*cp.normalVel; + // Скорость разделения неупругого контакта + cp.minSepVel = cp.penetration > allowedPenetration ? (cp.penetration - allowedPenetration)/(penResolutionSteps*dt) : 0; + if (cp.minSepVel > maxPenResolutionSpeed) cp.minSepVel = maxPenResolutionSpeed; + } + } + for (i = 0; i < constraintsNum; i++) Constraint(constraints[i]).preProcess(dt); + } + + /** + * + * @param dt + * @param forceInelastic + */ + private function processContacts(dt:Number, forceInelastic:Boolean):void { + var iterNum:int = forceInelastic ? contactIterations : collisionIterations; + var i:int; + var forwardLoop:Boolean = false; + for (var iter:int = 0; iter < iterNum; iter++) { + forwardLoop = !forwardLoop; + for (i = 0; i < contactsNum; i++) resolveContact(contacts[i], forceInelastic, forwardLoop); + // Ограничения + for (i = 0; i < constraintsNum; i++) Constraint(constraints[i]).apply(dt); + } + + // Разделение контактов путём непосредственного изменения координат и ориентации. Экспериментально. +// if (forceInelastic && useStaticSeparation) performStaticSeparation(); + } + + /** + * + */ + private function resolveContact(contactInfo:Contact, forceInelastic:Boolean, forwardLoop:Boolean):void { + var b1:Body = contactInfo.body1; + var b2:Body = contactInfo.body2; + var normal:Vector3 = contactInfo.normal; + var i:int; + if (forwardLoop) { + for (i = 0; i < contactInfo.pcount; i++) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } else { + for (i = contactInfo.pcount - 1; i >= 0; i--) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } + } + + /** + * + * @param idx + * @param b1 + * @param b2 + * @param colInfo + * @param normal + * @param forceInelastic + */ + private function resolveContactPoint(idx:int, b1:Body, b2:Body, contact:Contact, normal:Vector3, forceInelastic:Boolean):void { + var cp:ContactPoint = contact.points[idx]; + if (!forceInelastic) cp.satisfied = true; + + var newVel:Number = 0; + calcSepVelocity(b1, b2, cp, _v); + var sepVel:Number = _v.vDot(contact.normal); + if (forceInelastic) { + var minSpeVel:Number = useStaticSeparation ? 0 : cp.minSepVel; + if (sepVel < minSpeVel) cp.satisfied = false; + else if (cp.satisfied) return; + newVel = minSpeVel; + } else { + newVel = cp.normalVel; + } + var deltaVel:Number = newVel - sepVel; + var impulse:Number = deltaVel/cp.velByUnitImpulseN; + var accumImpulse:Number = cp.accumImpulseN + impulse; + if (accumImpulse < 0) accumImpulse = 0; + var deltaImpulse:Number = accumImpulse - cp.accumImpulseN; + cp.accumImpulseN = accumImpulse; + // Применяем импульс к телам + if (b1.movable) b1.applyRelPosWorldImpulse(cp.r1, normal, deltaImpulse); + if (b2 != null && b2.movable) b2.applyRelPosWorldImpulse(cp.r2, normal, -deltaImpulse); + + // Учёт силы трения + calcSepVelocity(b1, b2, cp, _v); + // Расчитываем изменение касательной скорости на единицу касательного импульса + var tanSpeedByUnitImpulse:Number = 0; + _v.vAddScaled(-_v.vDot(contact.normal), contact.normal); + var tanSpeed:Number = _v.vLength(); + if (tanSpeed < 0.001) return; + _t.vCopy(_v).vNormalize().vReverse(); + // dV = b.invMass + ((invI * (r % t)) % r) * t + if (b1.movable) { + _v.vCross2(cp.r1, _t).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1); + tanSpeedByUnitImpulse += b1.invMass + _v.vDot(_t); + } + if (b2 != null && b2.movable) { + _v.vCross2(cp.r2, _t).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2); + tanSpeedByUnitImpulse += b2.invMass + _v.vDot(_t); + } + + var tanImpulse:Number = tanSpeed/tanSpeedByUnitImpulse; + var max:Number = contact.friction*cp.accumImpulseN; + if (max < 0) { + if (tanImpulse < max) tanImpulse = max; + } else { + if (tanImpulse > max) tanImpulse = max; + } + + // Применяем импульс к телам + if (b1.movable) b1.applyRelPosWorldImpulse(cp.r1, _t, tanImpulse); + if (b2 != null && b2.movable) b2.applyRelPosWorldImpulse(cp.r2, _t, -tanImpulse); + } + + /** + * + * @param cp + * @param normal + * @return + */ + private function calcSepVelocity(body1:Body, body2:Body, cp:ContactPoint, result:Vector3):void { + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 +// result.vCopy(body1.state.velocity).vAdd(_v1.vCross2(body1.state.rotation, cp.r1)); + var rot:Vector3 = body1.state.rotation; + var v:Vector3 = cp.r1; + var x:Number = rot.y*v.z - rot.z*v.y; + var y:Number = rot.z*v.x - rot.x*v.z; + var z:Number = rot.x*v.y - rot.y*v.x; + v = body1.state.velocity; + result.x = v.x + x; + result.y = v.y + y; + result.z = v.z + z; + // V2 = V2_c + w2%r2 + if (body2 != null) { +// result.vSubtract(body2.state.velocity).vSubtract(_v2.vCross2(body2.state.rotation, cp.r2)); + rot = body2.state.rotation; + v = cp.r2; + x = rot.y*v.z - rot.z*v.y; + y = rot.z*v.x - rot.x*v.z; + z = rot.x*v.y - rot.y*v.x; + v = body2.state.velocity; + result.x -= v.x + x; + result.y -= v.y + y; + result.z -= v.z + z; + } + } + + /** + * + * @param dt + */ + private function intergateVelocities(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + (bodies[i] as Body).integrateVelocity(dt); + } + } + + /** + * + * @param dt + */ + private function integratePositions(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i] as Body; + if (body.movable && !body.frozen) body.integratePosition(dt); + } + } + + /** + * + */ + private function performStaticSeparation():void { + var iterNum:int = staticSeparationIterations; +// iterNum = 100; + + // 1. В начале каждой итерации для всех контактов сбрасывается флаг satisfied с одновременным поиском наихудшего контакта, т.е. имеющего наибольшую величину пересечения тел. + // 2. Если найденный контакт имеет величину пересечения меньше предельно допустимой процедура прерывается, т.к. разделение контактов не требуется. + // 3. Если процедура продолжается, то выполняется разделение найденного контакта и установка его флага satisfied в true, после чего запускается внутренний цикл, состоящий + // из contactsNum - 1 итераций. На каждой итерации ищется наихудший контакт среди оставшихся (satisfied == false). Если такой контакт найден, то выполняется его разделение + // и переход к следующей итерации внутреннего цикла, иначе внутренний цикл прерывается и выполняется переход к следующей итерации внешнего цикла (шаг 1.). + for (var iter:int = 0; iter < iterNum; iter++) { + // Ищем контакт с максимальной величиной пересечения тел, одновременно сбрасывая флаги satisfied в false + var worstContact:Contact = contacts[0]; + var i:int; + for (i = 1; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + contact.satisfied = false; + if (contact.maxPenetration > worstContact.maxPenetration) worstContact = contact; + } + if (worstContact.maxPenetration <= allowedPenetration) return; + + resolveInterpenetration(worstContact); + // Внутренний цикл по оставшимся контактам + for (i = 1; i < contactsNum; i++) { + worstContact = getWorstContact(); + if (worstContact == null) break; + resolveInterpenetration(worstContact); + } + } + } + + /** + * + */ + private function getWorstContact():Contact { + var maxPen:Number = 0; + var worst:Contact = null; + for (var i:int = 0; i < contactsNum; i++) { + var c:Contact = contacts[i]; + if (!c.satisfied && c.maxPenetration > maxPen) { + worst = c; + maxPen = c.maxPenetration; + } + } + return maxPen > allowedPenetration ? worst : null; + } + + /** + * Разделяет указанный контакт, выполняя staticSeparationSteps итераций по списку точек контакта. + * 1. В начале каждой итерации ищется точка с наибольшим пересечением, одновременно сбрасываются флаги satisfied у точек. + * 2. Для найденной точки выполняется процедура разделения. + * 3. После выполняется pcount - 1 итерация по списку точек, каждый раз ищется наихудшая среди имеющих satisfied == false, для которой выполняется процедура разделения. + * Если наихудшая точка не найдена, то выполнется переход к следующей итерации внешнего цикла (шаг 1). + * + * В конце процедуры разделения обновляются значения пересечений для остальных точек контакта, а также для всех прочих контактов, относящихся к телам текущего. + * + * @param contact контакт для разделения + */ + private function resolveInterpenetration(contact:Contact):void { + contact.satisfied = true; + + for (var step:int = 0; step < staticSeparationSteps; step++) { + var worstCp:ContactPoint = contact.points[0]; + var cp:ContactPoint; + var i:int; + for (i = 1; i < contact.pcount; i++) { + cp = contact.points[i]; + cp.satisfied = false; + if (cp.penetration > worstCp.penetration) worstCp = cp; + } + if (worstCp.penetration <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + // Разделяем оставшиеся точки + var maxPen:Number = 0; + for (i = 1; i < contact.pcount; i++) { + // Поиск наихудшей точки + for (var j:int = 0; j < contact.pcount; j++) { + cp = contact.points[j]; + if (cp.satisfied) continue; + if (cp.penetration > maxPen) { + maxPen = cp.penetration; + worstCp = cp; + } + } + if (maxPen <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + } + } + } + + /** + * + * @param cp + * @param contact + */ + private function separateContactPoint(cp:ContactPoint, contact:Contact):void { + cp.satisfied = true; + + var b1:Body = contact.body1; + var b2:Body = contact.body2; + var totalMove:Number = cp.penetration - allowedPenetration; + var moveCoeff:Number = totalMove/cp.velByUnitImpulseN; + var linMove1:Number; + var angleMove1:Number; + if (b1.movable) { + linMove1 = b1.invMass*moveCoeff; + angleMove1 = cp.angularInertia1*moveCoeff; + if (angleMove1 > maxAngleMove) { + linMove1 += angleMove1 - maxAngleMove; + angleMove1 -= maxAngleMove; + } + b1.state.pos.vAddScaled(linMove1, contact.normal); + _v1.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vScale(angleMove1); + b1.state.orientation.addScaledVector(_v1, 1); + } + var linMove2:Number; + var angleMove2:Number; + if (b2 != null && b2.movable) { + linMove2 = b2.invMass*moveCoeff; + angleMove2 = cp.angularInertia2*moveCoeff; + if (angleMove2 > maxAngleMove) { + linMove2 += angleMove2 - maxAngleMove; + angleMove2 -= maxAngleMove; + } + b2.state.pos.vAddScaled(-linMove2, contact.normal); + _v2.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vScale(angleMove2); + _v2.vReverse(); + b2.state.orientation.addScaledVector(_v2, 1); + } + cp.penetration = allowedPenetration; + // Обновляем пересечения в других точках + var i:int; + for (i = 0; i < contact.pcount; i++) { + var cp1:ContactPoint = contact.points[i]; + if (cp1 == cp) continue; + var angularMove:Number; + if (b1.movable) { + angularMove = _v.vCross2(_v1, cp1.r1).vDot(contact.normal); + cp1.penetration -= linMove1 + angularMove; + } + if (b2 != null && b2.movable) { + angularMove = _v.vCross2(_v2, cp1.r2).vDot(contact.normal); + cp1.penetration -= linMove2 - angularMove; + } + // Обновление максимального значения для контакта + if (cp1.penetration > contact.maxPenetration) contact.maxPenetration = cp1.penetration; + } + // Обновляем пересечения для других контактов + var c:Contact; + var j:int; + if (b1.movable) { + for (i = 0; i < b1.contactsNum; i++) { + c = b1.contacts[i]; + if (c == contact) continue; + for (j = 0; j < c.pcount; j++) { + cp1 = c.points[j]; + if (b1 == c.body1) cp1.penetration -= linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r1).vDot(c.normal); + else cp1.penetration += linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r2).vDot(c.normal); + if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; + } + } + } + if (b2 != null && b2.movable) { + for (i = 0; i < b2.contactsNum; i++) { + c = b2.contacts[i]; + if (c == contact) continue; + for (j = 0; j < c.pcount; j++) { + cp1 = c.points[j]; + if (b2 == c.body1) cp1.penetration -= linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r1).vDot(c.normal); + else cp1.penetration += linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r2).vDot(c.normal); + if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; + } + } + } + } + + /** + * + */ + private function postPhysics():void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i]; + body.clearAccumulators(); + body.calcDerivedData(); + if (body.canFreeze) { + if (body.state.velocity.vLength() < linSpeedFreezeLimit && body.state.rotation.vLength() < angSpeedFreezeLimit) { + if (!body.frozen) { + body.freezeCounter++; + if (body.freezeCounter >= freezeSteps) body.frozen = true; + } + } else { + body.freezeCounter = 0; + body.frozen = false; + } + } + } + } + + /** + * + * @param dt + */ + public function runPhysics(dt:Number):void { + timeStamp++; + applyForces(dt); + detectCollisions(dt); + preProcessContacts(dt); + processContacts(dt, false); + intergateVelocities(dt); + processContacts(dt, true); + integratePositions(dt); + postPhysics(); + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/rigid/constraints/.svn/all-wcprops b/0.0.4.0/src/alternativa/physics/rigid/constraints/.svn/all-wcprops new file mode 100644 index 0000000..9ee1fba --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/rigid/constraints/.svn/all-wcprops @@ -0,0 +1,17 @@ +K 25 +svn:wc:ra_dav:version-url +V 121 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/rigid/constraints +END +Constraint.as +K 25 +svn:wc:ra_dav:version-url +V 135 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/rigid/constraints/Constraint.as +END +MaxDistanceConstraint.as +K 25 +svn:wc:ra_dav:version-url +V 146 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/rigid/constraints/MaxDistanceConstraint.as +END diff --git a/0.0.4.0/src/alternativa/physics/rigid/constraints/.svn/entries b/0.0.4.0/src/alternativa/physics/rigid/constraints/.svn/entries new file mode 100644 index 0000000..95496ee --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/rigid/constraints/.svn/entries @@ -0,0 +1,52 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/rigid/constraints +http://svndev.alternativaplatform.com + + + +2009-06-29T03:58:47.301371Z +15198 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Constraint.as +file + + + + +2010-10-28T04:32:41.000000Z +757824f9eeb3c89243d104910e0d0529 +2009-04-23T13:43:38.903278Z +11779 +mike + +MaxDistanceConstraint.as +file + + + + +2010-10-28T04:32:41.000000Z +025a27c81aeaf18c860f3a4c4c63cf96 +2009-06-29T03:58:47.301371Z +15198 +mike + diff --git a/0.0.4.0/src/alternativa/physics/rigid/constraints/.svn/format b/0.0.4.0/src/alternativa/physics/rigid/constraints/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/rigid/constraints/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.4.0/src/alternativa/physics/rigid/constraints/.svn/text-base/Constraint.as.svn-base b/0.0.4.0/src/alternativa/physics/rigid/constraints/.svn/text-base/Constraint.as.svn-base new file mode 100644 index 0000000..c2cd912 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/rigid/constraints/.svn/text-base/Constraint.as.svn-base @@ -0,0 +1,35 @@ +package alternativa.physics.rigid.constraints { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.RigidWorld; + + use namespace altphysics; + + /** + * + */ + public class Constraint { + + altphysics var satisfied:Boolean; + altphysics var world:RigidWorld; + + /** + * + */ + public function Constraint() { + } + + /** + * @param dt + */ + public function preProcess(dt:Number):void { + } + + /** + * @param dt + */ + public function apply(dt:Number):void { + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/rigid/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base b/0.0.4.0/src/alternativa/physics/rigid/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base new file mode 100644 index 0000000..008d4a4 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/rigid/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base @@ -0,0 +1,155 @@ +package alternativa.physics.rigid.constraints { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.BodyState; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class MaxDistanceConstraint extends Constraint { + // Первое тело ограничения + altphysics var body1:Body; + // Второе тело ограничения. Может быть null. + altphysics var body2:Body; + // Первая точка крепления в системе координат первого тела + altphysics var r1:Vector3; + // Вторая точка привязки в системе координат второго тела. В случае отсутствия второго тела, координаты задаются в мировой системе. + altphysics var r2:Vector3; + // Максимально допустимое расстояние между точками крепления + altphysics var maxDistance:Number; + // Радиус-вектор первой точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr1:Vector3 = new Vector3(); + // Радиус-вектор второй точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr2:Vector3 = new Vector3(); + // Минимальная скорость сближения для удовлетворения ограничения + private var minClosingVel:Number; + // Изменение относительной скорости на единицу импульса + private var velByUnitImpulseN:Number; + // Направление корректирующего импульса + private var impulseDirection:Vector3 = new Vector3(); + + // Вспомогательные переменные + private static var _v:Vector3 = new Vector3(); + private static var _v1:Vector3 = new Vector3(); + private static var _v2:Vector3 = new Vector3(); + + /** + * @param body1 + * @param body2 + * @param r1 + * @param r2 + * @param maxDistance + */ + public function MaxDistanceConstraint(body1:Body, body2:Body, r1:Vector3, r2:Vector3, maxDistance:Number) { + super(); + this.body1 = body1; + this.body2 = body2; + this.r1 = r1.vClone(); + this.r2 = r2.vClone(); + this.maxDistance = maxDistance; + } + + /** + * @param dt + */ + override public function preProcess(dt:Number):void { + // Вычислим расстояние между точками + body1.baseMatrix.transformVector(r1, wr1); + if (body2 != null) body2.baseMatrix.transformVector(r2, wr2); + else { + wr2.x = r2.x; + wr2.y = r2.y; + wr2.z = r2.z; + } + var p1:Vector3 = body1.state.pos; + impulseDirection.x = wr2.x - wr1.x - p1.x; + impulseDirection.y = wr2.y - wr1.y - p1.y; + impulseDirection.z = wr2.z - wr1.z - p1.z; + if (body2 != null) { + var p2:Vector3 = body2.state.pos; + impulseDirection.x += p2.x; + impulseDirection.y += p2.y; + impulseDirection.z += p2.z; + } + + var len:Number = Math.sqrt(impulseDirection.x*impulseDirection.x + impulseDirection.y*impulseDirection.y + impulseDirection.z*impulseDirection.z); + var delta:Number = len - maxDistance; + if (delta > 0) { + satisfied = false; + if (len == 0) impulseDirection.x = 1; + else { + impulseDirection.x /= len; + impulseDirection.y /= len; + impulseDirection.z /= len; + } + minClosingVel = delta/(world.penResolutionSteps*dt); + if (minClosingVel > world.maxPenResolutionSpeed) minClosingVel = world.maxPenResolutionSpeed; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + velByUnitImpulseN = 0; + if (body1.movable) { +// velByUnitImpulseN += body1.invMass + _v.vCross2(wr1, impulseDirection).vTransformBy3(body1.invInertiaWorld).vCross(wr1).vDot(impulseDirection); + _v.x = wr1.y*impulseDirection.z - wr1.z*impulseDirection.y; + _v.y = wr1.z*impulseDirection.x - wr1.x*impulseDirection.z; + _v.z = wr1.x*impulseDirection.y - wr1.y*impulseDirection.x; + _v.vTransformBy3(body1.invInertiaWorld); + _v1.x = _v.y*wr1.z - _v.z*wr1.y; + _v1.y = _v.z*wr1.x - _v.x*wr1.z; + _v1.z = _v.x*wr1.y - _v.y*wr1.x; + velByUnitImpulseN += body1.invMass + _v1.x*impulseDirection.x + _v1.y*impulseDirection.y + _v1.z*impulseDirection.z; + } + if (body2 != null && body2.movable) { +// velByUnitImpulseN += body2.invMass + _v.vCross2(wr2, impulseDirection).vTransformBy3(body2.invInertiaWorld).vCross(wr2).vDot(impulseDirection); + _v.x = wr2.y*impulseDirection.z - wr2.z*impulseDirection.y; + _v.y = wr2.z*impulseDirection.x - wr2.x*impulseDirection.z; + _v.z = wr2.x*impulseDirection.y - wr2.y*impulseDirection.x; + _v.vTransformBy3(body2.invInertiaWorld); + _v1.x = _v.y*wr2.z - _v.z*wr2.y; + _v1.y = _v.z*wr2.x - _v.x*wr2.z; + _v1.z = _v.x*wr2.y - _v.y*wr2.x; + velByUnitImpulseN += body2.invMass + _v1.x*impulseDirection.x + _v1.y*impulseDirection.y + _v1.z*impulseDirection.z; + } + } else satisfied = true; + } + + /** + * @param dt + */ + override public function apply(dt:Number):void { + if (satisfied) return; + // Расчитываем проекцию скорости относительной скорости + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 + var state:BodyState = body1.state; + var rot:Vector3 = state.rotation; +// _v1.vCross2(state.rotation, wr1); + _v1.x = rot.y*wr1.z - rot.z*wr1.y; + _v1.y = rot.z*wr1.x - rot.x*wr1.z; + _v1.z = rot.x*wr1.y - rot.y*wr1.x; + _v.x = state.velocity.x + _v1.x; + _v.y = state.velocity.y + _v1.y; + _v.z = state.velocity.z + _v1.z; + // V2 = V2_c + w2%r2 + if (body2 != null) { + state = body2.state; +// _v2.vCross2(state.rotation, wr2); + _v2.x = rot.y*wr2.z - rot.z*wr2.y; + _v2.y = rot.z*wr2.x - rot.x*wr2.z; + _v2.z = rot.x*wr2.y - rot.y*wr2.x; + _v.x -= state.velocity.x - _v2.x; + _v.y -= state.velocity.y - _v2.y; + _v.z -= state.velocity.z - _v2.z; + } + var closingVel:Number = _v.x*impulseDirection.x + _v.y*impulseDirection.y + _v.z*impulseDirection.z; + if (closingVel > minClosingVel) return; + var impulse:Number = (minClosingVel - closingVel)/velByUnitImpulseN; + if (body1.movable) body1.applyRelPosWorldImpulse(wr1, impulseDirection, impulse); + if (body2 != null && body2.movable) body2.applyRelPosWorldImpulse(wr2, impulseDirection, -impulse); + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/rigid/constraints/Constraint.as b/0.0.4.0/src/alternativa/physics/rigid/constraints/Constraint.as new file mode 100644 index 0000000..c2cd912 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/rigid/constraints/Constraint.as @@ -0,0 +1,35 @@ +package alternativa.physics.rigid.constraints { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.RigidWorld; + + use namespace altphysics; + + /** + * + */ + public class Constraint { + + altphysics var satisfied:Boolean; + altphysics var world:RigidWorld; + + /** + * + */ + public function Constraint() { + } + + /** + * @param dt + */ + public function preProcess(dt:Number):void { + } + + /** + * @param dt + */ + public function apply(dt:Number):void { + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/rigid/constraints/MaxDistanceConstraint.as b/0.0.4.0/src/alternativa/physics/rigid/constraints/MaxDistanceConstraint.as new file mode 100644 index 0000000..008d4a4 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/rigid/constraints/MaxDistanceConstraint.as @@ -0,0 +1,155 @@ +package alternativa.physics.rigid.constraints { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.BodyState; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class MaxDistanceConstraint extends Constraint { + // Первое тело ограничения + altphysics var body1:Body; + // Второе тело ограничения. Может быть null. + altphysics var body2:Body; + // Первая точка крепления в системе координат первого тела + altphysics var r1:Vector3; + // Вторая точка привязки в системе координат второго тела. В случае отсутствия второго тела, координаты задаются в мировой системе. + altphysics var r2:Vector3; + // Максимально допустимое расстояние между точками крепления + altphysics var maxDistance:Number; + // Радиус-вектор первой точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr1:Vector3 = new Vector3(); + // Радиус-вектор второй точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr2:Vector3 = new Vector3(); + // Минимальная скорость сближения для удовлетворения ограничения + private var minClosingVel:Number; + // Изменение относительной скорости на единицу импульса + private var velByUnitImpulseN:Number; + // Направление корректирующего импульса + private var impulseDirection:Vector3 = new Vector3(); + + // Вспомогательные переменные + private static var _v:Vector3 = new Vector3(); + private static var _v1:Vector3 = new Vector3(); + private static var _v2:Vector3 = new Vector3(); + + /** + * @param body1 + * @param body2 + * @param r1 + * @param r2 + * @param maxDistance + */ + public function MaxDistanceConstraint(body1:Body, body2:Body, r1:Vector3, r2:Vector3, maxDistance:Number) { + super(); + this.body1 = body1; + this.body2 = body2; + this.r1 = r1.vClone(); + this.r2 = r2.vClone(); + this.maxDistance = maxDistance; + } + + /** + * @param dt + */ + override public function preProcess(dt:Number):void { + // Вычислим расстояние между точками + body1.baseMatrix.transformVector(r1, wr1); + if (body2 != null) body2.baseMatrix.transformVector(r2, wr2); + else { + wr2.x = r2.x; + wr2.y = r2.y; + wr2.z = r2.z; + } + var p1:Vector3 = body1.state.pos; + impulseDirection.x = wr2.x - wr1.x - p1.x; + impulseDirection.y = wr2.y - wr1.y - p1.y; + impulseDirection.z = wr2.z - wr1.z - p1.z; + if (body2 != null) { + var p2:Vector3 = body2.state.pos; + impulseDirection.x += p2.x; + impulseDirection.y += p2.y; + impulseDirection.z += p2.z; + } + + var len:Number = Math.sqrt(impulseDirection.x*impulseDirection.x + impulseDirection.y*impulseDirection.y + impulseDirection.z*impulseDirection.z); + var delta:Number = len - maxDistance; + if (delta > 0) { + satisfied = false; + if (len == 0) impulseDirection.x = 1; + else { + impulseDirection.x /= len; + impulseDirection.y /= len; + impulseDirection.z /= len; + } + minClosingVel = delta/(world.penResolutionSteps*dt); + if (minClosingVel > world.maxPenResolutionSpeed) minClosingVel = world.maxPenResolutionSpeed; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + velByUnitImpulseN = 0; + if (body1.movable) { +// velByUnitImpulseN += body1.invMass + _v.vCross2(wr1, impulseDirection).vTransformBy3(body1.invInertiaWorld).vCross(wr1).vDot(impulseDirection); + _v.x = wr1.y*impulseDirection.z - wr1.z*impulseDirection.y; + _v.y = wr1.z*impulseDirection.x - wr1.x*impulseDirection.z; + _v.z = wr1.x*impulseDirection.y - wr1.y*impulseDirection.x; + _v.vTransformBy3(body1.invInertiaWorld); + _v1.x = _v.y*wr1.z - _v.z*wr1.y; + _v1.y = _v.z*wr1.x - _v.x*wr1.z; + _v1.z = _v.x*wr1.y - _v.y*wr1.x; + velByUnitImpulseN += body1.invMass + _v1.x*impulseDirection.x + _v1.y*impulseDirection.y + _v1.z*impulseDirection.z; + } + if (body2 != null && body2.movable) { +// velByUnitImpulseN += body2.invMass + _v.vCross2(wr2, impulseDirection).vTransformBy3(body2.invInertiaWorld).vCross(wr2).vDot(impulseDirection); + _v.x = wr2.y*impulseDirection.z - wr2.z*impulseDirection.y; + _v.y = wr2.z*impulseDirection.x - wr2.x*impulseDirection.z; + _v.z = wr2.x*impulseDirection.y - wr2.y*impulseDirection.x; + _v.vTransformBy3(body2.invInertiaWorld); + _v1.x = _v.y*wr2.z - _v.z*wr2.y; + _v1.y = _v.z*wr2.x - _v.x*wr2.z; + _v1.z = _v.x*wr2.y - _v.y*wr2.x; + velByUnitImpulseN += body2.invMass + _v1.x*impulseDirection.x + _v1.y*impulseDirection.y + _v1.z*impulseDirection.z; + } + } else satisfied = true; + } + + /** + * @param dt + */ + override public function apply(dt:Number):void { + if (satisfied) return; + // Расчитываем проекцию скорости относительной скорости + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 + var state:BodyState = body1.state; + var rot:Vector3 = state.rotation; +// _v1.vCross2(state.rotation, wr1); + _v1.x = rot.y*wr1.z - rot.z*wr1.y; + _v1.y = rot.z*wr1.x - rot.x*wr1.z; + _v1.z = rot.x*wr1.y - rot.y*wr1.x; + _v.x = state.velocity.x + _v1.x; + _v.y = state.velocity.y + _v1.y; + _v.z = state.velocity.z + _v1.z; + // V2 = V2_c + w2%r2 + if (body2 != null) { + state = body2.state; +// _v2.vCross2(state.rotation, wr2); + _v2.x = rot.y*wr2.z - rot.z*wr2.y; + _v2.y = rot.z*wr2.x - rot.x*wr2.z; + _v2.z = rot.x*wr2.y - rot.y*wr2.x; + _v.x -= state.velocity.x - _v2.x; + _v.y -= state.velocity.y - _v2.y; + _v.z -= state.velocity.z - _v2.z; + } + var closingVel:Number = _v.x*impulseDirection.x + _v.y*impulseDirection.y + _v.z*impulseDirection.z; + if (closingVel > minClosingVel) return; + var impulse:Number = (minClosingVel - closingVel)/velByUnitImpulseN; + if (body1.movable) body1.applyRelPosWorldImpulse(wr1, impulseDirection, impulse); + if (body2 != null && body2.movable) body2.applyRelPosWorldImpulse(wr2, impulseDirection, -impulse); + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/rigid/primitives/.svn/all-wcprops b/0.0.4.0/src/alternativa/physics/rigid/primitives/.svn/all-wcprops new file mode 100644 index 0000000..cdf9805 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/rigid/primitives/.svn/all-wcprops @@ -0,0 +1,35 @@ +K 25 +svn:wc:ra_dav:version-url +V 120 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/rigid/primitives +END +RigidPlane.as +K 25 +svn:wc:ra_dav:version-url +V 134 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/rigid/primitives/RigidPlane.as +END +RigidSphere.as +K 25 +svn:wc:ra_dav:version-url +V 135 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/rigid/primitives/RigidSphere.as +END +RigidBox.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/rigid/primitives/RigidBox.as +END +RigidCylinder.as +K 25 +svn:wc:ra_dav:version-url +V 137 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/rigid/primitives/RigidCylinder.as +END +RigidRect.as +K 25 +svn:wc:ra_dav:version-url +V 133 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/rigid/primitives/RigidRect.as +END diff --git a/0.0.4.0/src/alternativa/physics/rigid/primitives/.svn/entries b/0.0.4.0/src/alternativa/physics/rigid/primitives/.svn/entries new file mode 100644 index 0000000..dd78ade --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/rigid/primitives/.svn/entries @@ -0,0 +1,88 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/rigid/primitives +http://svndev.alternativaplatform.com + + + +2009-06-08T06:46:18.310732Z +14099 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +RigidPlane.as +file + + + + +2010-10-28T04:32:41.000000Z +c4713421e357ee1967dca9ab3ffa7290 +2009-06-08T06:46:18.310732Z +14099 +mike + +RigidSphere.as +file + + + + +2010-10-28T04:32:41.000000Z +79ef38eab05657ad9627bdac4ade7271 +2009-04-23T13:43:38.903278Z +11779 +mike + +RigidBox.as +file + + + + +2010-10-28T04:32:41.000000Z +da834a5d208ff30f3a74321f75301cd7 +2009-06-08T06:46:18.310732Z +14099 +mike + +RigidCylinder.as +file + + + + +2010-10-28T04:32:41.000000Z +61512d31f9e211dcb0864164aa6f5210 +2009-04-23T13:43:38.903278Z +11779 +mike + +RigidRect.as +file + + + + +2010-10-28T04:32:41.000000Z +4cff4e313a1f41d4ccfa6480ea78173e +2009-04-23T13:43:38.903278Z +11779 +mike + diff --git a/0.0.4.0/src/alternativa/physics/rigid/primitives/.svn/format b/0.0.4.0/src/alternativa/physics/rigid/primitives/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/rigid/primitives/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.4.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidBox.as.svn-base b/0.0.4.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidBox.as.svn-base new file mode 100644 index 0000000..c01ff61 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidBox.as.svn-base @@ -0,0 +1,56 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class RigidBox extends Body { + + /** + * + * @param halfSize + * @param mass + */ + public function RigidBox(halfSize:Vector3, mass:Number) { + super(0, Matrix3.ZERO); + addCollisionPrimitive(new CollisionBox(halfSize, 1)); + setParams(halfSize, mass); + } + + /** + * @param halfSize + * @param mass + */ + public function setParams(halfSize:Vector3, mass:Number):void { + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + invInertia.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + invInertia.a = 3*invMass/(yy + zz); + invInertia.f = 3*invMass/(zz + xx); + invInertia.k = 3*invMass/(xx + yy); + } + var prim:CollisionBox = collisionPrimitives[0] as CollisionBox; + prim.hs.vCopy(halfSize); + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidCylinder.as.svn-base b/0.0.4.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidCylinder.as.svn-base new file mode 100644 index 0000000..0f9f2b3 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidCylinder.as.svn-base @@ -0,0 +1,32 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + + use namespace altphysics; + + public class RigidCylinder extends Body { + + altphysics var r:Number; + altphysics var h:Number; + + public function RigidCylinder(mass:Number, radius:Number, height:Number) { + super(0, Matrix3.ZERO); + setParams(mass, radius, height); + } + + public function setParams(mass:Number, radius:Number, height:Number):void { + r = radius; + h = height; + invInertiaWorld.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + invInertia.a = invInertia.f = mass*(h*h/12 + r*r/4); + invInertia.k = 0.5*mass*r*r; + invInertia.invert(); + } + } + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidPlane.as.svn-base b/0.0.4.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidPlane.as.svn-base new file mode 100644 index 0000000..26f74b2 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidPlane.as.svn-base @@ -0,0 +1,23 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + public class RigidPlane extends Body { + + public var normal:Vector3 = new Vector3(); + public var offset:Number; + + public function RigidPlane() { + super(0, Matrix3.ZERO); + this.normal.vCopy(Vector3.Z_AXIS); + this.offset = 0; + movable = false; + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidRect.as.svn-base b/0.0.4.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidRect.as.svn-base new file mode 100644 index 0000000..3c3913d --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidRect.as.svn-base @@ -0,0 +1,20 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + public class RigidRect extends Body { + + public function RigidRect(halfWidth:Number, halfLength:Number) { + super(0, Matrix3.ZERO); + movable = false; + addCollisionPrimitive(new CollisionRect(new Vector3(halfWidth, halfLength, 0), 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidSphere.as.svn-base b/0.0.4.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidSphere.as.svn-base new file mode 100644 index 0000000..9668716 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidSphere.as.svn-base @@ -0,0 +1,29 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + + use namespace altphysics; + + public class RigidSphere extends Body { + + public function RigidSphere(radius:Number, mass:Number) { + var m:Matrix3; + if (mass == Infinity) { + mass = 0; + m = Matrix3.ZERO; + } else { + mass = 1/mass; + var r2:Number = radius*radius; + m = new Matrix3(); + m.a = m.f = m.k = 2.5*mass/r2; + } + super(mass, m); + if (mass == 0) movable = false; + addCollisionPrimitive(new CollisionSphere(radius, 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/rigid/primitives/RigidBox.as b/0.0.4.0/src/alternativa/physics/rigid/primitives/RigidBox.as new file mode 100644 index 0000000..c01ff61 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/rigid/primitives/RigidBox.as @@ -0,0 +1,56 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class RigidBox extends Body { + + /** + * + * @param halfSize + * @param mass + */ + public function RigidBox(halfSize:Vector3, mass:Number) { + super(0, Matrix3.ZERO); + addCollisionPrimitive(new CollisionBox(halfSize, 1)); + setParams(halfSize, mass); + } + + /** + * @param halfSize + * @param mass + */ + public function setParams(halfSize:Vector3, mass:Number):void { + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + invInertia.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + invInertia.a = 3*invMass/(yy + zz); + invInertia.f = 3*invMass/(zz + xx); + invInertia.k = 3*invMass/(xx + yy); + } + var prim:CollisionBox = collisionPrimitives[0] as CollisionBox; + prim.hs.vCopy(halfSize); + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/rigid/primitives/RigidCylinder.as b/0.0.4.0/src/alternativa/physics/rigid/primitives/RigidCylinder.as new file mode 100644 index 0000000..0f9f2b3 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/rigid/primitives/RigidCylinder.as @@ -0,0 +1,32 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + + use namespace altphysics; + + public class RigidCylinder extends Body { + + altphysics var r:Number; + altphysics var h:Number; + + public function RigidCylinder(mass:Number, radius:Number, height:Number) { + super(0, Matrix3.ZERO); + setParams(mass, radius, height); + } + + public function setParams(mass:Number, radius:Number, height:Number):void { + r = radius; + h = height; + invInertiaWorld.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + invInertia.a = invInertia.f = mass*(h*h/12 + r*r/4); + invInertia.k = 0.5*mass*r*r; + invInertia.invert(); + } + } + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/rigid/primitives/RigidPlane.as b/0.0.4.0/src/alternativa/physics/rigid/primitives/RigidPlane.as new file mode 100644 index 0000000..26f74b2 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/rigid/primitives/RigidPlane.as @@ -0,0 +1,23 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + public class RigidPlane extends Body { + + public var normal:Vector3 = new Vector3(); + public var offset:Number; + + public function RigidPlane() { + super(0, Matrix3.ZERO); + this.normal.vCopy(Vector3.Z_AXIS); + this.offset = 0; + movable = false; + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/rigid/primitives/RigidRect.as b/0.0.4.0/src/alternativa/physics/rigid/primitives/RigidRect.as new file mode 100644 index 0000000..3c3913d --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/rigid/primitives/RigidRect.as @@ -0,0 +1,20 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + public class RigidRect extends Body { + + public function RigidRect(halfWidth:Number, halfLength:Number) { + super(0, Matrix3.ZERO); + movable = false; + addCollisionPrimitive(new CollisionRect(new Vector3(halfWidth, halfLength, 0), 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/rigid/primitives/RigidSphere.as b/0.0.4.0/src/alternativa/physics/rigid/primitives/RigidSphere.as new file mode 100644 index 0000000..9668716 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/rigid/primitives/RigidSphere.as @@ -0,0 +1,29 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + + use namespace altphysics; + + public class RigidSphere extends Body { + + public function RigidSphere(radius:Number, mass:Number) { + var m:Matrix3; + if (mass == Infinity) { + mass = 0; + m = Matrix3.ZERO; + } else { + mass = 1/mass; + var r2:Number = radius*radius; + m = new Matrix3(); + m.a = m.f = m.k = 2.5*mass/r2; + } + super(mass, m); + if (mass == 0) movable = false; + addCollisionPrimitive(new CollisionSphere(radius, 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/types/.svn/all-wcprops b/0.0.4.0/src/alternativa/physics/types/.svn/all-wcprops new file mode 100644 index 0000000..bdc33da --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/types/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 109 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/types +END +Quaternion.as +K 25 +svn:wc:ra_dav:version-url +V 123 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/types/Quaternion.as +END +Vector3.as +K 25 +svn:wc:ra_dav:version-url +V 120 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/types/Vector3.as +END +Matrix3.as +K 25 +svn:wc:ra_dav:version-url +V 120 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/types/Matrix3.as +END +Matrix4.as +K 25 +svn:wc:ra_dav:version-url +V 120 +/!svn/ver/16620/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/types/Matrix4.as +END diff --git a/0.0.4.0/src/alternativa/physics/types/.svn/entries b/0.0.4.0/src/alternativa/physics/types/.svn/entries new file mode 100644 index 0000000..df9bd9c --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/types/.svn/entries @@ -0,0 +1,76 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.4.0/src/alternativa/physics/types +http://svndev.alternativaplatform.com + + + +2009-06-25T09:18:57.805913Z +15117 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Quaternion.as +file + + + + +2010-10-28T04:32:41.000000Z +595f36fdfea8e82e752bb989e97e3a9a +2009-05-04T04:55:56.466789Z +12306 +mike + +Vector3.as +file + + + + +2010-10-28T04:32:41.000000Z +c8476935b1206abe041e8822204346cd +2009-06-25T09:18:57.805913Z +15117 +mike + +Matrix3.as +file + + + + +2010-10-28T04:32:41.000000Z +3fde68593cd3eb5a7046c441a76c9c5c +2009-06-25T09:18:57.805913Z +15117 +mike + +Matrix4.as +file + + + + +2010-10-28T04:32:41.000000Z +b2ed6277a2052c2568a1b4f19c469223 +2009-05-04T04:55:56.466789Z +12306 +mike + diff --git a/0.0.4.0/src/alternativa/physics/types/.svn/format b/0.0.4.0/src/alternativa/physics/types/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/types/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.4.0/src/alternativa/physics/types/.svn/text-base/Matrix3.as.svn-base b/0.0.4.0/src/alternativa/physics/types/.svn/text-base/Matrix3.as.svn-base new file mode 100644 index 0000000..e07d0d1 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/types/.svn/text-base/Matrix3.as.svn-base @@ -0,0 +1,396 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + /** + * + */ + public class Matrix3 { + + public static const ZERO:Matrix3 = new Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0); + public static const IDENTITY:Matrix3 = new Matrix3(); + + public var a:Number; + public var b:Number; + public var c:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix3(a:Number = 1, b:Number = 0, c:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1) { + this.a = a; + this.b = b; + this.c = c; + + this.e = e; + this.f = f; + this.g = g; + + this.i = i; + this.j = j; + this.k = k; + } + + /** + * + * @return + */ + public function toIdentity():Matrix3 { + a = f = k = 1; + b = c = e = g = i = j = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + var det:Number = 1/(-cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk); + + a = (ff*kk - gg*jj)*det; + b = (cc*jj - bb*kk)*det; + c = (bb*gg - cc*ff)*det; + e = (gg*ii - ee*kk)*det; + f = (aa*kk - cc*ii)*det; + g = (cc*ee - aa*gg)*det; + i = (ee*jj - ff*ii)*det; + j = (bb*ii - aa*jj)*det; + k = (aa*ff - bb*ee)*det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prependTransposed(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.b + cc*m.c; + b = aa*m.e + bb*m.f + cc*m.g; + c = aa*m.i + bb*m.j + cc*m.k; + e = ee*m.a + ff*m.b + gg*m.c; + f = ee*m.e + ff*m.f + gg*m.g; + g = ee*m.i + ff*m.j + gg*m.k; + i = ii*m.a + jj*m.b + kk*m.c; + j = ii*m.e + jj*m.f + kk*m.g; + k = ii*m.i + jj*m.j + kk*m.k; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix3):Matrix3 { + a += m.a; + b += m.b; + c += m.c; + e += m.e; + f += m.f; + g += m.g; + i += m.i; + j += m.j; + k += m.k; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix3):Matrix3 { + a -= m.a; + b -= m.b; + c -= m.c; + e -= m.e; + f -= m.f; + g -= m.g; + i -= m.i; + j -= m.j; + k -= m.k; + + return this; + } + + /** + * + * @return + */ + public function transpose():Matrix3 { + var tmp:Number = b; + b = e; + e = tmp; + tmp = c; + c = i; + i = tmp; + tmp = g; + g = j; + j = tmp; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector3To3D(vin:Vector3, vout:Vector3D):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * + * @param matrix + */ + public function createSkewSymmetric(v:Vector3):Matrix3 { + a = f = k = 0; + b = -v.z; + c = v.y; + e = v.z; + g = -v.x; + i = -v.y; + j = v.x; + return this; + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(i:int, axis:Vector3):void { + switch (i) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + } + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix3):Matrix3 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @return + */ + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):Matrix3 { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + + return this; + } + + /** + * Метод формирует матрицу поворота на заданный угол относительно заданной оси. + * + * @param axis нормализованный вектор, задающий ось, относительно которой выполняется поворот + * @param angle угол поворота в радианах + */ + public function fromAxisAngle(axis:Vector3, angle:Number):void { + var c1:Number = Math.cos(angle); + var s:Number = Math.sin(angle); + var t:Number = 1 - c1; + var x:Number = axis.x; + var y:Number = axis.y; + var z:Number = axis.z; + + a = t*x*x + c1; + b = t*x*y - z*s; + c = t*x*z + y*s; + + e = t*x*y + z*s; + f = t*y*y + c1; + g = t*y*z - x*s; + + i = t*x*z - y*s; + j = t*y*z + x*s; + k = t*z*z + c1; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix3 { + return new Matrix3(a, b, c, e, f, g, i, j, k); + } + + public function toString():String { + return "[Matrix3 (" + a + ", " + b + ", " + c + "), (" + e + ", " + f + ", " + g + "), (" + i + ", " + j + ", " + k + ")]"; + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/types/.svn/text-base/Matrix4.as.svn-base b/0.0.4.0/src/alternativa/physics/types/.svn/text-base/Matrix4.as.svn-base new file mode 100644 index 0000000..8cc23b2 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/types/.svn/text-base/Matrix4.as.svn-base @@ -0,0 +1,402 @@ +package alternativa.physics.types { + import __AS3__.vec.Vector; + + /** + * + */ + public class Matrix4 { + + public var a:Number; + public var b:Number; + public var c:Number; + public var d:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + public var h:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + public var l:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix4(a:Number = 1, b:Number = 0, c:Number = 0, d:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, h:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1, l:Number = 0) { + this.a = a; + this.b = b; + this.c = c; + this.d = h; + + this.e = e; + this.f = f; + this.g = g; + this.h = h; + + this.i = i; + this.j = j; + this.k = k; + this.l = l; + } + + /** + * + * @return + */ + public function toIdentity():Matrix4 { + a = f = k = 1; + b = c = e = g = i = j = d = h = l = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + var det:Number = -cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk; + + a = (-gg*jj + ff*kk)/det; + b = (cc*jj - bb*kk)/det; + c = (-cc*ff + bb*gg)/det; + d = (dd*gg*jj - cc*hh*jj - dd*ff*kk + bb*hh*kk + cc*ff*ll - bb*gg*ll)/det; + e = (gg*ii - ee*kk)/det; + f = (-cc*ii + aa*kk)/det; + g = (cc*ee - aa*gg)/det; + h = (cc*hh*ii - dd*gg*ii + dd*ee*kk - aa*hh*kk - cc*ee*ll + aa*gg*ll)/det; + i = (-ff*ii + ee*jj)/det; + j = (bb*ii - aa*jj)/det; + k = (-bb*ee + aa*ff)/det; + l = (dd*ff*ii - bb*hh*ii - dd*ee*jj + aa*hh*jj + bb*ee*ll - aa*ff*ll)/det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + d = m.a*dd + m.b*hh + m.c*ll + m.d; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + h = m.e*dd + m.f*hh + m.g*ll + m.h; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + l = m.i*dd + m.j*hh + m.k*ll + m.l; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + d = aa*m.d + bb*m.h + cc*m.l + dd; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + h = ee*m.d + ff*m.h + gg*m.l + hh; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + l = ii*m.d + jj*m.h + kk*m.l + ll; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix4):Matrix4 { + a += m.a; + b += m.b; + c += m.c; + d += m.d; + e += m.e; + f += m.f; + g += m.g; + h += m.h; + i += m.i; + j += m.j; + k += m.k; + l += m.l; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix4):Matrix4 { + a -= m.a; + b -= m.b; + c -= m.c; + d -= m.d; + e -= m.e; + f -= m.f; + g -= m.g; + h -= m.h; + i -= m.i; + j -= m.j; + k -= m.k; + l -= m.l; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectors(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverse(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(idx:int, axis:Vector3):void { + switch (idx) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + case 3: + axis.x = d; + axis.y = h; + axis.z = l; + return; + } + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix4):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = m.d; + e = m.e; + f = m.f; + g = m.g; + h = m.h; + i = m.i; + j = m.j; + k = m.k; + l = m.l; + + return this; + } + + public function setFromMatrix3(m:Matrix3, offset:Vector3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = offset.x; + e = m.e; + f = m.f; + g = m.g; + h = offset.y; + i = m.i; + j = m.j; + k = m.k; + l = offset.z; + + return this; + } + + /** + * @param rotations + */ + public function getEulerAngles(rotations:Vector3):void { + if (-1 < i && i < 1) { + rotations.x = Math.atan2(j, k); + rotations.y = -Math.asin(i); + rotations.z = Math.atan2(e, a); + } else { + rotations.x = 0; + rotations.y = 0.5*((i <= -1) ? Math.PI : -Math.PI); + rotations.z = Math.atan2(-b, f); + } + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix4 { + return new Matrix4(a, b, c, d, e, f, g, h, i, j, k, l); + } + + public function toString():String { + return "[Matrix4 [" + a.toFixed(3) + " " + b.toFixed(3) + " " + c.toFixed(3) + " " + d.toFixed(3) + "] [" + e.toFixed(3) + " " + f.toFixed(3) + " " + g.toFixed(3) + " " + h.toFixed(3) + "] [" + i.toFixed(3) + " " + j.toFixed(3) + " " + k.toFixed(3) + " " + l.toFixed(3) + "]]"; + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/types/.svn/text-base/Quaternion.as.svn-base b/0.0.4.0/src/alternativa/physics/types/.svn/text-base/Quaternion.as.svn-base new file mode 100644 index 0000000..7ad2af0 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/types/.svn/text-base/Quaternion.as.svn-base @@ -0,0 +1,396 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + public class Quaternion { + + public var w:Number; + public var x:Number; + public var y:Number; + public var z:Number; + + public static function multiply(q1:Quaternion, q2:Quaternion, result:Quaternion):void { + result.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z; + result.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y; + result.y = q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z; + result.z = q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x; + } + + public static function createFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngle(axis, angle); + return q; + } + + public static function createFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngleComponents(x, y, z, angle); + return q; + } + + public function Quaternion(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0) { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + } + + public function reset(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0):Quaternion { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + return this; + } + + public function normalize():Quaternion { + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * Умножает на указанный кватернион слева: this * q + * + * @param q множитель + */ + public function prepend(q:Quaternion):Quaternion { + var ww:Number = w*q.w - x*q.x - y*q.y - z*q.z; + var xx:Number = w*q.x + x*q.w + y*q.z - z*q.y; + var yy:Number = w*q.y + y*q.w + z*q.x - x*q.z; + var zz:Number = w*q.z + z*q.w + x*q.y - y*q.x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Умножает на указанный кватернион справа: q * this + * + * @param q множитель + */ + public function append(q:Quaternion):Quaternion { + var ww:Number = q.w*w - q.x*x - q.y*y - q.z*z; + var xx:Number = q.w*x + q.x*w + q.y*z - q.z*y; + var yy:Number = q.w*y + q.y*w + q.z*x - q.x*z; + var zz:Number = q.w*z + q.z*w + q.x*y - q.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * + * @param vector + */ + public function rotateByVector(v:Vector3):Quaternion { + var ww:Number = -v.x*x - v.y*y - v.z*z; + var xx:Number = v.x*w + v.y*z - v.z*y; + var yy:Number = v.y*w + v.z*x - v.x*z; + var zz:Number = v.z*w + v.x*y - v.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Добавляет вращение, приданное вектором угловой скорости за указанное время. + * + * @param v + * @param scale + */ + public function addScaledVector(v:Vector3, scale:Number):Quaternion { + var vx:Number = v.x*scale; + var vy:Number = v.y*scale; + var vz:Number = v.z*scale; + var ww:Number = -x*vx - y*vy - z*vz; + var xx:Number = vx*w + vy*z - vz*y; + var yy:Number = vy*w + vz*x - vx*z; + var zz:Number = vz*w + vx*y - vy*x; + w += 0.5*ww; + x += 0.5*xx; + y += 0.5*yy; + z += 0.5*zz; + // inlined normalize + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param m + * @return + */ + public function toMatrix3(m:Matrix3):Quaternion { + var xx2:Number = 2*x*x; + var yy2:Number = 2*y*y; + var zz2:Number = 2*z*z; + var xy2:Number = 2*x*y; + var yz2:Number = 2*y*z; + var zx2:Number = 2*z*x; + var wx2:Number = 2*w*x; + var wy2:Number = 2*w*y; + var wz2:Number = 2*w*z; + + m.a = 1 - yy2 - zz2; + m.b = xy2 - wz2; + m.c = zx2 + wy2; + + m.e = xy2 + wz2; + m.f = 1 - xx2 - zz2; + m.g = yz2 - wx2; + + m.i = zx2 - wy2; + m.j = yz2 + wx2; + m.k = 1 - xx2 - yy2; + return this; + } + + /** + * + * @return + */ + public function length():Number { + return Math.sqrt(w*w + x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function lengthSqr():Number { + return w*w + x*x + y*y + z*z; + } + + /** + * + * @param axis + * @param angle + * @return + */ + public function setFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z); + x = axis.x*k; + y = axis.y*k; + z = axis.z*k; + return this; + } + + /** + * + * @param x + * @param y + * @param z + * @param angle + */ + public function setFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(x*x + y*y + z*z); + this.x = x*k; + this.y = y*k; + this.z = z*k; + return this; + } + + /** + * + * @param vector + */ + public function toAxisVector(v:Vector3 = null):Vector3 { + if (w < -1 || w > 1) { + normalize(); + } + if (v == null) { + v = new Vector3(); + } + if (w > -1 && w < 1) { + if (w == 0) { + v.x = x; + v.y = y; + v.z = z; + } else { + var angle:Number = 2*Math.acos(w); + var coeff:Number = 1/Math.sqrt(1 - w*w); + v.x = x*coeff*angle; + v.y = y*coeff*angle; + v.z = z*coeff*angle; + } + } else { + v.x = 0; + v.y = 0; + v.z = 0; + } + return v; + } + + /** + * + * @param rotations + */ + public function getEulerAngles(angles:Vector3):Vector3 { + var qi2:Number = 2*x*x; + var qj2:Number = 2*y*y; + var qk2:Number = 2*z*z; + var qij:Number = 2*x*y; + var qjk:Number = 2*y*z; + var qki:Number = 2*z*x; + var qri:Number = 2*w*x; + var qrj:Number = 2*w*y; + var qrk:Number = 2*w*z; + + var aa:Number = 1 - qj2 - qk2; + var bb:Number = qij - qrk; + var ee:Number = qij + qrk; + var ff:Number = 1 - qi2 - qk2; + var ii:Number = qki - qrj; + var jj:Number = qjk + qri; + var kk:Number = 1 - qi2 - qj2; + + if (-1 < ii && ii < 1) { + if (angles == null) { + angles = new Vector3(Math.atan2(jj, kk), -Math.asin(ii), Math.atan2(ee, aa)); + } else { + angles.x = Math.atan2(jj, kk); + angles.y = -Math.asin(ii); + angles.z = Math.atan2(ee, aa); + } + } else { + if (angles == null) { + angles = new Vector3(0, 0.5*((ii <= -1) ? Math.PI : -Math.PI), Math.atan2(-bb, ff)); + } else { + angles.x = 0; + angles.y = 0.5*((ii <= -1) ? Math.PI : -Math.PI); + angles.z = Math.atan2(-bb, ff); + } + } + return angles; + } + + /** + * + */ + public function conjugate():void { + x = -x; + y = -y; + z = -z; + } + + /** + * Выполняет линейную интерполяцию. + * + * @param q1 начало отрезка + * @param q2 конец отрезка + * @param t время, обычно задаётся в интервале [0, 1] + */ + public function nlerp(q1:Quaternion, q2:Quaternion, t:Number):Quaternion { + var d:Number = 1 - t; + w = q1.w*d + q2.w*t; + x = q1.x*d + q2.x*t; + y = q1.y*d + q2.y*t; + z = q1.z*d + q2.z*t; + // inlined normalize + d = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param q + * @return + */ + public function subtract(q:Quaternion):Quaternion { + w -= q.w; + x -= q.x; + y -= q.y; + z -= q.z; + return this; + } + + /** + * + * @param q1 + * @param q2 + * @return + */ + public function diff(q1:Quaternion, q2:Quaternion):Quaternion { + w = q2.w - q1.w; + x = q2.x - q1.x; + y = q2.y - q1.y; + z = q2.z - q1.z; + return this; + } + + /** + * + * @param q + * @return + */ + public function copy(q:Quaternion):Quaternion { + w = q.w; + x = q.x; + y = q.y; + z = q.z; + return this; + } + + /** + * @param reslult + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + result.w = w; + return result; + } + + /** + * + * @return + */ + public function clone():Quaternion { + return new Quaternion(w, x, y, z); + } + + /** + * + * @return + */ + public function toString():String { + return "[" + w + ", " + x + ", " + y + ", " + z + "]"; + } + } +} diff --git a/0.0.4.0/src/alternativa/physics/types/.svn/text-base/Vector3.as.svn-base b/0.0.4.0/src/alternativa/physics/types/.svn/text-base/Vector3.as.svn-base new file mode 100644 index 0000000..13ffda0 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/types/.svn/text-base/Vector3.as.svn-base @@ -0,0 +1,331 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + /** + * + */ + public class Vector3 extends Vector3D { + + public static const ZERO:Vector3 = new Vector3(0, 0, 0); + public static const X_AXIS:Vector3 = new Vector3(1, 0, 0); + public static const Y_AXIS:Vector3 = new Vector3(0, 1, 0); + public static const Z_AXIS:Vector3 = new Vector3(0, 0, 1); + + /** + * + * @param x + * @param y + * @param z + */ + public function Vector3(x:Number = 0, y:Number = 0, z:Number = 0) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * + * @return + */ + public function vLength():Number { + return Math.sqrt(x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function vLengthSqr():Number { + return x*x + y*y + z*z; + } + + /** + * + * @param length + * @return + */ + public function vSetLength(length:Number):Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = length; + } else { + var k:Number = length/Math.sqrt(x*x + y*y + z*z); + x *= k; + y *= k; + z *= k; + } + return this; + } + + /** + * + * @return + */ + public function vNormalize():Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = 1; + } else { + d = Math.sqrt(d); + x /= d; + y /= d; + z /= d; + } + return this; + } + + /** + * Сложение координат. + * + * @param v точка, координаты которой прибавляются к собственным + */ + public function vAdd(v:Vector3):Vector3 { + x += v.x; + y += v.y; + z += v.z; + return this; + } + + /** + * + * @param k + * @param v + * @return + */ + public function vAddScaled(k:Number, v:Vector3):Vector3 { + x += k*v.x; + y += k*v.y; + z += k*v.z; + return this; + } + + /** + * Вычитание координат. + * + * @param v точка, координаты которой вычитаются из собственных + */ + public function vSubtract(v:Vector3):Vector3 { + x -= v.x; + y -= v.y; + z -= v.z; + return this; + } + + /** + * Вычисляет сумму векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + * @return this + */ + public function vSum(a:Vector3, b:Vector3):Vector3 { + x = a.x + b.x; + y = a.y + b.y; + z = a.z + b.z; + return this; + } + + /** + * Вычисление разности векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + */ + public function vDiff(a:Vector3, b:Vector3):Vector3 { + x = a.x - b.x; + y = a.y - b.y; + z = a.z - b.z; + return this; + } + + /** + * Умножение на скаляр. + * + * @param k число, на которое умножаются координаты + */ + public function vScale(k:Number):Vector3 { + x *= k; + y *= k; + z *= k; + return this; + } + + /** + * Инвертирование вектора. + */ + public function vReverse():Vector3 { + x = -x; + y = -y; + z = -z; + return this; + } + + /** + * + * @param v + * @return + */ + public function vDot(v:Vector3):Number { + return x*v.x + y*v.y + z*v.z; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross(v:Vector3):Vector3 { + var xx:Number = y*v.z - z*v.y; + var yy:Number = z*v.x - x*v.z; + var zz:Number = x*v.y - y*v.x; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross2(a:Vector3, b:Vector3):Vector3 { + x = a.y*b.z - a.z*b.y; + y = a.z*b.x - a.x*b.z; + z = a.x*b.y - a.y*b.x; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy3(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * + * @param matrix + */ + public function vTransformBy3Tr(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz + m.d; + y = m.e*xx + m.f*yy + m.g*zz + m.h; + z = m.i*xx + m.j*yy + m.k*zz + m.l; + return this; + } + + /** + * + * @param m + * @return + */ + public function vTransformInverseBy4(m:Matrix4):Vector3 { + var xx:Number = x - m.d; + var yy:Number = y - m.h; + var zz:Number = z - m.l; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * + * @param m + */ + public function vDeltaTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * Установка координат. + */ + public function vReset(x:Number = 0, y:Number = 0, z:Number = 0):Vector3 { + this.x = x; + this.y = y; + this.z = z; + return this; + } + + /** + * Копирование координат точки. + * + * @param v точка, координаты которой копируются + */ + public function vCopy(v:Vector3):Vector3 { + x = v.x; + y = v.y; + z = v.z; + return this; + } + + /** + * Клонирование точки. + * + * @return клонированная точка + */ + public function vClone():Vector3 { + return new Vector3(x, y, z); + } + + /** + * Копирует координаты вектора в экземпляр класса Vector3D. + * + * @param result экземпляр, в который копируются координаты + * @return переданный параметр + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + return result; + } + + /** + * @param source + * @return + */ + public function copyFromVector3D(source:Vector3D):Vector3 { + x = source.x; + y = source.y; + z = source.z; + return this; + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/types/Matrix3.as b/0.0.4.0/src/alternativa/physics/types/Matrix3.as new file mode 100644 index 0000000..e07d0d1 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/types/Matrix3.as @@ -0,0 +1,396 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + /** + * + */ + public class Matrix3 { + + public static const ZERO:Matrix3 = new Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0); + public static const IDENTITY:Matrix3 = new Matrix3(); + + public var a:Number; + public var b:Number; + public var c:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix3(a:Number = 1, b:Number = 0, c:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1) { + this.a = a; + this.b = b; + this.c = c; + + this.e = e; + this.f = f; + this.g = g; + + this.i = i; + this.j = j; + this.k = k; + } + + /** + * + * @return + */ + public function toIdentity():Matrix3 { + a = f = k = 1; + b = c = e = g = i = j = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + var det:Number = 1/(-cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk); + + a = (ff*kk - gg*jj)*det; + b = (cc*jj - bb*kk)*det; + c = (bb*gg - cc*ff)*det; + e = (gg*ii - ee*kk)*det; + f = (aa*kk - cc*ii)*det; + g = (cc*ee - aa*gg)*det; + i = (ee*jj - ff*ii)*det; + j = (bb*ii - aa*jj)*det; + k = (aa*ff - bb*ee)*det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prependTransposed(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.b + cc*m.c; + b = aa*m.e + bb*m.f + cc*m.g; + c = aa*m.i + bb*m.j + cc*m.k; + e = ee*m.a + ff*m.b + gg*m.c; + f = ee*m.e + ff*m.f + gg*m.g; + g = ee*m.i + ff*m.j + gg*m.k; + i = ii*m.a + jj*m.b + kk*m.c; + j = ii*m.e + jj*m.f + kk*m.g; + k = ii*m.i + jj*m.j + kk*m.k; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix3):Matrix3 { + a += m.a; + b += m.b; + c += m.c; + e += m.e; + f += m.f; + g += m.g; + i += m.i; + j += m.j; + k += m.k; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix3):Matrix3 { + a -= m.a; + b -= m.b; + c -= m.c; + e -= m.e; + f -= m.f; + g -= m.g; + i -= m.i; + j -= m.j; + k -= m.k; + + return this; + } + + /** + * + * @return + */ + public function transpose():Matrix3 { + var tmp:Number = b; + b = e; + e = tmp; + tmp = c; + c = i; + i = tmp; + tmp = g; + g = j; + j = tmp; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector3To3D(vin:Vector3, vout:Vector3D):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * + * @param matrix + */ + public function createSkewSymmetric(v:Vector3):Matrix3 { + a = f = k = 0; + b = -v.z; + c = v.y; + e = v.z; + g = -v.x; + i = -v.y; + j = v.x; + return this; + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(i:int, axis:Vector3):void { + switch (i) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + } + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix3):Matrix3 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @return + */ + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):Matrix3 { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + + return this; + } + + /** + * Метод формирует матрицу поворота на заданный угол относительно заданной оси. + * + * @param axis нормализованный вектор, задающий ось, относительно которой выполняется поворот + * @param angle угол поворота в радианах + */ + public function fromAxisAngle(axis:Vector3, angle:Number):void { + var c1:Number = Math.cos(angle); + var s:Number = Math.sin(angle); + var t:Number = 1 - c1; + var x:Number = axis.x; + var y:Number = axis.y; + var z:Number = axis.z; + + a = t*x*x + c1; + b = t*x*y - z*s; + c = t*x*z + y*s; + + e = t*x*y + z*s; + f = t*y*y + c1; + g = t*y*z - x*s; + + i = t*x*z - y*s; + j = t*y*z + x*s; + k = t*z*z + c1; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix3 { + return new Matrix3(a, b, c, e, f, g, i, j, k); + } + + public function toString():String { + return "[Matrix3 (" + a + ", " + b + ", " + c + "), (" + e + ", " + f + ", " + g + "), (" + i + ", " + j + ", " + k + ")]"; + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/types/Matrix4.as b/0.0.4.0/src/alternativa/physics/types/Matrix4.as new file mode 100644 index 0000000..8cc23b2 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/types/Matrix4.as @@ -0,0 +1,402 @@ +package alternativa.physics.types { + import __AS3__.vec.Vector; + + /** + * + */ + public class Matrix4 { + + public var a:Number; + public var b:Number; + public var c:Number; + public var d:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + public var h:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + public var l:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix4(a:Number = 1, b:Number = 0, c:Number = 0, d:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, h:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1, l:Number = 0) { + this.a = a; + this.b = b; + this.c = c; + this.d = h; + + this.e = e; + this.f = f; + this.g = g; + this.h = h; + + this.i = i; + this.j = j; + this.k = k; + this.l = l; + } + + /** + * + * @return + */ + public function toIdentity():Matrix4 { + a = f = k = 1; + b = c = e = g = i = j = d = h = l = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + var det:Number = -cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk; + + a = (-gg*jj + ff*kk)/det; + b = (cc*jj - bb*kk)/det; + c = (-cc*ff + bb*gg)/det; + d = (dd*gg*jj - cc*hh*jj - dd*ff*kk + bb*hh*kk + cc*ff*ll - bb*gg*ll)/det; + e = (gg*ii - ee*kk)/det; + f = (-cc*ii + aa*kk)/det; + g = (cc*ee - aa*gg)/det; + h = (cc*hh*ii - dd*gg*ii + dd*ee*kk - aa*hh*kk - cc*ee*ll + aa*gg*ll)/det; + i = (-ff*ii + ee*jj)/det; + j = (bb*ii - aa*jj)/det; + k = (-bb*ee + aa*ff)/det; + l = (dd*ff*ii - bb*hh*ii - dd*ee*jj + aa*hh*jj + bb*ee*ll - aa*ff*ll)/det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + d = m.a*dd + m.b*hh + m.c*ll + m.d; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + h = m.e*dd + m.f*hh + m.g*ll + m.h; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + l = m.i*dd + m.j*hh + m.k*ll + m.l; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + d = aa*m.d + bb*m.h + cc*m.l + dd; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + h = ee*m.d + ff*m.h + gg*m.l + hh; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + l = ii*m.d + jj*m.h + kk*m.l + ll; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix4):Matrix4 { + a += m.a; + b += m.b; + c += m.c; + d += m.d; + e += m.e; + f += m.f; + g += m.g; + h += m.h; + i += m.i; + j += m.j; + k += m.k; + l += m.l; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix4):Matrix4 { + a -= m.a; + b -= m.b; + c -= m.c; + d -= m.d; + e -= m.e; + f -= m.f; + g -= m.g; + h -= m.h; + i -= m.i; + j -= m.j; + k -= m.k; + l -= m.l; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectors(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverse(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(idx:int, axis:Vector3):void { + switch (idx) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + case 3: + axis.x = d; + axis.y = h; + axis.z = l; + return; + } + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix4):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = m.d; + e = m.e; + f = m.f; + g = m.g; + h = m.h; + i = m.i; + j = m.j; + k = m.k; + l = m.l; + + return this; + } + + public function setFromMatrix3(m:Matrix3, offset:Vector3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = offset.x; + e = m.e; + f = m.f; + g = m.g; + h = offset.y; + i = m.i; + j = m.j; + k = m.k; + l = offset.z; + + return this; + } + + /** + * @param rotations + */ + public function getEulerAngles(rotations:Vector3):void { + if (-1 < i && i < 1) { + rotations.x = Math.atan2(j, k); + rotations.y = -Math.asin(i); + rotations.z = Math.atan2(e, a); + } else { + rotations.x = 0; + rotations.y = 0.5*((i <= -1) ? Math.PI : -Math.PI); + rotations.z = Math.atan2(-b, f); + } + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix4 { + return new Matrix4(a, b, c, d, e, f, g, h, i, j, k, l); + } + + public function toString():String { + return "[Matrix4 [" + a.toFixed(3) + " " + b.toFixed(3) + " " + c.toFixed(3) + " " + d.toFixed(3) + "] [" + e.toFixed(3) + " " + f.toFixed(3) + " " + g.toFixed(3) + " " + h.toFixed(3) + "] [" + i.toFixed(3) + " " + j.toFixed(3) + " " + k.toFixed(3) + " " + l.toFixed(3) + "]]"; + } + + } +} \ No newline at end of file diff --git a/0.0.4.0/src/alternativa/physics/types/Quaternion.as b/0.0.4.0/src/alternativa/physics/types/Quaternion.as new file mode 100644 index 0000000..7ad2af0 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/types/Quaternion.as @@ -0,0 +1,396 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + public class Quaternion { + + public var w:Number; + public var x:Number; + public var y:Number; + public var z:Number; + + public static function multiply(q1:Quaternion, q2:Quaternion, result:Quaternion):void { + result.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z; + result.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y; + result.y = q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z; + result.z = q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x; + } + + public static function createFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngle(axis, angle); + return q; + } + + public static function createFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngleComponents(x, y, z, angle); + return q; + } + + public function Quaternion(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0) { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + } + + public function reset(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0):Quaternion { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + return this; + } + + public function normalize():Quaternion { + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * Умножает на указанный кватернион слева: this * q + * + * @param q множитель + */ + public function prepend(q:Quaternion):Quaternion { + var ww:Number = w*q.w - x*q.x - y*q.y - z*q.z; + var xx:Number = w*q.x + x*q.w + y*q.z - z*q.y; + var yy:Number = w*q.y + y*q.w + z*q.x - x*q.z; + var zz:Number = w*q.z + z*q.w + x*q.y - y*q.x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Умножает на указанный кватернион справа: q * this + * + * @param q множитель + */ + public function append(q:Quaternion):Quaternion { + var ww:Number = q.w*w - q.x*x - q.y*y - q.z*z; + var xx:Number = q.w*x + q.x*w + q.y*z - q.z*y; + var yy:Number = q.w*y + q.y*w + q.z*x - q.x*z; + var zz:Number = q.w*z + q.z*w + q.x*y - q.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * + * @param vector + */ + public function rotateByVector(v:Vector3):Quaternion { + var ww:Number = -v.x*x - v.y*y - v.z*z; + var xx:Number = v.x*w + v.y*z - v.z*y; + var yy:Number = v.y*w + v.z*x - v.x*z; + var zz:Number = v.z*w + v.x*y - v.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Добавляет вращение, приданное вектором угловой скорости за указанное время. + * + * @param v + * @param scale + */ + public function addScaledVector(v:Vector3, scale:Number):Quaternion { + var vx:Number = v.x*scale; + var vy:Number = v.y*scale; + var vz:Number = v.z*scale; + var ww:Number = -x*vx - y*vy - z*vz; + var xx:Number = vx*w + vy*z - vz*y; + var yy:Number = vy*w + vz*x - vx*z; + var zz:Number = vz*w + vx*y - vy*x; + w += 0.5*ww; + x += 0.5*xx; + y += 0.5*yy; + z += 0.5*zz; + // inlined normalize + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param m + * @return + */ + public function toMatrix3(m:Matrix3):Quaternion { + var xx2:Number = 2*x*x; + var yy2:Number = 2*y*y; + var zz2:Number = 2*z*z; + var xy2:Number = 2*x*y; + var yz2:Number = 2*y*z; + var zx2:Number = 2*z*x; + var wx2:Number = 2*w*x; + var wy2:Number = 2*w*y; + var wz2:Number = 2*w*z; + + m.a = 1 - yy2 - zz2; + m.b = xy2 - wz2; + m.c = zx2 + wy2; + + m.e = xy2 + wz2; + m.f = 1 - xx2 - zz2; + m.g = yz2 - wx2; + + m.i = zx2 - wy2; + m.j = yz2 + wx2; + m.k = 1 - xx2 - yy2; + return this; + } + + /** + * + * @return + */ + public function length():Number { + return Math.sqrt(w*w + x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function lengthSqr():Number { + return w*w + x*x + y*y + z*z; + } + + /** + * + * @param axis + * @param angle + * @return + */ + public function setFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z); + x = axis.x*k; + y = axis.y*k; + z = axis.z*k; + return this; + } + + /** + * + * @param x + * @param y + * @param z + * @param angle + */ + public function setFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(x*x + y*y + z*z); + this.x = x*k; + this.y = y*k; + this.z = z*k; + return this; + } + + /** + * + * @param vector + */ + public function toAxisVector(v:Vector3 = null):Vector3 { + if (w < -1 || w > 1) { + normalize(); + } + if (v == null) { + v = new Vector3(); + } + if (w > -1 && w < 1) { + if (w == 0) { + v.x = x; + v.y = y; + v.z = z; + } else { + var angle:Number = 2*Math.acos(w); + var coeff:Number = 1/Math.sqrt(1 - w*w); + v.x = x*coeff*angle; + v.y = y*coeff*angle; + v.z = z*coeff*angle; + } + } else { + v.x = 0; + v.y = 0; + v.z = 0; + } + return v; + } + + /** + * + * @param rotations + */ + public function getEulerAngles(angles:Vector3):Vector3 { + var qi2:Number = 2*x*x; + var qj2:Number = 2*y*y; + var qk2:Number = 2*z*z; + var qij:Number = 2*x*y; + var qjk:Number = 2*y*z; + var qki:Number = 2*z*x; + var qri:Number = 2*w*x; + var qrj:Number = 2*w*y; + var qrk:Number = 2*w*z; + + var aa:Number = 1 - qj2 - qk2; + var bb:Number = qij - qrk; + var ee:Number = qij + qrk; + var ff:Number = 1 - qi2 - qk2; + var ii:Number = qki - qrj; + var jj:Number = qjk + qri; + var kk:Number = 1 - qi2 - qj2; + + if (-1 < ii && ii < 1) { + if (angles == null) { + angles = new Vector3(Math.atan2(jj, kk), -Math.asin(ii), Math.atan2(ee, aa)); + } else { + angles.x = Math.atan2(jj, kk); + angles.y = -Math.asin(ii); + angles.z = Math.atan2(ee, aa); + } + } else { + if (angles == null) { + angles = new Vector3(0, 0.5*((ii <= -1) ? Math.PI : -Math.PI), Math.atan2(-bb, ff)); + } else { + angles.x = 0; + angles.y = 0.5*((ii <= -1) ? Math.PI : -Math.PI); + angles.z = Math.atan2(-bb, ff); + } + } + return angles; + } + + /** + * + */ + public function conjugate():void { + x = -x; + y = -y; + z = -z; + } + + /** + * Выполняет линейную интерполяцию. + * + * @param q1 начало отрезка + * @param q2 конец отрезка + * @param t время, обычно задаётся в интервале [0, 1] + */ + public function nlerp(q1:Quaternion, q2:Quaternion, t:Number):Quaternion { + var d:Number = 1 - t; + w = q1.w*d + q2.w*t; + x = q1.x*d + q2.x*t; + y = q1.y*d + q2.y*t; + z = q1.z*d + q2.z*t; + // inlined normalize + d = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param q + * @return + */ + public function subtract(q:Quaternion):Quaternion { + w -= q.w; + x -= q.x; + y -= q.y; + z -= q.z; + return this; + } + + /** + * + * @param q1 + * @param q2 + * @return + */ + public function diff(q1:Quaternion, q2:Quaternion):Quaternion { + w = q2.w - q1.w; + x = q2.x - q1.x; + y = q2.y - q1.y; + z = q2.z - q1.z; + return this; + } + + /** + * + * @param q + * @return + */ + public function copy(q:Quaternion):Quaternion { + w = q.w; + x = q.x; + y = q.y; + z = q.z; + return this; + } + + /** + * @param reslult + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + result.w = w; + return result; + } + + /** + * + * @return + */ + public function clone():Quaternion { + return new Quaternion(w, x, y, z); + } + + /** + * + * @return + */ + public function toString():String { + return "[" + w + ", " + x + ", " + y + ", " + z + "]"; + } + } +} diff --git a/0.0.4.0/src/alternativa/physics/types/Vector3.as b/0.0.4.0/src/alternativa/physics/types/Vector3.as new file mode 100644 index 0000000..13ffda0 --- /dev/null +++ b/0.0.4.0/src/alternativa/physics/types/Vector3.as @@ -0,0 +1,331 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + /** + * + */ + public class Vector3 extends Vector3D { + + public static const ZERO:Vector3 = new Vector3(0, 0, 0); + public static const X_AXIS:Vector3 = new Vector3(1, 0, 0); + public static const Y_AXIS:Vector3 = new Vector3(0, 1, 0); + public static const Z_AXIS:Vector3 = new Vector3(0, 0, 1); + + /** + * + * @param x + * @param y + * @param z + */ + public function Vector3(x:Number = 0, y:Number = 0, z:Number = 0) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * + * @return + */ + public function vLength():Number { + return Math.sqrt(x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function vLengthSqr():Number { + return x*x + y*y + z*z; + } + + /** + * + * @param length + * @return + */ + public function vSetLength(length:Number):Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = length; + } else { + var k:Number = length/Math.sqrt(x*x + y*y + z*z); + x *= k; + y *= k; + z *= k; + } + return this; + } + + /** + * + * @return + */ + public function vNormalize():Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = 1; + } else { + d = Math.sqrt(d); + x /= d; + y /= d; + z /= d; + } + return this; + } + + /** + * Сложение координат. + * + * @param v точка, координаты которой прибавляются к собственным + */ + public function vAdd(v:Vector3):Vector3 { + x += v.x; + y += v.y; + z += v.z; + return this; + } + + /** + * + * @param k + * @param v + * @return + */ + public function vAddScaled(k:Number, v:Vector3):Vector3 { + x += k*v.x; + y += k*v.y; + z += k*v.z; + return this; + } + + /** + * Вычитание координат. + * + * @param v точка, координаты которой вычитаются из собственных + */ + public function vSubtract(v:Vector3):Vector3 { + x -= v.x; + y -= v.y; + z -= v.z; + return this; + } + + /** + * Вычисляет сумму векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + * @return this + */ + public function vSum(a:Vector3, b:Vector3):Vector3 { + x = a.x + b.x; + y = a.y + b.y; + z = a.z + b.z; + return this; + } + + /** + * Вычисление разности векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + */ + public function vDiff(a:Vector3, b:Vector3):Vector3 { + x = a.x - b.x; + y = a.y - b.y; + z = a.z - b.z; + return this; + } + + /** + * Умножение на скаляр. + * + * @param k число, на которое умножаются координаты + */ + public function vScale(k:Number):Vector3 { + x *= k; + y *= k; + z *= k; + return this; + } + + /** + * Инвертирование вектора. + */ + public function vReverse():Vector3 { + x = -x; + y = -y; + z = -z; + return this; + } + + /** + * + * @param v + * @return + */ + public function vDot(v:Vector3):Number { + return x*v.x + y*v.y + z*v.z; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross(v:Vector3):Vector3 { + var xx:Number = y*v.z - z*v.y; + var yy:Number = z*v.x - x*v.z; + var zz:Number = x*v.y - y*v.x; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross2(a:Vector3, b:Vector3):Vector3 { + x = a.y*b.z - a.z*b.y; + y = a.z*b.x - a.x*b.z; + z = a.x*b.y - a.y*b.x; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy3(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * + * @param matrix + */ + public function vTransformBy3Tr(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz + m.d; + y = m.e*xx + m.f*yy + m.g*zz + m.h; + z = m.i*xx + m.j*yy + m.k*zz + m.l; + return this; + } + + /** + * + * @param m + * @return + */ + public function vTransformInverseBy4(m:Matrix4):Vector3 { + var xx:Number = x - m.d; + var yy:Number = y - m.h; + var zz:Number = z - m.l; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * + * @param m + */ + public function vDeltaTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * Установка координат. + */ + public function vReset(x:Number = 0, y:Number = 0, z:Number = 0):Vector3 { + this.x = x; + this.y = y; + this.z = z; + return this; + } + + /** + * Копирование координат точки. + * + * @param v точка, координаты которой копируются + */ + public function vCopy(v:Vector3):Vector3 { + x = v.x; + y = v.y; + z = v.z; + return this; + } + + /** + * Клонирование точки. + * + * @return клонированная точка + */ + public function vClone():Vector3 { + return new Vector3(x, y, z); + } + + /** + * Копирует координаты вектора в экземпляр класса Vector3D. + * + * @param result экземпляр, в который копируются координаты + * @return переданный параметр + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + return result; + } + + /** + * @param source + * @return + */ + public function copyFromVector3D(source:Vector3D):Vector3 { + x = source.x; + y = source.y; + z = source.z; + return this; + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/.actionScriptProperties b/0.0.5.0/.actionScriptProperties new file mode 100644 index 0000000..e5b1b89 --- /dev/null +++ b/0.0.5.0/.actionScriptProperties @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/0.0.5.0/.flexLibProperties b/0.0.5.0/.flexLibProperties new file mode 100644 index 0000000..f39915e --- /dev/null +++ b/0.0.5.0/.flexLibProperties @@ -0,0 +1,51 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/0.0.5.0/.project b/0.0.5.0/.project new file mode 100644 index 0000000..1f1425f --- /dev/null +++ b/0.0.5.0/.project @@ -0,0 +1,19 @@ + + + AlternativaPhysics + + + + + + com.adobe.flexbuilder.project.flexbuilder + + + + + + org.maven.ide.eclipse.maven2Nature + com.adobe.flexbuilder.project.flexlibnature + com.adobe.flexbuilder.project.actionscriptnature + + diff --git a/0.0.5.0/.settings/.svn/all-wcprops b/0.0.5.0/.settings/.svn/all-wcprops new file mode 100644 index 0000000..4986306 --- /dev/null +++ b/0.0.5.0/.settings/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 89 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/.settings +END +org.eclipse.core.resources.prefs +K 25 +svn:wc:ra_dav:version-url +V 122 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/.settings/org.eclipse.core.resources.prefs +END diff --git a/0.0.5.0/.settings/.svn/entries b/0.0.5.0/.settings/.svn/entries new file mode 100644 index 0000000..37ecbde --- /dev/null +++ b/0.0.5.0/.settings/.svn/entries @@ -0,0 +1,40 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/.settings +http://svndev.alternativaplatform.com + + + +2009-04-01T12:25:29.638016Z +10301 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +org.eclipse.core.resources.prefs +file + + + + +2010-10-28T04:32:41.000000Z +4f70b476f3e5075e399505021df2f89b +2009-04-01T12:25:29.638016Z +10301 +mike + diff --git a/0.0.5.0/.settings/.svn/format b/0.0.5.0/.settings/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.5.0/.settings/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.5.0/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base b/0.0.5.0/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base new file mode 100644 index 0000000..b3d0d49 --- /dev/null +++ b/0.0.5.0/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base @@ -0,0 +1,3 @@ +#Fri Mar 20 00:19:33 YEKT 2009 +eclipse.preferences.version=1 +encoding/=UTF-8 diff --git a/0.0.5.0/.settings/org.eclipse.core.resources.prefs b/0.0.5.0/.settings/org.eclipse.core.resources.prefs new file mode 100644 index 0000000..b3d0d49 --- /dev/null +++ b/0.0.5.0/.settings/org.eclipse.core.resources.prefs @@ -0,0 +1,3 @@ +#Fri Mar 20 00:19:33 YEKT 2009 +eclipse.preferences.version=1 +encoding/=UTF-8 diff --git a/0.0.5.0/.svn/all-wcprops b/0.0.5.0/.svn/all-wcprops new file mode 100644 index 0000000..c53ea8a --- /dev/null +++ b/0.0.5.0/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 79 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0 +END +.flexLibProperties +K 25 +svn:wc:ra_dav:version-url +V 98 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/.flexLibProperties +END +.project +K 25 +svn:wc:ra_dav:version-url +V 88 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/.project +END +pom.xml +K 25 +svn:wc:ra_dav:version-url +V 87 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/pom.xml +END +.actionScriptProperties +K 25 +svn:wc:ra_dav:version-url +V 103 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/.actionScriptProperties +END diff --git a/0.0.5.0/.svn/dir-prop-base b/0.0.5.0/.svn/dir-prop-base new file mode 100644 index 0000000..8eda4b3 --- /dev/null +++ b/0.0.5.0/.svn/dir-prop-base @@ -0,0 +1,5 @@ +K 13 +svn:mergeinfo +V 85 +/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D:13892-14098 +END diff --git a/0.0.5.0/.svn/entries b/0.0.5.0/.svn/entries new file mode 100644 index 0000000..8c1e189 --- /dev/null +++ b/0.0.5.0/.svn/entries @@ -0,0 +1,85 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0 +http://svndev.alternativaplatform.com + + + +2009-09-10T20:48:12.872816Z +19726 +mike +has-props + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +META-INF +dir + +.flexLibProperties +file + + + + +2010-10-28T04:32:41.000000Z +3ac589a87a5983147605d64c4c08e3c2 +2009-07-17T13:10:32.947052Z +16516 +mike + +.project +file + + + + +2010-10-28T04:32:41.000000Z +1e90cc68b52b93173b2b5de88eb5c3a3 +2009-04-20T17:15:55.260110Z +11541 +mike + +src +dir + +pom.xml +file + + + + +2010-10-28T04:32:41.000000Z +ea7cfd8913a0d83216b1c06b9f8379a8 +2009-09-10T20:48:12.872816Z +19726 +mike + +.actionScriptProperties +file + + + + +2010-10-28T04:32:41.000000Z +918335cb945c3f8866aeb8e6cfd1b110 +2009-07-20T09:23:34.939854Z +16623 +mike + +.settings +dir + diff --git a/0.0.5.0/.svn/format b/0.0.5.0/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.5.0/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.5.0/.svn/text-base/.actionScriptProperties.svn-base b/0.0.5.0/.svn/text-base/.actionScriptProperties.svn-base new file mode 100644 index 0000000..e5b1b89 --- /dev/null +++ b/0.0.5.0/.svn/text-base/.actionScriptProperties.svn-base @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/0.0.5.0/.svn/text-base/.flexLibProperties.svn-base b/0.0.5.0/.svn/text-base/.flexLibProperties.svn-base new file mode 100644 index 0000000..f39915e --- /dev/null +++ b/0.0.5.0/.svn/text-base/.flexLibProperties.svn-base @@ -0,0 +1,51 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/0.0.5.0/.svn/text-base/.project.svn-base b/0.0.5.0/.svn/text-base/.project.svn-base new file mode 100644 index 0000000..1f1425f --- /dev/null +++ b/0.0.5.0/.svn/text-base/.project.svn-base @@ -0,0 +1,19 @@ + + + AlternativaPhysics + + + + + + com.adobe.flexbuilder.project.flexbuilder + + + + + + org.maven.ide.eclipse.maven2Nature + com.adobe.flexbuilder.project.flexlibnature + com.adobe.flexbuilder.project.actionscriptnature + + diff --git a/0.0.5.0/.svn/text-base/pom.xml.svn-base b/0.0.5.0/.svn/text-base/pom.xml.svn-base new file mode 100644 index 0000000..24a5d12 --- /dev/null +++ b/0.0.5.0/.svn/text-base/pom.xml.svn-base @@ -0,0 +1,17 @@ + + 4.0.0 + platform.clients.fp10.libraries + AlternativaPhysics + swc + 0.0.5.0 + + platform.tools.maven + FlashBasePom + 1.0 + + + scm:svn:http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0 + + + + \ No newline at end of file diff --git a/0.0.5.0/META-INF/.svn/all-wcprops b/0.0.5.0/META-INF/.svn/all-wcprops new file mode 100644 index 0000000..261ff5b --- /dev/null +++ b/0.0.5.0/META-INF/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 88 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/META-INF +END +MANIFEST.MF +K 25 +svn:wc:ra_dav:version-url +V 100 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/META-INF/MANIFEST.MF +END diff --git a/0.0.5.0/META-INF/.svn/entries b/0.0.5.0/META-INF/.svn/entries new file mode 100644 index 0000000..0bb0185 --- /dev/null +++ b/0.0.5.0/META-INF/.svn/entries @@ -0,0 +1,40 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/META-INF +http://svndev.alternativaplatform.com + + + +2009-04-20T17:15:55.260110Z +11541 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +MANIFEST.MF +file + + + + +2010-10-28T04:32:41.000000Z +7b64dceb50a6905850ff00a0bfbe77eb +2009-04-20T17:15:55.260110Z +11541 +mike + diff --git a/0.0.5.0/META-INF/.svn/format b/0.0.5.0/META-INF/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.5.0/META-INF/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.5.0/META-INF/.svn/text-base/MANIFEST.MF.svn-base b/0.0.5.0/META-INF/.svn/text-base/MANIFEST.MF.svn-base new file mode 100644 index 0000000..5066a55 --- /dev/null +++ b/0.0.5.0/META-INF/.svn/text-base/MANIFEST.MF.svn-base @@ -0,0 +1 @@ +Bundle-Name: platform.clients.fp10.libraries.AlternativaPhysics diff --git a/0.0.5.0/META-INF/MANIFEST.MF b/0.0.5.0/META-INF/MANIFEST.MF new file mode 100644 index 0000000..5066a55 --- /dev/null +++ b/0.0.5.0/META-INF/MANIFEST.MF @@ -0,0 +1 @@ +Bundle-Name: platform.clients.fp10.libraries.AlternativaPhysics diff --git a/0.0.5.0/pom.xml b/0.0.5.0/pom.xml new file mode 100644 index 0000000..24a5d12 --- /dev/null +++ b/0.0.5.0/pom.xml @@ -0,0 +1,17 @@ + + 4.0.0 + platform.clients.fp10.libraries + AlternativaPhysics + swc + 0.0.5.0 + + platform.tools.maven + FlashBasePom + 1.0 + + + scm:svn:http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0 + + + + \ No newline at end of file diff --git a/0.0.5.0/src/.svn/all-wcprops b/0.0.5.0/src/.svn/all-wcprops new file mode 100644 index 0000000..97df39b --- /dev/null +++ b/0.0.5.0/src/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 83 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src +END diff --git a/0.0.5.0/src/.svn/entries b/0.0.5.0/src/.svn/entries new file mode 100644 index 0000000..1af780a --- /dev/null +++ b/0.0.5.0/src/.svn/entries @@ -0,0 +1,31 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src +http://svndev.alternativaplatform.com + + + +2009-09-10T19:17:30.639579Z +19708 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +alternativa +dir + diff --git a/0.0.5.0/src/.svn/format b/0.0.5.0/src/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.5.0/src/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.5.0/src/alternativa/.svn/all-wcprops b/0.0.5.0/src/alternativa/.svn/all-wcprops new file mode 100644 index 0000000..fac966c --- /dev/null +++ b/0.0.5.0/src/alternativa/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 95 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa +END diff --git a/0.0.5.0/src/alternativa/.svn/entries b/0.0.5.0/src/alternativa/.svn/entries new file mode 100644 index 0000000..04e2a12 --- /dev/null +++ b/0.0.5.0/src/alternativa/.svn/entries @@ -0,0 +1,31 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa +http://svndev.alternativaplatform.com + + + +2009-09-10T19:17:30.639579Z +19708 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +physics +dir + diff --git a/0.0.5.0/src/alternativa/.svn/format b/0.0.5.0/src/alternativa/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.5.0/src/alternativa/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.5.0/src/alternativa/physics/.svn/all-wcprops b/0.0.5.0/src/alternativa/physics/.svn/all-wcprops new file mode 100644 index 0000000..ae3c596 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 103 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics +END +altphysics.as +K 25 +svn:wc:ra_dav:version-url +V 117 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/altphysics.as +END diff --git a/0.0.5.0/src/alternativa/physics/.svn/entries b/0.0.5.0/src/alternativa/physics/.svn/entries new file mode 100644 index 0000000..b39d2b2 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/.svn/entries @@ -0,0 +1,49 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics +http://svndev.alternativaplatform.com + + + +2009-09-10T19:17:30.639579Z +19708 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +types +dir + +altphysics.as +file + + + + +2010-10-28T04:32:41.000000Z +04fbe40a27502dbbd0dba02a76b2df50 +2009-04-20T20:09:14.715403Z +11579 +mike + +collision +dir + +rigid +dir + diff --git a/0.0.5.0/src/alternativa/physics/.svn/format b/0.0.5.0/src/alternativa/physics/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.5.0/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base b/0.0.5.0/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base new file mode 100644 index 0000000..2715307 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base @@ -0,0 +1,3 @@ +package alternativa.physics { + public namespace altphysics = "http://alternativaplatform.com/en/altphysics"; +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/altphysics.as b/0.0.5.0/src/alternativa/physics/altphysics.as new file mode 100644 index 0000000..2715307 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/altphysics.as @@ -0,0 +1,3 @@ +package alternativa.physics { + public namespace altphysics = "http://alternativaplatform.com/en/altphysics"; +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/.svn/all-wcprops b/0.0.5.0/src/alternativa/physics/collision/.svn/all-wcprops new file mode 100644 index 0000000..4dfe429 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/.svn/all-wcprops @@ -0,0 +1,107 @@ +K 25 +svn:wc:ra_dav:version-url +V 113 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/collision +END +BoxSphereCollider.as +K 25 +svn:wc:ra_dav:version-url +V 134 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/collision/BoxSphereCollider.as +END +BoxBoxCollider.as +K 25 +svn:wc:ra_dav:version-url +V 131 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/collision/BoxBoxCollider.as +END +CollisionKdTree2D.as +K 25 +svn:wc:ra_dav:version-url +V 134 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/collision/CollisionKdTree2D.as +END +CollisionKdNode.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/collision/CollisionKdNode.as +END +SpherePlaneCollider.as +K 25 +svn:wc:ra_dav:version-url +V 136 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/collision/SpherePlaneCollider.as +END +KdTreeCollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/collision/KdTreeCollisionDetector.as +END +BoxRectCollider.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/collision/BoxRectCollider.as +END +ICollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 136 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/collision/ICollisionPredicate.as +END +BoxPlaneCollider.as +K 25 +svn:wc:ra_dav:version-url +V 133 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/collision/BoxPlaneCollider.as +END +CollisionKdTree.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/collision/CollisionKdTree.as +END +BoxTriangleCollider.as +K 25 +svn:wc:ra_dav:version-url +V 136 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/collision/BoxTriangleCollider.as +END +ICollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 135 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/collision/ICollisionDetector.as +END +IRayCollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 139 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/collision/IRayCollisionPredicate.as +END +IBodyCollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/collision/IBodyCollisionPredicate.as +END +BruteForceCollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 144 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/collision/BruteForceCollisionDetector.as +END +ICollider.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/collision/ICollider.as +END +SphereSphereCollider.as +K 25 +svn:wc:ra_dav:version-url +V 137 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/collision/SphereSphereCollider.as +END diff --git a/0.0.5.0/src/alternativa/physics/collision/.svn/entries b/0.0.5.0/src/alternativa/physics/collision/.svn/entries new file mode 100644 index 0000000..9139b89 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/.svn/entries @@ -0,0 +1,238 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/collision +http://svndev.alternativaplatform.com + + + +2009-09-10T19:17:30.639579Z +19708 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +BoxSphereCollider.as +file + + + + +2010-10-28T04:32:41.000000Z +8bfabb1784a5eb6f6b5a76cbd30d9e88 +2009-09-09T15:57:45.246374Z +19648 +mike + +BoxBoxCollider.as +file + + + + +2010-10-28T04:32:41.000000Z +0ac713b55f30a1e8c0112b1d3dcfd315 +2009-09-09T15:57:45.246374Z +19648 +mike + +CollisionKdNode.as +file + + + + +2010-10-28T04:32:41.000000Z +1d52aba28ce259ca1f9916d70619263d +2009-07-20T03:48:26.517738Z +16522 +mike + +CollisionKdTree2D.as +file + + + + +2010-10-28T04:32:41.000000Z +d8211949ef77817543bc2882480da12f +2009-07-20T03:48:26.517738Z +16522 +mike + +BoxRectCollider.as +file + + + + +2010-10-28T04:32:41.000000Z +bf1f4e6fada4948aec0c7346c45537be +2009-09-10T19:17:30.639579Z +19708 +mike + +KdTreeCollisionDetector.as +file + + + + +2010-10-28T04:32:41.000000Z +ed2340f19df5496ea226eed8a196014e +2009-09-10T19:17:30.639579Z +19708 +mike + +SpherePlaneCollider.as +file + + + + +2010-10-28T04:32:41.000000Z +9416a2154f699ed3330ebf270ac760f9 +2009-09-09T15:57:45.246374Z +19648 +mike + +ICollisionPredicate.as +file + + + + +2010-10-28T04:32:41.000000Z +866471ea582e95e46e94eb7ae437c827 +2009-04-26T15:39:06.673206Z +11995 +mike + +BoxPlaneCollider.as +file + + + + +2010-10-28T04:32:41.000000Z +827b78750c00050de610aa5b17ef33bd +2009-09-09T15:57:45.246374Z +19648 +mike + +primitives +dir + +CollisionKdTree.as +file + + + + +2010-10-28T04:32:41.000000Z +45bb93cc65be7d8a55015c4ac75b1242 +2009-07-20T03:48:26.517738Z +16522 +mike + +BoxTriangleCollider.as +file + + + + +2010-10-28T04:32:41.000000Z +3d1c9d8812beb82734b33a2222b4c075 +2009-09-10T19:17:30.639579Z +19708 +mike + +ICollisionDetector.as +file + + + + +2010-10-28T04:32:41.000000Z +c415c22df9a1b1064c0501d5d525a5cb +2009-07-13T10:34:12.822563Z +16296 +mike + +IRayCollisionPredicate.as +file + + + + +2010-10-28T04:32:41.000000Z +f7443fb728cf4e6157dcfb4f39665889 +2009-04-26T15:39:06.673206Z +11995 +mike + +types +dir + +IBodyCollisionPredicate.as +file + + + + +2010-10-28T04:32:41.000000Z +fce7e70f95b8ef96709157dcdc8433dd +2009-06-29T14:07:24.833358Z +15390 +mike + +ICollider.as +file + + + + +2010-10-28T04:32:41.000000Z +4eedf93d1111e060a54aadf29b86c63f +2009-09-09T15:57:45.246374Z +19648 +mike + +BruteForceCollisionDetector.as +file + + + + +2010-10-28T04:32:41.000000Z +a313af2ee5d9c84ec2551058b1b4c763 +2009-07-13T10:34:12.822563Z +16296 +mike + +SphereSphereCollider.as +file + + + + +2010-10-28T04:32:41.000000Z +2e720e302145d54e8391df879f68d6d8 +2009-09-09T15:57:45.246374Z +19648 +mike + diff --git a/0.0.5.0/src/alternativa/physics/collision/.svn/format b/0.0.5.0/src/alternativa/physics/collision/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/BoxBoxCollider.as.svn-base b/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/BoxBoxCollider.as.svn-base new file mode 100644 index 0000000..0262b99 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/BoxBoxCollider.as.svn-base @@ -0,0 +1,848 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Расчитывает точки контакта двух боксов. Нормаль контакта направляется в сторону бокса с меньшим ID. + */ + public class BoxBoxCollider implements ICollider { + + private var tolerance:Number = 0.001; + + private var pos1:Vector3 = new Vector3(); + private var pos2:Vector3 = new Vector3(); + private var vectorToBox1:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var colAxis:Vector3 = new Vector3(); + private var tmpAxis:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var tmpPoints:Vector. = new Vector.(8, true); + private var pcount:int; + + /** + * + */ + public function BoxBoxCollider() { + for (var i:int = 0; i < 8; i++) { + tmpPoints[i] = new ContactPoint(); + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + /** + * + * @param body1 + * @param body2 + * @param contactInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + minOverlap = 1e10; + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + // Вектор из центра второго бокса в центр первого + pos1.x = transform1.d; pos1.y = transform1.h; pos1.z = transform1.l; + pos2.x = transform2.d; pos2.y = transform2.h; pos2.z = transform2.l; + vectorToBox1.x = pos1.x - pos2.x; + vectorToBox1.y = pos1.y - pos2.y; + vectorToBox1.z = pos1.z - pos2.z; + + // Проверка пересечения по основным осям +// box1.transform.getAxis(0, axis10); + axis10.x = transform1.a; axis10.y = transform1.e; axis10.z = transform1.i; + if (!testAxis(box1, box2, axis10, 0, vectorToBox1)) return false; +// box1.transform.getAxis(1, axis11); + axis11.x = transform1.b; axis11.y = transform1.f; axis11.z = transform1.j; + if (!testAxis(box1, box2, axis11, 1, vectorToBox1)) return false; +// box1.transform.getAxis(2, axis12); + axis12.x = transform1.c; axis12.y = transform1.g; axis12.z = transform1.k; + if (!testAxis(box1, box2, axis12, 2, vectorToBox1)) return false; + +// box2.transform.getAxis(0, axis20); + axis20.x = transform2.a; axis20.y = transform2.e; axis20.z = transform2.i; + if (!testAxis(box1, box2, axis20, 3, vectorToBox1)) return false; +// box2.transform.getAxis(1, axis21); + axis21.x = transform2.b; axis21.y = transform2.f; axis21.z = transform2.j; + if (!testAxis(box1, box2, axis21, 4, vectorToBox1)) return false; +// box2.transform.getAxis(2, axis22); + axis22.x = transform2.c; axis22.y = transform2.g; axis22.z = transform2.k; + if (!testAxis(box1, box2, axis22, 5, vectorToBox1)) return false; + + // Проверка производных осей + if (!testAxis(box1, box2, axis.vCross2(axis10, axis20), 6, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis10, axis21), 7, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis10, axis22), 8, vectorToBox1)) return false; + + if (!testAxis(box1, box2, axis.vCross2(axis11, axis20), 9, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis11, axis21), 10, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis11, axis22), 11, vectorToBox1)) return false; + + if (!testAxis(box1, box2, axis.vCross2(axis12, axis20), 12, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis12, axis21), 13, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis12, axis22), 14, vectorToBox1)) return false; + + if (bestAxisIndex < 6) { + // Контакт грань-(грань|ребро|вершина) + findFaceContactPoints(box1, box2, vectorToBox1, bestAxisIndex, contact); + } else { + // Контакт ребро-ребро + bestAxisIndex -= 6; + findEdgesIntersection(box1, box2, vectorToBox1, int(bestAxisIndex/3), bestAxisIndex%3, contact); + } + contact.body1 = box1.body; + contact.body2 = box2.body; + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + // Вектор из центра второго бокса в центр первого + pos1.x = transform1.d; pos1.y = transform1.h; pos1.z = transform1.l; + pos2.x = transform2.d; pos2.y = transform2.h; pos2.z = transform2.l; + vectorToBox1.x = pos1.x - pos2.x; + vectorToBox1.y = pos1.y - pos2.y; + vectorToBox1.z = pos1.z - pos2.z; + + // Проверка пересечения по основным осям + axis10.x = transform1.a; axis10.y = transform1.e; axis10.z = transform1.i; + if (!testAxis(box1, box2, axis10, 0, vectorToBox1)) return false; + axis11.x = transform1.b; axis11.y = transform1.f; axis11.z = transform1.j; + if (!testAxis(box1, box2, axis11, 1, vectorToBox1)) return false; + axis12.x = transform1.c; axis12.y = transform1.g; axis12.z = transform1.k; + if (!testAxis(box1, box2, axis12, 2, vectorToBox1)) return false; + + axis20.x = transform2.a; axis20.y = transform2.e; axis20.z = transform2.i; + if (!testAxis(box1, box2, axis20, 3, vectorToBox1)) return false; + axis21.x = transform2.b; axis21.y = transform2.f; axis21.z = transform2.j; + if (!testAxis(box1, box2, axis21, 4, vectorToBox1)) return false; + axis22.x = transform2.c; axis22.y = transform2.g; axis22.z = transform2.k; + if (!testAxis(box1, box2, axis22, 5, vectorToBox1)) return false; + + // Проверка производных осей + if (!testAxis(box1, box2, axis.vCross2(axis10, axis20), 6, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis10, axis21), 7, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis10, axis22), 8, vectorToBox1)) return false; + + if (!testAxis(box1, box2, axis.vCross2(axis11, axis20), 9, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis11, axis21), 10, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis11, axis22), 11, vectorToBox1)) return false; + + if (!testAxis(box1, box2, axis.vCross2(axis12, axis20), 12, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis12, axis21), 13, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis12, axis22), 14, vectorToBox1)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта грани одного бокса с гранью/ребром/вершиной другого. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param faceAxisIdx индекс оси первого бокса, перпендикулярной грани, с которой произошло столкновение + * @param contactInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, faceAxisIdx:int, contactInfo:Contact):void { + var swapNormal:Boolean = false; + if (faceAxisIdx > 2) { + // Столкновение с гранью второго бокса. Для дальнейших расчётов боксы меняются местами, + // но нормаль контакта всё равно должна быть направлена в сторону первоначального box1 + var tmpBox:CollisionBox = box1; + box1 = box2; + box2 = tmpBox; + vectorToBox1.vReverse(); + faceAxisIdx -= 3; + swapNormal = true; + } + box1.transform.getAxis(faceAxisIdx, colAxis); + var faceReversed:Boolean = colAxis.vDot(vectorToBox1) > 0; + if (!faceReversed) colAxis.vReverse(); + // Ищем ось второго бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + box2.transform.getAxis(axisIdx, axis); + var dot:Number = axis.vDot(colAxis); + if (dot < 0) dot = -dot; + if (dot > maxDot) { + maxDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Получаем список вершин грани второго бокса, переводим их в систему координат первого бокса и выполняем обрезку + // по грани первого бокса. Таким образом получается список потенциальных точек контакта. + box2.transform.getAxis(incFaceAxisIdx, axis); + getFaceVertsByAxis(box2, incFaceAxisIdx, axis.vDot(colAxis) < 0, verts1); + box2.transform.transformVectors(verts1, verts2); + box1.transform.transformVectorsInverse(verts2, verts1); + var pnum:int = clip(box1.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + pcount = 0; + for (var i:int = 0; i < pnum; i++) { + if ((pen = getPointBoxPenetration(box1.hs, verts1[i], faceAxisIdx, faceReversed)) > -tolerance) { + var cp:ContactPoint = tmpPoints[pcount++]; + box1.transform.transformVector(verts1[i], cp.pos); + cp.r1.vDiff(cp.pos, pos1); + cp.r2.vDiff(cp.pos, pos2); + cp.penetration = pen; + } + } + contactInfo.normal.vCopy(colAxis); + if (swapNormal) contactInfo.normal.vReverse(); + + if (pcount > 4) reducePoints(); + for (i = 0; i < pcount; i++) (contactInfo.points[i] as ContactPoint).copyFrom(tmpPoints[i]); + contactInfo.pcount = pcount; + } + + /** + * + * @param contactInfo + */ + private function reducePoints():void { + var i:int; + var minIdx:int; + var cp1:ContactPoint; + var cp2:ContactPoint; + while (pcount > 4) { + var minLen:Number = 1e10; + var p1:Vector3 = (tmpPoints[pcount - 1] as ContactPoint).pos; + var p2:Vector3; + for (i = 0; i < pcount; i++) { + p2 = (tmpPoints[i] as ContactPoint).pos; + var dx:Number = p2.x - p1.x; + var dy:Number = p2.y - p1.y; + var dz:Number = p2.z - p1.z; + var len:Number = dx*dx + dy*dy + dz*dz; + if (len < minLen) { + minLen = len; + minIdx = i; + } + p1 = p2; + } + cp1 = tmpPoints[minIdx == 0 ? (pcount - 1) : (minIdx - 1)]; + cp2 = tmpPoints[minIdx]; + p1 = cp1.pos; + p2 = cp2.pos; + p2.x = 0.5*(p1.x + p2.x); + p2.y = 0.5*(p1.y + p2.y); + p2.z = 0.5*(p1.z + p2.z); + cp2.penetration = 0.5*(cp1.penetration + cp2.penetration); + if (minIdx > 0) { + for (i = minIdx; i < pcount; i++) tmpPoints[i - 1] = tmpPoints[i]; + tmpPoints[pcount - 1] = cp1; + } + pcount--; + } + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + private function getFaceVertsByAxis(box:CollisionBox, axisIdx:int, reverse:Boolean, result:Vector.):void { + var hs:Vector3 = box.hs; + switch (axisIdx) { + case 0: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, -hs.y, hs.z); + } + break; + case 1: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, hs.y, hs.z); + } + break; + case 2: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + } else { + (result[0] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } + break; + } + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clip(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + return clipHighX(hs.x, pnum, verts2, verts1); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + } + return 0; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param contactInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):void { + box1.transform.getAxis(axisIdx1, axis10); + box2.transform.getAxis(axisIdx2, axis20); + colAxis.vCross2(axis10, axis20).vNormalize(); + // Разворот оси в сторону первого бокса + if (colAxis.vDot(vectorToBox1) < 0) colAxis.vReverse(); + + /* Кодирование рёбер + бит 0: 1 (тип контакта ребро-ребро) + биты 1-2: индекс направляющей оси ребра + бит 3: + бит 4: + бит 5: + */ + var edgeCode1:int = 1; + var edgeCode2:int = 1; + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + point1.vCopy(box1.hs); + point2.vCopy(box2.hs); + // x + if (axisIdx1 == 0) { + point1.x = 0; + halfLen1 = box1.hs.x; + } else { + box1.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.x = -point1.x; + edgeCode1 |= 8; // 1 << 3 + } + } + if (axisIdx2 == 0) { + point2.x = 0; + halfLen2 = box2.hs.x; + } else { + box2.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.x = -point2.x; + edgeCode2 |= 8; // 1 << 3 + } + } + // y + if (axisIdx1 == 1) { + point1.y = 0; + halfLen1 = box1.hs.y; + edgeCode1 |= 2; // 1 << 1 + } else { + box1.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.y = -point1.y; + edgeCode1 |= 16; // 1 << 4 + } + } + if (axisIdx2 == 1) { + point2.y = 0; + halfLen2 = box2.hs.y; + edgeCode2 |= 2; // 1 << 1 + } else { + box2.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.y = -point2.y; + edgeCode2 |= 16; // 1 << 4 + } + } + // z + if (axisIdx1 == 2) { + point1.z = 0; + halfLen1 = box1.hs.z; + edgeCode1 |= 4; // 2 << 1 + } else { + box1.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.z = -point1.z; + edgeCode1 |= 32; // 1 << 5 + } + } + if (axisIdx2 == 2) { + point2.z = 0; + halfLen2 = box2.hs.z; + edgeCode2 |= 4; // 2 << 1 + } else { + box2.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.z = -point2.z; + edgeCode2 |= 32; // 1 << 5 + } + } + // Получаем глобальные координаты средних точек рёбер + point1.vTransformBy4(box1.transform); + point2.vTransformBy4(box2.transform); + // Находим точку пересечения рёбер, решая систему уравнений + var k:Number = axis10.vDot(axis20); + var det:Number = k*k - 1; + vector.vDiff(point2, point1); + var c1:Number = axis10.vDot(vector); + var c2:Number = axis20.vDot(vector); + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.normal.vCopy(colAxis); + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(point1.x + axis10.x*t1 + point2.x + axis20.x*t2); + cp.pos.y = 0.5*(point1.y + axis10.y*t1 + point2.y + axis20.y*t2); + cp.pos.z = 0.5*(point1.z + axis10.z*t1 + point2.z + axis20.z*t2); + cp.r1.vDiff(cp.pos, pos1); + cp.r2.vDiff(cp.pos, pos2); + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение боксов вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box1 + * @param box2 + * @param axis + * @param axisIndex + * @param vectorToBox1 + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, axisIndex:int, vectorToBox1:Vector3):Boolean { + if (axis.vLengthSqr() < 0.0001) { + return true; + } + axis.vNormalize(); + + var overlap:Number = overlapOnAxis(box1, box2, axis, vectorToBox1); + if (overlap < -tolerance) return false; + if (overlap + tolerance < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия двух боксов вдоль заданной оси. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param axis ось + * @param vectorToBox1 вектор, соединяющий центр второго бокса с центром первого + * @return величина перекрытия боксов вдоль оси + */ + public function overlapOnAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, vectorToBox1:Vector3):Number { + var m:Matrix4 = box1.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box1.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box1.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box1.hs.z; + if (d < 0) d = -d; + projection += d; + + m = box2.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box2.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box2.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box2.hs.z; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox1.x*axis.x + vectorToBox1.y*axis.y + vectorToBox1.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} + import alternativa.physics.types.Vector3; + + class CollisionPointTmp { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + } diff --git a/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/BoxPlaneCollider.as.svn-base b/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/BoxPlaneCollider.as.svn-base new file mode 100644 index 0000000..9817aac --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/BoxPlaneCollider.as.svn-base @@ -0,0 +1,87 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + public class BoxPlaneCollider implements ICollider { + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var normal:Vector3 = new Vector3(); + + public function BoxPlaneCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var box:RigidBox = body1 as RigidBox; +// var plane:RigidPlane; +// if (box == null) { +// box = body2 as RigidBox; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные координаты вершин бокса +// var sx:Number = box.halfSize.x; +// var sy:Number = box.halfSize.y; +// var sz:Number = box.halfSize.z; +// (verts1[0] as Vector3).reset(-sx, -sy, -sz); +// (verts1[1] as Vector3).reset(sx, -sy, -sz); +// (verts1[2] as Vector3).reset(sx, sy, -sz); +// (verts1[3] as Vector3).reset(-sx, sy, -sz); +// (verts1[4] as Vector3).reset(-sx, -sy, sz); +// (verts1[5] as Vector3).reset(sx, -sy, sz); +// (verts1[6] as Vector3).reset(sx, sy, sz); +// (verts1[7] as Vector3).reset(-sx, sy, sz); +// +// box.transform.transformVectors(verts1, verts2); +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// // Проверяем наличие столкновений с каждой вершиной +// collisionInfo.pcount = 0; +// for (var i:int = 0; i < 8; i++) { +// // Вершина добавляется в список точек столкновения, если лежит под плоскостью +// var dist:Number = (verts2[i] as Vector3).dot(normal); +// if (dist < offset) { +// var cp:ContactPoint; +// if (collisionInfo.pcount == collisionInfo.points.length) { +// cp = new ContactPoint(); +// collisionInfo.points[collisionInfo.pcount] = cp; +// } else { +// cp = collisionInfo.points[collisionInfo.pcount]; +// } +// cp.pos.copy(verts2[i]); +// cp.r1.diff(cp.pos, box.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); +// cp.penetration = offset - dist; +// collisionInfo.pcount++; +// } +// } +// if (collisionInfo.pcount > 0) { +// collisionInfo.body1 = box; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// return true; +// } + return false; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/BoxRectCollider.as.svn-base b/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/BoxRectCollider.as.svn-base new file mode 100644 index 0000000..dafaec7 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/BoxRectCollider.as.svn-base @@ -0,0 +1,750 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + /** + * + */ + public class BoxRectCollider implements ICollider { + + private var epsilon:Number = 0.001; + + private var bPos:Vector3 = new Vector3(); + private var rPos:Vector3 = new Vector3(); + + private var vectorToBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var colAxis:Vector3 = new Vector3(); + private var tmpAxis:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxRectCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + minOverlap = 1e10; + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + + // Вектор из центра прямоугольника в центр бокса + box.transform.getAxis(3, bPos); + rect.transform.getAxis(3, rPos); + vectorToBox.vDiff(bPos, rPos); + + // Проверка пересечения по нормали прямоугольника + rect.transform.getAxis(2, axis22); + if (!testAxis(box, rect, axis22, 0, vectorToBox)) return false; + + // Проверка пересечения по основным осям бокса + box.transform.getAxis(0, axis10); + if (!testAxis(box, rect, axis10, 1, vectorToBox)) return false; + box.transform.getAxis(1, axis11); + if (!testAxis(box, rect, axis11, 2, vectorToBox)) return false; + box.transform.getAxis(2, axis12); + if (!testAxis(box, rect, axis12, 3, vectorToBox)) return false; + + // Получаем направляющие рёбер прямоугольника + rect.transform.getAxis(0, axis20); + rect.transform.getAxis(1, axis21); + + // Проверка производных осей + if (!testAxis(box, rect, axis.vCross2(axis10, axis20), 4, vectorToBox)) return false; + if (!testAxis(box, rect, axis.vCross2(axis10, axis21), 5, vectorToBox)) return false; + + if (!testAxis(box, rect, axis.vCross2(axis11, axis20), 6, vectorToBox)) return false; + if (!testAxis(box, rect, axis.vCross2(axis11, axis21), 7, vectorToBox)) return false; + + if (!testAxis(box, rect, axis.vCross2(axis12, axis20), 8, vectorToBox)) return false; + if (!testAxis(box, rect, axis.vCross2(axis12, axis21), 9, vectorToBox)) return false; + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, rect, vectorToBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + findEdgesIntersection(box, rect, vectorToBox, int(bestAxisIndex/2), bestAxisIndex%2, contact); + } + contact.body1 = box.body; + contact.body2 = rect.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + /** + * Выполняет поиск точек контакта бокса с прямоугольником. + * + * @param box бокс + * @param rect прямоугольник + * @param vectorToBox вектор, направленный из центра прямоугольника в центр бокса + * @param faceAxisIdx индекс оси, идентифицирующей полскость столкновения (грань бокса или полскость прямоугольника) + * @param colInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, faceAxisIdx:int, colInfo:Contact):Boolean { + var pnum:int, i:int, v:Vector3, cp:ContactPoint; + if (faceAxisIdx == 0) { + // Столкновение с плоскостью прямоугольника + + // Проверим положение бокса относительно плоскости прямоугольника + rect.transform.getAxis(2, colAxis); + var offset:Number = colAxis.vDot(rPos); +// if (bPos.vDot(colAxis) < offset) return false; + + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + box.transform.getAxis(axisIdx, axis); + var dot:Number = axis.vDot(colAxis); + if (dot < 0) dot = -dot; + if (dot > maxDot) { + maxDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Получаем список вершин грани бокса, переводим их в систему координат прямоугольника и выполняем обрезку + // по прямоугольнику. Таким образом получается список потенциальных точек контакта. + box.transform.getAxis(incFaceAxisIdx, axis); + getFaceVertsByAxis(box.hs, incFaceAxisIdx, axis.vDot(colAxis) > 0, verts1); + box.transform.transformVectors(verts1, verts2); + rect.transform.transformVectorsInverse(verts2, verts1); + pnum = clipByRect(rect.hs); + // Проверяем каждую потенциальную точку на принадлежность нижней полуплоскости прямоугольника и добавляем такие точки в список контактов + colInfo.pcount = 0; + for (i = 0; i < pnum; i++) { + v = verts1[i]; + if (v.z < epsilon) { + cp = colInfo.points[colInfo.pcount++]; + rect.transform.transformVector(v, cp.pos); + cp.r1.vDiff(cp.pos, bPos); + cp.r2.vDiff(cp.pos, rPos); + cp.penetration = -v.z; + } + } + colInfo.normal.vCopy(colAxis); + } else { + // Столкновение с гранью бокса + faceAxisIdx--; + box.transform.getAxis(faceAxisIdx, colAxis); + var faceReversed:Boolean = colAxis.vDot(vectorToBox) > 0; + if (!faceReversed) colAxis.vReverse(); + + rect.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) return false; + + getFaceVertsByAxis(rect.hs, 2, false, verts1); + rect.transform.transformVectors(verts1, verts2); + box.transform.transformVectorsInverse(verts2, verts1); + pnum = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + colInfo.pcount = 0; + for (i = 0; i < pnum; i++) { + if ((pen = getPointBoxPenetration(box.hs, verts1[i], faceAxisIdx, faceReversed)) > -epsilon) { + cp = colInfo.points[colInfo.pcount++]; + box.transform.transformVector(verts1[i], cp.pos); + cp.r1.vDiff(cp.pos, bPos); + cp.r2.vDiff(cp.pos, rPos); + cp.penetration = pen; + } + } + colInfo.normal.vCopy(colAxis); + } + return true; + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + private function getFaceVertsByAxis(hs:Vector3, axisIdx:int, reverse:Boolean, result:Vector.):void { + switch (axisIdx) { + case 0: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, -hs.y, hs.z); + } + break; + case 1: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, hs.y, hs.z); + } + break; + case 2: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + } else { + (result[0] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } + break; + } + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + return clipHighX(hs.x, pnum, verts2, verts1); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + } + return 0; + } + + /** + * + * @param hs + * @return + */ + private function clipByRect(hs:Vector3):int { + var pnum:int = 4; + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box первый бокс + * @param rect второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param colInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, axisIdx1:int, axisIdx2:int, colInfo:Contact):void { + box.transform.getAxis(axisIdx1, axis10); + rect.transform.getAxis(axisIdx2, axis20); + colAxis.vCross2(axis10, axis20).vNormalize(); + // Разворот оси в сторону бокса + if (colAxis.vDot(vectorToBox) < 0) colAxis.vReverse(); + + /* Кодирование рёбер + бит 0: 1 (тип контакта ребро-ребро) + биты 1-2: индекс направляющей оси ребра + бит 3: + бит 4: + бит 5: + */ + var edgeCode1:int = 1; + var edgeCode2:int = 1; + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + point1.vCopy(box.hs); + point2.vCopy(rect.hs); + // x + if (axisIdx1 == 0) { + point1.x = 0; + halfLen1 = box.hs.x; + } else { + box.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.x = -point1.x; + edgeCode1 |= 8; // 1 << 3 + } + } + if (axisIdx2 == 0) { + point2.x = 0; + halfLen2 = rect.hs.x; + } else { + rect.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.x = -point2.x; + edgeCode2 |= 8; // 1 << 3 + } + } + // y + if (axisIdx1 == 1) { + point1.y = 0; + halfLen1 = box.hs.y; + edgeCode1 |= 2; // 1 << 1 + } else { + box.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.y = -point1.y; + edgeCode1 |= 16; // 1 << 4 + } + } + if (axisIdx2 == 1) { + point2.y = 0; + halfLen2 = rect.hs.y; + edgeCode2 |= 2; // 1 << 1 + } else { + rect.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.y = -point2.y; + edgeCode2 |= 16; // 1 << 4 + } + } + // z + if (axisIdx1 == 2) { + point1.z = 0; + halfLen1 = box.hs.z; + edgeCode1 |= 4; // 2 << 1 + } else { + box.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.z = -point1.z; + edgeCode1 |= 32; // 1 << 5 + } + } + // Получаем глобальные координаты средних точек рёбер + point1.vTransformBy4(box.transform); + point2.vTransformBy4(rect.transform); + // Находим точку пересечения рёбер, решая систему уравнений + var k:Number = axis10.vDot(axis20); + var det:Number = k*k - 1; + vector.vDiff(point2, point1); + var c1:Number = axis10.vDot(vector); + var c2:Number = axis20.vDot(vector); + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + colInfo.normal.vCopy(colAxis); + colInfo.pcount = 1; + var cp:ContactPoint = colInfo.points[0]; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(point1.x + axis10.x*t1 + point2.x + axis20.x*t2); + cp.pos.y = 0.5*(point1.y + axis10.y*t1 + point2.y + axis20.y*t2); + cp.pos.z = 0.5*(point1.z + axis10.z*t1 + point2.z + axis20.z*t2); + cp.r1.vDiff(cp.pos, bPos); + cp.r2.vDiff(cp.pos, rPos); + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box + * @param rect + * @param axis + * @param axisIndex + * @param vectorToBox + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + if (axis.vLengthSqr() < 0.0001) return true; + axis.vNormalize(); + + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия вдоль заданной оси. + * + * @param box бокс + * @param rect прямоугольник + * @param axis ось + * @param vectorToBox вектор, соединяющий центр прямоугольника с центром бокса + * @return величина перекрытия вдоль оси + */ + public function overlapOnAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, vectorToBox:Vector3):Number { + var m:Matrix4 = box.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box.hs.z; + if (d < 0) d = -d; + projection += d; + + m = rect.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*rect.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*rect.hs.y; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox.x*axis.x + vectorToBox.y*axis.y + vectorToBox.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/BoxSphereCollider.as.svn-base b/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/BoxSphereCollider.as.svn-base new file mode 100644 index 0000000..27ae562 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/BoxSphereCollider.as.svn-base @@ -0,0 +1,113 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + use namespace altphysics; + /** + * + */ + public class BoxSphereCollider implements ICollider { + + private var center:Vector3 = new Vector3(); + private var closestPt:Vector3 = new Vector3(); + + private var bPos:Vector3 = new Vector3(); + private var sPos:Vector3 = new Vector3(); + + /** + * + */ + public function BoxSphereCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + // Зафиксированно столкновение + contact.body1 = sphere.body; + contact.body2 = box.body; + contact.normal.vCopy(closestPt).vTransformBy4(box.transform).vSubtract(sPos).vNormalize().vReverse(); + contact.pcount = 1; + + var cp:ContactPoint = contact.points[0]; + cp.penetration = sphere.r - Math.sqrt(distSqr); + cp.pos.vCopy(contact.normal).vScale(-sphere.r).vAdd(sPos); + cp.r1.vDiff(cp.pos, sPos); + cp.r2.vDiff(cp.pos, bPos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/BoxTriangleCollider.as.svn-base b/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/BoxTriangleCollider.as.svn-base new file mode 100644 index 0000000..5167133 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/BoxTriangleCollider.as.svn-base @@ -0,0 +1,871 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionTriangle; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + + /** + * + */ + public class BoxTriangleCollider implements ICollider { + + public var epsilon:Number = 0.001; + + private var bestAxisIndex:int; + private var minOverlap:Number; + private var toBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var colNormal:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxTriangleCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + var mTri:Matrix4 = tri.transform; + var mBox:Matrix4 = box.transform; + + toBox.x = mBox.d - mTri.d; + toBox.y = mBox.h - mTri.h; + toBox.z = mBox.l - mTri.l; + + minOverlap = 1e308; + + // Сначала проверяется нормаль треугольника + axis.x = mTri.d; + axis.y = mTri.h; + axis.z = mTri.l; + if (!testAxis(box, tri, axis, 0, toBox)) return false; + + // Проверка основных осей бокса + axis10.x = mBox.a; + axis10.y = mBox.e; + axis10.z = mBox.i; + if (!testAxis(box, tri, axis10, 1, toBox)) return false; + axis11.x = mBox.b; + axis11.y = mBox.f; + axis11.z = mBox.j; + if (!testAxis(box, tri, axis11, 2, toBox)) return false; + axis12.x = mBox.c; + axis12.y = mBox.g; + axis12.z = mBox.k; + if (!testAxis(box, tri, axis10, 3, toBox)) return false; + + // Проверка производных осей + // TODO: заменить вычисления векторных произведений инлайнами + var v:Vector3 = tri.e0; + axis20.x = mTri.a*v.x + mTri.b*v.y + mTri.c*v.z; + axis20.y = mTri.e*v.x + mTri.f*v.y + mTri.g*v.z; + axis20.z = mTri.i*v.x + mTri.j*v.y + mTri.k*v.z; + if (!testAxis(box, tri, axis.vCross2(axis10, axis20), 4, toBox)) return false; + if (!testAxis(box, tri, axis.vCross2(axis11, axis20), 5, toBox)) return false; + if (!testAxis(box, tri, axis.vCross2(axis12, axis20), 6, toBox)) return false; + + v = tri.e1; + axis21.x = mTri.a*v.x + mTri.b*v.y + mTri.c*v.z; + axis21.y = mTri.e*v.x + mTri.f*v.y + mTri.g*v.z; + axis21.z = mTri.i*v.x + mTri.j*v.y + mTri.k*v.z; + if (!testAxis(box, tri, axis.vCross2(axis10, axis21), 7, toBox)) return false; + if (!testAxis(box, tri, axis.vCross2(axis11, axis21), 8, toBox)) return false; + if (!testAxis(box, tri, axis.vCross2(axis12, axis21), 9, toBox)) return false; + + v = tri.e2; + axis22.x = mTri.a*v.x + mTri.b*v.y + mTri.c*v.z; + axis22.y = mTri.e*v.x + mTri.f*v.y + mTri.g*v.z; + axis22.z = mTri.i*v.x + mTri.j*v.y + mTri.k*v.z; + if (!testAxis(box, tri, axis.vCross2(axis10, axis22), 10, toBox)) return false; + if (!testAxis(box, tri, axis.vCross2(axis11, axis22), 11, toBox)) return false; + if (!testAxis(box, tri, axis.vCross2(axis12, axis22), 12, toBox)) return false; + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, tri, toBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + findEdgesIntersection(box, tri, toBox, bestAxisIndex%3, int(bestAxisIndex/3), contact); + } + + contact.body1 = box.body; + contact.body2 = tri.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + /** + * Тестирует пересечение примитивов вдоль заданной оси. + * + * @param box бокс + * @param tri треугольник + * @param axis ось + * @param axisIndex индекс оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return true, если примитивы имеют перекрытие проекций вдоль указанной оси, иначе false + */ + private function testAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, axisIndex:int, toBox:Vector3):Boolean { + var len:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (len < 0.0001) return true; + len = 1/Math.sqrt(len); + axis.x *= len; + axis.y *= len; + axis.z *= len; + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Рассчитывает величину перекрытия проекций бокса и треугольника на заданную ось. + * + * @param box бокс + * @param tri треугольник + * @param axis единичный направляющий вектор оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return величина перекрытия. Положительное значение указывает на наличие перекрытия, нулевое или отрицательное значение указывает на отсутствие перекрытия. + */ + private function overlapOnAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, toBox:Vector3):Number { + var t:Matrix4 = box.transform; + var projection:Number = (t.a*axis.x + t.e*axis.y + t.i*axis.z)*box.hs.x; + if (projection < 0) projection = -projection; + var d:Number = (t.b*axis.x + t.f*axis.y + t.j*axis.z)*box.hs.y; + projection += d < 0 ? -d : d; + d = (t.c*axis.x + t.g*axis.y + t.k*axis.z)*box.hs.z; + projection += d < 0 ? -d : d; + + var vectorProjection:Number = toBox.x*axis.x + toBox.y*axis.y + toBox.z*axis.z; + // Для оптимизации ось преобразуется в систему треугольника, а не его вершины в мировую систему + t = tri.transform; + var ax:Number = t.a*axis.x + t.e*axis.y + t.i*axis.z; + var ay:Number = t.b*axis.x + t.f*axis.y + t.j*axis.z; + var az:Number = t.c*axis.x + t.g*axis.y + t.k*axis.z; + var max:Number = 0; + if (vectorProjection < 0) { + vectorProjection = -vectorProjection; + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d < max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d < max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d < max) max = d; + max = -max; + } else { + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d > max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d > max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d > max) max = d; + } + + return projection + max - vectorProjection; + } + + /** + * Определяет точки контакта бокса и треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function findFaceContactPoints(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIndex:int, contact:Contact):Boolean { + if (faceAxisIndex == 0) { + // Столкновение с плоскостью треугольника + getBoxToTriContact(box, tri, toBox, contact); + } else { + // Столкновение с гранью бокса + getTriToBoxContact(box, tri, toBox, faceAxisIndex, contact); + } + return true; + } + + /** + * + * @param box + * @param tri + * @param toBox + * @param contact + */ + private function getBoxToTriContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, contact:Contact):void { + tri.transform.getAxis(2, colNormal); + var over:Boolean = toBox.vDot(colNormal) > 0; + if (!over) colNormal.vReverse(); + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var incAxisDot:Number = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + box.transform.getAxis(axisIdx, axis); + var dot:Number = axis.vDot(colNormal); + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incAxisDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Обрезка грани + var negativeFace:Boolean = incAxisDot > 0; + getFaceVertsByAxis(box.hs, incFaceAxisIdx, negativeFace, points1); + box.transform.transformVectorsN(points1, points2, 4); + tri.transform.transformVectorsInverseN(points2, points1, 4); + var pnum:int = clipByTriangle(tri); + // Среди конечного списка точек определяются лежащие под плоскостью треугольника + var cp:ContactPoint; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points2[i]; + if ((over && v.z < 0) || (!over && v.z > 0)) { + cp = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + tri.transform.transformVector(v, cpPos); + var r:Vector3 = cp.r1; + r.x = cpPos.x - box.transform.d; + r.y = cpPos.y - box.transform.h; + r.z = cpPos.z - box.transform.l; + r = cp.r2; + r.x = cpPos.x - tri.transform.d; + r.y = cpPos.y - tri.transform.h; + r.z = cpPos.z - tri.transform.l; + cp.penetration = over ? -v.z : v.z; + } + } + if (contact.pcount > 0) { + contact.normal.vCopy(colNormal); + } + } + + /** + * + * @param box + * @param tri + * @param toBox + * @param faceAxisIndex + * @param contact + */ + private function getTriToBoxContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIdx:int, contact:Contact):void { + faceAxisIdx--; + box.transform.getAxis(faceAxisIdx, colNormal); + var negativeFace:Boolean = colNormal.vDot(toBox) > 0; + if (!negativeFace) { + colNormal.vReverse(); + } + Vector3(points1[0]).vCopy(tri.v0); + Vector3(points1[1]).vCopy(tri.v1); + Vector3(points1[2]).vCopy(tri.v2); + tri.transform.transformVectorsN(points1, points2, 3); + box.transform.transformVectorsInverseN(points2, points1, 3); + var pnum:int = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность боксу и добавляем такие точки в список контактов + var pen:Number; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points1[i]; + pen = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace); + if (pen > -epsilon) { + var cp:ContactPoint = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + box.transform.transformVector(v, cp.pos); + var r:Vector3 = cp.r1; + r.x = cpPos.x - box.transform.d; + r.y = cpPos.y - box.transform.h; + r.z = cpPos.z - box.transform.l; + r = cp.r2; + r.x = cpPos.x - tri.transform.d; + r.y = cpPos.y - tri.transform.h; + r.z = cpPos.z - tri.transform.l; + cp.penetration = pen; + } + } + contact.normal.vCopy(colNormal); + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, negativeFace:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (negativeFace) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (negativeFace) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (negativeFace) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, вершины которой заданы в списке points1. Результат сохраняется в этом же списке. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1); + } + return 0; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + Vector3(result[num++]).vCopy(p1); + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + Vector3(result[num++]).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + Vector3(result[num++]).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + Vector3(result[num++]).vCopy(p1); + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + Vector3(result[num++]).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + Vector3(result[num++]).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + Vector3(result[num++]).vCopy(p1); + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + Vector3(result[num++]).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + Vector3(result[num++]).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + Vector3(result[num++]).vCopy(p1); + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + Vector3(result[num++]).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + Vector3(result[num++]).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + Vector3(result[num++]).vCopy(p1); + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + Vector3(result[num++]).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + Vector3(result[num++]).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + Vector3(result[num++]).vCopy(p1); + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + Vector3(result[num++]).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + Vector3(result[num++]).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param negativeFace если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + private function getFaceVertsByAxis(hs:Vector3, axisIdx:int, negativeFace:Boolean, result:Vector.):void { + switch (axisIdx) { + case 0: + if (negativeFace) { + Vector3(result[0]).vReset(-hs.x, hs.y, -hs.z); + Vector3(result[1]).vReset(-hs.x, -hs.y, -hs.z); + Vector3(result[2]).vReset(-hs.x, -hs.y, hs.z); + Vector3(result[3]).vReset(-hs.x, hs.y, hs.z); + } else { + Vector3(result[0]).vReset(hs.x, -hs.y, -hs.z); + Vector3(result[1]).vReset(hs.x, hs.y, -hs.z); + Vector3(result[2]).vReset(hs.x, hs.y, hs.z); + Vector3(result[3]).vReset(hs.x, -hs.y, hs.z); + } + break; + case 1: + if (negativeFace) { + Vector3(result[0]).vReset(-hs.x, -hs.y, -hs.z); + Vector3(result[1]).vReset(hs.x, -hs.y, -hs.z); + Vector3(result[2]).vReset(hs.x, -hs.y, hs.z); + Vector3(result[3]).vReset(-hs.x, -hs.y, hs.z); + } else { + Vector3(result[0]).vReset(hs.x, hs.y, -hs.z); + Vector3(result[1]).vReset(-hs.x, hs.y, -hs.z); + Vector3(result[2]).vReset(-hs.x, hs.y, hs.z); + Vector3(result[3]).vReset(hs.x, hs.y, hs.z); + } + break; + case 2: + if (negativeFace) { + Vector3(result[0]).vReset(-hs.x, hs.y, -hs.z); + Vector3(result[1]).vReset(hs.x, hs.y, -hs.z); + Vector3(result[2]).vReset(hs.x, -hs.y, -hs.z); + Vector3(result[3]).vReset(-hs.x, -hs.y, -hs.z); + } else { + Vector3(result[0]).vReset(-hs.x, -hs.y, hs.z); + Vector3(result[1]).vReset(hs.x, -hs.y, hs.z); + Vector3(result[2]).vReset(hs.x, hs.y, hs.z); + Vector3(result[3]).vReset(-hs.x, hs.y, hs.z); + } + break; + } + } + + /** + * Обрезает грань прямоугольника сторонами треугольника. Обрезка выполняется в системе координат и в проекции на + * плоскость треугольника. Входные вершины грани в количестве четырёх штук должны находиться в списке points1. + * Конечные вершины сохраняются в списке points2. + * + * @param tri треугольник + * @return количество врешин в конечном списке + */ + private function clipByTriangle(tri:CollisionTriangle):int { + var vnum:int = 4; + vnum = clipByLine(tri.v0, tri.e0, points1, vnum, points2); + if (vnum == 0) return 0; + vnum = clipByLine(tri.v1, tri.e1, points2, vnum, points1); + if (vnum == 0) return 0; + return clipByLine(tri.v2, tri.e2, points1, vnum, points2);; + } + + /** + * Обрезает полигон указанной прямой. + * + * @param linePoint точка на прямой + * @param lineDir единичный направляющий вектор прямой + * @param verticesIn список вершин исходного полигона + * @param vnum количество вершин исходного полигона + * @param verticesOut список, куда будут записаны вершины конечного полигона + * @return количество вершин конечного полигона + */ + private function clipByLine(linePoint:Vector3, lineDir:Vector3, verticesIn:Vector., vnum:int, verticesOut:Vector.):int { + var nx:Number = lineDir.y; + var ny:Number = -lineDir.x; + var offset:Number = linePoint.x*nx + linePoint.y*ny; + var v1:Vector3 = verticesIn[vnum -1]; + var offset1:Number = v1.x*nx + v1.y*ny; + var t:Number; + var v:Vector3; + var num:int = 0; + for (var i:int = 0; i < vnum; i++) { + var v2:Vector3 = verticesIn[i]; + var offset2:Number = v2.x*nx + v2.y*ny; + if (offset1 < offset) { + // Первая точка ребра во внешней полуплоскости + if (offset2 > offset) { + // Вторая точка ребра во внутренней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } else { + // Первая точка ребра во внутренней полуплоскости. Добавляем её в конечный список. + v = verticesOut[num]; + v.x = v1.x; + v.y = v1.y; + v.z = v1.z; + num++; + if (offset2 < offset) { + // Вторая точка ребра во внешней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } + v1 = v2; + offset1 = offset2; + } + return num; + } + + /** + * + * @param box + * @param tri + * @param toBox + * @param boxAxisIdx + * @param triAxisIdx + * @param contact + */ + private function findEdgesIntersection(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, boxAxisIdx:int, triAxisIdx:int, contact:Contact):void { + box.transform.getAxis(boxAxisIdx, axis10); + point1.vCopy(box.hs); + var boxHalfLen:Number; + // X + if (boxAxisIdx == 0) { + point1.x = 0; + boxHalfLen = box.hs.x; + } else { + box.transform.getAxis(0, axis11); + if (axis11.vDot(toBox) > 0) { + point1.x = -point1.x; + } + } + // Y + if (boxAxisIdx == 1) { + point1.y = 0; + boxHalfLen = box.hs.y; + } else { + box.transform.getAxis(1, axis11); + if (axis11.vDot(toBox) > 0) { + point1.y = -point1.y; + } + } + // Z + if (boxAxisIdx == 2) { + point1.z = 0; + boxHalfLen = box.hs.z; + } else { + box.transform.getAxis(2, axis11); + if (axis11.vDot(toBox) > 0) { + point1.z = -point1.z; + } + } + point1.vTransformBy4(box.transform); + + var triLen:Number; + switch (triAxisIdx) { + case 0: + point2.vCopy(tri.v0); + axis20.vCopy(tri.e0); + triLen = tri.len0; + break; + case 1: + point2.vCopy(tri.v1); + axis20.vCopy(tri.e1); + triLen = tri.len1; + break; + case 2: + point2.vCopy(tri.v2); + axis20.vCopy(tri.e2); + triLen = tri.len2; + break; + } + axis20.vDeltaTransformBy4(tri.transform); + point2.vTransformBy4(tri.transform); + + colNormal.vCross2(axis10, axis20).vNormalize(); + // Разворот оси в сторону бокса + if (colNormal.vDot(toBox) < 0) colNormal.vReverse(); + + // Находим точку пересечения рёбер, решая систему уравнений + // axis10 - направляющий вектор ребра бокса + // point1 - средняя точка ребра бокса + // axis20 - направляющий вектор ребра треугольника + // point2 - начальная точка ребра треугольника + var k:Number = axis10.vDot(axis20); + var det:Number = k*k - 1; + vector.vDiff(point2, point1); + var c1:Number = axis10.vDot(vector); + var c2:Number = axis20.vDot(vector); + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.normal.vCopy(colNormal); + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cpPos.x = 0.5*(point1.x + axis10.x*t1 + point2.x + axis20.x*t2); + cpPos.y = 0.5*(point1.y + axis10.y*t1 + point2.y + axis20.y*t2); + cpPos.z = 0.5*(point1.z + axis10.z*t1 + point2.z + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = cpPos.x - box.transform.d; + r.y = cpPos.y - box.transform.h; + r.z = cpPos.z - box.transform.l; + r = cp.r2; + r.x = cpPos.x - tri.transform.d; + r.y = cpPos.y - tri.transform.h; + r.z = cpPos.z - tri.transform.l; + cp.penetration = minOverlap; + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/BruteForceCollisionDetector.as.svn-base b/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/BruteForceCollisionDetector.as.svn-base new file mode 100644 index 0000000..85d84fa --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/BruteForceCollisionDetector.as.svn-base @@ -0,0 +1,63 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + public class BruteForceCollisionDetector implements ICollisionDetector { + public function BruteForceCollisionDetector() { + } + + public function addPrimitive(primitive:CollisionPrimitive, isStatic:Boolean=true):Boolean { + return false; + } + + public function removePrimitive(primitive:CollisionPrimitive, isStatic:Boolean=true):Boolean { + return false; + } + + public function init():void { + } + + public function getAllCollisions(contacts:Vector.):int { + return 0; + } + + public function intersectRay(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, intersection:RayIntersection):Boolean { + return false; + } + + public function intersectRayWithStatic(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean { + return false; + } + + public function testBodyPrimitiveCollision(body:Body, primitive:CollisionPrimitive):Boolean { + return false; + } + + /** + * + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + return false; + } + + /** + * + * @param prim1 + * @param prim2 + * @return + */ + public function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base b/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base new file mode 100644 index 0000000..e24922c --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base @@ -0,0 +1,18 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.types.BoundBox; + + public class CollisionKdNode { + public var indices:Vector.; + public var splitIndices:Vector.; + public var boundBox:BoundBox; + public var parent:CollisionKdNode; + public var splitTree:CollisionKdTree2D; + + public var axis:int = -1; // 0 - x, 1 - y, 2 - z + public var coord:Number; + public var positiveNode:CollisionKdNode; + public var negativeNode:CollisionKdNode; + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base b/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base new file mode 100644 index 0000000..c046f17 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base @@ -0,0 +1,280 @@ +package alternativa.physics.collision { + + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + + /** + * @author mike + */ + public class CollisionKdTree { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + public var rootNode:CollisionKdNode; + public var staticChildren:Vector. = new Vector.(); + public var numStaticChildren:int; + public var staticBoundBoxes:Vector. = new Vector.(); + + private var splitAxis:int; + private var splitCoord:Number; + private var splitCost:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + + /** + * @param primitive + */ + public function addStaticPrimitive(primitive:CollisionPrimitive):void { + staticChildren[numStaticChildren++] = primitive; + } + + /** + * @param primitive + * @return + */ + public function removeStaticPrimitive(primitive:CollisionPrimitive):Boolean { + var idx:int = staticChildren.indexOf(primitive); + if (idx < 0) return false; + staticChildren.splice(idx, 1); + numStaticChildren--; + return true; + } + + /** + * @param boundBox + */ + public function createTree(boundBox:BoundBox = null):void { + if (numStaticChildren == 0) return; + // Создаём корневую ноду + rootNode = new CollisionKdNode(); + rootNode.indices = new Vector.(); + // Расчитываем баунды объектов и рутовой ноды + var rootNodeBoundBox:BoundBox = rootNode.boundBox = (boundBox != null) ? boundBox : new BoundBox(); + for (var i:int = 0; i < numStaticChildren; ++i) { + var child:CollisionPrimitive = staticChildren[i]; + var childBoundBox:BoundBox = staticBoundBoxes[i] = child.calculateAABB(); + rootNodeBoundBox.addBoundBox(childBoundBox); + rootNode.indices[i] = i; + } + staticBoundBoxes.length = numStaticChildren; + // Разделяем рутовую ноду + splitNode(rootNode); + + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + var indices:Vector. = node.indices; + var numPrimitives:int = indices.length; + if (numPrimitives <= minPrimitivesPerNode) return; + + // Подготовка баунда с погрешностями + var nodeBoundBox:BoundBox = node.boundBox; + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + // Собираем опорные координаты + var i:int; + var j:int; + var numSplitCoordsX:int = 0 + var numSplitCoordsY:int = 0; + var numSplitCoordsZ:int = 0; + for (i = 0; i < numPrimitives; ++i) { + var boundBox:BoundBox = staticBoundBoxes[indices[i]]; + + if (boundBox.maxX - boundBox.minX <= doubleThreshold) { + if (boundBox.minX <= nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.minX; + else if (boundBox.maxX >= nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.maxX; + else splitCoordsX[numSplitCoordsX++] = (boundBox.minX + boundBox.maxX)*0.5; + } else { + if (boundBox.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = boundBox.minX; + if (boundBox.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = boundBox.maxX; + } + + if (boundBox.maxY - boundBox.minY <= doubleThreshold) { + if (boundBox.minY <= nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.minY; + else if (boundBox.maxY >= nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.maxY; + else splitCoordsY[numSplitCoordsY++] = (boundBox.minY + boundBox.maxY)*0.5; + } else { + if (boundBox.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = boundBox.minY; + if (boundBox.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = boundBox.maxY; + } + + if (boundBox.maxZ - boundBox.minZ <= doubleThreshold) { + if (boundBox.minZ <= nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.minZ; + else if (boundBox.maxZ >= nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.maxZ; + else splitCoordsZ[numSplitCoordsZ++] = (boundBox.minZ + boundBox.maxZ)*0.5; + } else { + if (boundBox.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.minZ; + if (boundBox.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numPrimitives; ++i) { + boundBox = staticBoundBoxes[indices[i]]; + var min:Number = axisX ? boundBox.minX : (axisY ? boundBox.minY : boundBox.minZ); + var max:Number = axisX ? boundBox.maxX : (axisY ? boundBox.maxY : boundBox.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(indices[i]); + indices[i] = -1; + } else { + if (node.splitIndices == null) node.splitIndices = new Vector.(); + node.splitIndices.push(indices[i]); + indices[i] = -1; + } + } else { + if (min >= coordMin) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(indices[i]); + indices[i] = -1; + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numPrimitives; ++i) { + if (indices[i] >= 0) indices[j++] = indices[i]; + } + if (j > 0) indices.length = j; + else node.indices = null; + + if (node.splitIndices != null) { + node.splitTree = new CollisionKdTree2D(this, node); + node.splitTree.createTree(); + } + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + /** + * + */ + public function traceTree():void { + traceNode("", rootNode); + } + + /** + * @param str + * @param node + */ + private function traceNode(str:String, node:CollisionKdNode):void { + if (node == null) return; + trace(str, node.axis == -1 ? "end" : ((node.axis == 0) ? "X" : ((node.axis == 1) ? "Y" : "Z")), "splitCoord=" + splitCoord, "bound", node.boundBox, "objs:", node.indices); + traceNode(str + "-", node.negativeNode); + traceNode(str + "+", node.positiveNode); + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base b/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base new file mode 100644 index 0000000..78aaa95 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base @@ -0,0 +1,226 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.types.BoundBox; + + /** + * + */ + public class CollisionKdTree2D { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + + public var parentTree:CollisionKdTree; + public var parentNode:CollisionKdNode; + public var rootNode:CollisionKdNode; + + private var splitAxis:int; + private var splitCost:Number; + private var splitCoord:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + /** + * + * @param parentTree + * @param parentNode + */ + public function CollisionKdTree2D(parentTree:CollisionKdTree, parentNode:CollisionKdNode) { + this.parentTree = parentTree; + this.parentNode = parentNode; + } + + /** + * + */ + public function createTree():void { + rootNode = new CollisionKdNode(); + rootNode.boundBox = parentNode.boundBox.clone(); + rootNode.indices = new Vector.(); + var numObjects:int = parentNode.splitIndices.length; + for (var i:int = 0; i < numObjects; ++i) rootNode.indices[i] = parentNode.splitIndices[i]; + + splitNode(rootNode); + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + if (node.indices.length <= minPrimitivesPerNode) return; + + var objects:Vector. = node.indices; + var i:int; + var j:int; + + var nodeBoundBox:BoundBox = node.boundBox; + + // Подготовка баунда с погрешностями + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + // Собираем опорные координаты + var numSplitCoordsX:int; + var numSplitCoordsY:int; + var numSplitCoordsZ:int; + var numObjects:int = objects.length; + for (i = 0; i < numObjects; ++i) { + var bb:BoundBox = staticBoundBoxes[objects[i]]; + + if (parentNode.axis != 0) { + if (bb.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = bb.minX; + if (bb.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = bb.maxX; + } + + if (parentNode.axis != 1) { + if (bb.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = bb.minY; + if (bb.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = bb.maxY; + } + + if (parentNode.axis != 2) { + if (bb.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = bb.minZ; + if (bb.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = bb.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + if (parentNode.axis != 0) checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + if (parentNode.axis != 1) checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + if (parentNode.axis != 2) checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numObjects; ++i) { + bb = staticBoundBoxes[objects[i]]; + var min:Number = axisX ? bb.minX : (axisY ? bb.minY : bb.minZ); + var max:Number = axisX ? bb.maxX : (axisY ? bb.maxY : bb.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(objects[i]); + objects[i] = -1; + } + } else { + if (min >= coordMin) { + if (max > coordMax) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(objects[i]); + objects[i] = -1; + } + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numObjects; ++i) { + if (objects[i] >= 0) objects[j++] = objects[i]; + } + if (j > 0) objects.length = j; + else node.indices = null; + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base b/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base new file mode 100644 index 0000000..9cfdca5 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Body; + + public interface IBodyCollisionPredicate { + function considerBodies(body1:Body, body2:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base b/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base new file mode 100644 index 0000000..220e717 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base @@ -0,0 +1,30 @@ +package alternativa.physics.collision { + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + + /** + * Интерфейс определителя столкновений между двумя примитивами. + */ + public interface ICollider { + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base b/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base new file mode 100644 index 0000000..af43dfb --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base @@ -0,0 +1,92 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + /** + * Интерфейс детектора столкновений. + */ + public interface ICollisionDetector { + + /** + * Добавляет физический примитив в коллайдер. + * + * @param primitive добавляемый примитив + * @param isStatic указывает тип примитива: статический или динамический + * @return true если примитив был успешно добавлен, иначе false + */ + function addPrimitive(primitive:CollisionPrimitive, isStatic:Boolean = true):Boolean; + + /** + * Удаляет физический примитив из коллайдера. + * + * @param primitive удаляемый примитив + * @param isStatic указывает тип примитива: статический или динамический + * @return true если примитив был успшено удалён, иначе false + */ + function removePrimitive(primitive:CollisionPrimitive, isStatic:Boolean = true):Boolean; + + /** + * Выполняет инициализацию детектора после обновления списка примитивов. + */ + function init():void; + + /** + * Получает все столкновения в текущей конфигурации физической геометрии. + * + * @param contacts список контактов, в кторые будет записана информация о столкновении + * @return количество найденных столкновений + */ + function getAllCollisions(contacts:Vector.):int; + + /** + * Тестирует луч на пересечение с физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param dir направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRay(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * Тестирует луч на пересечение со статической физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param dir направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRayWithStatic(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @return + */ + function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base b/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base new file mode 100644 index 0000000..bb1b756 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base @@ -0,0 +1,9 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + + public interface ICollisionPredicate { + + function considerCollision(primitive:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base b/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base new file mode 100644 index 0000000..0f3d929 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Body; + + public interface IRayCollisionPredicate { + function considerBody(body:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base b/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base new file mode 100644 index 0000000..7b0b78e --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base @@ -0,0 +1,484 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Детектор, хранящий статическую геометрию в kD-дереве и использующий дерево для ускорения тестов на пересечения. + */ + public class KdTreeCollisionDetector implements ICollisionDetector { + + altphysics var tree:CollisionKdTree; + altphysics var dynamicPrimitives:Vector.; + 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.(); + + 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():void { + tree.createTree(); +// tree.traceTree(); + } + + /** + * + * @param contacts + * @return + */ + public function getAllCollisions(contacts:Vector.):int { + var colNum:int = 0; + for (var i:int = 0; i < dynamicPrimitivesNum; i++) { + var primitive:CollisionPrimitive = dynamicPrimitives[i]; + primitive.calculateAABB(); + if (primitive.body != null && primitive.body.frozen) continue; + colNum += getPrimitiveNodeCollisions(tree.rootNode, primitive, contacts, colNum); + + // Столкновения динамических примитивов между собой + // TODO: Попробовать различные оптимизации (сортировка по баундам, встраивание в дерево) + for (var j:int = i + 1; j < dynamicPrimitivesNum; j++) { + if (collide(primitive, dynamicPrimitives[j], contacts[colNum])) colNum++; + } + } + + return colNum; + } + + /** + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + public function collide(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 = (contact.points[0] as ContactPoint).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:Vector., startIndex:int):int { + var colNum:int = 0; + if (node.indices != null) { + // Поиск столкновений со статическими примитивами узла + var primitives:Vector. = tree.staticChildren; + var indices:Vector. = node.indices; + for (var i:int = indices.length - 1; i >= 0; i--) + if (collide(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; + } + + 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., indices:Vector., 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; + } \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/SpherePlaneCollider.as.svn-base b/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/SpherePlaneCollider.as.svn-base new file mode 100644 index 0000000..77eb1b0 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/SpherePlaneCollider.as.svn-base @@ -0,0 +1,67 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + /** + * + */ + public class SpherePlaneCollider implements ICollider { + + private var normal:Vector3 = new Vector3(); + + /** + * + */ + public function SpherePlaneCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var sphere:RigidSphere = body1 as RigidSphere; +// var plane:RigidPlane; +// if (sphere == null) { +// sphere = body2 as RigidSphere; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// +// var dist:Number = sphere.state.pos.dot(normal) - offset; +// if (dist > sphere.r) return false; +// +// collisionInfo.body1 = sphere; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// collisionInfo.pcount = 1; +// +// var cp:ContactPoint = collisionInfo.points[0]; +// cp.penetration = sphere.r - dist; +// cp.pos.copy(normal).reverse().scale(sphere.r).add(sphere.state.pos); +// cp.r1.diff(cp.pos, sphere.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/SphereSphereCollider.as.svn-base b/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/SphereSphereCollider.as.svn-base new file mode 100644 index 0000000..25971d5 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/.svn/text-base/SphereSphereCollider.as.svn-base @@ -0,0 +1,69 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + public class SphereSphereCollider implements ICollider { + + private var p1:Vector3 = new Vector3(); + private var p2:Vector3 = new Vector3(); + + public function SphereSphereCollider() { + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var s1:CollisionSphere; + var s2:CollisionSphere; + if (prim1.body != null) { + s1 = prim1 as CollisionSphere; + s2 = prim2 as CollisionSphere; + } else { + s1 = prim2 as CollisionSphere; + s2 = prim1 as CollisionSphere; + } + + s1.transform.getAxis(3, p1); + s2.transform.getAxis(3, p2); + var dx:Number = p1.x - p2.x; + var dy:Number = p1.y - p2.y; + var dz:Number = p1.z - p2.z; + var len:Number = dx*dx + dy*dy + dz*dz; + var sum:Number = s1.r + s2.r; + if (len > sum*sum) return false; + len = Math.sqrt(len); + dx /= len; + dy /= len; + dz /= len; + + contact.body1 = s1.body; + contact.body2 = s2.body; + contact.normal.x = dx; + contact.normal.y = dy; + contact.normal.z = dz; + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = sum - len; + cp.pos.x = p1.x - dx*s1.r; + cp.pos.y = p1.y - dy*s1.r; + cp.pos.z = p1.z - dz*s1.r; + cp.r1.vDiff(cp.pos, p1); + cp.r2.vDiff(cp.pos, p2); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/BoxBoxCollider.as b/0.0.5.0/src/alternativa/physics/collision/BoxBoxCollider.as new file mode 100644 index 0000000..0262b99 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/BoxBoxCollider.as @@ -0,0 +1,848 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Расчитывает точки контакта двух боксов. Нормаль контакта направляется в сторону бокса с меньшим ID. + */ + public class BoxBoxCollider implements ICollider { + + private var tolerance:Number = 0.001; + + private var pos1:Vector3 = new Vector3(); + private var pos2:Vector3 = new Vector3(); + private var vectorToBox1:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var colAxis:Vector3 = new Vector3(); + private var tmpAxis:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var tmpPoints:Vector. = new Vector.(8, true); + private var pcount:int; + + /** + * + */ + public function BoxBoxCollider() { + for (var i:int = 0; i < 8; i++) { + tmpPoints[i] = new ContactPoint(); + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + /** + * + * @param body1 + * @param body2 + * @param contactInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + minOverlap = 1e10; + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + // Вектор из центра второго бокса в центр первого + pos1.x = transform1.d; pos1.y = transform1.h; pos1.z = transform1.l; + pos2.x = transform2.d; pos2.y = transform2.h; pos2.z = transform2.l; + vectorToBox1.x = pos1.x - pos2.x; + vectorToBox1.y = pos1.y - pos2.y; + vectorToBox1.z = pos1.z - pos2.z; + + // Проверка пересечения по основным осям +// box1.transform.getAxis(0, axis10); + axis10.x = transform1.a; axis10.y = transform1.e; axis10.z = transform1.i; + if (!testAxis(box1, box2, axis10, 0, vectorToBox1)) return false; +// box1.transform.getAxis(1, axis11); + axis11.x = transform1.b; axis11.y = transform1.f; axis11.z = transform1.j; + if (!testAxis(box1, box2, axis11, 1, vectorToBox1)) return false; +// box1.transform.getAxis(2, axis12); + axis12.x = transform1.c; axis12.y = transform1.g; axis12.z = transform1.k; + if (!testAxis(box1, box2, axis12, 2, vectorToBox1)) return false; + +// box2.transform.getAxis(0, axis20); + axis20.x = transform2.a; axis20.y = transform2.e; axis20.z = transform2.i; + if (!testAxis(box1, box2, axis20, 3, vectorToBox1)) return false; +// box2.transform.getAxis(1, axis21); + axis21.x = transform2.b; axis21.y = transform2.f; axis21.z = transform2.j; + if (!testAxis(box1, box2, axis21, 4, vectorToBox1)) return false; +// box2.transform.getAxis(2, axis22); + axis22.x = transform2.c; axis22.y = transform2.g; axis22.z = transform2.k; + if (!testAxis(box1, box2, axis22, 5, vectorToBox1)) return false; + + // Проверка производных осей + if (!testAxis(box1, box2, axis.vCross2(axis10, axis20), 6, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis10, axis21), 7, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis10, axis22), 8, vectorToBox1)) return false; + + if (!testAxis(box1, box2, axis.vCross2(axis11, axis20), 9, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis11, axis21), 10, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis11, axis22), 11, vectorToBox1)) return false; + + if (!testAxis(box1, box2, axis.vCross2(axis12, axis20), 12, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis12, axis21), 13, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis12, axis22), 14, vectorToBox1)) return false; + + if (bestAxisIndex < 6) { + // Контакт грань-(грань|ребро|вершина) + findFaceContactPoints(box1, box2, vectorToBox1, bestAxisIndex, contact); + } else { + // Контакт ребро-ребро + bestAxisIndex -= 6; + findEdgesIntersection(box1, box2, vectorToBox1, int(bestAxisIndex/3), bestAxisIndex%3, contact); + } + contact.body1 = box1.body; + contact.body2 = box2.body; + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + // Вектор из центра второго бокса в центр первого + pos1.x = transform1.d; pos1.y = transform1.h; pos1.z = transform1.l; + pos2.x = transform2.d; pos2.y = transform2.h; pos2.z = transform2.l; + vectorToBox1.x = pos1.x - pos2.x; + vectorToBox1.y = pos1.y - pos2.y; + vectorToBox1.z = pos1.z - pos2.z; + + // Проверка пересечения по основным осям + axis10.x = transform1.a; axis10.y = transform1.e; axis10.z = transform1.i; + if (!testAxis(box1, box2, axis10, 0, vectorToBox1)) return false; + axis11.x = transform1.b; axis11.y = transform1.f; axis11.z = transform1.j; + if (!testAxis(box1, box2, axis11, 1, vectorToBox1)) return false; + axis12.x = transform1.c; axis12.y = transform1.g; axis12.z = transform1.k; + if (!testAxis(box1, box2, axis12, 2, vectorToBox1)) return false; + + axis20.x = transform2.a; axis20.y = transform2.e; axis20.z = transform2.i; + if (!testAxis(box1, box2, axis20, 3, vectorToBox1)) return false; + axis21.x = transform2.b; axis21.y = transform2.f; axis21.z = transform2.j; + if (!testAxis(box1, box2, axis21, 4, vectorToBox1)) return false; + axis22.x = transform2.c; axis22.y = transform2.g; axis22.z = transform2.k; + if (!testAxis(box1, box2, axis22, 5, vectorToBox1)) return false; + + // Проверка производных осей + if (!testAxis(box1, box2, axis.vCross2(axis10, axis20), 6, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis10, axis21), 7, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis10, axis22), 8, vectorToBox1)) return false; + + if (!testAxis(box1, box2, axis.vCross2(axis11, axis20), 9, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis11, axis21), 10, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis11, axis22), 11, vectorToBox1)) return false; + + if (!testAxis(box1, box2, axis.vCross2(axis12, axis20), 12, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis12, axis21), 13, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis12, axis22), 14, vectorToBox1)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта грани одного бокса с гранью/ребром/вершиной другого. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param faceAxisIdx индекс оси первого бокса, перпендикулярной грани, с которой произошло столкновение + * @param contactInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, faceAxisIdx:int, contactInfo:Contact):void { + var swapNormal:Boolean = false; + if (faceAxisIdx > 2) { + // Столкновение с гранью второго бокса. Для дальнейших расчётов боксы меняются местами, + // но нормаль контакта всё равно должна быть направлена в сторону первоначального box1 + var tmpBox:CollisionBox = box1; + box1 = box2; + box2 = tmpBox; + vectorToBox1.vReverse(); + faceAxisIdx -= 3; + swapNormal = true; + } + box1.transform.getAxis(faceAxisIdx, colAxis); + var faceReversed:Boolean = colAxis.vDot(vectorToBox1) > 0; + if (!faceReversed) colAxis.vReverse(); + // Ищем ось второго бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + box2.transform.getAxis(axisIdx, axis); + var dot:Number = axis.vDot(colAxis); + if (dot < 0) dot = -dot; + if (dot > maxDot) { + maxDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Получаем список вершин грани второго бокса, переводим их в систему координат первого бокса и выполняем обрезку + // по грани первого бокса. Таким образом получается список потенциальных точек контакта. + box2.transform.getAxis(incFaceAxisIdx, axis); + getFaceVertsByAxis(box2, incFaceAxisIdx, axis.vDot(colAxis) < 0, verts1); + box2.transform.transformVectors(verts1, verts2); + box1.transform.transformVectorsInverse(verts2, verts1); + var pnum:int = clip(box1.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + pcount = 0; + for (var i:int = 0; i < pnum; i++) { + if ((pen = getPointBoxPenetration(box1.hs, verts1[i], faceAxisIdx, faceReversed)) > -tolerance) { + var cp:ContactPoint = tmpPoints[pcount++]; + box1.transform.transformVector(verts1[i], cp.pos); + cp.r1.vDiff(cp.pos, pos1); + cp.r2.vDiff(cp.pos, pos2); + cp.penetration = pen; + } + } + contactInfo.normal.vCopy(colAxis); + if (swapNormal) contactInfo.normal.vReverse(); + + if (pcount > 4) reducePoints(); + for (i = 0; i < pcount; i++) (contactInfo.points[i] as ContactPoint).copyFrom(tmpPoints[i]); + contactInfo.pcount = pcount; + } + + /** + * + * @param contactInfo + */ + private function reducePoints():void { + var i:int; + var minIdx:int; + var cp1:ContactPoint; + var cp2:ContactPoint; + while (pcount > 4) { + var minLen:Number = 1e10; + var p1:Vector3 = (tmpPoints[pcount - 1] as ContactPoint).pos; + var p2:Vector3; + for (i = 0; i < pcount; i++) { + p2 = (tmpPoints[i] as ContactPoint).pos; + var dx:Number = p2.x - p1.x; + var dy:Number = p2.y - p1.y; + var dz:Number = p2.z - p1.z; + var len:Number = dx*dx + dy*dy + dz*dz; + if (len < minLen) { + minLen = len; + minIdx = i; + } + p1 = p2; + } + cp1 = tmpPoints[minIdx == 0 ? (pcount - 1) : (minIdx - 1)]; + cp2 = tmpPoints[minIdx]; + p1 = cp1.pos; + p2 = cp2.pos; + p2.x = 0.5*(p1.x + p2.x); + p2.y = 0.5*(p1.y + p2.y); + p2.z = 0.5*(p1.z + p2.z); + cp2.penetration = 0.5*(cp1.penetration + cp2.penetration); + if (minIdx > 0) { + for (i = minIdx; i < pcount; i++) tmpPoints[i - 1] = tmpPoints[i]; + tmpPoints[pcount - 1] = cp1; + } + pcount--; + } + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + private function getFaceVertsByAxis(box:CollisionBox, axisIdx:int, reverse:Boolean, result:Vector.):void { + var hs:Vector3 = box.hs; + switch (axisIdx) { + case 0: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, -hs.y, hs.z); + } + break; + case 1: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, hs.y, hs.z); + } + break; + case 2: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + } else { + (result[0] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } + break; + } + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clip(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + return clipHighX(hs.x, pnum, verts2, verts1); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + } + return 0; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param contactInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):void { + box1.transform.getAxis(axisIdx1, axis10); + box2.transform.getAxis(axisIdx2, axis20); + colAxis.vCross2(axis10, axis20).vNormalize(); + // Разворот оси в сторону первого бокса + if (colAxis.vDot(vectorToBox1) < 0) colAxis.vReverse(); + + /* Кодирование рёбер + бит 0: 1 (тип контакта ребро-ребро) + биты 1-2: индекс направляющей оси ребра + бит 3: + бит 4: + бит 5: + */ + var edgeCode1:int = 1; + var edgeCode2:int = 1; + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + point1.vCopy(box1.hs); + point2.vCopy(box2.hs); + // x + if (axisIdx1 == 0) { + point1.x = 0; + halfLen1 = box1.hs.x; + } else { + box1.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.x = -point1.x; + edgeCode1 |= 8; // 1 << 3 + } + } + if (axisIdx2 == 0) { + point2.x = 0; + halfLen2 = box2.hs.x; + } else { + box2.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.x = -point2.x; + edgeCode2 |= 8; // 1 << 3 + } + } + // y + if (axisIdx1 == 1) { + point1.y = 0; + halfLen1 = box1.hs.y; + edgeCode1 |= 2; // 1 << 1 + } else { + box1.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.y = -point1.y; + edgeCode1 |= 16; // 1 << 4 + } + } + if (axisIdx2 == 1) { + point2.y = 0; + halfLen2 = box2.hs.y; + edgeCode2 |= 2; // 1 << 1 + } else { + box2.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.y = -point2.y; + edgeCode2 |= 16; // 1 << 4 + } + } + // z + if (axisIdx1 == 2) { + point1.z = 0; + halfLen1 = box1.hs.z; + edgeCode1 |= 4; // 2 << 1 + } else { + box1.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.z = -point1.z; + edgeCode1 |= 32; // 1 << 5 + } + } + if (axisIdx2 == 2) { + point2.z = 0; + halfLen2 = box2.hs.z; + edgeCode2 |= 4; // 2 << 1 + } else { + box2.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.z = -point2.z; + edgeCode2 |= 32; // 1 << 5 + } + } + // Получаем глобальные координаты средних точек рёбер + point1.vTransformBy4(box1.transform); + point2.vTransformBy4(box2.transform); + // Находим точку пересечения рёбер, решая систему уравнений + var k:Number = axis10.vDot(axis20); + var det:Number = k*k - 1; + vector.vDiff(point2, point1); + var c1:Number = axis10.vDot(vector); + var c2:Number = axis20.vDot(vector); + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.normal.vCopy(colAxis); + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(point1.x + axis10.x*t1 + point2.x + axis20.x*t2); + cp.pos.y = 0.5*(point1.y + axis10.y*t1 + point2.y + axis20.y*t2); + cp.pos.z = 0.5*(point1.z + axis10.z*t1 + point2.z + axis20.z*t2); + cp.r1.vDiff(cp.pos, pos1); + cp.r2.vDiff(cp.pos, pos2); + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение боксов вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box1 + * @param box2 + * @param axis + * @param axisIndex + * @param vectorToBox1 + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, axisIndex:int, vectorToBox1:Vector3):Boolean { + if (axis.vLengthSqr() < 0.0001) { + return true; + } + axis.vNormalize(); + + var overlap:Number = overlapOnAxis(box1, box2, axis, vectorToBox1); + if (overlap < -tolerance) return false; + if (overlap + tolerance < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия двух боксов вдоль заданной оси. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param axis ось + * @param vectorToBox1 вектор, соединяющий центр второго бокса с центром первого + * @return величина перекрытия боксов вдоль оси + */ + public function overlapOnAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, vectorToBox1:Vector3):Number { + var m:Matrix4 = box1.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box1.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box1.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box1.hs.z; + if (d < 0) d = -d; + projection += d; + + m = box2.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box2.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box2.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box2.hs.z; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox1.x*axis.x + vectorToBox1.y*axis.y + vectorToBox1.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} + import alternativa.physics.types.Vector3; + + class CollisionPointTmp { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + } diff --git a/0.0.5.0/src/alternativa/physics/collision/BoxPlaneCollider.as b/0.0.5.0/src/alternativa/physics/collision/BoxPlaneCollider.as new file mode 100644 index 0000000..9817aac --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/BoxPlaneCollider.as @@ -0,0 +1,87 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + public class BoxPlaneCollider implements ICollider { + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var normal:Vector3 = new Vector3(); + + public function BoxPlaneCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var box:RigidBox = body1 as RigidBox; +// var plane:RigidPlane; +// if (box == null) { +// box = body2 as RigidBox; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные координаты вершин бокса +// var sx:Number = box.halfSize.x; +// var sy:Number = box.halfSize.y; +// var sz:Number = box.halfSize.z; +// (verts1[0] as Vector3).reset(-sx, -sy, -sz); +// (verts1[1] as Vector3).reset(sx, -sy, -sz); +// (verts1[2] as Vector3).reset(sx, sy, -sz); +// (verts1[3] as Vector3).reset(-sx, sy, -sz); +// (verts1[4] as Vector3).reset(-sx, -sy, sz); +// (verts1[5] as Vector3).reset(sx, -sy, sz); +// (verts1[6] as Vector3).reset(sx, sy, sz); +// (verts1[7] as Vector3).reset(-sx, sy, sz); +// +// box.transform.transformVectors(verts1, verts2); +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// // Проверяем наличие столкновений с каждой вершиной +// collisionInfo.pcount = 0; +// for (var i:int = 0; i < 8; i++) { +// // Вершина добавляется в список точек столкновения, если лежит под плоскостью +// var dist:Number = (verts2[i] as Vector3).dot(normal); +// if (dist < offset) { +// var cp:ContactPoint; +// if (collisionInfo.pcount == collisionInfo.points.length) { +// cp = new ContactPoint(); +// collisionInfo.points[collisionInfo.pcount] = cp; +// } else { +// cp = collisionInfo.points[collisionInfo.pcount]; +// } +// cp.pos.copy(verts2[i]); +// cp.r1.diff(cp.pos, box.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); +// cp.penetration = offset - dist; +// collisionInfo.pcount++; +// } +// } +// if (collisionInfo.pcount > 0) { +// collisionInfo.body1 = box; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// return true; +// } + return false; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/BoxRectCollider.as b/0.0.5.0/src/alternativa/physics/collision/BoxRectCollider.as new file mode 100644 index 0000000..dafaec7 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/BoxRectCollider.as @@ -0,0 +1,750 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + /** + * + */ + public class BoxRectCollider implements ICollider { + + private var epsilon:Number = 0.001; + + private var bPos:Vector3 = new Vector3(); + private var rPos:Vector3 = new Vector3(); + + private var vectorToBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var colAxis:Vector3 = new Vector3(); + private var tmpAxis:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxRectCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + minOverlap = 1e10; + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + + // Вектор из центра прямоугольника в центр бокса + box.transform.getAxis(3, bPos); + rect.transform.getAxis(3, rPos); + vectorToBox.vDiff(bPos, rPos); + + // Проверка пересечения по нормали прямоугольника + rect.transform.getAxis(2, axis22); + if (!testAxis(box, rect, axis22, 0, vectorToBox)) return false; + + // Проверка пересечения по основным осям бокса + box.transform.getAxis(0, axis10); + if (!testAxis(box, rect, axis10, 1, vectorToBox)) return false; + box.transform.getAxis(1, axis11); + if (!testAxis(box, rect, axis11, 2, vectorToBox)) return false; + box.transform.getAxis(2, axis12); + if (!testAxis(box, rect, axis12, 3, vectorToBox)) return false; + + // Получаем направляющие рёбер прямоугольника + rect.transform.getAxis(0, axis20); + rect.transform.getAxis(1, axis21); + + // Проверка производных осей + if (!testAxis(box, rect, axis.vCross2(axis10, axis20), 4, vectorToBox)) return false; + if (!testAxis(box, rect, axis.vCross2(axis10, axis21), 5, vectorToBox)) return false; + + if (!testAxis(box, rect, axis.vCross2(axis11, axis20), 6, vectorToBox)) return false; + if (!testAxis(box, rect, axis.vCross2(axis11, axis21), 7, vectorToBox)) return false; + + if (!testAxis(box, rect, axis.vCross2(axis12, axis20), 8, vectorToBox)) return false; + if (!testAxis(box, rect, axis.vCross2(axis12, axis21), 9, vectorToBox)) return false; + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, rect, vectorToBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + findEdgesIntersection(box, rect, vectorToBox, int(bestAxisIndex/2), bestAxisIndex%2, contact); + } + contact.body1 = box.body; + contact.body2 = rect.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + /** + * Выполняет поиск точек контакта бокса с прямоугольником. + * + * @param box бокс + * @param rect прямоугольник + * @param vectorToBox вектор, направленный из центра прямоугольника в центр бокса + * @param faceAxisIdx индекс оси, идентифицирующей полскость столкновения (грань бокса или полскость прямоугольника) + * @param colInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, faceAxisIdx:int, colInfo:Contact):Boolean { + var pnum:int, i:int, v:Vector3, cp:ContactPoint; + if (faceAxisIdx == 0) { + // Столкновение с плоскостью прямоугольника + + // Проверим положение бокса относительно плоскости прямоугольника + rect.transform.getAxis(2, colAxis); + var offset:Number = colAxis.vDot(rPos); +// if (bPos.vDot(colAxis) < offset) return false; + + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + box.transform.getAxis(axisIdx, axis); + var dot:Number = axis.vDot(colAxis); + if (dot < 0) dot = -dot; + if (dot > maxDot) { + maxDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Получаем список вершин грани бокса, переводим их в систему координат прямоугольника и выполняем обрезку + // по прямоугольнику. Таким образом получается список потенциальных точек контакта. + box.transform.getAxis(incFaceAxisIdx, axis); + getFaceVertsByAxis(box.hs, incFaceAxisIdx, axis.vDot(colAxis) > 0, verts1); + box.transform.transformVectors(verts1, verts2); + rect.transform.transformVectorsInverse(verts2, verts1); + pnum = clipByRect(rect.hs); + // Проверяем каждую потенциальную точку на принадлежность нижней полуплоскости прямоугольника и добавляем такие точки в список контактов + colInfo.pcount = 0; + for (i = 0; i < pnum; i++) { + v = verts1[i]; + if (v.z < epsilon) { + cp = colInfo.points[colInfo.pcount++]; + rect.transform.transformVector(v, cp.pos); + cp.r1.vDiff(cp.pos, bPos); + cp.r2.vDiff(cp.pos, rPos); + cp.penetration = -v.z; + } + } + colInfo.normal.vCopy(colAxis); + } else { + // Столкновение с гранью бокса + faceAxisIdx--; + box.transform.getAxis(faceAxisIdx, colAxis); + var faceReversed:Boolean = colAxis.vDot(vectorToBox) > 0; + if (!faceReversed) colAxis.vReverse(); + + rect.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) return false; + + getFaceVertsByAxis(rect.hs, 2, false, verts1); + rect.transform.transformVectors(verts1, verts2); + box.transform.transformVectorsInverse(verts2, verts1); + pnum = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + colInfo.pcount = 0; + for (i = 0; i < pnum; i++) { + if ((pen = getPointBoxPenetration(box.hs, verts1[i], faceAxisIdx, faceReversed)) > -epsilon) { + cp = colInfo.points[colInfo.pcount++]; + box.transform.transformVector(verts1[i], cp.pos); + cp.r1.vDiff(cp.pos, bPos); + cp.r2.vDiff(cp.pos, rPos); + cp.penetration = pen; + } + } + colInfo.normal.vCopy(colAxis); + } + return true; + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + private function getFaceVertsByAxis(hs:Vector3, axisIdx:int, reverse:Boolean, result:Vector.):void { + switch (axisIdx) { + case 0: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, -hs.y, hs.z); + } + break; + case 1: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, hs.y, hs.z); + } + break; + case 2: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + } else { + (result[0] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } + break; + } + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + return clipHighX(hs.x, pnum, verts2, verts1); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + } + return 0; + } + + /** + * + * @param hs + * @return + */ + private function clipByRect(hs:Vector3):int { + var pnum:int = 4; + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box первый бокс + * @param rect второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param colInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, axisIdx1:int, axisIdx2:int, colInfo:Contact):void { + box.transform.getAxis(axisIdx1, axis10); + rect.transform.getAxis(axisIdx2, axis20); + colAxis.vCross2(axis10, axis20).vNormalize(); + // Разворот оси в сторону бокса + if (colAxis.vDot(vectorToBox) < 0) colAxis.vReverse(); + + /* Кодирование рёбер + бит 0: 1 (тип контакта ребро-ребро) + биты 1-2: индекс направляющей оси ребра + бит 3: + бит 4: + бит 5: + */ + var edgeCode1:int = 1; + var edgeCode2:int = 1; + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + point1.vCopy(box.hs); + point2.vCopy(rect.hs); + // x + if (axisIdx1 == 0) { + point1.x = 0; + halfLen1 = box.hs.x; + } else { + box.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.x = -point1.x; + edgeCode1 |= 8; // 1 << 3 + } + } + if (axisIdx2 == 0) { + point2.x = 0; + halfLen2 = rect.hs.x; + } else { + rect.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.x = -point2.x; + edgeCode2 |= 8; // 1 << 3 + } + } + // y + if (axisIdx1 == 1) { + point1.y = 0; + halfLen1 = box.hs.y; + edgeCode1 |= 2; // 1 << 1 + } else { + box.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.y = -point1.y; + edgeCode1 |= 16; // 1 << 4 + } + } + if (axisIdx2 == 1) { + point2.y = 0; + halfLen2 = rect.hs.y; + edgeCode2 |= 2; // 1 << 1 + } else { + rect.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.y = -point2.y; + edgeCode2 |= 16; // 1 << 4 + } + } + // z + if (axisIdx1 == 2) { + point1.z = 0; + halfLen1 = box.hs.z; + edgeCode1 |= 4; // 2 << 1 + } else { + box.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.z = -point1.z; + edgeCode1 |= 32; // 1 << 5 + } + } + // Получаем глобальные координаты средних точек рёбер + point1.vTransformBy4(box.transform); + point2.vTransformBy4(rect.transform); + // Находим точку пересечения рёбер, решая систему уравнений + var k:Number = axis10.vDot(axis20); + var det:Number = k*k - 1; + vector.vDiff(point2, point1); + var c1:Number = axis10.vDot(vector); + var c2:Number = axis20.vDot(vector); + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + colInfo.normal.vCopy(colAxis); + colInfo.pcount = 1; + var cp:ContactPoint = colInfo.points[0]; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(point1.x + axis10.x*t1 + point2.x + axis20.x*t2); + cp.pos.y = 0.5*(point1.y + axis10.y*t1 + point2.y + axis20.y*t2); + cp.pos.z = 0.5*(point1.z + axis10.z*t1 + point2.z + axis20.z*t2); + cp.r1.vDiff(cp.pos, bPos); + cp.r2.vDiff(cp.pos, rPos); + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box + * @param rect + * @param axis + * @param axisIndex + * @param vectorToBox + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + if (axis.vLengthSqr() < 0.0001) return true; + axis.vNormalize(); + + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия вдоль заданной оси. + * + * @param box бокс + * @param rect прямоугольник + * @param axis ось + * @param vectorToBox вектор, соединяющий центр прямоугольника с центром бокса + * @return величина перекрытия вдоль оси + */ + public function overlapOnAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, vectorToBox:Vector3):Number { + var m:Matrix4 = box.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box.hs.z; + if (d < 0) d = -d; + projection += d; + + m = rect.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*rect.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*rect.hs.y; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox.x*axis.x + vectorToBox.y*axis.y + vectorToBox.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/BoxSphereCollider.as b/0.0.5.0/src/alternativa/physics/collision/BoxSphereCollider.as new file mode 100644 index 0000000..27ae562 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/BoxSphereCollider.as @@ -0,0 +1,113 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + use namespace altphysics; + /** + * + */ + public class BoxSphereCollider implements ICollider { + + private var center:Vector3 = new Vector3(); + private var closestPt:Vector3 = new Vector3(); + + private var bPos:Vector3 = new Vector3(); + private var sPos:Vector3 = new Vector3(); + + /** + * + */ + public function BoxSphereCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + // Зафиксированно столкновение + contact.body1 = sphere.body; + contact.body2 = box.body; + contact.normal.vCopy(closestPt).vTransformBy4(box.transform).vSubtract(sPos).vNormalize().vReverse(); + contact.pcount = 1; + + var cp:ContactPoint = contact.points[0]; + cp.penetration = sphere.r - Math.sqrt(distSqr); + cp.pos.vCopy(contact.normal).vScale(-sphere.r).vAdd(sPos); + cp.r1.vDiff(cp.pos, sPos); + cp.r2.vDiff(cp.pos, bPos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/BoxTriangleCollider.as b/0.0.5.0/src/alternativa/physics/collision/BoxTriangleCollider.as new file mode 100644 index 0000000..5167133 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/BoxTriangleCollider.as @@ -0,0 +1,871 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionTriangle; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + + /** + * + */ + public class BoxTriangleCollider implements ICollider { + + public var epsilon:Number = 0.001; + + private var bestAxisIndex:int; + private var minOverlap:Number; + private var toBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var colNormal:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxTriangleCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + var mTri:Matrix4 = tri.transform; + var mBox:Matrix4 = box.transform; + + toBox.x = mBox.d - mTri.d; + toBox.y = mBox.h - mTri.h; + toBox.z = mBox.l - mTri.l; + + minOverlap = 1e308; + + // Сначала проверяется нормаль треугольника + axis.x = mTri.d; + axis.y = mTri.h; + axis.z = mTri.l; + if (!testAxis(box, tri, axis, 0, toBox)) return false; + + // Проверка основных осей бокса + axis10.x = mBox.a; + axis10.y = mBox.e; + axis10.z = mBox.i; + if (!testAxis(box, tri, axis10, 1, toBox)) return false; + axis11.x = mBox.b; + axis11.y = mBox.f; + axis11.z = mBox.j; + if (!testAxis(box, tri, axis11, 2, toBox)) return false; + axis12.x = mBox.c; + axis12.y = mBox.g; + axis12.z = mBox.k; + if (!testAxis(box, tri, axis10, 3, toBox)) return false; + + // Проверка производных осей + // TODO: заменить вычисления векторных произведений инлайнами + var v:Vector3 = tri.e0; + axis20.x = mTri.a*v.x + mTri.b*v.y + mTri.c*v.z; + axis20.y = mTri.e*v.x + mTri.f*v.y + mTri.g*v.z; + axis20.z = mTri.i*v.x + mTri.j*v.y + mTri.k*v.z; + if (!testAxis(box, tri, axis.vCross2(axis10, axis20), 4, toBox)) return false; + if (!testAxis(box, tri, axis.vCross2(axis11, axis20), 5, toBox)) return false; + if (!testAxis(box, tri, axis.vCross2(axis12, axis20), 6, toBox)) return false; + + v = tri.e1; + axis21.x = mTri.a*v.x + mTri.b*v.y + mTri.c*v.z; + axis21.y = mTri.e*v.x + mTri.f*v.y + mTri.g*v.z; + axis21.z = mTri.i*v.x + mTri.j*v.y + mTri.k*v.z; + if (!testAxis(box, tri, axis.vCross2(axis10, axis21), 7, toBox)) return false; + if (!testAxis(box, tri, axis.vCross2(axis11, axis21), 8, toBox)) return false; + if (!testAxis(box, tri, axis.vCross2(axis12, axis21), 9, toBox)) return false; + + v = tri.e2; + axis22.x = mTri.a*v.x + mTri.b*v.y + mTri.c*v.z; + axis22.y = mTri.e*v.x + mTri.f*v.y + mTri.g*v.z; + axis22.z = mTri.i*v.x + mTri.j*v.y + mTri.k*v.z; + if (!testAxis(box, tri, axis.vCross2(axis10, axis22), 10, toBox)) return false; + if (!testAxis(box, tri, axis.vCross2(axis11, axis22), 11, toBox)) return false; + if (!testAxis(box, tri, axis.vCross2(axis12, axis22), 12, toBox)) return false; + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, tri, toBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + findEdgesIntersection(box, tri, toBox, bestAxisIndex%3, int(bestAxisIndex/3), contact); + } + + contact.body1 = box.body; + contact.body2 = tri.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + /** + * Тестирует пересечение примитивов вдоль заданной оси. + * + * @param box бокс + * @param tri треугольник + * @param axis ось + * @param axisIndex индекс оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return true, если примитивы имеют перекрытие проекций вдоль указанной оси, иначе false + */ + private function testAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, axisIndex:int, toBox:Vector3):Boolean { + var len:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (len < 0.0001) return true; + len = 1/Math.sqrt(len); + axis.x *= len; + axis.y *= len; + axis.z *= len; + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Рассчитывает величину перекрытия проекций бокса и треугольника на заданную ось. + * + * @param box бокс + * @param tri треугольник + * @param axis единичный направляющий вектор оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return величина перекрытия. Положительное значение указывает на наличие перекрытия, нулевое или отрицательное значение указывает на отсутствие перекрытия. + */ + private function overlapOnAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, toBox:Vector3):Number { + var t:Matrix4 = box.transform; + var projection:Number = (t.a*axis.x + t.e*axis.y + t.i*axis.z)*box.hs.x; + if (projection < 0) projection = -projection; + var d:Number = (t.b*axis.x + t.f*axis.y + t.j*axis.z)*box.hs.y; + projection += d < 0 ? -d : d; + d = (t.c*axis.x + t.g*axis.y + t.k*axis.z)*box.hs.z; + projection += d < 0 ? -d : d; + + var vectorProjection:Number = toBox.x*axis.x + toBox.y*axis.y + toBox.z*axis.z; + // Для оптимизации ось преобразуется в систему треугольника, а не его вершины в мировую систему + t = tri.transform; + var ax:Number = t.a*axis.x + t.e*axis.y + t.i*axis.z; + var ay:Number = t.b*axis.x + t.f*axis.y + t.j*axis.z; + var az:Number = t.c*axis.x + t.g*axis.y + t.k*axis.z; + var max:Number = 0; + if (vectorProjection < 0) { + vectorProjection = -vectorProjection; + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d < max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d < max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d < max) max = d; + max = -max; + } else { + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d > max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d > max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d > max) max = d; + } + + return projection + max - vectorProjection; + } + + /** + * Определяет точки контакта бокса и треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function findFaceContactPoints(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIndex:int, contact:Contact):Boolean { + if (faceAxisIndex == 0) { + // Столкновение с плоскостью треугольника + getBoxToTriContact(box, tri, toBox, contact); + } else { + // Столкновение с гранью бокса + getTriToBoxContact(box, tri, toBox, faceAxisIndex, contact); + } + return true; + } + + /** + * + * @param box + * @param tri + * @param toBox + * @param contact + */ + private function getBoxToTriContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, contact:Contact):void { + tri.transform.getAxis(2, colNormal); + var over:Boolean = toBox.vDot(colNormal) > 0; + if (!over) colNormal.vReverse(); + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var incAxisDot:Number = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + box.transform.getAxis(axisIdx, axis); + var dot:Number = axis.vDot(colNormal); + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incAxisDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Обрезка грани + var negativeFace:Boolean = incAxisDot > 0; + getFaceVertsByAxis(box.hs, incFaceAxisIdx, negativeFace, points1); + box.transform.transformVectorsN(points1, points2, 4); + tri.transform.transformVectorsInverseN(points2, points1, 4); + var pnum:int = clipByTriangle(tri); + // Среди конечного списка точек определяются лежащие под плоскостью треугольника + var cp:ContactPoint; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points2[i]; + if ((over && v.z < 0) || (!over && v.z > 0)) { + cp = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + tri.transform.transformVector(v, cpPos); + var r:Vector3 = cp.r1; + r.x = cpPos.x - box.transform.d; + r.y = cpPos.y - box.transform.h; + r.z = cpPos.z - box.transform.l; + r = cp.r2; + r.x = cpPos.x - tri.transform.d; + r.y = cpPos.y - tri.transform.h; + r.z = cpPos.z - tri.transform.l; + cp.penetration = over ? -v.z : v.z; + } + } + if (contact.pcount > 0) { + contact.normal.vCopy(colNormal); + } + } + + /** + * + * @param box + * @param tri + * @param toBox + * @param faceAxisIndex + * @param contact + */ + private function getTriToBoxContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIdx:int, contact:Contact):void { + faceAxisIdx--; + box.transform.getAxis(faceAxisIdx, colNormal); + var negativeFace:Boolean = colNormal.vDot(toBox) > 0; + if (!negativeFace) { + colNormal.vReverse(); + } + Vector3(points1[0]).vCopy(tri.v0); + Vector3(points1[1]).vCopy(tri.v1); + Vector3(points1[2]).vCopy(tri.v2); + tri.transform.transformVectorsN(points1, points2, 3); + box.transform.transformVectorsInverseN(points2, points1, 3); + var pnum:int = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность боксу и добавляем такие точки в список контактов + var pen:Number; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points1[i]; + pen = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace); + if (pen > -epsilon) { + var cp:ContactPoint = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + box.transform.transformVector(v, cp.pos); + var r:Vector3 = cp.r1; + r.x = cpPos.x - box.transform.d; + r.y = cpPos.y - box.transform.h; + r.z = cpPos.z - box.transform.l; + r = cp.r2; + r.x = cpPos.x - tri.transform.d; + r.y = cpPos.y - tri.transform.h; + r.z = cpPos.z - tri.transform.l; + cp.penetration = pen; + } + } + contact.normal.vCopy(colNormal); + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, negativeFace:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (negativeFace) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (negativeFace) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (negativeFace) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, вершины которой заданы в списке points1. Результат сохраняется в этом же списке. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1); + } + return 0; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + Vector3(result[num++]).vCopy(p1); + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + Vector3(result[num++]).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + Vector3(result[num++]).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + Vector3(result[num++]).vCopy(p1); + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + Vector3(result[num++]).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + Vector3(result[num++]).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + Vector3(result[num++]).vCopy(p1); + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + Vector3(result[num++]).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + Vector3(result[num++]).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + Vector3(result[num++]).vCopy(p1); + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + Vector3(result[num++]).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + Vector3(result[num++]).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + Vector3(result[num++]).vCopy(p1); + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + Vector3(result[num++]).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + Vector3(result[num++]).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + Vector3(result[num++]).vCopy(p1); + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + Vector3(result[num++]).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + Vector3(result[num++]).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param negativeFace если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + private function getFaceVertsByAxis(hs:Vector3, axisIdx:int, negativeFace:Boolean, result:Vector.):void { + switch (axisIdx) { + case 0: + if (negativeFace) { + Vector3(result[0]).vReset(-hs.x, hs.y, -hs.z); + Vector3(result[1]).vReset(-hs.x, -hs.y, -hs.z); + Vector3(result[2]).vReset(-hs.x, -hs.y, hs.z); + Vector3(result[3]).vReset(-hs.x, hs.y, hs.z); + } else { + Vector3(result[0]).vReset(hs.x, -hs.y, -hs.z); + Vector3(result[1]).vReset(hs.x, hs.y, -hs.z); + Vector3(result[2]).vReset(hs.x, hs.y, hs.z); + Vector3(result[3]).vReset(hs.x, -hs.y, hs.z); + } + break; + case 1: + if (negativeFace) { + Vector3(result[0]).vReset(-hs.x, -hs.y, -hs.z); + Vector3(result[1]).vReset(hs.x, -hs.y, -hs.z); + Vector3(result[2]).vReset(hs.x, -hs.y, hs.z); + Vector3(result[3]).vReset(-hs.x, -hs.y, hs.z); + } else { + Vector3(result[0]).vReset(hs.x, hs.y, -hs.z); + Vector3(result[1]).vReset(-hs.x, hs.y, -hs.z); + Vector3(result[2]).vReset(-hs.x, hs.y, hs.z); + Vector3(result[3]).vReset(hs.x, hs.y, hs.z); + } + break; + case 2: + if (negativeFace) { + Vector3(result[0]).vReset(-hs.x, hs.y, -hs.z); + Vector3(result[1]).vReset(hs.x, hs.y, -hs.z); + Vector3(result[2]).vReset(hs.x, -hs.y, -hs.z); + Vector3(result[3]).vReset(-hs.x, -hs.y, -hs.z); + } else { + Vector3(result[0]).vReset(-hs.x, -hs.y, hs.z); + Vector3(result[1]).vReset(hs.x, -hs.y, hs.z); + Vector3(result[2]).vReset(hs.x, hs.y, hs.z); + Vector3(result[3]).vReset(-hs.x, hs.y, hs.z); + } + break; + } + } + + /** + * Обрезает грань прямоугольника сторонами треугольника. Обрезка выполняется в системе координат и в проекции на + * плоскость треугольника. Входные вершины грани в количестве четырёх штук должны находиться в списке points1. + * Конечные вершины сохраняются в списке points2. + * + * @param tri треугольник + * @return количество врешин в конечном списке + */ + private function clipByTriangle(tri:CollisionTriangle):int { + var vnum:int = 4; + vnum = clipByLine(tri.v0, tri.e0, points1, vnum, points2); + if (vnum == 0) return 0; + vnum = clipByLine(tri.v1, tri.e1, points2, vnum, points1); + if (vnum == 0) return 0; + return clipByLine(tri.v2, tri.e2, points1, vnum, points2);; + } + + /** + * Обрезает полигон указанной прямой. + * + * @param linePoint точка на прямой + * @param lineDir единичный направляющий вектор прямой + * @param verticesIn список вершин исходного полигона + * @param vnum количество вершин исходного полигона + * @param verticesOut список, куда будут записаны вершины конечного полигона + * @return количество вершин конечного полигона + */ + private function clipByLine(linePoint:Vector3, lineDir:Vector3, verticesIn:Vector., vnum:int, verticesOut:Vector.):int { + var nx:Number = lineDir.y; + var ny:Number = -lineDir.x; + var offset:Number = linePoint.x*nx + linePoint.y*ny; + var v1:Vector3 = verticesIn[vnum -1]; + var offset1:Number = v1.x*nx + v1.y*ny; + var t:Number; + var v:Vector3; + var num:int = 0; + for (var i:int = 0; i < vnum; i++) { + var v2:Vector3 = verticesIn[i]; + var offset2:Number = v2.x*nx + v2.y*ny; + if (offset1 < offset) { + // Первая точка ребра во внешней полуплоскости + if (offset2 > offset) { + // Вторая точка ребра во внутренней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } else { + // Первая точка ребра во внутренней полуплоскости. Добавляем её в конечный список. + v = verticesOut[num]; + v.x = v1.x; + v.y = v1.y; + v.z = v1.z; + num++; + if (offset2 < offset) { + // Вторая точка ребра во внешней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } + v1 = v2; + offset1 = offset2; + } + return num; + } + + /** + * + * @param box + * @param tri + * @param toBox + * @param boxAxisIdx + * @param triAxisIdx + * @param contact + */ + private function findEdgesIntersection(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, boxAxisIdx:int, triAxisIdx:int, contact:Contact):void { + box.transform.getAxis(boxAxisIdx, axis10); + point1.vCopy(box.hs); + var boxHalfLen:Number; + // X + if (boxAxisIdx == 0) { + point1.x = 0; + boxHalfLen = box.hs.x; + } else { + box.transform.getAxis(0, axis11); + if (axis11.vDot(toBox) > 0) { + point1.x = -point1.x; + } + } + // Y + if (boxAxisIdx == 1) { + point1.y = 0; + boxHalfLen = box.hs.y; + } else { + box.transform.getAxis(1, axis11); + if (axis11.vDot(toBox) > 0) { + point1.y = -point1.y; + } + } + // Z + if (boxAxisIdx == 2) { + point1.z = 0; + boxHalfLen = box.hs.z; + } else { + box.transform.getAxis(2, axis11); + if (axis11.vDot(toBox) > 0) { + point1.z = -point1.z; + } + } + point1.vTransformBy4(box.transform); + + var triLen:Number; + switch (triAxisIdx) { + case 0: + point2.vCopy(tri.v0); + axis20.vCopy(tri.e0); + triLen = tri.len0; + break; + case 1: + point2.vCopy(tri.v1); + axis20.vCopy(tri.e1); + triLen = tri.len1; + break; + case 2: + point2.vCopy(tri.v2); + axis20.vCopy(tri.e2); + triLen = tri.len2; + break; + } + axis20.vDeltaTransformBy4(tri.transform); + point2.vTransformBy4(tri.transform); + + colNormal.vCross2(axis10, axis20).vNormalize(); + // Разворот оси в сторону бокса + if (colNormal.vDot(toBox) < 0) colNormal.vReverse(); + + // Находим точку пересечения рёбер, решая систему уравнений + // axis10 - направляющий вектор ребра бокса + // point1 - средняя точка ребра бокса + // axis20 - направляющий вектор ребра треугольника + // point2 - начальная точка ребра треугольника + var k:Number = axis10.vDot(axis20); + var det:Number = k*k - 1; + vector.vDiff(point2, point1); + var c1:Number = axis10.vDot(vector); + var c2:Number = axis20.vDot(vector); + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.normal.vCopy(colNormal); + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cpPos.x = 0.5*(point1.x + axis10.x*t1 + point2.x + axis20.x*t2); + cpPos.y = 0.5*(point1.y + axis10.y*t1 + point2.y + axis20.y*t2); + cpPos.z = 0.5*(point1.z + axis10.z*t1 + point2.z + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = cpPos.x - box.transform.d; + r.y = cpPos.y - box.transform.h; + r.z = cpPos.z - box.transform.l; + r = cp.r2; + r.x = cpPos.x - tri.transform.d; + r.y = cpPos.y - tri.transform.h; + r.z = cpPos.z - tri.transform.l; + cp.penetration = minOverlap; + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/BruteForceCollisionDetector.as b/0.0.5.0/src/alternativa/physics/collision/BruteForceCollisionDetector.as new file mode 100644 index 0000000..85d84fa --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/BruteForceCollisionDetector.as @@ -0,0 +1,63 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + public class BruteForceCollisionDetector implements ICollisionDetector { + public function BruteForceCollisionDetector() { + } + + public function addPrimitive(primitive:CollisionPrimitive, isStatic:Boolean=true):Boolean { + return false; + } + + public function removePrimitive(primitive:CollisionPrimitive, isStatic:Boolean=true):Boolean { + return false; + } + + public function init():void { + } + + public function getAllCollisions(contacts:Vector.):int { + return 0; + } + + public function intersectRay(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, intersection:RayIntersection):Boolean { + return false; + } + + public function intersectRayWithStatic(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean { + return false; + } + + public function testBodyPrimitiveCollision(body:Body, primitive:CollisionPrimitive):Boolean { + return false; + } + + /** + * + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + return false; + } + + /** + * + * @param prim1 + * @param prim2 + * @return + */ + public function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/CollisionKdNode.as b/0.0.5.0/src/alternativa/physics/collision/CollisionKdNode.as new file mode 100644 index 0000000..e24922c --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/CollisionKdNode.as @@ -0,0 +1,18 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.types.BoundBox; + + public class CollisionKdNode { + public var indices:Vector.; + public var splitIndices:Vector.; + public var boundBox:BoundBox; + public var parent:CollisionKdNode; + public var splitTree:CollisionKdTree2D; + + public var axis:int = -1; // 0 - x, 1 - y, 2 - z + public var coord:Number; + public var positiveNode:CollisionKdNode; + public var negativeNode:CollisionKdNode; + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/CollisionKdTree.as b/0.0.5.0/src/alternativa/physics/collision/CollisionKdTree.as new file mode 100644 index 0000000..c046f17 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/CollisionKdTree.as @@ -0,0 +1,280 @@ +package alternativa.physics.collision { + + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + + /** + * @author mike + */ + public class CollisionKdTree { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + public var rootNode:CollisionKdNode; + public var staticChildren:Vector. = new Vector.(); + public var numStaticChildren:int; + public var staticBoundBoxes:Vector. = new Vector.(); + + private var splitAxis:int; + private var splitCoord:Number; + private var splitCost:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + + /** + * @param primitive + */ + public function addStaticPrimitive(primitive:CollisionPrimitive):void { + staticChildren[numStaticChildren++] = primitive; + } + + /** + * @param primitive + * @return + */ + public function removeStaticPrimitive(primitive:CollisionPrimitive):Boolean { + var idx:int = staticChildren.indexOf(primitive); + if (idx < 0) return false; + staticChildren.splice(idx, 1); + numStaticChildren--; + return true; + } + + /** + * @param boundBox + */ + public function createTree(boundBox:BoundBox = null):void { + if (numStaticChildren == 0) return; + // Создаём корневую ноду + rootNode = new CollisionKdNode(); + rootNode.indices = new Vector.(); + // Расчитываем баунды объектов и рутовой ноды + var rootNodeBoundBox:BoundBox = rootNode.boundBox = (boundBox != null) ? boundBox : new BoundBox(); + for (var i:int = 0; i < numStaticChildren; ++i) { + var child:CollisionPrimitive = staticChildren[i]; + var childBoundBox:BoundBox = staticBoundBoxes[i] = child.calculateAABB(); + rootNodeBoundBox.addBoundBox(childBoundBox); + rootNode.indices[i] = i; + } + staticBoundBoxes.length = numStaticChildren; + // Разделяем рутовую ноду + splitNode(rootNode); + + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + var indices:Vector. = node.indices; + var numPrimitives:int = indices.length; + if (numPrimitives <= minPrimitivesPerNode) return; + + // Подготовка баунда с погрешностями + var nodeBoundBox:BoundBox = node.boundBox; + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + // Собираем опорные координаты + var i:int; + var j:int; + var numSplitCoordsX:int = 0 + var numSplitCoordsY:int = 0; + var numSplitCoordsZ:int = 0; + for (i = 0; i < numPrimitives; ++i) { + var boundBox:BoundBox = staticBoundBoxes[indices[i]]; + + if (boundBox.maxX - boundBox.minX <= doubleThreshold) { + if (boundBox.minX <= nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.minX; + else if (boundBox.maxX >= nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.maxX; + else splitCoordsX[numSplitCoordsX++] = (boundBox.minX + boundBox.maxX)*0.5; + } else { + if (boundBox.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = boundBox.minX; + if (boundBox.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = boundBox.maxX; + } + + if (boundBox.maxY - boundBox.minY <= doubleThreshold) { + if (boundBox.minY <= nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.minY; + else if (boundBox.maxY >= nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.maxY; + else splitCoordsY[numSplitCoordsY++] = (boundBox.minY + boundBox.maxY)*0.5; + } else { + if (boundBox.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = boundBox.minY; + if (boundBox.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = boundBox.maxY; + } + + if (boundBox.maxZ - boundBox.minZ <= doubleThreshold) { + if (boundBox.minZ <= nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.minZ; + else if (boundBox.maxZ >= nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.maxZ; + else splitCoordsZ[numSplitCoordsZ++] = (boundBox.minZ + boundBox.maxZ)*0.5; + } else { + if (boundBox.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.minZ; + if (boundBox.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numPrimitives; ++i) { + boundBox = staticBoundBoxes[indices[i]]; + var min:Number = axisX ? boundBox.minX : (axisY ? boundBox.minY : boundBox.minZ); + var max:Number = axisX ? boundBox.maxX : (axisY ? boundBox.maxY : boundBox.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(indices[i]); + indices[i] = -1; + } else { + if (node.splitIndices == null) node.splitIndices = new Vector.(); + node.splitIndices.push(indices[i]); + indices[i] = -1; + } + } else { + if (min >= coordMin) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(indices[i]); + indices[i] = -1; + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numPrimitives; ++i) { + if (indices[i] >= 0) indices[j++] = indices[i]; + } + if (j > 0) indices.length = j; + else node.indices = null; + + if (node.splitIndices != null) { + node.splitTree = new CollisionKdTree2D(this, node); + node.splitTree.createTree(); + } + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + /** + * + */ + public function traceTree():void { + traceNode("", rootNode); + } + + /** + * @param str + * @param node + */ + private function traceNode(str:String, node:CollisionKdNode):void { + if (node == null) return; + trace(str, node.axis == -1 ? "end" : ((node.axis == 0) ? "X" : ((node.axis == 1) ? "Y" : "Z")), "splitCoord=" + splitCoord, "bound", node.boundBox, "objs:", node.indices); + traceNode(str + "-", node.negativeNode); + traceNode(str + "+", node.positiveNode); + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/CollisionKdTree2D.as b/0.0.5.0/src/alternativa/physics/collision/CollisionKdTree2D.as new file mode 100644 index 0000000..78aaa95 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/CollisionKdTree2D.as @@ -0,0 +1,226 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.types.BoundBox; + + /** + * + */ + public class CollisionKdTree2D { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + + public var parentTree:CollisionKdTree; + public var parentNode:CollisionKdNode; + public var rootNode:CollisionKdNode; + + private var splitAxis:int; + private var splitCost:Number; + private var splitCoord:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + /** + * + * @param parentTree + * @param parentNode + */ + public function CollisionKdTree2D(parentTree:CollisionKdTree, parentNode:CollisionKdNode) { + this.parentTree = parentTree; + this.parentNode = parentNode; + } + + /** + * + */ + public function createTree():void { + rootNode = new CollisionKdNode(); + rootNode.boundBox = parentNode.boundBox.clone(); + rootNode.indices = new Vector.(); + var numObjects:int = parentNode.splitIndices.length; + for (var i:int = 0; i < numObjects; ++i) rootNode.indices[i] = parentNode.splitIndices[i]; + + splitNode(rootNode); + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + if (node.indices.length <= minPrimitivesPerNode) return; + + var objects:Vector. = node.indices; + var i:int; + var j:int; + + var nodeBoundBox:BoundBox = node.boundBox; + + // Подготовка баунда с погрешностями + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + // Собираем опорные координаты + var numSplitCoordsX:int; + var numSplitCoordsY:int; + var numSplitCoordsZ:int; + var numObjects:int = objects.length; + for (i = 0; i < numObjects; ++i) { + var bb:BoundBox = staticBoundBoxes[objects[i]]; + + if (parentNode.axis != 0) { + if (bb.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = bb.minX; + if (bb.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = bb.maxX; + } + + if (parentNode.axis != 1) { + if (bb.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = bb.minY; + if (bb.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = bb.maxY; + } + + if (parentNode.axis != 2) { + if (bb.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = bb.minZ; + if (bb.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = bb.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + if (parentNode.axis != 0) checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + if (parentNode.axis != 1) checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + if (parentNode.axis != 2) checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numObjects; ++i) { + bb = staticBoundBoxes[objects[i]]; + var min:Number = axisX ? bb.minX : (axisY ? bb.minY : bb.minZ); + var max:Number = axisX ? bb.maxX : (axisY ? bb.maxY : bb.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(objects[i]); + objects[i] = -1; + } + } else { + if (min >= coordMin) { + if (max > coordMax) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(objects[i]); + objects[i] = -1; + } + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numObjects; ++i) { + if (objects[i] >= 0) objects[j++] = objects[i]; + } + if (j > 0) objects.length = j; + else node.indices = null; + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/IBodyCollisionPredicate.as b/0.0.5.0/src/alternativa/physics/collision/IBodyCollisionPredicate.as new file mode 100644 index 0000000..9cfdca5 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/IBodyCollisionPredicate.as @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Body; + + public interface IBodyCollisionPredicate { + function considerBodies(body1:Body, body2:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/ICollider.as b/0.0.5.0/src/alternativa/physics/collision/ICollider.as new file mode 100644 index 0000000..220e717 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/ICollider.as @@ -0,0 +1,30 @@ +package alternativa.physics.collision { + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + + /** + * Интерфейс определителя столкновений между двумя примитивами. + */ + public interface ICollider { + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/ICollisionDetector.as b/0.0.5.0/src/alternativa/physics/collision/ICollisionDetector.as new file mode 100644 index 0000000..af43dfb --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/ICollisionDetector.as @@ -0,0 +1,92 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + /** + * Интерфейс детектора столкновений. + */ + public interface ICollisionDetector { + + /** + * Добавляет физический примитив в коллайдер. + * + * @param primitive добавляемый примитив + * @param isStatic указывает тип примитива: статический или динамический + * @return true если примитив был успешно добавлен, иначе false + */ + function addPrimitive(primitive:CollisionPrimitive, isStatic:Boolean = true):Boolean; + + /** + * Удаляет физический примитив из коллайдера. + * + * @param primitive удаляемый примитив + * @param isStatic указывает тип примитива: статический или динамический + * @return true если примитив был успшено удалён, иначе false + */ + function removePrimitive(primitive:CollisionPrimitive, isStatic:Boolean = true):Boolean; + + /** + * Выполняет инициализацию детектора после обновления списка примитивов. + */ + function init():void; + + /** + * Получает все столкновения в текущей конфигурации физической геометрии. + * + * @param contacts список контактов, в кторые будет записана информация о столкновении + * @return количество найденных столкновений + */ + function getAllCollisions(contacts:Vector.):int; + + /** + * Тестирует луч на пересечение с физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param dir направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRay(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * Тестирует луч на пересечение со статической физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param dir направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRayWithStatic(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @return + */ + function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/ICollisionPredicate.as b/0.0.5.0/src/alternativa/physics/collision/ICollisionPredicate.as new file mode 100644 index 0000000..bb1b756 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/ICollisionPredicate.as @@ -0,0 +1,9 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + + public interface ICollisionPredicate { + + function considerCollision(primitive:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/IRayCollisionPredicate.as b/0.0.5.0/src/alternativa/physics/collision/IRayCollisionPredicate.as new file mode 100644 index 0000000..0f3d929 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/IRayCollisionPredicate.as @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Body; + + public interface IRayCollisionPredicate { + function considerBody(body:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/KdTreeCollisionDetector.as b/0.0.5.0/src/alternativa/physics/collision/KdTreeCollisionDetector.as new file mode 100644 index 0000000..7b0b78e --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/KdTreeCollisionDetector.as @@ -0,0 +1,484 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Детектор, хранящий статическую геометрию в kD-дереве и использующий дерево для ускорения тестов на пересечения. + */ + public class KdTreeCollisionDetector implements ICollisionDetector { + + altphysics var tree:CollisionKdTree; + altphysics var dynamicPrimitives:Vector.; + 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.(); + + 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():void { + tree.createTree(); +// tree.traceTree(); + } + + /** + * + * @param contacts + * @return + */ + public function getAllCollisions(contacts:Vector.):int { + var colNum:int = 0; + for (var i:int = 0; i < dynamicPrimitivesNum; i++) { + var primitive:CollisionPrimitive = dynamicPrimitives[i]; + primitive.calculateAABB(); + if (primitive.body != null && primitive.body.frozen) continue; + colNum += getPrimitiveNodeCollisions(tree.rootNode, primitive, contacts, colNum); + + // Столкновения динамических примитивов между собой + // TODO: Попробовать различные оптимизации (сортировка по баундам, встраивание в дерево) + for (var j:int = i + 1; j < dynamicPrimitivesNum; j++) { + if (collide(primitive, dynamicPrimitives[j], contacts[colNum])) colNum++; + } + } + + return colNum; + } + + /** + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + public function collide(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 = (contact.points[0] as ContactPoint).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:Vector., startIndex:int):int { + var colNum:int = 0; + if (node.indices != null) { + // Поиск столкновений со статическими примитивами узла + var primitives:Vector. = tree.staticChildren; + var indices:Vector. = node.indices; + for (var i:int = indices.length - 1; i >= 0; i--) + if (collide(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; + } + + 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., indices:Vector., 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; + } \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/SpherePlaneCollider.as b/0.0.5.0/src/alternativa/physics/collision/SpherePlaneCollider.as new file mode 100644 index 0000000..77eb1b0 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/SpherePlaneCollider.as @@ -0,0 +1,67 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + /** + * + */ + public class SpherePlaneCollider implements ICollider { + + private var normal:Vector3 = new Vector3(); + + /** + * + */ + public function SpherePlaneCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var sphere:RigidSphere = body1 as RigidSphere; +// var plane:RigidPlane; +// if (sphere == null) { +// sphere = body2 as RigidSphere; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// +// var dist:Number = sphere.state.pos.dot(normal) - offset; +// if (dist > sphere.r) return false; +// +// collisionInfo.body1 = sphere; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// collisionInfo.pcount = 1; +// +// var cp:ContactPoint = collisionInfo.points[0]; +// cp.penetration = sphere.r - dist; +// cp.pos.copy(normal).reverse().scale(sphere.r).add(sphere.state.pos); +// cp.r1.diff(cp.pos, sphere.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/SphereSphereCollider.as b/0.0.5.0/src/alternativa/physics/collision/SphereSphereCollider.as new file mode 100644 index 0000000..25971d5 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/SphereSphereCollider.as @@ -0,0 +1,69 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + public class SphereSphereCollider implements ICollider { + + private var p1:Vector3 = new Vector3(); + private var p2:Vector3 = new Vector3(); + + public function SphereSphereCollider() { + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var s1:CollisionSphere; + var s2:CollisionSphere; + if (prim1.body != null) { + s1 = prim1 as CollisionSphere; + s2 = prim2 as CollisionSphere; + } else { + s1 = prim2 as CollisionSphere; + s2 = prim1 as CollisionSphere; + } + + s1.transform.getAxis(3, p1); + s2.transform.getAxis(3, p2); + var dx:Number = p1.x - p2.x; + var dy:Number = p1.y - p2.y; + var dz:Number = p1.z - p2.z; + var len:Number = dx*dx + dy*dy + dz*dz; + var sum:Number = s1.r + s2.r; + if (len > sum*sum) return false; + len = Math.sqrt(len); + dx /= len; + dy /= len; + dz /= len; + + contact.body1 = s1.body; + contact.body2 = s2.body; + contact.normal.x = dx; + contact.normal.y = dy; + contact.normal.z = dz; + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = sum - len; + cp.pos.x = p1.x - dx*s1.r; + cp.pos.y = p1.y - dy*s1.r; + cp.pos.z = p1.z - dz*s1.r; + cp.r1.vDiff(cp.pos, p1); + cp.r2.vDiff(cp.pos, p2); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/primitives/.svn/all-wcprops b/0.0.5.0/src/alternativa/physics/collision/primitives/.svn/all-wcprops new file mode 100644 index 0000000..7d23609 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/primitives/.svn/all-wcprops @@ -0,0 +1,35 @@ +K 25 +svn:wc:ra_dav:version-url +V 124 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/collision/primitives +END +CollisionTriangle.as +K 25 +svn:wc:ra_dav:version-url +V 145 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/collision/primitives/CollisionTriangle.as +END +CollisionSphere.as +K 25 +svn:wc:ra_dav:version-url +V 143 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/collision/primitives/CollisionSphere.as +END +CollisionBox.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/collision/primitives/CollisionBox.as +END +CollisionPrimitive.as +K 25 +svn:wc:ra_dav:version-url +V 146 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/collision/primitives/CollisionPrimitive.as +END +CollisionRect.as +K 25 +svn:wc:ra_dav:version-url +V 141 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/collision/primitives/CollisionRect.as +END diff --git a/0.0.5.0/src/alternativa/physics/collision/primitives/.svn/entries b/0.0.5.0/src/alternativa/physics/collision/primitives/.svn/entries new file mode 100644 index 0000000..205ebe8 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/primitives/.svn/entries @@ -0,0 +1,88 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/collision/primitives +http://svndev.alternativaplatform.com + + + +2009-09-09T14:41:32.057202Z +19641 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +CollisionTriangle.as +file + + + + +2010-10-28T04:32:41.000000Z +0083a2ba2b8b1d31cfae63b6f7c8b832 +2009-09-09T14:41:32.057202Z +19641 +mike + +CollisionSphere.as +file + + + + +2010-10-28T04:32:41.000000Z +d0d8b961b5e74673c357cd98e0963f4a +2009-09-09T12:16:47.747177Z +19632 +mike + +CollisionBox.as +file + + + + +2010-10-28T04:32:41.000000Z +27c7fef4bdd6c1341b6ca73f67b53fb7 +2009-09-09T14:41:32.057202Z +19641 +mike + +CollisionPrimitive.as +file + + + + +2010-10-28T04:32:41.000000Z +267988e82920cc9a334ef4cc4c305d9a +2009-09-09T14:41:32.057202Z +19641 +mike + +CollisionRect.as +file + + + + +2010-10-28T04:32:41.000000Z +5e19726d3c945f1191a432cff31549a4 +2009-09-09T12:16:47.747177Z +19632 +mike + diff --git a/0.0.5.0/src/alternativa/physics/collision/primitives/.svn/format b/0.0.5.0/src/alternativa/physics/collision/primitives/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/primitives/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.5.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base b/0.0.5.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base new file mode 100644 index 0000000..21704fa --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base @@ -0,0 +1,180 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + /** + * Ориентированный бокс. + */ + public class CollisionBox extends CollisionPrimitive { + + // Половинные размеры вдоль каждой из осей + public var hs:Vector3 = new Vector3(); + + // Используются в определении пересечения с лучом + private static var _o:Vector3 = new Vector3(); + private static var _v:Vector3 = new Vector3(); + + /** + * @param hs + * @param collisionGroup + */ + public function CollisionBox(hs:Vector3, collisionGroup:int) { + super(BOX, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + var t:Matrix4 = transform; + aabb.maxX = hs.x*(t.a < 0 ? -t.a : t.a) + hs.y*(t.b < 0 ? -t.b : t.b) + hs.z*(t.c < 0 ? -t.c : t.c); + aabb.minX = -aabb.maxX; + + aabb.maxY = hs.x*(t.e < 0 ? -t.e : t.e) + hs.y*(t.f < 0 ? -t.f : t.f) + hs.z*(t.g < 0 ? -t.g : t.g); + aabb.minY = -aabb.maxY; + + aabb.maxZ = hs.x*(t.i < 0 ? -t.i : t.i) + hs.y*(t.j < 0 ? -t.j : t.j) + hs.z*(t.k < 0 ? -t.k : t.k); + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var box:CollisionBox = source as CollisionBox; + if (box == null) return this; + super.copyFrom(box); + hs.vCopy(box.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionBox(hs, collisionGroup); + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + var tMin:Number = -1; + var tMax:Number = 1e308; + var t1:Number; + var t2:Number; + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var xx:Number = origin.x - transform.d; + var yy:Number = origin.y - transform.h; + var zz:Number = origin.z - transform.l; + _o.x = transform.a*xx + transform.e*yy + transform.i*zz; + _o.y = transform.b*xx + transform.f*yy + transform.j*zz; + _o.z = transform.c*xx + transform.g*yy + transform.k*zz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + _v.x = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + _v.y = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + _v.z = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + // X + if (_v.x < epsilon && _v.x > -epsilon) { + if (_o.x < -hs.x || _o.x > hs.x) return -1; + } else { + t1 = (-hs.x - _o.x)/_v.x; + t2 = (hs.x - _o.x)/_v.x; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.x = -1; + normal.y = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.x = 1; + normal.y = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Y + if (_v.y < epsilon && _v.y > -epsilon) { + if (_o.y < -hs.y || _o.y > hs.y) return -1; + } else { + t1 = (-hs.y - _o.y)/_v.y; + t2 = (hs.y - _o.y)/_v.y; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.y = -1; + normal.x = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.y = 1; + normal.x = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Z + if (_v.z < epsilon && _v.z > -epsilon) { + if (_o.z < -hs.z || _o.z > hs.z) return -1; + } else { + t1 = (-hs.z - _o.z)/_v.z; + t2 = (hs.z - _o.z)/_v.z; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.z = -1; + normal.x = normal.y = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.z = 1; + normal.x = normal.y = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Перевод нормали в мировую систему координат + // Inlined normal.vDeltaTransformBy4(transform); + xx = normal.x, yy = normal.y, zz = normal.z; + normal.x = transform.a*xx + transform.b*yy + transform.c*zz; + normal.y = transform.e*xx + transform.f*yy + transform.g*zz; + normal.z = transform.i*xx + transform.j*yy + transform.k*zz; + + return tMin; + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionPrimitive.as.svn-base b/0.0.5.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionPrimitive.as.svn-base new file mode 100644 index 0000000..ebfeeaf --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionPrimitive.as.svn-base @@ -0,0 +1,143 @@ +package alternativa.physics.collision.primitives { + + import alternativa.physics.collision.ICollisionPredicate; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + + /** + * Базовый класс для примитивов, использующихся детектором столкновений. + */ + public class CollisionPrimitive { + // Константы типов примитива + public static const BOX:int = 1; + public static const PLANE:int = 2; + public static const SPHERE:int = 3; + public static const RECT:int = 4; + public static const TRIANGLE:int = 5; + + // Тип примитива + public var type:int; + // Группы примитива. Каждая группа определяется установленным битом. Столкновения проверяются только для примитивов, + // имеющих хотя бы одну общую группу. + public var collisionGroup:int; + // Предикат примитива, вызывающийся детектором при нахождении столкновения. В зависимости от возвращённого + // предикатом результата столкновение либо регистрируется, либо игнорируется. + // Nullable + public var postCollisionPredicate:ICollisionPredicate; + // Тело, владеющее примитивом. Поле сделано открытым для быстрого доступа на чтение. Установка значения должна выполняться вызовом метода setBody(). + // Nullable + public var body:Body; + // Трансформация примитива в системе координат тела, если оно указано. Не допускается масштабирование матрицы. + public var localTransform:Matrix4 = new Matrix4(); + // Полная трансформация примитива. Не допускается масштабирование матрицы. + public var transform:Matrix4 = new Matrix4(); + // AABB в мировой системе координат. Расчитывается системой вызовом функции calculateBoundBox(). + public var aabb:BoundBox = new BoundBox(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param type тип примитива + * @param collisionGroup группа примитива + */ + public function CollisionPrimitive(type:int, collisionGroup:int) { + this.type = type; + this.collisionGroup = collisionGroup; + } + + /** + * Устанавливает тело, владеющее примитивом. + * + * @param body тело, которое владеет примитивом. Если указано значение null, матрица локальной трансформации удаляется. + * @param localTransform трансформация примитива в системе координат тела. Указание значения null равносильно + * заданию единичной матрицы, однако в первом случае не будет дополнительного умножения матриц при вычислении полной трансформации примитива. + */ + public function setBody(body:Body, localTransform:Matrix4 = null):void { + if (this.body == body) return; + this.body = body; + if (body == null) { + this.localTransform = null; + } else { + if (localTransform != null) { + if (this.localTransform == null) { + this.localTransform = new Matrix4(); + } + this.localTransform.copy(localTransform); + } else { + this.localTransform = null; + } + } + } + + /** + * Рассчитывает AABB примитива. Наследники должны переопределять этот метод, реализуя в нём корректный рассчёт. + * + * @return ссылка на свой AABB + */ + public function calculateAABB():BoundBox { + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + return -1; + } + + /** + * Клонирует примитив. Переопределять не рекомендуется. Вместо этого переопределяются методы createPrimitive() и copyFrom(). + * + * @return клон примитива + */ + public function clone():CollisionPrimitive { + var p:CollisionPrimitive = createPrimitive(); + return p.copyFrom(this); + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + if (source == null) { + throw new ArgumentError("Parameter source cannot be null"); + } + type = source.type; + transform.copy(source.transform); + collisionGroup = source.collisionGroup; + setBody(source.body, source.localTransform); + aabb.copyFrom(source.aabb); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + public function toString():String { + return "[CollisionPrimitive type=" + type + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + protected function createPrimitive():CollisionPrimitive { + return new CollisionPrimitive(type, collisionGroup); + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base b/0.0.5.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base new file mode 100644 index 0000000..0e39447 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base @@ -0,0 +1,126 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + /** + * Ориентированный прямоугольник. Задаётся половинами размеров вдоль осей X (ширина) и Y (длина) локальной системы + * координат примитива. Таким образом, прямоугольник лежит в плоскости XY, его стороны параллельны осям этой + * плоскости, а нормаль направлена вдоль локальной оси Z. + * + * Прямоугольник может быть одно- или двусторонним. В случае одностороннего прямоугольника, + * столкновения с внутренней стороны не регистрируются. + */ + public class CollisionRect extends CollisionPrimitive { + // Половинные размеры прямоугольника вдоль осей X и Y. Нормаль направлена вдоль оси Z. + public var hs:Vector3 = new Vector3(); + // Флаг указывает, является примитив одно- или двусторонним + public var twoSided:Boolean = true; + + // Малое значение. Используется для указания фиктивной высоты примитива и в функции определения пересечения с сегментом. + private static const EPSILON:Number = 0.005; + // Переменные используются в функции поиска пересечения с лучом + private static var _o:Vector3 = new Vector3(); + private static var _v:Vector3 = new Vector3(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param hs половинные размерв прямоугольника вдоль осей X и Y. Значение z игнорируется. + * @param collisionGroup группа примитива + */ + public function CollisionRect(hs:Vector3, collisionGroup:int) { + super(RECT, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * Расчитывает ограничивающий бокс прямоугольника. Для избежания проблем высота примитива принимается равной + * не нулю, а малому значению. + * + * @return + */ + override public function calculateAABB():BoundBox { + // Баунд бокс прямоугольника имеет минимальную высоту, отличную от нуля во избежание проблем с построением kd-дерева + var t:Matrix4 = transform; + aabb.maxX = hs.x*(t.a < 0 ? -t.a : t.a) + hs.y*(t.b < 0 ? -t.b : t.b) + EPSILON*(t.c < 0 ? -t.c : t.c); + aabb.minX = -aabb.maxX; + + aabb.maxY = hs.x*(t.e < 0 ? -t.e : t.e) + hs.y*(t.f < 0 ? -t.f : t.f) + EPSILON*(t.g < 0 ? -t.g : t.g); + aabb.minY = -aabb.maxY; + + aabb.maxZ = hs.x*(t.i < 0 ? -t.i : t.i) + hs.y*(t.j < 0 ? -t.j : t.j) + EPSILON*(t.k < 0 ? -t.k : t.k); + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var rect:CollisionRect = source as CollisionRect; + if (rect == null) return this; + super.copyFrom(rect); + hs.vCopy(rect.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionRect(hs, collisionGroup); + } + + /** + * @param origin + * @param vector + * @param collisionGroup + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var xx:Number = origin.x - transform.d; + var yy:Number = origin.y - transform.h; + var zz:Number = origin.z - transform.l; + _o.x = transform.a*xx + transform.e*yy + transform.i*zz; + _o.y = transform.b*xx + transform.f*yy + transform.j*zz; + _o.z = transform.c*xx + transform.g*yy + transform.k*zz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + _v.x = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + _v.y = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + _v.z = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + + // Проверка параллельности сегмента и плоскости примитива + if (_v.z > -threshold && _v.z < threshold) return -1; + var t:Number = -_o.z/_v.z; + if (t < 0) return -1; + // Проверка вхождения точки пересечения в прямоугольник + _o.x += _v.x*t; + _o.y += _v.y*t; + _o.z = 0; + if (_o.x < (-hs.x - EPSILON) || _o.x > (hs.x + EPSILON) || _o.y < (-hs.y - EPSILON) || _o.y > (hs.y + EPSILON)) return -1; + + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + return t; + } + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base b/0.0.5.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base new file mode 100644 index 0000000..dc2328b --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base @@ -0,0 +1,80 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + + /** + * Сфера. + */ + public class CollisionSphere extends CollisionPrimitive { + + // Радиус сферы + public var r:Number = 0; + + /** + * + * @param r + * @param collisionGroup + */ + public function CollisionSphere(r:Number, collisionGroup:int) { + super(SPHERE, collisionGroup); + this.r = r; + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + aabb.maxX = transform.d + r; + aabb.minX = transform.d - r; + + aabb.maxY = transform.h + r; + aabb.minY = transform.h - r; + + aabb.maxZ = transform.l + r; + aabb.minZ = transform.l - r; + + return aabb; + } + + /** + * @param origin + * @param vector + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var px:Number = origin.x - transform.d; + var py:Number = origin.y - transform.h; + var pz:Number = origin.z - transform.l; + var k:Number = vector.x*px + vector.y*py + vector.z*pz; + if (k > 0) return -1; + var a:Number = vector.x*vector.x + vector.y*vector.y + vector.z*vector.z; + var D:Number = k*k - a*(px*px + py*py + pz*pz - r*r); + if (D < 0) return -1; + return -(k + Math.sqrt(D))/a; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var sphere:CollisionSphere = source as CollisionSphere; + if (sphere == null) return this; + super.copyFrom(sphere); + r = sphere.r; + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionSphere(r, collisionGroup); + } + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base b/0.0.5.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base new file mode 100644 index 0000000..1bbdc0f --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base @@ -0,0 +1,243 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + /** + * Примитив треугольник. + */ + public class CollisionTriangle extends CollisionPrimitive { + + public var v0:Vector3 = new Vector3(); + public var v1:Vector3 = new Vector3(); + public var v2:Vector3 = new Vector3(); + + public var e0:Vector3 = new Vector3(); + public var e1:Vector3 = new Vector3(); + public var e2:Vector3 = new Vector3(); + + public var len0:Number; + public var len1:Number; + public var len2:Number; + + /** + * + * @param v0 + * @param v1 + * @param v2 + * @param collisionGroup + */ + public function CollisionTriangle(v0:Vector3, v1:Vector3, v2:Vector3, collisionGroup:int) { + super(TRIANGLE, collisionGroup); + initVertices(v0, v1, v2); + } + + /** + * Рассчитывает AABB примитива. + * + * @return ссылка на свой AABB + */ + override public function calculateAABB():BoundBox { + var epsilon:Number = 0.005; + var a:Number; + var b:Number; + var eps_c:Number = epsilon*transform.c; + var eps_g:Number = epsilon*transform.g; + var eps_k:Number = epsilon*transform.k; + + // Вершина 0 + // Ось X + a = v0.x*transform.a + v0.y*transform.b; + aabb.minX = aabb.maxX = a + eps_c; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v0.x*transform.e + v0.y*transform.f; + aabb.minY = aabb.maxY = a + eps_g; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v0.x*transform.i + v0.y*transform.j; + aabb.minZ = aabb.maxZ = a + eps_k; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 1 + // Ось X + a = v1.x*transform.a + v1.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v1.x*transform.e + v1.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v1.x*transform.i + v1.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 2 + // Ось X + a = v2.x*transform.a + v2.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v2.x*transform.e + v2.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v2.x*transform.i + v2.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + aabb.minX += transform.d; + aabb.maxX += transform.d; + + aabb.minY += transform.h; + aabb.maxY += transform.h; + + aabb.minZ += transform.l; + aabb.maxZ += transform.l; + + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + // Луч трансформируется в систему координат примитива, затем проверяется пересечение + + var vz:Number = vector.x*transform.c + vector.y*transform.g + vector.z*transform.k; + // Если луч параллелен плоскости птимитива, то пересечения нет + if (vz < epsilon && vz > -epsilon) return -1; + + var tx:Number = origin.x - transform.d; + var ty:Number = origin.y - transform.h; + var tz:Number = origin.z - transform.l; + + var oz:Number = tx*transform.c + ty*transform.g + tz*transform.k; + var t:Number = -oz/vz; + if (t < 0) return -1; + + var ox:Number = tx*transform.a + ty*transform.e + tz*transform.i; + var oy:Number = tx*transform.b + ty*transform.f + tz*transform.j; + + tx = ox + t*(vector.x*transform.a + vector.y*transform.e + vector.z*transform.i); + ty = oy + t*(vector.x*transform.b + vector.y*transform.f + vector.z*transform.j); + tz = oz + t*vz; + + // Проверка вхождения точки в треугольник + if ((e0.x*(ty - v0.y) - e0.y*(tx - v0.x) < 0) || (e1.x*(ty - v1.y) - e1.y*(tx - v1.x) < 0) || (e2.x*(ty - v2.y) - e2.y*(tx - v2.x) < 0)) return -1; + + // Запись нормали + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + + return t; + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + super.copyFrom(source); + var tri:CollisionTriangle = source as CollisionTriangle; + if (tri != null) { + v0.vCopy(tri.v0); + v1.vCopy(tri.v1); + v2.vCopy(tri.v2); + + e0.vCopy(tri.e0); + e1.vCopy(tri.e1); + e2.vCopy(tri.e2); + + len0 = tri.len0; + len1 = tri.len1; + len2 = tri.len2; + } + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionTriangle v0=" + v0 + ", v1=" + v1 + ", v2=" + v2 + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionTriangle(v0, v1, v2, collisionGroup); + } + + /** + * + * @param v0 + * @param v1 + * @param v2 + */ + private function initVertices(v0:Vector3, v1:Vector3, v2:Vector3):void { + this.v0.vCopy(v0); + this.v1.vCopy(v1); + this.v2.vCopy(v2); + + e0.vDiff(v1, v0); + len0 = e0.vLength(); + e0.vNormalize(); + + e1.vDiff(v2, v1); + len1 = e1.vLength(); + e1.vNormalize(); + + e2.vDiff(v0, v2); + len2 = e2.vLength(); + e2.vNormalize(); + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/primitives/CollisionBox.as b/0.0.5.0/src/alternativa/physics/collision/primitives/CollisionBox.as new file mode 100644 index 0000000..21704fa --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/primitives/CollisionBox.as @@ -0,0 +1,180 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + /** + * Ориентированный бокс. + */ + public class CollisionBox extends CollisionPrimitive { + + // Половинные размеры вдоль каждой из осей + public var hs:Vector3 = new Vector3(); + + // Используются в определении пересечения с лучом + private static var _o:Vector3 = new Vector3(); + private static var _v:Vector3 = new Vector3(); + + /** + * @param hs + * @param collisionGroup + */ + public function CollisionBox(hs:Vector3, collisionGroup:int) { + super(BOX, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + var t:Matrix4 = transform; + aabb.maxX = hs.x*(t.a < 0 ? -t.a : t.a) + hs.y*(t.b < 0 ? -t.b : t.b) + hs.z*(t.c < 0 ? -t.c : t.c); + aabb.minX = -aabb.maxX; + + aabb.maxY = hs.x*(t.e < 0 ? -t.e : t.e) + hs.y*(t.f < 0 ? -t.f : t.f) + hs.z*(t.g < 0 ? -t.g : t.g); + aabb.minY = -aabb.maxY; + + aabb.maxZ = hs.x*(t.i < 0 ? -t.i : t.i) + hs.y*(t.j < 0 ? -t.j : t.j) + hs.z*(t.k < 0 ? -t.k : t.k); + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var box:CollisionBox = source as CollisionBox; + if (box == null) return this; + super.copyFrom(box); + hs.vCopy(box.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionBox(hs, collisionGroup); + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + var tMin:Number = -1; + var tMax:Number = 1e308; + var t1:Number; + var t2:Number; + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var xx:Number = origin.x - transform.d; + var yy:Number = origin.y - transform.h; + var zz:Number = origin.z - transform.l; + _o.x = transform.a*xx + transform.e*yy + transform.i*zz; + _o.y = transform.b*xx + transform.f*yy + transform.j*zz; + _o.z = transform.c*xx + transform.g*yy + transform.k*zz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + _v.x = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + _v.y = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + _v.z = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + // X + if (_v.x < epsilon && _v.x > -epsilon) { + if (_o.x < -hs.x || _o.x > hs.x) return -1; + } else { + t1 = (-hs.x - _o.x)/_v.x; + t2 = (hs.x - _o.x)/_v.x; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.x = -1; + normal.y = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.x = 1; + normal.y = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Y + if (_v.y < epsilon && _v.y > -epsilon) { + if (_o.y < -hs.y || _o.y > hs.y) return -1; + } else { + t1 = (-hs.y - _o.y)/_v.y; + t2 = (hs.y - _o.y)/_v.y; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.y = -1; + normal.x = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.y = 1; + normal.x = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Z + if (_v.z < epsilon && _v.z > -epsilon) { + if (_o.z < -hs.z || _o.z > hs.z) return -1; + } else { + t1 = (-hs.z - _o.z)/_v.z; + t2 = (hs.z - _o.z)/_v.z; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.z = -1; + normal.x = normal.y = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.z = 1; + normal.x = normal.y = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Перевод нормали в мировую систему координат + // Inlined normal.vDeltaTransformBy4(transform); + xx = normal.x, yy = normal.y, zz = normal.z; + normal.x = transform.a*xx + transform.b*yy + transform.c*zz; + normal.y = transform.e*xx + transform.f*yy + transform.g*zz; + normal.z = transform.i*xx + transform.j*yy + transform.k*zz; + + return tMin; + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/primitives/CollisionPrimitive.as b/0.0.5.0/src/alternativa/physics/collision/primitives/CollisionPrimitive.as new file mode 100644 index 0000000..ebfeeaf --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/primitives/CollisionPrimitive.as @@ -0,0 +1,143 @@ +package alternativa.physics.collision.primitives { + + import alternativa.physics.collision.ICollisionPredicate; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + + /** + * Базовый класс для примитивов, использующихся детектором столкновений. + */ + public class CollisionPrimitive { + // Константы типов примитива + public static const BOX:int = 1; + public static const PLANE:int = 2; + public static const SPHERE:int = 3; + public static const RECT:int = 4; + public static const TRIANGLE:int = 5; + + // Тип примитива + public var type:int; + // Группы примитива. Каждая группа определяется установленным битом. Столкновения проверяются только для примитивов, + // имеющих хотя бы одну общую группу. + public var collisionGroup:int; + // Предикат примитива, вызывающийся детектором при нахождении столкновения. В зависимости от возвращённого + // предикатом результата столкновение либо регистрируется, либо игнорируется. + // Nullable + public var postCollisionPredicate:ICollisionPredicate; + // Тело, владеющее примитивом. Поле сделано открытым для быстрого доступа на чтение. Установка значения должна выполняться вызовом метода setBody(). + // Nullable + public var body:Body; + // Трансформация примитива в системе координат тела, если оно указано. Не допускается масштабирование матрицы. + public var localTransform:Matrix4 = new Matrix4(); + // Полная трансформация примитива. Не допускается масштабирование матрицы. + public var transform:Matrix4 = new Matrix4(); + // AABB в мировой системе координат. Расчитывается системой вызовом функции calculateBoundBox(). + public var aabb:BoundBox = new BoundBox(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param type тип примитива + * @param collisionGroup группа примитива + */ + public function CollisionPrimitive(type:int, collisionGroup:int) { + this.type = type; + this.collisionGroup = collisionGroup; + } + + /** + * Устанавливает тело, владеющее примитивом. + * + * @param body тело, которое владеет примитивом. Если указано значение null, матрица локальной трансформации удаляется. + * @param localTransform трансформация примитива в системе координат тела. Указание значения null равносильно + * заданию единичной матрицы, однако в первом случае не будет дополнительного умножения матриц при вычислении полной трансформации примитива. + */ + public function setBody(body:Body, localTransform:Matrix4 = null):void { + if (this.body == body) return; + this.body = body; + if (body == null) { + this.localTransform = null; + } else { + if (localTransform != null) { + if (this.localTransform == null) { + this.localTransform = new Matrix4(); + } + this.localTransform.copy(localTransform); + } else { + this.localTransform = null; + } + } + } + + /** + * Рассчитывает AABB примитива. Наследники должны переопределять этот метод, реализуя в нём корректный рассчёт. + * + * @return ссылка на свой AABB + */ + public function calculateAABB():BoundBox { + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + return -1; + } + + /** + * Клонирует примитив. Переопределять не рекомендуется. Вместо этого переопределяются методы createPrimitive() и copyFrom(). + * + * @return клон примитива + */ + public function clone():CollisionPrimitive { + var p:CollisionPrimitive = createPrimitive(); + return p.copyFrom(this); + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + if (source == null) { + throw new ArgumentError("Parameter source cannot be null"); + } + type = source.type; + transform.copy(source.transform); + collisionGroup = source.collisionGroup; + setBody(source.body, source.localTransform); + aabb.copyFrom(source.aabb); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + public function toString():String { + return "[CollisionPrimitive type=" + type + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + protected function createPrimitive():CollisionPrimitive { + return new CollisionPrimitive(type, collisionGroup); + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/primitives/CollisionRect.as b/0.0.5.0/src/alternativa/physics/collision/primitives/CollisionRect.as new file mode 100644 index 0000000..0e39447 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/primitives/CollisionRect.as @@ -0,0 +1,126 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + /** + * Ориентированный прямоугольник. Задаётся половинами размеров вдоль осей X (ширина) и Y (длина) локальной системы + * координат примитива. Таким образом, прямоугольник лежит в плоскости XY, его стороны параллельны осям этой + * плоскости, а нормаль направлена вдоль локальной оси Z. + * + * Прямоугольник может быть одно- или двусторонним. В случае одностороннего прямоугольника, + * столкновения с внутренней стороны не регистрируются. + */ + public class CollisionRect extends CollisionPrimitive { + // Половинные размеры прямоугольника вдоль осей X и Y. Нормаль направлена вдоль оси Z. + public var hs:Vector3 = new Vector3(); + // Флаг указывает, является примитив одно- или двусторонним + public var twoSided:Boolean = true; + + // Малое значение. Используется для указания фиктивной высоты примитива и в функции определения пересечения с сегментом. + private static const EPSILON:Number = 0.005; + // Переменные используются в функции поиска пересечения с лучом + private static var _o:Vector3 = new Vector3(); + private static var _v:Vector3 = new Vector3(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param hs половинные размерв прямоугольника вдоль осей X и Y. Значение z игнорируется. + * @param collisionGroup группа примитива + */ + public function CollisionRect(hs:Vector3, collisionGroup:int) { + super(RECT, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * Расчитывает ограничивающий бокс прямоугольника. Для избежания проблем высота примитива принимается равной + * не нулю, а малому значению. + * + * @return + */ + override public function calculateAABB():BoundBox { + // Баунд бокс прямоугольника имеет минимальную высоту, отличную от нуля во избежание проблем с построением kd-дерева + var t:Matrix4 = transform; + aabb.maxX = hs.x*(t.a < 0 ? -t.a : t.a) + hs.y*(t.b < 0 ? -t.b : t.b) + EPSILON*(t.c < 0 ? -t.c : t.c); + aabb.minX = -aabb.maxX; + + aabb.maxY = hs.x*(t.e < 0 ? -t.e : t.e) + hs.y*(t.f < 0 ? -t.f : t.f) + EPSILON*(t.g < 0 ? -t.g : t.g); + aabb.minY = -aabb.maxY; + + aabb.maxZ = hs.x*(t.i < 0 ? -t.i : t.i) + hs.y*(t.j < 0 ? -t.j : t.j) + EPSILON*(t.k < 0 ? -t.k : t.k); + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var rect:CollisionRect = source as CollisionRect; + if (rect == null) return this; + super.copyFrom(rect); + hs.vCopy(rect.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionRect(hs, collisionGroup); + } + + /** + * @param origin + * @param vector + * @param collisionGroup + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var xx:Number = origin.x - transform.d; + var yy:Number = origin.y - transform.h; + var zz:Number = origin.z - transform.l; + _o.x = transform.a*xx + transform.e*yy + transform.i*zz; + _o.y = transform.b*xx + transform.f*yy + transform.j*zz; + _o.z = transform.c*xx + transform.g*yy + transform.k*zz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + _v.x = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + _v.y = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + _v.z = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + + // Проверка параллельности сегмента и плоскости примитива + if (_v.z > -threshold && _v.z < threshold) return -1; + var t:Number = -_o.z/_v.z; + if (t < 0) return -1; + // Проверка вхождения точки пересечения в прямоугольник + _o.x += _v.x*t; + _o.y += _v.y*t; + _o.z = 0; + if (_o.x < (-hs.x - EPSILON) || _o.x > (hs.x + EPSILON) || _o.y < (-hs.y - EPSILON) || _o.y > (hs.y + EPSILON)) return -1; + + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + return t; + } + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/primitives/CollisionSphere.as b/0.0.5.0/src/alternativa/physics/collision/primitives/CollisionSphere.as new file mode 100644 index 0000000..dc2328b --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/primitives/CollisionSphere.as @@ -0,0 +1,80 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + + /** + * Сфера. + */ + public class CollisionSphere extends CollisionPrimitive { + + // Радиус сферы + public var r:Number = 0; + + /** + * + * @param r + * @param collisionGroup + */ + public function CollisionSphere(r:Number, collisionGroup:int) { + super(SPHERE, collisionGroup); + this.r = r; + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + aabb.maxX = transform.d + r; + aabb.minX = transform.d - r; + + aabb.maxY = transform.h + r; + aabb.minY = transform.h - r; + + aabb.maxZ = transform.l + r; + aabb.minZ = transform.l - r; + + return aabb; + } + + /** + * @param origin + * @param vector + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var px:Number = origin.x - transform.d; + var py:Number = origin.y - transform.h; + var pz:Number = origin.z - transform.l; + var k:Number = vector.x*px + vector.y*py + vector.z*pz; + if (k > 0) return -1; + var a:Number = vector.x*vector.x + vector.y*vector.y + vector.z*vector.z; + var D:Number = k*k - a*(px*px + py*py + pz*pz - r*r); + if (D < 0) return -1; + return -(k + Math.sqrt(D))/a; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var sphere:CollisionSphere = source as CollisionSphere; + if (sphere == null) return this; + super.copyFrom(sphere); + r = sphere.r; + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionSphere(r, collisionGroup); + } + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/primitives/CollisionTriangle.as b/0.0.5.0/src/alternativa/physics/collision/primitives/CollisionTriangle.as new file mode 100644 index 0000000..1bbdc0f --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/primitives/CollisionTriangle.as @@ -0,0 +1,243 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + /** + * Примитив треугольник. + */ + public class CollisionTriangle extends CollisionPrimitive { + + public var v0:Vector3 = new Vector3(); + public var v1:Vector3 = new Vector3(); + public var v2:Vector3 = new Vector3(); + + public var e0:Vector3 = new Vector3(); + public var e1:Vector3 = new Vector3(); + public var e2:Vector3 = new Vector3(); + + public var len0:Number; + public var len1:Number; + public var len2:Number; + + /** + * + * @param v0 + * @param v1 + * @param v2 + * @param collisionGroup + */ + public function CollisionTriangle(v0:Vector3, v1:Vector3, v2:Vector3, collisionGroup:int) { + super(TRIANGLE, collisionGroup); + initVertices(v0, v1, v2); + } + + /** + * Рассчитывает AABB примитива. + * + * @return ссылка на свой AABB + */ + override public function calculateAABB():BoundBox { + var epsilon:Number = 0.005; + var a:Number; + var b:Number; + var eps_c:Number = epsilon*transform.c; + var eps_g:Number = epsilon*transform.g; + var eps_k:Number = epsilon*transform.k; + + // Вершина 0 + // Ось X + a = v0.x*transform.a + v0.y*transform.b; + aabb.minX = aabb.maxX = a + eps_c; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v0.x*transform.e + v0.y*transform.f; + aabb.minY = aabb.maxY = a + eps_g; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v0.x*transform.i + v0.y*transform.j; + aabb.minZ = aabb.maxZ = a + eps_k; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 1 + // Ось X + a = v1.x*transform.a + v1.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v1.x*transform.e + v1.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v1.x*transform.i + v1.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 2 + // Ось X + a = v2.x*transform.a + v2.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v2.x*transform.e + v2.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v2.x*transform.i + v2.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + aabb.minX += transform.d; + aabb.maxX += transform.d; + + aabb.minY += transform.h; + aabb.maxY += transform.h; + + aabb.minZ += transform.l; + aabb.maxZ += transform.l; + + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + // Луч трансформируется в систему координат примитива, затем проверяется пересечение + + var vz:Number = vector.x*transform.c + vector.y*transform.g + vector.z*transform.k; + // Если луч параллелен плоскости птимитива, то пересечения нет + if (vz < epsilon && vz > -epsilon) return -1; + + var tx:Number = origin.x - transform.d; + var ty:Number = origin.y - transform.h; + var tz:Number = origin.z - transform.l; + + var oz:Number = tx*transform.c + ty*transform.g + tz*transform.k; + var t:Number = -oz/vz; + if (t < 0) return -1; + + var ox:Number = tx*transform.a + ty*transform.e + tz*transform.i; + var oy:Number = tx*transform.b + ty*transform.f + tz*transform.j; + + tx = ox + t*(vector.x*transform.a + vector.y*transform.e + vector.z*transform.i); + ty = oy + t*(vector.x*transform.b + vector.y*transform.f + vector.z*transform.j); + tz = oz + t*vz; + + // Проверка вхождения точки в треугольник + if ((e0.x*(ty - v0.y) - e0.y*(tx - v0.x) < 0) || (e1.x*(ty - v1.y) - e1.y*(tx - v1.x) < 0) || (e2.x*(ty - v2.y) - e2.y*(tx - v2.x) < 0)) return -1; + + // Запись нормали + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + + return t; + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + super.copyFrom(source); + var tri:CollisionTriangle = source as CollisionTriangle; + if (tri != null) { + v0.vCopy(tri.v0); + v1.vCopy(tri.v1); + v2.vCopy(tri.v2); + + e0.vCopy(tri.e0); + e1.vCopy(tri.e1); + e2.vCopy(tri.e2); + + len0 = tri.len0; + len1 = tri.len1; + len2 = tri.len2; + } + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionTriangle v0=" + v0 + ", v1=" + v1 + ", v2=" + v2 + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionTriangle(v0, v1, v2, collisionGroup); + } + + /** + * + * @param v0 + * @param v1 + * @param v2 + */ + private function initVertices(v0:Vector3, v1:Vector3, v2:Vector3):void { + this.v0.vCopy(v0); + this.v1.vCopy(v1); + this.v2.vCopy(v2); + + e0.vDiff(v1, v0); + len0 = e0.vLength(); + e0.vNormalize(); + + e1.vDiff(v2, v1); + len1 = e1.vLength(); + e1.vNormalize(); + + e2.vDiff(v0, v2); + len2 = e2.vLength(); + e2.vNormalize(); + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/types/.svn/all-wcprops b/0.0.5.0/src/alternativa/physics/collision/types/.svn/all-wcprops new file mode 100644 index 0000000..4a0bd7f --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/types/.svn/all-wcprops @@ -0,0 +1,23 @@ +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/collision/types +END +BoundBox.as +K 25 +svn:wc:ra_dav:version-url +V 131 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/collision/types/BoundBox.as +END +RayIntersection.as +K 25 +svn:wc:ra_dav:version-url +V 138 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/collision/types/RayIntersection.as +END +Ray.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/collision/types/Ray.as +END diff --git a/0.0.5.0/src/alternativa/physics/collision/types/.svn/entries b/0.0.5.0/src/alternativa/physics/collision/types/.svn/entries new file mode 100644 index 0000000..ba5b2cf --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/types/.svn/entries @@ -0,0 +1,64 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/collision/types +http://svndev.alternativaplatform.com + + + +2009-09-09T14:02:48.134744Z +19633 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +BoundBox.as +file + + + + +2010-10-28T04:32:41.000000Z +d92d4ab3dde698feb674fb9488f59678 +2009-09-09T14:02:48.134744Z +19633 +mike + +RayIntersection.as +file + + + + +2010-10-28T04:32:41.000000Z +a0a4cf3dd94468fdf44cd06734927c94 +2009-06-08T06:46:18.310732Z +14099 +mike + +Ray.as +file + + + + +2010-10-28T04:32:41.000000Z +55eeb1cf79a416bb863bee95f089dbcd +2009-04-17T17:39:21.367569Z +11341 +mike + diff --git a/0.0.5.0/src/alternativa/physics/collision/types/.svn/format b/0.0.5.0/src/alternativa/physics/collision/types/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/types/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.5.0/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base b/0.0.5.0/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base new file mode 100644 index 0000000..b4d5278 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base @@ -0,0 +1,75 @@ +package alternativa.physics.collision.types { + + /** + * Ограничивающий параллелепипед. + */ + public class BoundBox { + + public var minX:Number = 1e308; + public var minY:Number = 1e308; + public var minZ:Number = 1e308; + public var maxX:Number = -1e308; + public var maxY:Number = -1e308; + public var maxZ:Number = -1e308; + + public function setSize(minX:Number, minY:Number, minZ:Number, maxX:Number, maxY:Number, maxZ:Number):void { + this.minX = minX; + this.minY = minY; + this.minZ = minZ; + this.maxX = maxX; + this.maxY = maxY; + this.maxZ = maxZ; + } + + public function addBoundBox(boundBox:BoundBox):void { + minX = (boundBox.minX < minX) ? boundBox.minX : minX; + minY = (boundBox.minY < minY) ? boundBox.minY : minY; + minZ = (boundBox.minZ < minZ) ? boundBox.minZ : minZ; + maxX = (boundBox.maxX > maxX) ? boundBox.maxX : maxX; + maxY = (boundBox.maxY > maxY) ? boundBox.maxY : maxY; + maxZ = (boundBox.maxZ > maxZ) ? boundBox.maxZ : maxZ; + } + + public function addPoint(x:Number, y:Number, z:Number):void { + if (x < minX) minX = x; + if (x > maxX) maxX = x; + if (y < minY) minY = y; + if (y > maxY) maxY = y; + if (z < minZ) minZ = z; + if (z > maxZ) maxZ = z; + } + + public function infinity():void { + minX = 1e308; + minY = 1e308; + minZ = 1e308; + maxX = -1e308; + maxY = -1e308; + maxZ = -1e308; + } + + public function intersects(bb:BoundBox, epsilon:Number):Boolean { + return !(minX > bb.maxX + epsilon || maxX < bb.minX - epsilon || minY > bb.maxY + epsilon || maxY < bb.minY - epsilon || minZ > bb.maxZ + epsilon || maxZ < bb.minZ - epsilon); + } + + public function copyFrom(boundBox:BoundBox):void { + minX = boundBox.minX; + minY = boundBox.minY; + minZ = boundBox.minZ; + maxX = boundBox.maxX; + maxY = boundBox.maxY; + maxZ = boundBox.maxZ; + } + + public function clone():BoundBox { + var clone:BoundBox = new BoundBox(); + clone.copyFrom(this); + return clone; + } + + public function toString():String { + return "BoundBox [" + minX + ", " + minY + ", " + minZ + " : " + maxX + ", " + maxY + ", " + maxZ + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base b/0.0.5.0/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base new file mode 100644 index 0000000..f80620a --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base @@ -0,0 +1,13 @@ +package alternativa.physics.collision.types { + import alternativa.physics.types.Vector3; + + /** + * Структура, описывающая луч и сегмент прямой в пространстве. + */ + public class Ray { + // Начало луча. Начальная точка сегмента на прямой луча. + public var origin:Vector3 = new Vector3(); + // Направляющий вектор луча. Длина вектора задаёт длину сегмента на луче. Конечная точка сегмента вычисляется как origin + dir. + public var dir:Vector3 = new Vector3(); + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base b/0.0.5.0/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base new file mode 100644 index 0000000..8a89f75 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base @@ -0,0 +1,25 @@ +package alternativa.physics.collision.types { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.types.Vector3; + + /** + * Структура описывает точку пересечения луча с физической геометрией. + */ + public class RayIntersection { + // Физический примитив, с которым пересекается луч + public var primitive:CollisionPrimitive; + // Положенеи точки пересечения + public var pos:Vector3 = new Vector3(); + // Нормаль поверхности физического примитива в точке пересечения + public var normal:Vector3 = new Vector3(); + // Время пересечения (|pos - ray.origin|/|ray.dir|) + public var t:Number = 0; + + public function copy(source:RayIntersection):void { + primitive = source.primitive; + pos.vCopy(source.pos); + normal.vCopy(source.normal); + t = source.t; + } + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/types/BoundBox.as b/0.0.5.0/src/alternativa/physics/collision/types/BoundBox.as new file mode 100644 index 0000000..b4d5278 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/types/BoundBox.as @@ -0,0 +1,75 @@ +package alternativa.physics.collision.types { + + /** + * Ограничивающий параллелепипед. + */ + public class BoundBox { + + public var minX:Number = 1e308; + public var minY:Number = 1e308; + public var minZ:Number = 1e308; + public var maxX:Number = -1e308; + public var maxY:Number = -1e308; + public var maxZ:Number = -1e308; + + public function setSize(minX:Number, minY:Number, minZ:Number, maxX:Number, maxY:Number, maxZ:Number):void { + this.minX = minX; + this.minY = minY; + this.minZ = minZ; + this.maxX = maxX; + this.maxY = maxY; + this.maxZ = maxZ; + } + + public function addBoundBox(boundBox:BoundBox):void { + minX = (boundBox.minX < minX) ? boundBox.minX : minX; + minY = (boundBox.minY < minY) ? boundBox.minY : minY; + minZ = (boundBox.minZ < minZ) ? boundBox.minZ : minZ; + maxX = (boundBox.maxX > maxX) ? boundBox.maxX : maxX; + maxY = (boundBox.maxY > maxY) ? boundBox.maxY : maxY; + maxZ = (boundBox.maxZ > maxZ) ? boundBox.maxZ : maxZ; + } + + public function addPoint(x:Number, y:Number, z:Number):void { + if (x < minX) minX = x; + if (x > maxX) maxX = x; + if (y < minY) minY = y; + if (y > maxY) maxY = y; + if (z < minZ) minZ = z; + if (z > maxZ) maxZ = z; + } + + public function infinity():void { + minX = 1e308; + minY = 1e308; + minZ = 1e308; + maxX = -1e308; + maxY = -1e308; + maxZ = -1e308; + } + + public function intersects(bb:BoundBox, epsilon:Number):Boolean { + return !(minX > bb.maxX + epsilon || maxX < bb.minX - epsilon || minY > bb.maxY + epsilon || maxY < bb.minY - epsilon || minZ > bb.maxZ + epsilon || maxZ < bb.minZ - epsilon); + } + + public function copyFrom(boundBox:BoundBox):void { + minX = boundBox.minX; + minY = boundBox.minY; + minZ = boundBox.minZ; + maxX = boundBox.maxX; + maxY = boundBox.maxY; + maxZ = boundBox.maxZ; + } + + public function clone():BoundBox { + var clone:BoundBox = new BoundBox(); + clone.copyFrom(this); + return clone; + } + + public function toString():String { + return "BoundBox [" + minX + ", " + minY + ", " + minZ + " : " + maxX + ", " + maxY + ", " + maxZ + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/types/Ray.as b/0.0.5.0/src/alternativa/physics/collision/types/Ray.as new file mode 100644 index 0000000..f80620a --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/types/Ray.as @@ -0,0 +1,13 @@ +package alternativa.physics.collision.types { + import alternativa.physics.types.Vector3; + + /** + * Структура, описывающая луч и сегмент прямой в пространстве. + */ + public class Ray { + // Начало луча. Начальная точка сегмента на прямой луча. + public var origin:Vector3 = new Vector3(); + // Направляющий вектор луча. Длина вектора задаёт длину сегмента на луче. Конечная точка сегмента вычисляется как origin + dir. + public var dir:Vector3 = new Vector3(); + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/collision/types/RayIntersection.as b/0.0.5.0/src/alternativa/physics/collision/types/RayIntersection.as new file mode 100644 index 0000000..8a89f75 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/collision/types/RayIntersection.as @@ -0,0 +1,25 @@ +package alternativa.physics.collision.types { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.types.Vector3; + + /** + * Структура описывает точку пересечения луча с физической геометрией. + */ + public class RayIntersection { + // Физический примитив, с которым пересекается луч + public var primitive:CollisionPrimitive; + // Положенеи точки пересечения + public var pos:Vector3 = new Vector3(); + // Нормаль поверхности физического примитива в точке пересечения + public var normal:Vector3 = new Vector3(); + // Время пересечения (|pos - ray.origin|/|ray.dir|) + public var t:Number = 0; + + public function copy(source:RayIntersection):void { + primitive = source.primitive; + pos.vCopy(source.pos); + normal.vCopy(source.normal); + t = source.t; + } + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/rigid/.svn/all-wcprops b/0.0.5.0/src/alternativa/physics/rigid/.svn/all-wcprops new file mode 100644 index 0000000..46796b4 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/rigid/.svn/all-wcprops @@ -0,0 +1,47 @@ +K 25 +svn:wc:ra_dav:version-url +V 109 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/rigid +END +Body.as +K 25 +svn:wc:ra_dav:version-url +V 117 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/rigid/Body.as +END +BodyState.as +K 25 +svn:wc:ra_dav:version-url +V 122 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/rigid/BodyState.as +END +PhysicsUtils.as +K 25 +svn:wc:ra_dav:version-url +V 125 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/rigid/PhysicsUtils.as +END +ContactPoint.as +K 25 +svn:wc:ra_dav:version-url +V 125 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/rigid/ContactPoint.as +END +RigidWorld.as +K 25 +svn:wc:ra_dav:version-url +V 123 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/rigid/RigidWorld.as +END +Contact.as +K 25 +svn:wc:ra_dav:version-url +V 120 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/rigid/Contact.as +END +BodyMaterial.as +K 25 +svn:wc:ra_dav:version-url +V 125 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/rigid/BodyMaterial.as +END diff --git a/0.0.5.0/src/alternativa/physics/rigid/.svn/entries b/0.0.5.0/src/alternativa/physics/rigid/.svn/entries new file mode 100644 index 0000000..21237de --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/rigid/.svn/entries @@ -0,0 +1,118 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/rigid +http://svndev.alternativaplatform.com + + + +2009-07-03T12:54:42.818772Z +15744 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Body.as +file + + + + +2010-10-28T04:32:41.000000Z +23bad2eff16ce73dccb56a7a601cc3c0 +2009-06-29T14:07:24.833358Z +15390 +mike + +BodyState.as +file + + + + +2010-10-28T04:32:41.000000Z +8a089205beccac13a05eccfb9cfc6387 +2009-06-08T06:46:18.310732Z +14099 +mike + +PhysicsUtils.as +file + + + + +2010-10-28T04:32:41.000000Z +6d88004ddab2ae19e0d15f5b1b7c016a +2009-04-26T21:40:17.813142Z +12023 +mike + +constraints +dir + +ContactPoint.as +file + + + + +2010-10-28T04:32:41.000000Z +a94e73e3bfc85be2a79d6cc939512d91 +2009-06-08T06:46:18.310732Z +14099 +mike + +RigidWorld.as +file + + + + +2010-10-28T04:32:41.000000Z +b0b528a79a6fce55e5d800a15dc99534 +2009-07-03T12:54:42.818772Z +15744 +mike + +primitives +dir + +Contact.as +file + + + + +2010-10-28T04:32:41.000000Z +d65ad3d488d4a06addcbfa4aaf958f10 +2009-04-13T03:04:08.898183Z +11018 +mike + +BodyMaterial.as +file + + + + +2010-10-28T04:32:41.000000Z +49c7a55b5b26fdb03a60f24bac460f53 +2009-04-07T08:34:06.342994Z +10587 +mike + diff --git a/0.0.5.0/src/alternativa/physics/rigid/.svn/format b/0.0.5.0/src/alternativa/physics/rigid/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/rigid/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.5.0/src/alternativa/physics/rigid/.svn/text-base/Body.as.svn-base b/0.0.5.0/src/alternativa/physics/rigid/.svn/text-base/Body.as.svn-base new file mode 100644 index 0000000..7c08ce9 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/rigid/.svn/text-base/Body.as.svn-base @@ -0,0 +1,340 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.IBodyCollisionPredicate; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Quaternion; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class Body { + + public static var linDamping:Number = 0.997; + public static var rotDamping:Number = 0.997; + + public var name:String; + public var movable:Boolean = true; + public var canFreeze:Boolean = false; + + public var freezeCounter:int; + public var frozen:Boolean = false; + public var aabb:BoundBox = new BoundBox(); + public var postCollisionPredicate:IBodyCollisionPredicate; + + altphysics var id:int; + // Мир, в котором находится тело + altphysics var world:RigidWorld; + // Текущее и предыдущее состояние тела. Промежуточное состояние вычисляется линейной интерполяцией. + altphysics var state:BodyState = new BodyState(); + altphysics var prevState:BodyState = new BodyState(); + // Линейное и угловое ускорение тела на текущем шаге симуляции + altphysics var accel:Vector3 = new Vector3(); + altphysics var angleAccel:Vector3 = new Vector3(); + // Материал тела + altphysics var material:BodyMaterial = new BodyMaterial(); + + altphysics var invMass:Number = 1; + altphysics var invInertia:Matrix3 = new Matrix3(); + altphysics var invInertiaWorld:Matrix3 = new Matrix3(); + altphysics var baseMatrix:Matrix3 = new Matrix3(); + + altphysics const MAX_CONTACTS:int = 20; + altphysics var contacts:Vector. = new Vector.(MAX_CONTACTS); + altphysics var contactsNum:int; + + altphysics var collisionPrimitives:Vector.; + altphysics var collisionPrimitivesNum:int; + + // Аккумулятор сил + altphysics var forceAccum:Vector3 = new Vector3(); + // Аккумулятор моментов + altphysics var torqueAccum:Vector3 = new Vector3(); + + // Внутренние переменные для избежания создания экземпляров + private static var _r:Vector3 = new Vector3(); + private static var _f:Vector3 = new Vector3(); + + /** + * + * @param invMass + * @param invInertia + */ + public function Body(invMass:Number, invInertia:Matrix3) { + this.invMass = invMass; + this.invInertia.copy(invInertia); + } + + /** + * @param primitive + * @param localTransform + */ + public function addCollisionPrimitive(primitive:CollisionPrimitive, localTransform:Matrix4 = null):void { + if (primitive == null) throw new ArgumentError("Primitive cannot be null"); + if (collisionPrimitives == null) collisionPrimitives = new Vector.(); + collisionPrimitives[collisionPrimitivesNum++] = primitive; + primitive.setBody(this, localTransform); + } + + /** + * @param t + * @param result + */ + public function interpolate(t:Number, pos:Vector3, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.w = prevState.orientation.w*t1 + state.orientation.w*t; + orientation.x = prevState.orientation.x*t1 + state.orientation.x*t; + orientation.y = prevState.orientation.y*t1 + state.orientation.y*t; + orientation.z = prevState.orientation.z*t1 + state.orientation.z*t; + } + + /** + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + state.pos.vCopy(pos); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setPositionXYZ(x:Number, y:Number, z:Number):void { + state.pos.vReset(x, y, z); + } + + /** + * + * @param vel + */ + public function setVelocity(vel:Vector3):void { + state.velocity.vCopy(vel); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setVelocityXYZ(x:Number, y:Number, z:Number):void { + state.velocity.vReset(x, y, z); + } + + /** + * + * @param rot + */ + public function setRotation(rot:Vector3):void { + state.rotation.vCopy(rot); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setRotationXYZ(x:Number, y:Number, z:Number):void { + state.rotation.vReset(x, y, z); + } + + /** + * + * @param q + */ + public function setOrientation(q:Quaternion):void { + state.orientation.copy(q); + } + + /** + * @param r + * @param dir + * @param magnitude + */ + public function applyRelPosWorldImpulse(r:Vector3, dir:Vector3, magnitude:Number):void { + var d:Number = magnitude*invMass; + // Линейная часть + state.velocity.x += d*dir.x; + state.velocity.y += d*dir.y; + state.velocity.z += d*dir.z; + + // Вращательная часть + var rx:Number = r.x; + var ry:Number = r.y; + var rz:Number = r.z; + _r.x = (ry*dir.z - rz*dir.y)*magnitude; + _r.y = (rz*dir.x - rx*dir.z)*magnitude; + _r.z = (rx*dir.y - ry*dir.x)*magnitude; + _r.vTransformBy3(invInertiaWorld); + state.rotation.x += _r.x; + state.rotation.y += _r.y; + state.rotation.z += _r.z; + } + + /** + * @param f + */ + public function addForce(f:Vector3):void { + forceAccum.vAdd(f); + } + + /** + * @param pos + * @param f + */ + public function addWorldForce(pos:Vector3, force:Vector3):void { + forceAccum.vAdd(force); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(force)); + } + + /** + * @param pos + * @param f + */ + public function addWorldForceScaled(pos:Vector3, force:Vector3, scale:Number):void { + _f.x = scale*force.x, _f.y = scale*force.y, _f.z = scale*force.z; + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(_f)); + } + + /** + * @param pos + * @param f + */ + public function addLocalForce(pos:Vector3, force:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(pos, _r); + // Трансформируем вектор силы в мировую систему + baseMatrix.transformVector(force, _f); + // Добавляем силу и момент + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vCross(_f)); + } + + /** + * + * @param localPos + * @param worldForce + */ + public function addWorldForceAtLocalPoint(localPos:Vector3, worldForce:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(localPos, _r); + // Добавляем силу и момент + forceAccum.vAdd(worldForce); + torqueAccum.vAdd(_r.vCross(worldForce)); + } + + /** + * @param dt + */ + public function addExternalForces(dt:Number):void { + } + + /** + * @param t + */ + public function addTorque(t:Vector3):void { + torqueAccum.vAdd(t); + } + + /** + * + */ + altphysics function clearAccumulators():void { + forceAccum.vReset(); + torqueAccum.vReset(); + } + + /** + * + */ + altphysics function calcAccelerations():void { + accel.x = forceAccum.x*invMass; + accel.y = forceAccum.y*invMass; + accel.z = forceAccum.z*invMass; + invInertiaWorld.transformVector(torqueAccum, angleAccel); + } + + /** + * Вычисляет производные данные. + */ + altphysics function calcDerivedData():void { + // Вычисление базисной матрицы и обратного тензора инерции в мировых координатах + state.orientation.toMatrix3(baseMatrix); + invInertiaWorld.copy(invInertia).append(baseMatrix).prependTransposed(baseMatrix); + if (collisionPrimitives != null) { + aabb.infinity(); + for (var i:int = 0; i < collisionPrimitivesNum; i++) { + var primitive:CollisionPrimitive = collisionPrimitives[i]; + primitive.transform.setFromMatrix3(baseMatrix, state.pos); + if (primitive.localTransform != null) primitive.transform.prepend(primitive.localTransform); + primitive.calculateAABB(); + aabb.addBoundBox(primitive.aabb); + } + } + } + + /** + * + */ + altphysics function saveState():void { + prevState.copy(state); + } + + /** + * + */ + altphysics function restoreState():void { + state.copy(prevState); + } + + /** + * @param dt + */ + altphysics function integrateVelocity(dt:Number):void { + // v = v + a*t + state.velocity.x += accel.x*dt; + state.velocity.y += accel.y*dt; + state.velocity.z += accel.z*dt; + // rot = rot + eps*t + state.rotation.x += angleAccel.x*dt; + state.rotation.y += angleAccel.y*dt; + state.rotation.z += angleAccel.z*dt; + + state.velocity.x *= linDamping; + state.velocity.y *= linDamping; + state.velocity.z *= linDamping; + + state.rotation.x *= rotDamping; + state.rotation.y *= rotDamping; + state.rotation.z *= rotDamping; + } + + /** + * + */ + altphysics function integratePosition(dt:Number):void { + // pos = pos + v*t + state.pos.x += state.velocity.x*dt; + state.pos.y += state.velocity.y*dt; + state.pos.z += state.velocity.z*dt; + // q = q + 0.5*rot*q + state.orientation.addScaledVector(state.rotation, dt); + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/rigid/.svn/text-base/BodyMaterial.as.svn-base b/0.0.5.0/src/alternativa/physics/rigid/.svn/text-base/BodyMaterial.as.svn-base new file mode 100644 index 0000000..61d4382 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/rigid/.svn/text-base/BodyMaterial.as.svn-base @@ -0,0 +1,11 @@ +package alternativa.physics.rigid { + + public class BodyMaterial { + + public var restitution:Number = 0; + public var friction:Number = 0.3; +// public var dynamicFriction:Number = 0.2; +// public var dynamicFriction:Number = 0.2; + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/rigid/.svn/text-base/BodyState.as.svn-base b/0.0.5.0/src/alternativa/physics/rigid/.svn/text-base/BodyState.as.svn-base new file mode 100644 index 0000000..408b0ea --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/rigid/.svn/text-base/BodyState.as.svn-base @@ -0,0 +1,39 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Quaternion; + import alternativa.physics.types.Vector3; + + /** + * Класс описывает состояние твёрдого тела. + */ + public class BodyState { + /** + * Положение тела. + */ + public var pos:Vector3 = new Vector3(); + /** + * Ориентация тела. + */ + public var orientation:Quaternion = new Quaternion(); + /** + * Скорость тела. + */ + public var velocity:Vector3 = new Vector3(); + /** + * Угловая скорость тела. + */ + public var rotation:Vector3 = new Vector3(); + + /** + * Копирует значение указанного объекта. + * + * @param state + */ + public function copy(state:BodyState):void { + pos.vCopy(state.pos); + orientation.copy(state.orientation); + velocity.vCopy(state.velocity); + rotation.vCopy(state.rotation); + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/rigid/.svn/text-base/Contact.as.svn-base b/0.0.5.0/src/alternativa/physics/rigid/.svn/text-base/Contact.as.svn-base new file mode 100644 index 0000000..b73fc55 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/rigid/.svn/text-base/Contact.as.svn-base @@ -0,0 +1,53 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.types.Vector3; + + /** + * Информация о контакте между двумя телами. Содержит нормаль, множество точек контакта и прочие величины. + */ + public class Contact { + +// private static var pool:Vector. = new Vector.(); +// +// public static function create():ContactInfo { +// if (pool.length > 0) { +// return pool.pop(); +// } +// return new ContactInfo(); +// } +// +// public static function destroy(collInfo:ContactInfo):void { +// collInfo.body1 = collInfo.body2 = null; +// pool.push(collInfo); +// } + + // Максимальное количество точек контакта + private const N:int = 8; + + public var body1:Body; + public var body2:Body; + // Взаимный коэффициент отскока + public var restitution:Number; + // Взаимный коэффициент трения + public var friction:Number; + // Нормаль контакта. Направлена от второго тела к первому. + public var normal:Vector3 = new Vector3(); + // Список точек контакта + public var points:Vector. = new Vector.(N, true); + // Количество точек контакта + public var pcount:int; + // Максимальная глубина пересечения тел + public var maxPenetration:Number = 0; + + public var satisfied:Boolean; + + /** + * + */ + public function Contact() { + for (var i:int = 0; i < N; i++) points[i] = new ContactPoint(); + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/rigid/.svn/text-base/ContactPoint.as.svn-base b/0.0.5.0/src/alternativa/physics/rigid/.svn/text-base/ContactPoint.as.svn-base new file mode 100644 index 0000000..c36f958 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/rigid/.svn/text-base/ContactPoint.as.svn-base @@ -0,0 +1,52 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Vector3; + + public class ContactPoint { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + + // Величины, расчитываемые перед началом фазы решения контактов + + // Требуемая проекция конечной скорости на нормаль для упругого контакта + public var normalVel:Number; + // Минимальная скорость разделения неупругого контакта + public var minSepVel:Number; + // Изменение проекции скорости на единицу нормального импульса + public var velByUnitImpulseN:Number; + + public var angularInertia1:Number; + public var angularInertia2:Number; + + // Радиус-вектор точки контакта относительно центра первого тела + public var r1:Vector3 = new Vector3(); + // Радиус-вектор точки контакта относительно центра второго тела + public var r2:Vector3 = new Vector3(); + + // Величины, накапливаемые во время фазы решения контактов + + // Накопленный импульс, применяемый для получения требуемой относительной скорости в точке контакта + public var accumImpulseN:Number; + // Накопленный импульс, применяемый для разделения тел в точке контакта. Не создаёт момента. +// public var accumSepImpulse:Number; + + public var satisfied:Boolean; + + /** + * + * @param cp + */ + public function copyFrom(cp:ContactPoint):void { + pos.vCopy(cp.pos); + penetration = cp.penetration; + feature1 = cp.feature1; + feature2 = cp.feature2; + r1.vCopy(cp.r1); + r2.vCopy(cp.r2); + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/rigid/.svn/text-base/PhysicsUtils.as.svn-base b/0.0.5.0/src/alternativa/physics/rigid/.svn/text-base/PhysicsUtils.as.svn-base new file mode 100644 index 0000000..a9aaa99 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/rigid/.svn/text-base/PhysicsUtils.as.svn-base @@ -0,0 +1,49 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + public class PhysicsUtils { + + /** + * @param mass + * @param halfSize + * @param result + */ + public static function getBoxInvInertia(mass:Number, halfSize:Vector3, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + result.a = 3/(mass*(yy + zz)); + result.f = 3/(mass*(zz + xx)); + result.k = 3/(mass*(xx + yy)); + } + + /** + * @param mass + * @param r + * @param h + * @param result + */ + public static function getCylinderInvInertia(mass:Number, r:Number, h:Number, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + + result.a = result.f = 1/(mass*(h*h/12 + r*r/4)); + result.k = 2/(mass*r*r); + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/rigid/.svn/text-base/RigidWorld.as.svn-base b/0.0.5.0/src/alternativa/physics/rigid/.svn/text-base/RigidWorld.as.svn-base new file mode 100644 index 0000000..696580b --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/rigid/.svn/text-base/RigidWorld.as.svn-base @@ -0,0 +1,653 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.ICollisionDetector; + import alternativa.physics.collision.KdTreeCollisionDetector; + import alternativa.physics.rigid.constraints.Constraint; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Класс реализует физическую симуляцию поведения твёрдых тел. + */ + public class RigidWorld { + + private static var lastBodyId:int; + + // Максимальное количество контактов + altphysics const MAX_CONTACTS:int = 1000; + + ////////////////////////////////////////////////////////////////// + // Настроечные параметры симуляции. Могут быть изменены в любой + // момент времени без нарушения корректной работы симуляции. + ////////////////////////////////////////////////////////////////// + // Количество шагов, за которое пересекающиеся тела должны разделиться + public var penResolutionSteps:int = 10; + // Величина допустимой глубины пересечения + public var allowedPenetration:Number = 0.1; + // Максимальная скорость, добавляемая с целью разделения тел + public var maxPenResolutionSpeed:Number = 0.5; + // Количество итераций для обработки упругих контактов + public var collisionIterations:int = 5; + // Количество итераций для обработки неупругих контактов + public var contactIterations:int = 5; + // Флаг использования предсказания состояний + public var usePrediction:Boolean = false; + + public var freezeSteps:int = 10; + public var linSpeedFreezeLimit:Number = 1; + public var angSpeedFreezeLimit:Number = 0.01; + + // Переменные для процедуры разделения тел путём непосредственного их перемещенеия. + // !!!!!!!!!!!!!!!!!!!!!!!! + // !!! Экспериментально !!! + // !!!!!!!!!!!!!!!!!!!!!!!! + public var staticSeparationIterations:int = 10; + public var staticSeparationSteps:int = 10; + public var maxAngleMove:Number = 10; + public var useStaticSeparation:Boolean = false; + + // Вектор гравитации + altphysics var _gravity:Vector3 = new Vector3(0, 0, -9.8); + // Использующийся детектор столкновений + altphysics var collisionDetector:ICollisionDetector; + // Список тел, участвующих в симуляции + altphysics var bodies:Vector. = new Vector.(); + // Количество тел, участвующих в симуляции + altphysics var bodiesNum:int; + // Список контактов на текущем шаге симуляции + altphysics var contacts:Vector. = new Vector.(MAX_CONTACTS, true); + // Количество контактов на текущем шаге симуляции + altphysics var contactsNum:int; + // Список ограничений + altphysics var constraints:Vector. = new Vector.(); + // Количество ограничений + altphysics var constraintsNum:int; + // Временная метка. Число прошедших шагов с начала симуляции. + altphysics var timeStamp:uint; + + // Временные переменные для избежания создания экземпляров + private var _r:Vector3 = new Vector3(); + private var _t:Vector3 = new Vector3(); + private var _v:Vector3 = new Vector3(); + private var _v1:Vector3 = new Vector3(); + private var _v2:Vector3 = new Vector3(); + + /** + * + */ + public function RigidWorld() { + for (var i:int = 0; i < MAX_CONTACTS; i++) contacts[i] = new Contact(); + collisionDetector = new KdTreeCollisionDetector(); + } + + /** + * Вектор гравитации. + */ + public function get gravity():Vector3 { + return _gravity.vClone(); + } + + /** + * @private + */ + public function set gravity(value:Vector3):void { + _gravity.vCopy(value); + } + + /** + * Добавляет тело в симуляцию. + * @param body + * @return + */ + public function addBody(body:Body, isStatic:Boolean):Boolean { + var i:int = bodies.indexOf(body); + if (i > -1) return false; + bodies[bodiesNum++] = body; + body.world = this; + body.id = lastBodyId++; +// if (body.collisionPrimitives != null) { +// for (i = 0; i < body.collisionPrimitivesNum; i++) collisionDetector.addPrimitive(body.collisionPrimitives[i], isStatic); +// } + return true; + } + + /** + * Удаляет тело из симуляции. + * @param body + * @return + */ + public function removeBody(body:Body, isStatic:Boolean):Boolean { + var i:int = bodies.indexOf(body); + if (i == -1) return false; + bodies.splice(i, 1); + body.world = null; +// if (body.collisionPrimitives != null) { +// for (i = 0; i < body.collisionPrimitivesNum; i++) collisionDetector.removePrimitive(body.collisionPrimitives[i], isStatic); +// } + bodiesNum--; + return true; + } + + /** + * Добавляет ограничение. + * @param c + */ + public function addConstraint(c:Constraint):void { + constraints[constraintsNum++] = c; + c.world = this; + } + + /** + * Удаляет ограничение. + * @param c + */ + public function removeConstraint(c:Constraint):Boolean { + var idx:int = constraints.indexOf(c); + if (idx < 0) return false; + constraints.splice(idx, 1) + constraintsNum--; + c.world = null; + return true; + } + + /** + * Применяет к телам действующте на них силы. Аккумуляторы сил и моментов тел очищаются после завершения шага + * симуляции, поэтому на момент вызова метода могут уже содержать некоторые значения. + * + * @param dt промежуток времени, в течении которого действуют силы + */ + private function applyForces(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i]; + body.addExternalForces(dt); + body.calcAccelerations(); + // Ускорение свободного падения применяется только к подвижным телам во избежание некорректного изменения + // фиктивной скорости неподвижных тел. + if (body.movable && !body.frozen) { + body.accel.x += _gravity.x; + body.accel.y += _gravity.y; + body.accel.z += _gravity.z; + } + } + } + + /** + * Определяет все столкновения на текущем шаге симуляции и заполняет список получившихся контактов. + * + * @param dt длительность шага симуляции + */ + private function detectCollisions(dt:Number):void { + var i:int; + var j:int; + var body:Body; + + for (i = 0; i < bodiesNum; i++) { + body = bodies[i]; + if (body.frozen) continue; + body.contactsNum = 0; + body.saveState(); + // При включённом режиме предсказания состояние тел интегрируется на один шаг вперёд + if (usePrediction) { + body.integrateVelocity(dt); + body.integratePosition(dt); + } + body.calcDerivedData(); + } + + contactsNum = collisionDetector.getAllCollisions(contacts); + + // Расчёт относительных векторов точки контакта вынесен сюда из-за необходимости учитывать + // положение тел в предсказанном состоянии + for (i = 0; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + var b1:Body = contact.body1; + var b2:Body = contact.body2; + for (j = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.r1.x = cp.pos.x - b1.state.pos.x; + cp.r1.y = cp.pos.y - b1.state.pos.y; + cp.r1.z = cp.pos.z - b1.state.pos.z; + if (b2 != null) { + cp.r2.x = cp.pos.x - b2.state.pos.x; + cp.r2.y = cp.pos.y - b2.state.pos.y; + cp.r2.z = cp.pos.z - b2.state.pos.z; + } + } + } + + // Восстановление состояния тел + if (usePrediction) { + for (i = 0; i < bodiesNum; i++) { + body = bodies[i]; + if (body.frozen) continue; + body.restoreState(); + body.calcDerivedData(); + } + } + } + + /** + * Подготваливает полученные из детектора столкновений контакты, расчитывая значения, не меняющиеся + * в ходе шага симуляции. + */ + private function preProcessContacts(dt:Number):void { + var i:int; + for (i = 0; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + var b1:Body = contact.body1; + var b2:Body = contact.body2; + // Столкнувшиеся тела размораживаются + if (b1.frozen) { + b1.frozen = false; + b1.freezeCounter = 0; + } + if (b2 != null && b2.frozen) { + b2.frozen = false; + b2.freezeCounter = 0; + } + contact.restitution = b1.material.restitution; + if (b2 != null && b2.material.restitution < contact.restitution) contact.restitution = b2.material.restitution; + contact.friction = b1.material.friction; + if (b2 != null && b2.material.friction < contact.friction) contact.friction = b2.material.friction; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.accumImpulseN = 0; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + cp.velByUnitImpulseN = 0; + if (b1.movable) { + cp.angularInertia1 = _v.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1).vDot(contact.normal); + cp.velByUnitImpulseN += b1.invMass + cp.angularInertia1; + } + if (b2 != null && b2.movable) { + cp.angularInertia2 = _v.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2).vDot(contact.normal); + cp.velByUnitImpulseN += b2.invMass + cp.angularInertia2; + } + // Расчёт требуемой конечной скорости для упругого контакта + calcSepVelocity(b1, b2, cp, _v); + cp.normalVel = _v.vDot(contact.normal); + if (cp.normalVel < 0) cp.normalVel = - contact.restitution*cp.normalVel; + // Скорость разделения неупругого контакта + cp.minSepVel = cp.penetration > allowedPenetration ? (cp.penetration - allowedPenetration)/(penResolutionSteps*dt) : 0; + if (cp.minSepVel > maxPenResolutionSpeed) cp.minSepVel = maxPenResolutionSpeed; + } + } + for (i = 0; i < constraintsNum; i++) Constraint(constraints[i]).preProcess(dt); + } + + /** + * + * @param dt + * @param forceInelastic + */ + private function processContacts(dt:Number, forceInelastic:Boolean):void { + var iterNum:int = forceInelastic ? contactIterations : collisionIterations; + var i:int; + var forwardLoop:Boolean = false; + for (var iter:int = 0; iter < iterNum; iter++) { + forwardLoop = !forwardLoop; + for (i = 0; i < contactsNum; i++) resolveContact(contacts[i], forceInelastic, forwardLoop); + // Ограничения + for (i = 0; i < constraintsNum; i++) Constraint(constraints[i]).apply(dt); + } + + // Разделение контактов путём непосредственного изменения координат и ориентации. Экспериментально. +// if (forceInelastic && useStaticSeparation) performStaticSeparation(); + } + + /** + * + */ + private function resolveContact(contactInfo:Contact, forceInelastic:Boolean, forwardLoop:Boolean):void { + var b1:Body = contactInfo.body1; + var b2:Body = contactInfo.body2; + var normal:Vector3 = contactInfo.normal; + var i:int; + if (forwardLoop) { + for (i = 0; i < contactInfo.pcount; i++) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } else { + for (i = contactInfo.pcount - 1; i >= 0; i--) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } + } + + /** + * + * @param idx + * @param b1 + * @param b2 + * @param colInfo + * @param normal + * @param forceInelastic + */ + private function resolveContactPoint(idx:int, b1:Body, b2:Body, contact:Contact, normal:Vector3, forceInelastic:Boolean):void { + var cp:ContactPoint = contact.points[idx]; + if (!forceInelastic) cp.satisfied = true; + + var newVel:Number = 0; + calcSepVelocity(b1, b2, cp, _v); + var sepVel:Number = _v.vDot(contact.normal); + if (forceInelastic) { + var minSpeVel:Number = useStaticSeparation ? 0 : cp.minSepVel; + if (sepVel < minSpeVel) cp.satisfied = false; + else if (cp.satisfied) return; + newVel = minSpeVel; + } else { + newVel = cp.normalVel; + } + var deltaVel:Number = newVel - sepVel; + var impulse:Number = deltaVel/cp.velByUnitImpulseN; + var accumImpulse:Number = cp.accumImpulseN + impulse; + if (accumImpulse < 0) accumImpulse = 0; + var deltaImpulse:Number = accumImpulse - cp.accumImpulseN; + cp.accumImpulseN = accumImpulse; + // Применяем импульс к телам + if (b1.movable) b1.applyRelPosWorldImpulse(cp.r1, normal, deltaImpulse); + if (b2 != null && b2.movable) b2.applyRelPosWorldImpulse(cp.r2, normal, -deltaImpulse); + + // Учёт силы трения + calcSepVelocity(b1, b2, cp, _v); + // Расчитываем изменение касательной скорости на единицу касательного импульса + var tanSpeedByUnitImpulse:Number = 0; + _v.vAddScaled(-_v.vDot(contact.normal), contact.normal); + var tanSpeed:Number = _v.vLength(); + if (tanSpeed < 0.001) return; + _t.vCopy(_v).vNormalize().vReverse(); + // dV = b.invMass + ((invI * (r % t)) % r) * t + if (b1.movable) { + _v.vCross2(cp.r1, _t).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1); + tanSpeedByUnitImpulse += b1.invMass + _v.vDot(_t); + } + if (b2 != null && b2.movable) { + _v.vCross2(cp.r2, _t).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2); + tanSpeedByUnitImpulse += b2.invMass + _v.vDot(_t); + } + + var tanImpulse:Number = tanSpeed/tanSpeedByUnitImpulse; + var max:Number = contact.friction*cp.accumImpulseN; + if (max < 0) { + if (tanImpulse < max) tanImpulse = max; + } else { + if (tanImpulse > max) tanImpulse = max; + } + + // Применяем импульс к телам + if (b1.movable) b1.applyRelPosWorldImpulse(cp.r1, _t, tanImpulse); + if (b2 != null && b2.movable) b2.applyRelPosWorldImpulse(cp.r2, _t, -tanImpulse); + } + + /** + * + * @param cp + * @param normal + * @return + */ + private function calcSepVelocity(body1:Body, body2:Body, cp:ContactPoint, result:Vector3):void { + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 +// result.vCopy(body1.state.velocity).vAdd(_v1.vCross2(body1.state.rotation, cp.r1)); + var rot:Vector3 = body1.state.rotation; + var v:Vector3 = cp.r1; + var x:Number = rot.y*v.z - rot.z*v.y; + var y:Number = rot.z*v.x - rot.x*v.z; + var z:Number = rot.x*v.y - rot.y*v.x; + v = body1.state.velocity; + result.x = v.x + x; + result.y = v.y + y; + result.z = v.z + z; + // V2 = V2_c + w2%r2 + if (body2 != null) { +// result.vSubtract(body2.state.velocity).vSubtract(_v2.vCross2(body2.state.rotation, cp.r2)); + rot = body2.state.rotation; + v = cp.r2; + x = rot.y*v.z - rot.z*v.y; + y = rot.z*v.x - rot.x*v.z; + z = rot.x*v.y - rot.y*v.x; + v = body2.state.velocity; + result.x -= v.x + x; + result.y -= v.y + y; + result.z -= v.z + z; + } + } + + /** + * + * @param dt + */ + private function intergateVelocities(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + (bodies[i] as Body).integrateVelocity(dt); + } + } + + /** + * + * @param dt + */ + private function integratePositions(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i] as Body; + if (body.movable && !body.frozen) body.integratePosition(dt); + } + } + + /** + * + */ + private function performStaticSeparation():void { + var iterNum:int = staticSeparationIterations; +// iterNum = 100; + + // 1. В начале каждой итерации для всех контактов сбрасывается флаг satisfied с одновременным поиском наихудшего контакта, т.е. имеющего наибольшую величину пересечения тел. + // 2. Если найденный контакт имеет величину пересечения меньше предельно допустимой процедура прерывается, т.к. разделение контактов не требуется. + // 3. Если процедура продолжается, то выполняется разделение найденного контакта и установка его флага satisfied в true, после чего запускается внутренний цикл, состоящий + // из contactsNum - 1 итераций. На каждой итерации ищется наихудший контакт среди оставшихся (satisfied == false). Если такой контакт найден, то выполняется его разделение + // и переход к следующей итерации внутреннего цикла, иначе внутренний цикл прерывается и выполняется переход к следующей итерации внешнего цикла (шаг 1.). + for (var iter:int = 0; iter < iterNum; iter++) { + // Ищем контакт с максимальной величиной пересечения тел, одновременно сбрасывая флаги satisfied в false + var worstContact:Contact = contacts[0]; + var i:int; + for (i = 1; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + contact.satisfied = false; + if (contact.maxPenetration > worstContact.maxPenetration) worstContact = contact; + } + if (worstContact.maxPenetration <= allowedPenetration) return; + + resolveInterpenetration(worstContact); + // Внутренний цикл по оставшимся контактам + for (i = 1; i < contactsNum; i++) { + worstContact = getWorstContact(); + if (worstContact == null) break; + resolveInterpenetration(worstContact); + } + } + } + + /** + * + */ + private function getWorstContact():Contact { + var maxPen:Number = 0; + var worst:Contact = null; + for (var i:int = 0; i < contactsNum; i++) { + var c:Contact = contacts[i]; + if (!c.satisfied && c.maxPenetration > maxPen) { + worst = c; + maxPen = c.maxPenetration; + } + } + return maxPen > allowedPenetration ? worst : null; + } + + /** + * Разделяет указанный контакт, выполняя staticSeparationSteps итераций по списку точек контакта. + * 1. В начале каждой итерации ищется точка с наибольшим пересечением, одновременно сбрасываются флаги satisfied у точек. + * 2. Для найденной точки выполняется процедура разделения. + * 3. После выполняется pcount - 1 итерация по списку точек, каждый раз ищется наихудшая среди имеющих satisfied == false, для которой выполняется процедура разделения. + * Если наихудшая точка не найдена, то выполнется переход к следующей итерации внешнего цикла (шаг 1). + * + * В конце процедуры разделения обновляются значения пересечений для остальных точек контакта, а также для всех прочих контактов, относящихся к телам текущего. + * + * @param contact контакт для разделения + */ + private function resolveInterpenetration(contact:Contact):void { + contact.satisfied = true; + + for (var step:int = 0; step < staticSeparationSteps; step++) { + var worstCp:ContactPoint = contact.points[0]; + var cp:ContactPoint; + var i:int; + for (i = 1; i < contact.pcount; i++) { + cp = contact.points[i]; + cp.satisfied = false; + if (cp.penetration > worstCp.penetration) worstCp = cp; + } + if (worstCp.penetration <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + // Разделяем оставшиеся точки + var maxPen:Number = 0; + for (i = 1; i < contact.pcount; i++) { + // Поиск наихудшей точки + for (var j:int = 0; j < contact.pcount; j++) { + cp = contact.points[j]; + if (cp.satisfied) continue; + if (cp.penetration > maxPen) { + maxPen = cp.penetration; + worstCp = cp; + } + } + if (maxPen <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + } + } + } + + /** + * + * @param cp + * @param contact + */ + private function separateContactPoint(cp:ContactPoint, contact:Contact):void { + cp.satisfied = true; + + var b1:Body = contact.body1; + var b2:Body = contact.body2; + var totalMove:Number = cp.penetration - allowedPenetration; + var moveCoeff:Number = totalMove/cp.velByUnitImpulseN; + var linMove1:Number; + var angleMove1:Number; + if (b1.movable) { + linMove1 = b1.invMass*moveCoeff; + angleMove1 = cp.angularInertia1*moveCoeff; + if (angleMove1 > maxAngleMove) { + linMove1 += angleMove1 - maxAngleMove; + angleMove1 -= maxAngleMove; + } + b1.state.pos.vAddScaled(linMove1, contact.normal); + _v1.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vScale(angleMove1); + b1.state.orientation.addScaledVector(_v1, 1); + } + var linMove2:Number; + var angleMove2:Number; + if (b2 != null && b2.movable) { + linMove2 = b2.invMass*moveCoeff; + angleMove2 = cp.angularInertia2*moveCoeff; + if (angleMove2 > maxAngleMove) { + linMove2 += angleMove2 - maxAngleMove; + angleMove2 -= maxAngleMove; + } + b2.state.pos.vAddScaled(-linMove2, contact.normal); + _v2.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vScale(angleMove2); + _v2.vReverse(); + b2.state.orientation.addScaledVector(_v2, 1); + } + cp.penetration = allowedPenetration; + // Обновляем пересечения в других точках + var i:int; + for (i = 0; i < contact.pcount; i++) { + var cp1:ContactPoint = contact.points[i]; + if (cp1 == cp) continue; + var angularMove:Number; + if (b1.movable) { + angularMove = _v.vCross2(_v1, cp1.r1).vDot(contact.normal); + cp1.penetration -= linMove1 + angularMove; + } + if (b2 != null && b2.movable) { + angularMove = _v.vCross2(_v2, cp1.r2).vDot(contact.normal); + cp1.penetration -= linMove2 - angularMove; + } + // Обновление максимального значения для контакта + if (cp1.penetration > contact.maxPenetration) contact.maxPenetration = cp1.penetration; + } + // Обновляем пересечения для других контактов + var c:Contact; + var j:int; + if (b1.movable) { + for (i = 0; i < b1.contactsNum; i++) { + c = b1.contacts[i]; + if (c == contact) continue; + for (j = 0; j < c.pcount; j++) { + cp1 = c.points[j]; + if (b1 == c.body1) cp1.penetration -= linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r1).vDot(c.normal); + else cp1.penetration += linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r2).vDot(c.normal); + if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; + } + } + } + if (b2 != null && b2.movable) { + for (i = 0; i < b2.contactsNum; i++) { + c = b2.contacts[i]; + if (c == contact) continue; + for (j = 0; j < c.pcount; j++) { + cp1 = c.points[j]; + if (b2 == c.body1) cp1.penetration -= linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r1).vDot(c.normal); + else cp1.penetration += linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r2).vDot(c.normal); + if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; + } + } + } + } + + /** + * + */ + private function postPhysics():void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i]; + body.clearAccumulators(); + body.calcDerivedData(); + if (body.canFreeze) { + if (body.state.velocity.vLength() < linSpeedFreezeLimit && body.state.rotation.vLength() < angSpeedFreezeLimit) { + if (!body.frozen) { + body.freezeCounter++; + if (body.freezeCounter >= freezeSteps) body.frozen = true; + } + } else { + body.freezeCounter = 0; + body.frozen = false; + } + } + } + } + + /** + * + * @param dt + */ + public function runPhysics(dt:Number):void { + timeStamp++; + applyForces(dt); + detectCollisions(dt); + preProcessContacts(dt); + processContacts(dt, false); + intergateVelocities(dt); + processContacts(dt, true); + integratePositions(dt); + postPhysics(); + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/rigid/Body.as b/0.0.5.0/src/alternativa/physics/rigid/Body.as new file mode 100644 index 0000000..7c08ce9 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/rigid/Body.as @@ -0,0 +1,340 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.IBodyCollisionPredicate; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Quaternion; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class Body { + + public static var linDamping:Number = 0.997; + public static var rotDamping:Number = 0.997; + + public var name:String; + public var movable:Boolean = true; + public var canFreeze:Boolean = false; + + public var freezeCounter:int; + public var frozen:Boolean = false; + public var aabb:BoundBox = new BoundBox(); + public var postCollisionPredicate:IBodyCollisionPredicate; + + altphysics var id:int; + // Мир, в котором находится тело + altphysics var world:RigidWorld; + // Текущее и предыдущее состояние тела. Промежуточное состояние вычисляется линейной интерполяцией. + altphysics var state:BodyState = new BodyState(); + altphysics var prevState:BodyState = new BodyState(); + // Линейное и угловое ускорение тела на текущем шаге симуляции + altphysics var accel:Vector3 = new Vector3(); + altphysics var angleAccel:Vector3 = new Vector3(); + // Материал тела + altphysics var material:BodyMaterial = new BodyMaterial(); + + altphysics var invMass:Number = 1; + altphysics var invInertia:Matrix3 = new Matrix3(); + altphysics var invInertiaWorld:Matrix3 = new Matrix3(); + altphysics var baseMatrix:Matrix3 = new Matrix3(); + + altphysics const MAX_CONTACTS:int = 20; + altphysics var contacts:Vector. = new Vector.(MAX_CONTACTS); + altphysics var contactsNum:int; + + altphysics var collisionPrimitives:Vector.; + altphysics var collisionPrimitivesNum:int; + + // Аккумулятор сил + altphysics var forceAccum:Vector3 = new Vector3(); + // Аккумулятор моментов + altphysics var torqueAccum:Vector3 = new Vector3(); + + // Внутренние переменные для избежания создания экземпляров + private static var _r:Vector3 = new Vector3(); + private static var _f:Vector3 = new Vector3(); + + /** + * + * @param invMass + * @param invInertia + */ + public function Body(invMass:Number, invInertia:Matrix3) { + this.invMass = invMass; + this.invInertia.copy(invInertia); + } + + /** + * @param primitive + * @param localTransform + */ + public function addCollisionPrimitive(primitive:CollisionPrimitive, localTransform:Matrix4 = null):void { + if (primitive == null) throw new ArgumentError("Primitive cannot be null"); + if (collisionPrimitives == null) collisionPrimitives = new Vector.(); + collisionPrimitives[collisionPrimitivesNum++] = primitive; + primitive.setBody(this, localTransform); + } + + /** + * @param t + * @param result + */ + public function interpolate(t:Number, pos:Vector3, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.w = prevState.orientation.w*t1 + state.orientation.w*t; + orientation.x = prevState.orientation.x*t1 + state.orientation.x*t; + orientation.y = prevState.orientation.y*t1 + state.orientation.y*t; + orientation.z = prevState.orientation.z*t1 + state.orientation.z*t; + } + + /** + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + state.pos.vCopy(pos); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setPositionXYZ(x:Number, y:Number, z:Number):void { + state.pos.vReset(x, y, z); + } + + /** + * + * @param vel + */ + public function setVelocity(vel:Vector3):void { + state.velocity.vCopy(vel); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setVelocityXYZ(x:Number, y:Number, z:Number):void { + state.velocity.vReset(x, y, z); + } + + /** + * + * @param rot + */ + public function setRotation(rot:Vector3):void { + state.rotation.vCopy(rot); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setRotationXYZ(x:Number, y:Number, z:Number):void { + state.rotation.vReset(x, y, z); + } + + /** + * + * @param q + */ + public function setOrientation(q:Quaternion):void { + state.orientation.copy(q); + } + + /** + * @param r + * @param dir + * @param magnitude + */ + public function applyRelPosWorldImpulse(r:Vector3, dir:Vector3, magnitude:Number):void { + var d:Number = magnitude*invMass; + // Линейная часть + state.velocity.x += d*dir.x; + state.velocity.y += d*dir.y; + state.velocity.z += d*dir.z; + + // Вращательная часть + var rx:Number = r.x; + var ry:Number = r.y; + var rz:Number = r.z; + _r.x = (ry*dir.z - rz*dir.y)*magnitude; + _r.y = (rz*dir.x - rx*dir.z)*magnitude; + _r.z = (rx*dir.y - ry*dir.x)*magnitude; + _r.vTransformBy3(invInertiaWorld); + state.rotation.x += _r.x; + state.rotation.y += _r.y; + state.rotation.z += _r.z; + } + + /** + * @param f + */ + public function addForce(f:Vector3):void { + forceAccum.vAdd(f); + } + + /** + * @param pos + * @param f + */ + public function addWorldForce(pos:Vector3, force:Vector3):void { + forceAccum.vAdd(force); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(force)); + } + + /** + * @param pos + * @param f + */ + public function addWorldForceScaled(pos:Vector3, force:Vector3, scale:Number):void { + _f.x = scale*force.x, _f.y = scale*force.y, _f.z = scale*force.z; + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(_f)); + } + + /** + * @param pos + * @param f + */ + public function addLocalForce(pos:Vector3, force:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(pos, _r); + // Трансформируем вектор силы в мировую систему + baseMatrix.transformVector(force, _f); + // Добавляем силу и момент + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vCross(_f)); + } + + /** + * + * @param localPos + * @param worldForce + */ + public function addWorldForceAtLocalPoint(localPos:Vector3, worldForce:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(localPos, _r); + // Добавляем силу и момент + forceAccum.vAdd(worldForce); + torqueAccum.vAdd(_r.vCross(worldForce)); + } + + /** + * @param dt + */ + public function addExternalForces(dt:Number):void { + } + + /** + * @param t + */ + public function addTorque(t:Vector3):void { + torqueAccum.vAdd(t); + } + + /** + * + */ + altphysics function clearAccumulators():void { + forceAccum.vReset(); + torqueAccum.vReset(); + } + + /** + * + */ + altphysics function calcAccelerations():void { + accel.x = forceAccum.x*invMass; + accel.y = forceAccum.y*invMass; + accel.z = forceAccum.z*invMass; + invInertiaWorld.transformVector(torqueAccum, angleAccel); + } + + /** + * Вычисляет производные данные. + */ + altphysics function calcDerivedData():void { + // Вычисление базисной матрицы и обратного тензора инерции в мировых координатах + state.orientation.toMatrix3(baseMatrix); + invInertiaWorld.copy(invInertia).append(baseMatrix).prependTransposed(baseMatrix); + if (collisionPrimitives != null) { + aabb.infinity(); + for (var i:int = 0; i < collisionPrimitivesNum; i++) { + var primitive:CollisionPrimitive = collisionPrimitives[i]; + primitive.transform.setFromMatrix3(baseMatrix, state.pos); + if (primitive.localTransform != null) primitive.transform.prepend(primitive.localTransform); + primitive.calculateAABB(); + aabb.addBoundBox(primitive.aabb); + } + } + } + + /** + * + */ + altphysics function saveState():void { + prevState.copy(state); + } + + /** + * + */ + altphysics function restoreState():void { + state.copy(prevState); + } + + /** + * @param dt + */ + altphysics function integrateVelocity(dt:Number):void { + // v = v + a*t + state.velocity.x += accel.x*dt; + state.velocity.y += accel.y*dt; + state.velocity.z += accel.z*dt; + // rot = rot + eps*t + state.rotation.x += angleAccel.x*dt; + state.rotation.y += angleAccel.y*dt; + state.rotation.z += angleAccel.z*dt; + + state.velocity.x *= linDamping; + state.velocity.y *= linDamping; + state.velocity.z *= linDamping; + + state.rotation.x *= rotDamping; + state.rotation.y *= rotDamping; + state.rotation.z *= rotDamping; + } + + /** + * + */ + altphysics function integratePosition(dt:Number):void { + // pos = pos + v*t + state.pos.x += state.velocity.x*dt; + state.pos.y += state.velocity.y*dt; + state.pos.z += state.velocity.z*dt; + // q = q + 0.5*rot*q + state.orientation.addScaledVector(state.rotation, dt); + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/rigid/BodyMaterial.as b/0.0.5.0/src/alternativa/physics/rigid/BodyMaterial.as new file mode 100644 index 0000000..61d4382 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/rigid/BodyMaterial.as @@ -0,0 +1,11 @@ +package alternativa.physics.rigid { + + public class BodyMaterial { + + public var restitution:Number = 0; + public var friction:Number = 0.3; +// public var dynamicFriction:Number = 0.2; +// public var dynamicFriction:Number = 0.2; + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/rigid/BodyState.as b/0.0.5.0/src/alternativa/physics/rigid/BodyState.as new file mode 100644 index 0000000..408b0ea --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/rigid/BodyState.as @@ -0,0 +1,39 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Quaternion; + import alternativa.physics.types.Vector3; + + /** + * Класс описывает состояние твёрдого тела. + */ + public class BodyState { + /** + * Положение тела. + */ + public var pos:Vector3 = new Vector3(); + /** + * Ориентация тела. + */ + public var orientation:Quaternion = new Quaternion(); + /** + * Скорость тела. + */ + public var velocity:Vector3 = new Vector3(); + /** + * Угловая скорость тела. + */ + public var rotation:Vector3 = new Vector3(); + + /** + * Копирует значение указанного объекта. + * + * @param state + */ + public function copy(state:BodyState):void { + pos.vCopy(state.pos); + orientation.copy(state.orientation); + velocity.vCopy(state.velocity); + rotation.vCopy(state.rotation); + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/rigid/Contact.as b/0.0.5.0/src/alternativa/physics/rigid/Contact.as new file mode 100644 index 0000000..b73fc55 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/rigid/Contact.as @@ -0,0 +1,53 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.types.Vector3; + + /** + * Информация о контакте между двумя телами. Содержит нормаль, множество точек контакта и прочие величины. + */ + public class Contact { + +// private static var pool:Vector. = new Vector.(); +// +// public static function create():ContactInfo { +// if (pool.length > 0) { +// return pool.pop(); +// } +// return new ContactInfo(); +// } +// +// public static function destroy(collInfo:ContactInfo):void { +// collInfo.body1 = collInfo.body2 = null; +// pool.push(collInfo); +// } + + // Максимальное количество точек контакта + private const N:int = 8; + + public var body1:Body; + public var body2:Body; + // Взаимный коэффициент отскока + public var restitution:Number; + // Взаимный коэффициент трения + public var friction:Number; + // Нормаль контакта. Направлена от второго тела к первому. + public var normal:Vector3 = new Vector3(); + // Список точек контакта + public var points:Vector. = new Vector.(N, true); + // Количество точек контакта + public var pcount:int; + // Максимальная глубина пересечения тел + public var maxPenetration:Number = 0; + + public var satisfied:Boolean; + + /** + * + */ + public function Contact() { + for (var i:int = 0; i < N; i++) points[i] = new ContactPoint(); + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/rigid/ContactPoint.as b/0.0.5.0/src/alternativa/physics/rigid/ContactPoint.as new file mode 100644 index 0000000..c36f958 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/rigid/ContactPoint.as @@ -0,0 +1,52 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Vector3; + + public class ContactPoint { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + + // Величины, расчитываемые перед началом фазы решения контактов + + // Требуемая проекция конечной скорости на нормаль для упругого контакта + public var normalVel:Number; + // Минимальная скорость разделения неупругого контакта + public var minSepVel:Number; + // Изменение проекции скорости на единицу нормального импульса + public var velByUnitImpulseN:Number; + + public var angularInertia1:Number; + public var angularInertia2:Number; + + // Радиус-вектор точки контакта относительно центра первого тела + public var r1:Vector3 = new Vector3(); + // Радиус-вектор точки контакта относительно центра второго тела + public var r2:Vector3 = new Vector3(); + + // Величины, накапливаемые во время фазы решения контактов + + // Накопленный импульс, применяемый для получения требуемой относительной скорости в точке контакта + public var accumImpulseN:Number; + // Накопленный импульс, применяемый для разделения тел в точке контакта. Не создаёт момента. +// public var accumSepImpulse:Number; + + public var satisfied:Boolean; + + /** + * + * @param cp + */ + public function copyFrom(cp:ContactPoint):void { + pos.vCopy(cp.pos); + penetration = cp.penetration; + feature1 = cp.feature1; + feature2 = cp.feature2; + r1.vCopy(cp.r1); + r2.vCopy(cp.r2); + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/rigid/PhysicsUtils.as b/0.0.5.0/src/alternativa/physics/rigid/PhysicsUtils.as new file mode 100644 index 0000000..a9aaa99 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/rigid/PhysicsUtils.as @@ -0,0 +1,49 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + public class PhysicsUtils { + + /** + * @param mass + * @param halfSize + * @param result + */ + public static function getBoxInvInertia(mass:Number, halfSize:Vector3, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + result.a = 3/(mass*(yy + zz)); + result.f = 3/(mass*(zz + xx)); + result.k = 3/(mass*(xx + yy)); + } + + /** + * @param mass + * @param r + * @param h + * @param result + */ + public static function getCylinderInvInertia(mass:Number, r:Number, h:Number, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + + result.a = result.f = 1/(mass*(h*h/12 + r*r/4)); + result.k = 2/(mass*r*r); + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/rigid/RigidWorld.as b/0.0.5.0/src/alternativa/physics/rigid/RigidWorld.as new file mode 100644 index 0000000..696580b --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/rigid/RigidWorld.as @@ -0,0 +1,653 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.ICollisionDetector; + import alternativa.physics.collision.KdTreeCollisionDetector; + import alternativa.physics.rigid.constraints.Constraint; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Класс реализует физическую симуляцию поведения твёрдых тел. + */ + public class RigidWorld { + + private static var lastBodyId:int; + + // Максимальное количество контактов + altphysics const MAX_CONTACTS:int = 1000; + + ////////////////////////////////////////////////////////////////// + // Настроечные параметры симуляции. Могут быть изменены в любой + // момент времени без нарушения корректной работы симуляции. + ////////////////////////////////////////////////////////////////// + // Количество шагов, за которое пересекающиеся тела должны разделиться + public var penResolutionSteps:int = 10; + // Величина допустимой глубины пересечения + public var allowedPenetration:Number = 0.1; + // Максимальная скорость, добавляемая с целью разделения тел + public var maxPenResolutionSpeed:Number = 0.5; + // Количество итераций для обработки упругих контактов + public var collisionIterations:int = 5; + // Количество итераций для обработки неупругих контактов + public var contactIterations:int = 5; + // Флаг использования предсказания состояний + public var usePrediction:Boolean = false; + + public var freezeSteps:int = 10; + public var linSpeedFreezeLimit:Number = 1; + public var angSpeedFreezeLimit:Number = 0.01; + + // Переменные для процедуры разделения тел путём непосредственного их перемещенеия. + // !!!!!!!!!!!!!!!!!!!!!!!! + // !!! Экспериментально !!! + // !!!!!!!!!!!!!!!!!!!!!!!! + public var staticSeparationIterations:int = 10; + public var staticSeparationSteps:int = 10; + public var maxAngleMove:Number = 10; + public var useStaticSeparation:Boolean = false; + + // Вектор гравитации + altphysics var _gravity:Vector3 = new Vector3(0, 0, -9.8); + // Использующийся детектор столкновений + altphysics var collisionDetector:ICollisionDetector; + // Список тел, участвующих в симуляции + altphysics var bodies:Vector. = new Vector.(); + // Количество тел, участвующих в симуляции + altphysics var bodiesNum:int; + // Список контактов на текущем шаге симуляции + altphysics var contacts:Vector. = new Vector.(MAX_CONTACTS, true); + // Количество контактов на текущем шаге симуляции + altphysics var contactsNum:int; + // Список ограничений + altphysics var constraints:Vector. = new Vector.(); + // Количество ограничений + altphysics var constraintsNum:int; + // Временная метка. Число прошедших шагов с начала симуляции. + altphysics var timeStamp:uint; + + // Временные переменные для избежания создания экземпляров + private var _r:Vector3 = new Vector3(); + private var _t:Vector3 = new Vector3(); + private var _v:Vector3 = new Vector3(); + private var _v1:Vector3 = new Vector3(); + private var _v2:Vector3 = new Vector3(); + + /** + * + */ + public function RigidWorld() { + for (var i:int = 0; i < MAX_CONTACTS; i++) contacts[i] = new Contact(); + collisionDetector = new KdTreeCollisionDetector(); + } + + /** + * Вектор гравитации. + */ + public function get gravity():Vector3 { + return _gravity.vClone(); + } + + /** + * @private + */ + public function set gravity(value:Vector3):void { + _gravity.vCopy(value); + } + + /** + * Добавляет тело в симуляцию. + * @param body + * @return + */ + public function addBody(body:Body, isStatic:Boolean):Boolean { + var i:int = bodies.indexOf(body); + if (i > -1) return false; + bodies[bodiesNum++] = body; + body.world = this; + body.id = lastBodyId++; +// if (body.collisionPrimitives != null) { +// for (i = 0; i < body.collisionPrimitivesNum; i++) collisionDetector.addPrimitive(body.collisionPrimitives[i], isStatic); +// } + return true; + } + + /** + * Удаляет тело из симуляции. + * @param body + * @return + */ + public function removeBody(body:Body, isStatic:Boolean):Boolean { + var i:int = bodies.indexOf(body); + if (i == -1) return false; + bodies.splice(i, 1); + body.world = null; +// if (body.collisionPrimitives != null) { +// for (i = 0; i < body.collisionPrimitivesNum; i++) collisionDetector.removePrimitive(body.collisionPrimitives[i], isStatic); +// } + bodiesNum--; + return true; + } + + /** + * Добавляет ограничение. + * @param c + */ + public function addConstraint(c:Constraint):void { + constraints[constraintsNum++] = c; + c.world = this; + } + + /** + * Удаляет ограничение. + * @param c + */ + public function removeConstraint(c:Constraint):Boolean { + var idx:int = constraints.indexOf(c); + if (idx < 0) return false; + constraints.splice(idx, 1) + constraintsNum--; + c.world = null; + return true; + } + + /** + * Применяет к телам действующте на них силы. Аккумуляторы сил и моментов тел очищаются после завершения шага + * симуляции, поэтому на момент вызова метода могут уже содержать некоторые значения. + * + * @param dt промежуток времени, в течении которого действуют силы + */ + private function applyForces(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i]; + body.addExternalForces(dt); + body.calcAccelerations(); + // Ускорение свободного падения применяется только к подвижным телам во избежание некорректного изменения + // фиктивной скорости неподвижных тел. + if (body.movable && !body.frozen) { + body.accel.x += _gravity.x; + body.accel.y += _gravity.y; + body.accel.z += _gravity.z; + } + } + } + + /** + * Определяет все столкновения на текущем шаге симуляции и заполняет список получившихся контактов. + * + * @param dt длительность шага симуляции + */ + private function detectCollisions(dt:Number):void { + var i:int; + var j:int; + var body:Body; + + for (i = 0; i < bodiesNum; i++) { + body = bodies[i]; + if (body.frozen) continue; + body.contactsNum = 0; + body.saveState(); + // При включённом режиме предсказания состояние тел интегрируется на один шаг вперёд + if (usePrediction) { + body.integrateVelocity(dt); + body.integratePosition(dt); + } + body.calcDerivedData(); + } + + contactsNum = collisionDetector.getAllCollisions(contacts); + + // Расчёт относительных векторов точки контакта вынесен сюда из-за необходимости учитывать + // положение тел в предсказанном состоянии + for (i = 0; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + var b1:Body = contact.body1; + var b2:Body = contact.body2; + for (j = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.r1.x = cp.pos.x - b1.state.pos.x; + cp.r1.y = cp.pos.y - b1.state.pos.y; + cp.r1.z = cp.pos.z - b1.state.pos.z; + if (b2 != null) { + cp.r2.x = cp.pos.x - b2.state.pos.x; + cp.r2.y = cp.pos.y - b2.state.pos.y; + cp.r2.z = cp.pos.z - b2.state.pos.z; + } + } + } + + // Восстановление состояния тел + if (usePrediction) { + for (i = 0; i < bodiesNum; i++) { + body = bodies[i]; + if (body.frozen) continue; + body.restoreState(); + body.calcDerivedData(); + } + } + } + + /** + * Подготваливает полученные из детектора столкновений контакты, расчитывая значения, не меняющиеся + * в ходе шага симуляции. + */ + private function preProcessContacts(dt:Number):void { + var i:int; + for (i = 0; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + var b1:Body = contact.body1; + var b2:Body = contact.body2; + // Столкнувшиеся тела размораживаются + if (b1.frozen) { + b1.frozen = false; + b1.freezeCounter = 0; + } + if (b2 != null && b2.frozen) { + b2.frozen = false; + b2.freezeCounter = 0; + } + contact.restitution = b1.material.restitution; + if (b2 != null && b2.material.restitution < contact.restitution) contact.restitution = b2.material.restitution; + contact.friction = b1.material.friction; + if (b2 != null && b2.material.friction < contact.friction) contact.friction = b2.material.friction; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.accumImpulseN = 0; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + cp.velByUnitImpulseN = 0; + if (b1.movable) { + cp.angularInertia1 = _v.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1).vDot(contact.normal); + cp.velByUnitImpulseN += b1.invMass + cp.angularInertia1; + } + if (b2 != null && b2.movable) { + cp.angularInertia2 = _v.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2).vDot(contact.normal); + cp.velByUnitImpulseN += b2.invMass + cp.angularInertia2; + } + // Расчёт требуемой конечной скорости для упругого контакта + calcSepVelocity(b1, b2, cp, _v); + cp.normalVel = _v.vDot(contact.normal); + if (cp.normalVel < 0) cp.normalVel = - contact.restitution*cp.normalVel; + // Скорость разделения неупругого контакта + cp.minSepVel = cp.penetration > allowedPenetration ? (cp.penetration - allowedPenetration)/(penResolutionSteps*dt) : 0; + if (cp.minSepVel > maxPenResolutionSpeed) cp.minSepVel = maxPenResolutionSpeed; + } + } + for (i = 0; i < constraintsNum; i++) Constraint(constraints[i]).preProcess(dt); + } + + /** + * + * @param dt + * @param forceInelastic + */ + private function processContacts(dt:Number, forceInelastic:Boolean):void { + var iterNum:int = forceInelastic ? contactIterations : collisionIterations; + var i:int; + var forwardLoop:Boolean = false; + for (var iter:int = 0; iter < iterNum; iter++) { + forwardLoop = !forwardLoop; + for (i = 0; i < contactsNum; i++) resolveContact(contacts[i], forceInelastic, forwardLoop); + // Ограничения + for (i = 0; i < constraintsNum; i++) Constraint(constraints[i]).apply(dt); + } + + // Разделение контактов путём непосредственного изменения координат и ориентации. Экспериментально. +// if (forceInelastic && useStaticSeparation) performStaticSeparation(); + } + + /** + * + */ + private function resolveContact(contactInfo:Contact, forceInelastic:Boolean, forwardLoop:Boolean):void { + var b1:Body = contactInfo.body1; + var b2:Body = contactInfo.body2; + var normal:Vector3 = contactInfo.normal; + var i:int; + if (forwardLoop) { + for (i = 0; i < contactInfo.pcount; i++) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } else { + for (i = contactInfo.pcount - 1; i >= 0; i--) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } + } + + /** + * + * @param idx + * @param b1 + * @param b2 + * @param colInfo + * @param normal + * @param forceInelastic + */ + private function resolveContactPoint(idx:int, b1:Body, b2:Body, contact:Contact, normal:Vector3, forceInelastic:Boolean):void { + var cp:ContactPoint = contact.points[idx]; + if (!forceInelastic) cp.satisfied = true; + + var newVel:Number = 0; + calcSepVelocity(b1, b2, cp, _v); + var sepVel:Number = _v.vDot(contact.normal); + if (forceInelastic) { + var minSpeVel:Number = useStaticSeparation ? 0 : cp.minSepVel; + if (sepVel < minSpeVel) cp.satisfied = false; + else if (cp.satisfied) return; + newVel = minSpeVel; + } else { + newVel = cp.normalVel; + } + var deltaVel:Number = newVel - sepVel; + var impulse:Number = deltaVel/cp.velByUnitImpulseN; + var accumImpulse:Number = cp.accumImpulseN + impulse; + if (accumImpulse < 0) accumImpulse = 0; + var deltaImpulse:Number = accumImpulse - cp.accumImpulseN; + cp.accumImpulseN = accumImpulse; + // Применяем импульс к телам + if (b1.movable) b1.applyRelPosWorldImpulse(cp.r1, normal, deltaImpulse); + if (b2 != null && b2.movable) b2.applyRelPosWorldImpulse(cp.r2, normal, -deltaImpulse); + + // Учёт силы трения + calcSepVelocity(b1, b2, cp, _v); + // Расчитываем изменение касательной скорости на единицу касательного импульса + var tanSpeedByUnitImpulse:Number = 0; + _v.vAddScaled(-_v.vDot(contact.normal), contact.normal); + var tanSpeed:Number = _v.vLength(); + if (tanSpeed < 0.001) return; + _t.vCopy(_v).vNormalize().vReverse(); + // dV = b.invMass + ((invI * (r % t)) % r) * t + if (b1.movable) { + _v.vCross2(cp.r1, _t).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1); + tanSpeedByUnitImpulse += b1.invMass + _v.vDot(_t); + } + if (b2 != null && b2.movable) { + _v.vCross2(cp.r2, _t).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2); + tanSpeedByUnitImpulse += b2.invMass + _v.vDot(_t); + } + + var tanImpulse:Number = tanSpeed/tanSpeedByUnitImpulse; + var max:Number = contact.friction*cp.accumImpulseN; + if (max < 0) { + if (tanImpulse < max) tanImpulse = max; + } else { + if (tanImpulse > max) tanImpulse = max; + } + + // Применяем импульс к телам + if (b1.movable) b1.applyRelPosWorldImpulse(cp.r1, _t, tanImpulse); + if (b2 != null && b2.movable) b2.applyRelPosWorldImpulse(cp.r2, _t, -tanImpulse); + } + + /** + * + * @param cp + * @param normal + * @return + */ + private function calcSepVelocity(body1:Body, body2:Body, cp:ContactPoint, result:Vector3):void { + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 +// result.vCopy(body1.state.velocity).vAdd(_v1.vCross2(body1.state.rotation, cp.r1)); + var rot:Vector3 = body1.state.rotation; + var v:Vector3 = cp.r1; + var x:Number = rot.y*v.z - rot.z*v.y; + var y:Number = rot.z*v.x - rot.x*v.z; + var z:Number = rot.x*v.y - rot.y*v.x; + v = body1.state.velocity; + result.x = v.x + x; + result.y = v.y + y; + result.z = v.z + z; + // V2 = V2_c + w2%r2 + if (body2 != null) { +// result.vSubtract(body2.state.velocity).vSubtract(_v2.vCross2(body2.state.rotation, cp.r2)); + rot = body2.state.rotation; + v = cp.r2; + x = rot.y*v.z - rot.z*v.y; + y = rot.z*v.x - rot.x*v.z; + z = rot.x*v.y - rot.y*v.x; + v = body2.state.velocity; + result.x -= v.x + x; + result.y -= v.y + y; + result.z -= v.z + z; + } + } + + /** + * + * @param dt + */ + private function intergateVelocities(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + (bodies[i] as Body).integrateVelocity(dt); + } + } + + /** + * + * @param dt + */ + private function integratePositions(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i] as Body; + if (body.movable && !body.frozen) body.integratePosition(dt); + } + } + + /** + * + */ + private function performStaticSeparation():void { + var iterNum:int = staticSeparationIterations; +// iterNum = 100; + + // 1. В начале каждой итерации для всех контактов сбрасывается флаг satisfied с одновременным поиском наихудшего контакта, т.е. имеющего наибольшую величину пересечения тел. + // 2. Если найденный контакт имеет величину пересечения меньше предельно допустимой процедура прерывается, т.к. разделение контактов не требуется. + // 3. Если процедура продолжается, то выполняется разделение найденного контакта и установка его флага satisfied в true, после чего запускается внутренний цикл, состоящий + // из contactsNum - 1 итераций. На каждой итерации ищется наихудший контакт среди оставшихся (satisfied == false). Если такой контакт найден, то выполняется его разделение + // и переход к следующей итерации внутреннего цикла, иначе внутренний цикл прерывается и выполняется переход к следующей итерации внешнего цикла (шаг 1.). + for (var iter:int = 0; iter < iterNum; iter++) { + // Ищем контакт с максимальной величиной пересечения тел, одновременно сбрасывая флаги satisfied в false + var worstContact:Contact = contacts[0]; + var i:int; + for (i = 1; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + contact.satisfied = false; + if (contact.maxPenetration > worstContact.maxPenetration) worstContact = contact; + } + if (worstContact.maxPenetration <= allowedPenetration) return; + + resolveInterpenetration(worstContact); + // Внутренний цикл по оставшимся контактам + for (i = 1; i < contactsNum; i++) { + worstContact = getWorstContact(); + if (worstContact == null) break; + resolveInterpenetration(worstContact); + } + } + } + + /** + * + */ + private function getWorstContact():Contact { + var maxPen:Number = 0; + var worst:Contact = null; + for (var i:int = 0; i < contactsNum; i++) { + var c:Contact = contacts[i]; + if (!c.satisfied && c.maxPenetration > maxPen) { + worst = c; + maxPen = c.maxPenetration; + } + } + return maxPen > allowedPenetration ? worst : null; + } + + /** + * Разделяет указанный контакт, выполняя staticSeparationSteps итераций по списку точек контакта. + * 1. В начале каждой итерации ищется точка с наибольшим пересечением, одновременно сбрасываются флаги satisfied у точек. + * 2. Для найденной точки выполняется процедура разделения. + * 3. После выполняется pcount - 1 итерация по списку точек, каждый раз ищется наихудшая среди имеющих satisfied == false, для которой выполняется процедура разделения. + * Если наихудшая точка не найдена, то выполнется переход к следующей итерации внешнего цикла (шаг 1). + * + * В конце процедуры разделения обновляются значения пересечений для остальных точек контакта, а также для всех прочих контактов, относящихся к телам текущего. + * + * @param contact контакт для разделения + */ + private function resolveInterpenetration(contact:Contact):void { + contact.satisfied = true; + + for (var step:int = 0; step < staticSeparationSteps; step++) { + var worstCp:ContactPoint = contact.points[0]; + var cp:ContactPoint; + var i:int; + for (i = 1; i < contact.pcount; i++) { + cp = contact.points[i]; + cp.satisfied = false; + if (cp.penetration > worstCp.penetration) worstCp = cp; + } + if (worstCp.penetration <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + // Разделяем оставшиеся точки + var maxPen:Number = 0; + for (i = 1; i < contact.pcount; i++) { + // Поиск наихудшей точки + for (var j:int = 0; j < contact.pcount; j++) { + cp = contact.points[j]; + if (cp.satisfied) continue; + if (cp.penetration > maxPen) { + maxPen = cp.penetration; + worstCp = cp; + } + } + if (maxPen <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + } + } + } + + /** + * + * @param cp + * @param contact + */ + private function separateContactPoint(cp:ContactPoint, contact:Contact):void { + cp.satisfied = true; + + var b1:Body = contact.body1; + var b2:Body = contact.body2; + var totalMove:Number = cp.penetration - allowedPenetration; + var moveCoeff:Number = totalMove/cp.velByUnitImpulseN; + var linMove1:Number; + var angleMove1:Number; + if (b1.movable) { + linMove1 = b1.invMass*moveCoeff; + angleMove1 = cp.angularInertia1*moveCoeff; + if (angleMove1 > maxAngleMove) { + linMove1 += angleMove1 - maxAngleMove; + angleMove1 -= maxAngleMove; + } + b1.state.pos.vAddScaled(linMove1, contact.normal); + _v1.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vScale(angleMove1); + b1.state.orientation.addScaledVector(_v1, 1); + } + var linMove2:Number; + var angleMove2:Number; + if (b2 != null && b2.movable) { + linMove2 = b2.invMass*moveCoeff; + angleMove2 = cp.angularInertia2*moveCoeff; + if (angleMove2 > maxAngleMove) { + linMove2 += angleMove2 - maxAngleMove; + angleMove2 -= maxAngleMove; + } + b2.state.pos.vAddScaled(-linMove2, contact.normal); + _v2.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vScale(angleMove2); + _v2.vReverse(); + b2.state.orientation.addScaledVector(_v2, 1); + } + cp.penetration = allowedPenetration; + // Обновляем пересечения в других точках + var i:int; + for (i = 0; i < contact.pcount; i++) { + var cp1:ContactPoint = contact.points[i]; + if (cp1 == cp) continue; + var angularMove:Number; + if (b1.movable) { + angularMove = _v.vCross2(_v1, cp1.r1).vDot(contact.normal); + cp1.penetration -= linMove1 + angularMove; + } + if (b2 != null && b2.movable) { + angularMove = _v.vCross2(_v2, cp1.r2).vDot(contact.normal); + cp1.penetration -= linMove2 - angularMove; + } + // Обновление максимального значения для контакта + if (cp1.penetration > contact.maxPenetration) contact.maxPenetration = cp1.penetration; + } + // Обновляем пересечения для других контактов + var c:Contact; + var j:int; + if (b1.movable) { + for (i = 0; i < b1.contactsNum; i++) { + c = b1.contacts[i]; + if (c == contact) continue; + for (j = 0; j < c.pcount; j++) { + cp1 = c.points[j]; + if (b1 == c.body1) cp1.penetration -= linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r1).vDot(c.normal); + else cp1.penetration += linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r2).vDot(c.normal); + if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; + } + } + } + if (b2 != null && b2.movable) { + for (i = 0; i < b2.contactsNum; i++) { + c = b2.contacts[i]; + if (c == contact) continue; + for (j = 0; j < c.pcount; j++) { + cp1 = c.points[j]; + if (b2 == c.body1) cp1.penetration -= linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r1).vDot(c.normal); + else cp1.penetration += linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r2).vDot(c.normal); + if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; + } + } + } + } + + /** + * + */ + private function postPhysics():void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i]; + body.clearAccumulators(); + body.calcDerivedData(); + if (body.canFreeze) { + if (body.state.velocity.vLength() < linSpeedFreezeLimit && body.state.rotation.vLength() < angSpeedFreezeLimit) { + if (!body.frozen) { + body.freezeCounter++; + if (body.freezeCounter >= freezeSteps) body.frozen = true; + } + } else { + body.freezeCounter = 0; + body.frozen = false; + } + } + } + } + + /** + * + * @param dt + */ + public function runPhysics(dt:Number):void { + timeStamp++; + applyForces(dt); + detectCollisions(dt); + preProcessContacts(dt); + processContacts(dt, false); + intergateVelocities(dt); + processContacts(dt, true); + integratePositions(dt); + postPhysics(); + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/rigid/constraints/.svn/all-wcprops b/0.0.5.0/src/alternativa/physics/rigid/constraints/.svn/all-wcprops new file mode 100644 index 0000000..33a3527 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/rigid/constraints/.svn/all-wcprops @@ -0,0 +1,17 @@ +K 25 +svn:wc:ra_dav:version-url +V 121 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/rigid/constraints +END +Constraint.as +K 25 +svn:wc:ra_dav:version-url +V 135 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/rigid/constraints/Constraint.as +END +MaxDistanceConstraint.as +K 25 +svn:wc:ra_dav:version-url +V 146 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/rigid/constraints/MaxDistanceConstraint.as +END diff --git a/0.0.5.0/src/alternativa/physics/rigid/constraints/.svn/entries b/0.0.5.0/src/alternativa/physics/rigid/constraints/.svn/entries new file mode 100644 index 0000000..a72a4e7 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/rigid/constraints/.svn/entries @@ -0,0 +1,52 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/rigid/constraints +http://svndev.alternativaplatform.com + + + +2009-06-29T03:58:47.301371Z +15198 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Constraint.as +file + + + + +2010-10-28T04:32:41.000000Z +757824f9eeb3c89243d104910e0d0529 +2009-04-23T13:43:38.903278Z +11779 +mike + +MaxDistanceConstraint.as +file + + + + +2010-10-28T04:32:41.000000Z +025a27c81aeaf18c860f3a4c4c63cf96 +2009-06-29T03:58:47.301371Z +15198 +mike + diff --git a/0.0.5.0/src/alternativa/physics/rigid/constraints/.svn/format b/0.0.5.0/src/alternativa/physics/rigid/constraints/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/rigid/constraints/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.5.0/src/alternativa/physics/rigid/constraints/.svn/text-base/Constraint.as.svn-base b/0.0.5.0/src/alternativa/physics/rigid/constraints/.svn/text-base/Constraint.as.svn-base new file mode 100644 index 0000000..c2cd912 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/rigid/constraints/.svn/text-base/Constraint.as.svn-base @@ -0,0 +1,35 @@ +package alternativa.physics.rigid.constraints { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.RigidWorld; + + use namespace altphysics; + + /** + * + */ + public class Constraint { + + altphysics var satisfied:Boolean; + altphysics var world:RigidWorld; + + /** + * + */ + public function Constraint() { + } + + /** + * @param dt + */ + public function preProcess(dt:Number):void { + } + + /** + * @param dt + */ + public function apply(dt:Number):void { + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/rigid/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base b/0.0.5.0/src/alternativa/physics/rigid/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base new file mode 100644 index 0000000..008d4a4 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/rigid/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base @@ -0,0 +1,155 @@ +package alternativa.physics.rigid.constraints { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.BodyState; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class MaxDistanceConstraint extends Constraint { + // Первое тело ограничения + altphysics var body1:Body; + // Второе тело ограничения. Может быть null. + altphysics var body2:Body; + // Первая точка крепления в системе координат первого тела + altphysics var r1:Vector3; + // Вторая точка привязки в системе координат второго тела. В случае отсутствия второго тела, координаты задаются в мировой системе. + altphysics var r2:Vector3; + // Максимально допустимое расстояние между точками крепления + altphysics var maxDistance:Number; + // Радиус-вектор первой точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr1:Vector3 = new Vector3(); + // Радиус-вектор второй точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr2:Vector3 = new Vector3(); + // Минимальная скорость сближения для удовлетворения ограничения + private var minClosingVel:Number; + // Изменение относительной скорости на единицу импульса + private var velByUnitImpulseN:Number; + // Направление корректирующего импульса + private var impulseDirection:Vector3 = new Vector3(); + + // Вспомогательные переменные + private static var _v:Vector3 = new Vector3(); + private static var _v1:Vector3 = new Vector3(); + private static var _v2:Vector3 = new Vector3(); + + /** + * @param body1 + * @param body2 + * @param r1 + * @param r2 + * @param maxDistance + */ + public function MaxDistanceConstraint(body1:Body, body2:Body, r1:Vector3, r2:Vector3, maxDistance:Number) { + super(); + this.body1 = body1; + this.body2 = body2; + this.r1 = r1.vClone(); + this.r2 = r2.vClone(); + this.maxDistance = maxDistance; + } + + /** + * @param dt + */ + override public function preProcess(dt:Number):void { + // Вычислим расстояние между точками + body1.baseMatrix.transformVector(r1, wr1); + if (body2 != null) body2.baseMatrix.transformVector(r2, wr2); + else { + wr2.x = r2.x; + wr2.y = r2.y; + wr2.z = r2.z; + } + var p1:Vector3 = body1.state.pos; + impulseDirection.x = wr2.x - wr1.x - p1.x; + impulseDirection.y = wr2.y - wr1.y - p1.y; + impulseDirection.z = wr2.z - wr1.z - p1.z; + if (body2 != null) { + var p2:Vector3 = body2.state.pos; + impulseDirection.x += p2.x; + impulseDirection.y += p2.y; + impulseDirection.z += p2.z; + } + + var len:Number = Math.sqrt(impulseDirection.x*impulseDirection.x + impulseDirection.y*impulseDirection.y + impulseDirection.z*impulseDirection.z); + var delta:Number = len - maxDistance; + if (delta > 0) { + satisfied = false; + if (len == 0) impulseDirection.x = 1; + else { + impulseDirection.x /= len; + impulseDirection.y /= len; + impulseDirection.z /= len; + } + minClosingVel = delta/(world.penResolutionSteps*dt); + if (minClosingVel > world.maxPenResolutionSpeed) minClosingVel = world.maxPenResolutionSpeed; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + velByUnitImpulseN = 0; + if (body1.movable) { +// velByUnitImpulseN += body1.invMass + _v.vCross2(wr1, impulseDirection).vTransformBy3(body1.invInertiaWorld).vCross(wr1).vDot(impulseDirection); + _v.x = wr1.y*impulseDirection.z - wr1.z*impulseDirection.y; + _v.y = wr1.z*impulseDirection.x - wr1.x*impulseDirection.z; + _v.z = wr1.x*impulseDirection.y - wr1.y*impulseDirection.x; + _v.vTransformBy3(body1.invInertiaWorld); + _v1.x = _v.y*wr1.z - _v.z*wr1.y; + _v1.y = _v.z*wr1.x - _v.x*wr1.z; + _v1.z = _v.x*wr1.y - _v.y*wr1.x; + velByUnitImpulseN += body1.invMass + _v1.x*impulseDirection.x + _v1.y*impulseDirection.y + _v1.z*impulseDirection.z; + } + if (body2 != null && body2.movable) { +// velByUnitImpulseN += body2.invMass + _v.vCross2(wr2, impulseDirection).vTransformBy3(body2.invInertiaWorld).vCross(wr2).vDot(impulseDirection); + _v.x = wr2.y*impulseDirection.z - wr2.z*impulseDirection.y; + _v.y = wr2.z*impulseDirection.x - wr2.x*impulseDirection.z; + _v.z = wr2.x*impulseDirection.y - wr2.y*impulseDirection.x; + _v.vTransformBy3(body2.invInertiaWorld); + _v1.x = _v.y*wr2.z - _v.z*wr2.y; + _v1.y = _v.z*wr2.x - _v.x*wr2.z; + _v1.z = _v.x*wr2.y - _v.y*wr2.x; + velByUnitImpulseN += body2.invMass + _v1.x*impulseDirection.x + _v1.y*impulseDirection.y + _v1.z*impulseDirection.z; + } + } else satisfied = true; + } + + /** + * @param dt + */ + override public function apply(dt:Number):void { + if (satisfied) return; + // Расчитываем проекцию скорости относительной скорости + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 + var state:BodyState = body1.state; + var rot:Vector3 = state.rotation; +// _v1.vCross2(state.rotation, wr1); + _v1.x = rot.y*wr1.z - rot.z*wr1.y; + _v1.y = rot.z*wr1.x - rot.x*wr1.z; + _v1.z = rot.x*wr1.y - rot.y*wr1.x; + _v.x = state.velocity.x + _v1.x; + _v.y = state.velocity.y + _v1.y; + _v.z = state.velocity.z + _v1.z; + // V2 = V2_c + w2%r2 + if (body2 != null) { + state = body2.state; +// _v2.vCross2(state.rotation, wr2); + _v2.x = rot.y*wr2.z - rot.z*wr2.y; + _v2.y = rot.z*wr2.x - rot.x*wr2.z; + _v2.z = rot.x*wr2.y - rot.y*wr2.x; + _v.x -= state.velocity.x - _v2.x; + _v.y -= state.velocity.y - _v2.y; + _v.z -= state.velocity.z - _v2.z; + } + var closingVel:Number = _v.x*impulseDirection.x + _v.y*impulseDirection.y + _v.z*impulseDirection.z; + if (closingVel > minClosingVel) return; + var impulse:Number = (minClosingVel - closingVel)/velByUnitImpulseN; + if (body1.movable) body1.applyRelPosWorldImpulse(wr1, impulseDirection, impulse); + if (body2 != null && body2.movable) body2.applyRelPosWorldImpulse(wr2, impulseDirection, -impulse); + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/rigid/constraints/Constraint.as b/0.0.5.0/src/alternativa/physics/rigid/constraints/Constraint.as new file mode 100644 index 0000000..c2cd912 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/rigid/constraints/Constraint.as @@ -0,0 +1,35 @@ +package alternativa.physics.rigid.constraints { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.RigidWorld; + + use namespace altphysics; + + /** + * + */ + public class Constraint { + + altphysics var satisfied:Boolean; + altphysics var world:RigidWorld; + + /** + * + */ + public function Constraint() { + } + + /** + * @param dt + */ + public function preProcess(dt:Number):void { + } + + /** + * @param dt + */ + public function apply(dt:Number):void { + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/rigid/constraints/MaxDistanceConstraint.as b/0.0.5.0/src/alternativa/physics/rigid/constraints/MaxDistanceConstraint.as new file mode 100644 index 0000000..008d4a4 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/rigid/constraints/MaxDistanceConstraint.as @@ -0,0 +1,155 @@ +package alternativa.physics.rigid.constraints { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.BodyState; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class MaxDistanceConstraint extends Constraint { + // Первое тело ограничения + altphysics var body1:Body; + // Второе тело ограничения. Может быть null. + altphysics var body2:Body; + // Первая точка крепления в системе координат первого тела + altphysics var r1:Vector3; + // Вторая точка привязки в системе координат второго тела. В случае отсутствия второго тела, координаты задаются в мировой системе. + altphysics var r2:Vector3; + // Максимально допустимое расстояние между точками крепления + altphysics var maxDistance:Number; + // Радиус-вектор первой точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr1:Vector3 = new Vector3(); + // Радиус-вектор второй точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr2:Vector3 = new Vector3(); + // Минимальная скорость сближения для удовлетворения ограничения + private var minClosingVel:Number; + // Изменение относительной скорости на единицу импульса + private var velByUnitImpulseN:Number; + // Направление корректирующего импульса + private var impulseDirection:Vector3 = new Vector3(); + + // Вспомогательные переменные + private static var _v:Vector3 = new Vector3(); + private static var _v1:Vector3 = new Vector3(); + private static var _v2:Vector3 = new Vector3(); + + /** + * @param body1 + * @param body2 + * @param r1 + * @param r2 + * @param maxDistance + */ + public function MaxDistanceConstraint(body1:Body, body2:Body, r1:Vector3, r2:Vector3, maxDistance:Number) { + super(); + this.body1 = body1; + this.body2 = body2; + this.r1 = r1.vClone(); + this.r2 = r2.vClone(); + this.maxDistance = maxDistance; + } + + /** + * @param dt + */ + override public function preProcess(dt:Number):void { + // Вычислим расстояние между точками + body1.baseMatrix.transformVector(r1, wr1); + if (body2 != null) body2.baseMatrix.transformVector(r2, wr2); + else { + wr2.x = r2.x; + wr2.y = r2.y; + wr2.z = r2.z; + } + var p1:Vector3 = body1.state.pos; + impulseDirection.x = wr2.x - wr1.x - p1.x; + impulseDirection.y = wr2.y - wr1.y - p1.y; + impulseDirection.z = wr2.z - wr1.z - p1.z; + if (body2 != null) { + var p2:Vector3 = body2.state.pos; + impulseDirection.x += p2.x; + impulseDirection.y += p2.y; + impulseDirection.z += p2.z; + } + + var len:Number = Math.sqrt(impulseDirection.x*impulseDirection.x + impulseDirection.y*impulseDirection.y + impulseDirection.z*impulseDirection.z); + var delta:Number = len - maxDistance; + if (delta > 0) { + satisfied = false; + if (len == 0) impulseDirection.x = 1; + else { + impulseDirection.x /= len; + impulseDirection.y /= len; + impulseDirection.z /= len; + } + minClosingVel = delta/(world.penResolutionSteps*dt); + if (minClosingVel > world.maxPenResolutionSpeed) minClosingVel = world.maxPenResolutionSpeed; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + velByUnitImpulseN = 0; + if (body1.movable) { +// velByUnitImpulseN += body1.invMass + _v.vCross2(wr1, impulseDirection).vTransformBy3(body1.invInertiaWorld).vCross(wr1).vDot(impulseDirection); + _v.x = wr1.y*impulseDirection.z - wr1.z*impulseDirection.y; + _v.y = wr1.z*impulseDirection.x - wr1.x*impulseDirection.z; + _v.z = wr1.x*impulseDirection.y - wr1.y*impulseDirection.x; + _v.vTransformBy3(body1.invInertiaWorld); + _v1.x = _v.y*wr1.z - _v.z*wr1.y; + _v1.y = _v.z*wr1.x - _v.x*wr1.z; + _v1.z = _v.x*wr1.y - _v.y*wr1.x; + velByUnitImpulseN += body1.invMass + _v1.x*impulseDirection.x + _v1.y*impulseDirection.y + _v1.z*impulseDirection.z; + } + if (body2 != null && body2.movable) { +// velByUnitImpulseN += body2.invMass + _v.vCross2(wr2, impulseDirection).vTransformBy3(body2.invInertiaWorld).vCross(wr2).vDot(impulseDirection); + _v.x = wr2.y*impulseDirection.z - wr2.z*impulseDirection.y; + _v.y = wr2.z*impulseDirection.x - wr2.x*impulseDirection.z; + _v.z = wr2.x*impulseDirection.y - wr2.y*impulseDirection.x; + _v.vTransformBy3(body2.invInertiaWorld); + _v1.x = _v.y*wr2.z - _v.z*wr2.y; + _v1.y = _v.z*wr2.x - _v.x*wr2.z; + _v1.z = _v.x*wr2.y - _v.y*wr2.x; + velByUnitImpulseN += body2.invMass + _v1.x*impulseDirection.x + _v1.y*impulseDirection.y + _v1.z*impulseDirection.z; + } + } else satisfied = true; + } + + /** + * @param dt + */ + override public function apply(dt:Number):void { + if (satisfied) return; + // Расчитываем проекцию скорости относительной скорости + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 + var state:BodyState = body1.state; + var rot:Vector3 = state.rotation; +// _v1.vCross2(state.rotation, wr1); + _v1.x = rot.y*wr1.z - rot.z*wr1.y; + _v1.y = rot.z*wr1.x - rot.x*wr1.z; + _v1.z = rot.x*wr1.y - rot.y*wr1.x; + _v.x = state.velocity.x + _v1.x; + _v.y = state.velocity.y + _v1.y; + _v.z = state.velocity.z + _v1.z; + // V2 = V2_c + w2%r2 + if (body2 != null) { + state = body2.state; +// _v2.vCross2(state.rotation, wr2); + _v2.x = rot.y*wr2.z - rot.z*wr2.y; + _v2.y = rot.z*wr2.x - rot.x*wr2.z; + _v2.z = rot.x*wr2.y - rot.y*wr2.x; + _v.x -= state.velocity.x - _v2.x; + _v.y -= state.velocity.y - _v2.y; + _v.z -= state.velocity.z - _v2.z; + } + var closingVel:Number = _v.x*impulseDirection.x + _v.y*impulseDirection.y + _v.z*impulseDirection.z; + if (closingVel > minClosingVel) return; + var impulse:Number = (minClosingVel - closingVel)/velByUnitImpulseN; + if (body1.movable) body1.applyRelPosWorldImpulse(wr1, impulseDirection, impulse); + if (body2 != null && body2.movable) body2.applyRelPosWorldImpulse(wr2, impulseDirection, -impulse); + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/rigid/primitives/.svn/all-wcprops b/0.0.5.0/src/alternativa/physics/rigid/primitives/.svn/all-wcprops new file mode 100644 index 0000000..5df987c --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/rigid/primitives/.svn/all-wcprops @@ -0,0 +1,35 @@ +K 25 +svn:wc:ra_dav:version-url +V 120 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/rigid/primitives +END +RigidPlane.as +K 25 +svn:wc:ra_dav:version-url +V 134 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/rigid/primitives/RigidPlane.as +END +RigidSphere.as +K 25 +svn:wc:ra_dav:version-url +V 135 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/rigid/primitives/RigidSphere.as +END +RigidBox.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/rigid/primitives/RigidBox.as +END +RigidCylinder.as +K 25 +svn:wc:ra_dav:version-url +V 137 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/rigid/primitives/RigidCylinder.as +END +RigidRect.as +K 25 +svn:wc:ra_dav:version-url +V 133 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/rigid/primitives/RigidRect.as +END diff --git a/0.0.5.0/src/alternativa/physics/rigid/primitives/.svn/entries b/0.0.5.0/src/alternativa/physics/rigid/primitives/.svn/entries new file mode 100644 index 0000000..a182759 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/rigid/primitives/.svn/entries @@ -0,0 +1,88 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/rigid/primitives +http://svndev.alternativaplatform.com + + + +2009-06-08T06:46:18.310732Z +14099 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +RigidPlane.as +file + + + + +2010-10-28T04:32:41.000000Z +c4713421e357ee1967dca9ab3ffa7290 +2009-06-08T06:46:18.310732Z +14099 +mike + +RigidSphere.as +file + + + + +2010-10-28T04:32:41.000000Z +79ef38eab05657ad9627bdac4ade7271 +2009-04-23T13:43:38.903278Z +11779 +mike + +RigidBox.as +file + + + + +2010-10-28T04:32:41.000000Z +da834a5d208ff30f3a74321f75301cd7 +2009-06-08T06:46:18.310732Z +14099 +mike + +RigidCylinder.as +file + + + + +2010-10-28T04:32:41.000000Z +61512d31f9e211dcb0864164aa6f5210 +2009-04-23T13:43:38.903278Z +11779 +mike + +RigidRect.as +file + + + + +2010-10-28T04:32:41.000000Z +4cff4e313a1f41d4ccfa6480ea78173e +2009-04-23T13:43:38.903278Z +11779 +mike + diff --git a/0.0.5.0/src/alternativa/physics/rigid/primitives/.svn/format b/0.0.5.0/src/alternativa/physics/rigid/primitives/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/rigid/primitives/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.5.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidBox.as.svn-base b/0.0.5.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidBox.as.svn-base new file mode 100644 index 0000000..c01ff61 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidBox.as.svn-base @@ -0,0 +1,56 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class RigidBox extends Body { + + /** + * + * @param halfSize + * @param mass + */ + public function RigidBox(halfSize:Vector3, mass:Number) { + super(0, Matrix3.ZERO); + addCollisionPrimitive(new CollisionBox(halfSize, 1)); + setParams(halfSize, mass); + } + + /** + * @param halfSize + * @param mass + */ + public function setParams(halfSize:Vector3, mass:Number):void { + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + invInertia.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + invInertia.a = 3*invMass/(yy + zz); + invInertia.f = 3*invMass/(zz + xx); + invInertia.k = 3*invMass/(xx + yy); + } + var prim:CollisionBox = collisionPrimitives[0] as CollisionBox; + prim.hs.vCopy(halfSize); + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidCylinder.as.svn-base b/0.0.5.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidCylinder.as.svn-base new file mode 100644 index 0000000..0f9f2b3 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidCylinder.as.svn-base @@ -0,0 +1,32 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + + use namespace altphysics; + + public class RigidCylinder extends Body { + + altphysics var r:Number; + altphysics var h:Number; + + public function RigidCylinder(mass:Number, radius:Number, height:Number) { + super(0, Matrix3.ZERO); + setParams(mass, radius, height); + } + + public function setParams(mass:Number, radius:Number, height:Number):void { + r = radius; + h = height; + invInertiaWorld.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + invInertia.a = invInertia.f = mass*(h*h/12 + r*r/4); + invInertia.k = 0.5*mass*r*r; + invInertia.invert(); + } + } + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidPlane.as.svn-base b/0.0.5.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidPlane.as.svn-base new file mode 100644 index 0000000..26f74b2 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidPlane.as.svn-base @@ -0,0 +1,23 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + public class RigidPlane extends Body { + + public var normal:Vector3 = new Vector3(); + public var offset:Number; + + public function RigidPlane() { + super(0, Matrix3.ZERO); + this.normal.vCopy(Vector3.Z_AXIS); + this.offset = 0; + movable = false; + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidRect.as.svn-base b/0.0.5.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidRect.as.svn-base new file mode 100644 index 0000000..3c3913d --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidRect.as.svn-base @@ -0,0 +1,20 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + public class RigidRect extends Body { + + public function RigidRect(halfWidth:Number, halfLength:Number) { + super(0, Matrix3.ZERO); + movable = false; + addCollisionPrimitive(new CollisionRect(new Vector3(halfWidth, halfLength, 0), 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidSphere.as.svn-base b/0.0.5.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidSphere.as.svn-base new file mode 100644 index 0000000..9668716 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidSphere.as.svn-base @@ -0,0 +1,29 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + + use namespace altphysics; + + public class RigidSphere extends Body { + + public function RigidSphere(radius:Number, mass:Number) { + var m:Matrix3; + if (mass == Infinity) { + mass = 0; + m = Matrix3.ZERO; + } else { + mass = 1/mass; + var r2:Number = radius*radius; + m = new Matrix3(); + m.a = m.f = m.k = 2.5*mass/r2; + } + super(mass, m); + if (mass == 0) movable = false; + addCollisionPrimitive(new CollisionSphere(radius, 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/rigid/primitives/RigidBox.as b/0.0.5.0/src/alternativa/physics/rigid/primitives/RigidBox.as new file mode 100644 index 0000000..c01ff61 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/rigid/primitives/RigidBox.as @@ -0,0 +1,56 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class RigidBox extends Body { + + /** + * + * @param halfSize + * @param mass + */ + public function RigidBox(halfSize:Vector3, mass:Number) { + super(0, Matrix3.ZERO); + addCollisionPrimitive(new CollisionBox(halfSize, 1)); + setParams(halfSize, mass); + } + + /** + * @param halfSize + * @param mass + */ + public function setParams(halfSize:Vector3, mass:Number):void { + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + invInertia.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + invInertia.a = 3*invMass/(yy + zz); + invInertia.f = 3*invMass/(zz + xx); + invInertia.k = 3*invMass/(xx + yy); + } + var prim:CollisionBox = collisionPrimitives[0] as CollisionBox; + prim.hs.vCopy(halfSize); + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/rigid/primitives/RigidCylinder.as b/0.0.5.0/src/alternativa/physics/rigid/primitives/RigidCylinder.as new file mode 100644 index 0000000..0f9f2b3 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/rigid/primitives/RigidCylinder.as @@ -0,0 +1,32 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + + use namespace altphysics; + + public class RigidCylinder extends Body { + + altphysics var r:Number; + altphysics var h:Number; + + public function RigidCylinder(mass:Number, radius:Number, height:Number) { + super(0, Matrix3.ZERO); + setParams(mass, radius, height); + } + + public function setParams(mass:Number, radius:Number, height:Number):void { + r = radius; + h = height; + invInertiaWorld.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + invInertia.a = invInertia.f = mass*(h*h/12 + r*r/4); + invInertia.k = 0.5*mass*r*r; + invInertia.invert(); + } + } + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/rigid/primitives/RigidPlane.as b/0.0.5.0/src/alternativa/physics/rigid/primitives/RigidPlane.as new file mode 100644 index 0000000..26f74b2 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/rigid/primitives/RigidPlane.as @@ -0,0 +1,23 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + public class RigidPlane extends Body { + + public var normal:Vector3 = new Vector3(); + public var offset:Number; + + public function RigidPlane() { + super(0, Matrix3.ZERO); + this.normal.vCopy(Vector3.Z_AXIS); + this.offset = 0; + movable = false; + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/rigid/primitives/RigidRect.as b/0.0.5.0/src/alternativa/physics/rigid/primitives/RigidRect.as new file mode 100644 index 0000000..3c3913d --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/rigid/primitives/RigidRect.as @@ -0,0 +1,20 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + public class RigidRect extends Body { + + public function RigidRect(halfWidth:Number, halfLength:Number) { + super(0, Matrix3.ZERO); + movable = false; + addCollisionPrimitive(new CollisionRect(new Vector3(halfWidth, halfLength, 0), 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/rigid/primitives/RigidSphere.as b/0.0.5.0/src/alternativa/physics/rigid/primitives/RigidSphere.as new file mode 100644 index 0000000..9668716 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/rigid/primitives/RigidSphere.as @@ -0,0 +1,29 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + + use namespace altphysics; + + public class RigidSphere extends Body { + + public function RigidSphere(radius:Number, mass:Number) { + var m:Matrix3; + if (mass == Infinity) { + mass = 0; + m = Matrix3.ZERO; + } else { + mass = 1/mass; + var r2:Number = radius*radius; + m = new Matrix3(); + m.a = m.f = m.k = 2.5*mass/r2; + } + super(mass, m); + if (mass == 0) movable = false; + addCollisionPrimitive(new CollisionSphere(radius, 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/types/.svn/all-wcprops b/0.0.5.0/src/alternativa/physics/types/.svn/all-wcprops new file mode 100644 index 0000000..0e5f633 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/types/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 109 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/types +END +Quaternion.as +K 25 +svn:wc:ra_dav:version-url +V 123 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/types/Quaternion.as +END +Vector3.as +K 25 +svn:wc:ra_dav:version-url +V 120 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/types/Vector3.as +END +Matrix3.as +K 25 +svn:wc:ra_dav:version-url +V 120 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/types/Matrix3.as +END +Matrix4.as +K 25 +svn:wc:ra_dav:version-url +V 120 +/!svn/ver/19726/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/types/Matrix4.as +END diff --git a/0.0.5.0/src/alternativa/physics/types/.svn/entries b/0.0.5.0/src/alternativa/physics/types/.svn/entries new file mode 100644 index 0000000..4d93551 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/types/.svn/entries @@ -0,0 +1,76 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.5.0/src/alternativa/physics/types +http://svndev.alternativaplatform.com + + + +2009-09-10T19:17:30.639579Z +19708 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Quaternion.as +file + + + + +2010-10-28T04:32:41.000000Z +595f36fdfea8e82e752bb989e97e3a9a +2009-05-04T04:55:56.466789Z +12306 +mike + +Vector3.as +file + + + + +2010-10-28T04:32:41.000000Z +843b476caf4f3879350b906d5a80e815 +2009-09-09T14:41:32.057202Z +19641 +mike + +Matrix3.as +file + + + + +2010-10-28T04:32:41.000000Z +3fde68593cd3eb5a7046c441a76c9c5c +2009-06-25T09:18:57.805913Z +15117 +mike + +Matrix4.as +file + + + + +2010-10-28T04:32:41.000000Z +2b87614adc88ffe3d96611cfdf918d45 +2009-09-10T19:17:30.639579Z +19708 +mike + diff --git a/0.0.5.0/src/alternativa/physics/types/.svn/format b/0.0.5.0/src/alternativa/physics/types/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/types/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.5.0/src/alternativa/physics/types/.svn/text-base/Matrix3.as.svn-base b/0.0.5.0/src/alternativa/physics/types/.svn/text-base/Matrix3.as.svn-base new file mode 100644 index 0000000..e07d0d1 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/types/.svn/text-base/Matrix3.as.svn-base @@ -0,0 +1,396 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + /** + * + */ + public class Matrix3 { + + public static const ZERO:Matrix3 = new Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0); + public static const IDENTITY:Matrix3 = new Matrix3(); + + public var a:Number; + public var b:Number; + public var c:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix3(a:Number = 1, b:Number = 0, c:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1) { + this.a = a; + this.b = b; + this.c = c; + + this.e = e; + this.f = f; + this.g = g; + + this.i = i; + this.j = j; + this.k = k; + } + + /** + * + * @return + */ + public function toIdentity():Matrix3 { + a = f = k = 1; + b = c = e = g = i = j = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + var det:Number = 1/(-cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk); + + a = (ff*kk - gg*jj)*det; + b = (cc*jj - bb*kk)*det; + c = (bb*gg - cc*ff)*det; + e = (gg*ii - ee*kk)*det; + f = (aa*kk - cc*ii)*det; + g = (cc*ee - aa*gg)*det; + i = (ee*jj - ff*ii)*det; + j = (bb*ii - aa*jj)*det; + k = (aa*ff - bb*ee)*det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prependTransposed(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.b + cc*m.c; + b = aa*m.e + bb*m.f + cc*m.g; + c = aa*m.i + bb*m.j + cc*m.k; + e = ee*m.a + ff*m.b + gg*m.c; + f = ee*m.e + ff*m.f + gg*m.g; + g = ee*m.i + ff*m.j + gg*m.k; + i = ii*m.a + jj*m.b + kk*m.c; + j = ii*m.e + jj*m.f + kk*m.g; + k = ii*m.i + jj*m.j + kk*m.k; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix3):Matrix3 { + a += m.a; + b += m.b; + c += m.c; + e += m.e; + f += m.f; + g += m.g; + i += m.i; + j += m.j; + k += m.k; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix3):Matrix3 { + a -= m.a; + b -= m.b; + c -= m.c; + e -= m.e; + f -= m.f; + g -= m.g; + i -= m.i; + j -= m.j; + k -= m.k; + + return this; + } + + /** + * + * @return + */ + public function transpose():Matrix3 { + var tmp:Number = b; + b = e; + e = tmp; + tmp = c; + c = i; + i = tmp; + tmp = g; + g = j; + j = tmp; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector3To3D(vin:Vector3, vout:Vector3D):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * + * @param matrix + */ + public function createSkewSymmetric(v:Vector3):Matrix3 { + a = f = k = 0; + b = -v.z; + c = v.y; + e = v.z; + g = -v.x; + i = -v.y; + j = v.x; + return this; + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(i:int, axis:Vector3):void { + switch (i) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + } + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix3):Matrix3 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @return + */ + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):Matrix3 { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + + return this; + } + + /** + * Метод формирует матрицу поворота на заданный угол относительно заданной оси. + * + * @param axis нормализованный вектор, задающий ось, относительно которой выполняется поворот + * @param angle угол поворота в радианах + */ + public function fromAxisAngle(axis:Vector3, angle:Number):void { + var c1:Number = Math.cos(angle); + var s:Number = Math.sin(angle); + var t:Number = 1 - c1; + var x:Number = axis.x; + var y:Number = axis.y; + var z:Number = axis.z; + + a = t*x*x + c1; + b = t*x*y - z*s; + c = t*x*z + y*s; + + e = t*x*y + z*s; + f = t*y*y + c1; + g = t*y*z - x*s; + + i = t*x*z - y*s; + j = t*y*z + x*s; + k = t*z*z + c1; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix3 { + return new Matrix3(a, b, c, e, f, g, i, j, k); + } + + public function toString():String { + return "[Matrix3 (" + a + ", " + b + ", " + c + "), (" + e + ", " + f + ", " + g + "), (" + i + ", " + j + ", " + k + ")]"; + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/types/.svn/text-base/Matrix4.as.svn-base b/0.0.5.0/src/alternativa/physics/types/.svn/text-base/Matrix4.as.svn-base new file mode 100644 index 0000000..25aa01c --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/types/.svn/text-base/Matrix4.as.svn-base @@ -0,0 +1,444 @@ +package alternativa.physics.types { + import __AS3__.vec.Vector; + + /** + * + */ + public class Matrix4 { + + public var a:Number; + public var b:Number; + public var c:Number; + public var d:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + public var h:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + public var l:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix4(a:Number = 1, b:Number = 0, c:Number = 0, d:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, h:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1, l:Number = 0) { + this.a = a; + this.b = b; + this.c = c; + this.d = h; + + this.e = e; + this.f = f; + this.g = g; + this.h = h; + + this.i = i; + this.j = j; + this.k = k; + this.l = l; + } + + /** + * + * @return + */ + public function toIdentity():Matrix4 { + a = f = k = 1; + b = c = e = g = i = j = d = h = l = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + var det:Number = -cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk; + + a = (-gg*jj + ff*kk)/det; + b = (cc*jj - bb*kk)/det; + c = (-cc*ff + bb*gg)/det; + d = (dd*gg*jj - cc*hh*jj - dd*ff*kk + bb*hh*kk + cc*ff*ll - bb*gg*ll)/det; + e = (gg*ii - ee*kk)/det; + f = (-cc*ii + aa*kk)/det; + g = (cc*ee - aa*gg)/det; + h = (cc*hh*ii - dd*gg*ii + dd*ee*kk - aa*hh*kk - cc*ee*ll + aa*gg*ll)/det; + i = (-ff*ii + ee*jj)/det; + j = (bb*ii - aa*jj)/det; + k = (-bb*ee + aa*ff)/det; + l = (dd*ff*ii - bb*hh*ii - dd*ee*jj + aa*hh*jj + bb*ee*ll - aa*ff*ll)/det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + d = m.a*dd + m.b*hh + m.c*ll + m.d; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + h = m.e*dd + m.f*hh + m.g*ll + m.h; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + l = m.i*dd + m.j*hh + m.k*ll + m.l; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + d = aa*m.d + bb*m.h + cc*m.l + dd; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + h = ee*m.d + ff*m.h + gg*m.l + hh; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + l = ii*m.d + jj*m.h + kk*m.l + ll; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix4):Matrix4 { + a += m.a; + b += m.b; + c += m.c; + d += m.d; + e += m.e; + f += m.f; + g += m.g; + h += m.h; + i += m.i; + j += m.j; + k += m.k; + l += m.l; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix4):Matrix4 { + a -= m.a; + b -= m.b; + c -= m.c; + d -= m.d; + e -= m.e; + f -= m.f; + g -= m.g; + h -= m.h; + i -= m.i; + j -= m.j; + k -= m.k; + l -= m.l; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectors(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + * @param len + */ + public function transformVectorsN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverse(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverseN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(idx:int, axis:Vector3):void { + switch (idx) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + case 3: + axis.x = d; + axis.y = h; + axis.z = l; + return; + } + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix4):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = m.d; + e = m.e; + f = m.f; + g = m.g; + h = m.h; + i = m.i; + j = m.j; + k = m.k; + l = m.l; + + return this; + } + + public function setFromMatrix3(m:Matrix3, offset:Vector3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = offset.x; + e = m.e; + f = m.f; + g = m.g; + h = offset.y; + i = m.i; + j = m.j; + k = m.k; + l = offset.z; + + return this; + } + + /** + * @param rotations + */ + public function getEulerAngles(rotations:Vector3):void { + if (-1 < i && i < 1) { + rotations.x = Math.atan2(j, k); + rotations.y = -Math.asin(i); + rotations.z = Math.atan2(e, a); + } else { + rotations.x = 0; + rotations.y = 0.5*((i <= -1) ? Math.PI : -Math.PI); + rotations.z = Math.atan2(-b, f); + } + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix4 { + return new Matrix4(a, b, c, d, e, f, g, h, i, j, k, l); + } + + public function toString():String { + return "[Matrix4 [" + a.toFixed(3) + " " + b.toFixed(3) + " " + c.toFixed(3) + " " + d.toFixed(3) + "] [" + e.toFixed(3) + " " + f.toFixed(3) + " " + g.toFixed(3) + " " + h.toFixed(3) + "] [" + i.toFixed(3) + " " + j.toFixed(3) + " " + k.toFixed(3) + " " + l.toFixed(3) + "]]"; + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/types/.svn/text-base/Quaternion.as.svn-base b/0.0.5.0/src/alternativa/physics/types/.svn/text-base/Quaternion.as.svn-base new file mode 100644 index 0000000..7ad2af0 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/types/.svn/text-base/Quaternion.as.svn-base @@ -0,0 +1,396 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + public class Quaternion { + + public var w:Number; + public var x:Number; + public var y:Number; + public var z:Number; + + public static function multiply(q1:Quaternion, q2:Quaternion, result:Quaternion):void { + result.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z; + result.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y; + result.y = q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z; + result.z = q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x; + } + + public static function createFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngle(axis, angle); + return q; + } + + public static function createFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngleComponents(x, y, z, angle); + return q; + } + + public function Quaternion(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0) { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + } + + public function reset(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0):Quaternion { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + return this; + } + + public function normalize():Quaternion { + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * Умножает на указанный кватернион слева: this * q + * + * @param q множитель + */ + public function prepend(q:Quaternion):Quaternion { + var ww:Number = w*q.w - x*q.x - y*q.y - z*q.z; + var xx:Number = w*q.x + x*q.w + y*q.z - z*q.y; + var yy:Number = w*q.y + y*q.w + z*q.x - x*q.z; + var zz:Number = w*q.z + z*q.w + x*q.y - y*q.x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Умножает на указанный кватернион справа: q * this + * + * @param q множитель + */ + public function append(q:Quaternion):Quaternion { + var ww:Number = q.w*w - q.x*x - q.y*y - q.z*z; + var xx:Number = q.w*x + q.x*w + q.y*z - q.z*y; + var yy:Number = q.w*y + q.y*w + q.z*x - q.x*z; + var zz:Number = q.w*z + q.z*w + q.x*y - q.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * + * @param vector + */ + public function rotateByVector(v:Vector3):Quaternion { + var ww:Number = -v.x*x - v.y*y - v.z*z; + var xx:Number = v.x*w + v.y*z - v.z*y; + var yy:Number = v.y*w + v.z*x - v.x*z; + var zz:Number = v.z*w + v.x*y - v.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Добавляет вращение, приданное вектором угловой скорости за указанное время. + * + * @param v + * @param scale + */ + public function addScaledVector(v:Vector3, scale:Number):Quaternion { + var vx:Number = v.x*scale; + var vy:Number = v.y*scale; + var vz:Number = v.z*scale; + var ww:Number = -x*vx - y*vy - z*vz; + var xx:Number = vx*w + vy*z - vz*y; + var yy:Number = vy*w + vz*x - vx*z; + var zz:Number = vz*w + vx*y - vy*x; + w += 0.5*ww; + x += 0.5*xx; + y += 0.5*yy; + z += 0.5*zz; + // inlined normalize + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param m + * @return + */ + public function toMatrix3(m:Matrix3):Quaternion { + var xx2:Number = 2*x*x; + var yy2:Number = 2*y*y; + var zz2:Number = 2*z*z; + var xy2:Number = 2*x*y; + var yz2:Number = 2*y*z; + var zx2:Number = 2*z*x; + var wx2:Number = 2*w*x; + var wy2:Number = 2*w*y; + var wz2:Number = 2*w*z; + + m.a = 1 - yy2 - zz2; + m.b = xy2 - wz2; + m.c = zx2 + wy2; + + m.e = xy2 + wz2; + m.f = 1 - xx2 - zz2; + m.g = yz2 - wx2; + + m.i = zx2 - wy2; + m.j = yz2 + wx2; + m.k = 1 - xx2 - yy2; + return this; + } + + /** + * + * @return + */ + public function length():Number { + return Math.sqrt(w*w + x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function lengthSqr():Number { + return w*w + x*x + y*y + z*z; + } + + /** + * + * @param axis + * @param angle + * @return + */ + public function setFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z); + x = axis.x*k; + y = axis.y*k; + z = axis.z*k; + return this; + } + + /** + * + * @param x + * @param y + * @param z + * @param angle + */ + public function setFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(x*x + y*y + z*z); + this.x = x*k; + this.y = y*k; + this.z = z*k; + return this; + } + + /** + * + * @param vector + */ + public function toAxisVector(v:Vector3 = null):Vector3 { + if (w < -1 || w > 1) { + normalize(); + } + if (v == null) { + v = new Vector3(); + } + if (w > -1 && w < 1) { + if (w == 0) { + v.x = x; + v.y = y; + v.z = z; + } else { + var angle:Number = 2*Math.acos(w); + var coeff:Number = 1/Math.sqrt(1 - w*w); + v.x = x*coeff*angle; + v.y = y*coeff*angle; + v.z = z*coeff*angle; + } + } else { + v.x = 0; + v.y = 0; + v.z = 0; + } + return v; + } + + /** + * + * @param rotations + */ + public function getEulerAngles(angles:Vector3):Vector3 { + var qi2:Number = 2*x*x; + var qj2:Number = 2*y*y; + var qk2:Number = 2*z*z; + var qij:Number = 2*x*y; + var qjk:Number = 2*y*z; + var qki:Number = 2*z*x; + var qri:Number = 2*w*x; + var qrj:Number = 2*w*y; + var qrk:Number = 2*w*z; + + var aa:Number = 1 - qj2 - qk2; + var bb:Number = qij - qrk; + var ee:Number = qij + qrk; + var ff:Number = 1 - qi2 - qk2; + var ii:Number = qki - qrj; + var jj:Number = qjk + qri; + var kk:Number = 1 - qi2 - qj2; + + if (-1 < ii && ii < 1) { + if (angles == null) { + angles = new Vector3(Math.atan2(jj, kk), -Math.asin(ii), Math.atan2(ee, aa)); + } else { + angles.x = Math.atan2(jj, kk); + angles.y = -Math.asin(ii); + angles.z = Math.atan2(ee, aa); + } + } else { + if (angles == null) { + angles = new Vector3(0, 0.5*((ii <= -1) ? Math.PI : -Math.PI), Math.atan2(-bb, ff)); + } else { + angles.x = 0; + angles.y = 0.5*((ii <= -1) ? Math.PI : -Math.PI); + angles.z = Math.atan2(-bb, ff); + } + } + return angles; + } + + /** + * + */ + public function conjugate():void { + x = -x; + y = -y; + z = -z; + } + + /** + * Выполняет линейную интерполяцию. + * + * @param q1 начало отрезка + * @param q2 конец отрезка + * @param t время, обычно задаётся в интервале [0, 1] + */ + public function nlerp(q1:Quaternion, q2:Quaternion, t:Number):Quaternion { + var d:Number = 1 - t; + w = q1.w*d + q2.w*t; + x = q1.x*d + q2.x*t; + y = q1.y*d + q2.y*t; + z = q1.z*d + q2.z*t; + // inlined normalize + d = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param q + * @return + */ + public function subtract(q:Quaternion):Quaternion { + w -= q.w; + x -= q.x; + y -= q.y; + z -= q.z; + return this; + } + + /** + * + * @param q1 + * @param q2 + * @return + */ + public function diff(q1:Quaternion, q2:Quaternion):Quaternion { + w = q2.w - q1.w; + x = q2.x - q1.x; + y = q2.y - q1.y; + z = q2.z - q1.z; + return this; + } + + /** + * + * @param q + * @return + */ + public function copy(q:Quaternion):Quaternion { + w = q.w; + x = q.x; + y = q.y; + z = q.z; + return this; + } + + /** + * @param reslult + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + result.w = w; + return result; + } + + /** + * + * @return + */ + public function clone():Quaternion { + return new Quaternion(w, x, y, z); + } + + /** + * + * @return + */ + public function toString():String { + return "[" + w + ", " + x + ", " + y + ", " + z + "]"; + } + } +} diff --git a/0.0.5.0/src/alternativa/physics/types/.svn/text-base/Vector3.as.svn-base b/0.0.5.0/src/alternativa/physics/types/.svn/text-base/Vector3.as.svn-base new file mode 100644 index 0000000..4f0e7e4 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/types/.svn/text-base/Vector3.as.svn-base @@ -0,0 +1,335 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + /** + * + */ + public class Vector3 extends Vector3D { + + public static const ZERO:Vector3 = new Vector3(0, 0, 0); + public static const X_AXIS:Vector3 = new Vector3(1, 0, 0); + public static const Y_AXIS:Vector3 = new Vector3(0, 1, 0); + public static const Z_AXIS:Vector3 = new Vector3(0, 0, 1); + + /** + * + * @param x + * @param y + * @param z + */ + public function Vector3(x:Number = 0, y:Number = 0, z:Number = 0) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * + * @return + */ + public function vLength():Number { + return Math.sqrt(x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function vLengthSqr():Number { + return x*x + y*y + z*z; + } + + /** + * + * @param length + * @return + */ + public function vSetLength(length:Number):Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = length; + } else { + var k:Number = length/Math.sqrt(x*x + y*y + z*z); + x *= k; + y *= k; + z *= k; + } + return this; + } + + /** + * + * @return + */ + public function vNormalize():Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = 1; + } else { + d = Math.sqrt(d); + x /= d; + y /= d; + z /= d; + } + return this; + } + + /** + * Сложение координат. + * + * @param v точка, координаты которой прибавляются к собственным + */ + public function vAdd(v:Vector3):Vector3 { + x += v.x; + y += v.y; + z += v.z; + return this; + } + + /** + * + * @param k + * @param v + * @return + */ + public function vAddScaled(k:Number, v:Vector3):Vector3 { + x += k*v.x; + y += k*v.y; + z += k*v.z; + return this; + } + + /** + * Вычитание координат. + * + * @param v точка, координаты которой вычитаются из собственных + */ + public function vSubtract(v:Vector3):Vector3 { + x -= v.x; + y -= v.y; + z -= v.z; + return this; + } + + /** + * Вычисляет сумму векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + * @return this + */ + public function vSum(a:Vector3, b:Vector3):Vector3 { + x = a.x + b.x; + y = a.y + b.y; + z = a.z + b.z; + return this; + } + + /** + * Вычисление разности векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + */ + public function vDiff(a:Vector3, b:Vector3):Vector3 { + x = a.x - b.x; + y = a.y - b.y; + z = a.z - b.z; + return this; + } + + /** + * Умножение на скаляр. + * + * @param k число, на которое умножаются координаты + */ + public function vScale(k:Number):Vector3 { + x *= k; + y *= k; + z *= k; + return this; + } + + /** + * Инвертирование вектора. + */ + public function vReverse():Vector3 { + x = -x; + y = -y; + z = -z; + return this; + } + + /** + * + * @param v + * @return + */ + public function vDot(v:Vector3):Number { + return x*v.x + y*v.y + z*v.z; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross(v:Vector3):Vector3 { + var xx:Number = y*v.z - z*v.y; + var yy:Number = z*v.x - x*v.z; + var zz:Number = x*v.y - y*v.x; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross2(a:Vector3, b:Vector3):Vector3 { + x = a.y*b.z - a.z*b.y; + y = a.z*b.x - a.x*b.z; + z = a.x*b.y - a.y*b.x; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy3(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * + * @param matrix + */ + public function vTransformBy3Tr(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz + m.d; + y = m.e*xx + m.f*yy + m.g*zz + m.h; + z = m.i*xx + m.j*yy + m.k*zz + m.l; + return this; + } + + /** + * + * @param m + * @return + */ + public function vTransformInverseBy4(m:Matrix4):Vector3 { + var xx:Number = x - m.d; + var yy:Number = y - m.h; + var zz:Number = z - m.l; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * + * @param m + */ + public function vDeltaTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * Установка координат. + */ + public function vReset(x:Number = 0, y:Number = 0, z:Number = 0):Vector3 { + this.x = x; + this.y = y; + this.z = z; + return this; + } + + /** + * Копирование координат точки. + * + * @param v точка, координаты которой копируются + */ + public function vCopy(v:Vector3):Vector3 { + x = v.x; + y = v.y; + z = v.z; + return this; + } + + /** + * Клонирование точки. + * + * @return клонированная точка + */ + public function vClone():Vector3 { + return new Vector3(x, y, z); + } + + /** + * Копирует координаты вектора в экземпляр класса Vector3D. + * + * @param result экземпляр, в который копируются координаты + * @return переданный параметр + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + return result; + } + + /** + * @param source + * @return + */ + public function copyFromVector3D(source:Vector3D):Vector3 { + x = source.x; + y = source.y; + z = source.z; + return this; + } + + override public function toString():String { + return "Vector3(" + x + "," + y + ", " + z + ")"; + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/types/Matrix3.as b/0.0.5.0/src/alternativa/physics/types/Matrix3.as new file mode 100644 index 0000000..e07d0d1 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/types/Matrix3.as @@ -0,0 +1,396 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + /** + * + */ + public class Matrix3 { + + public static const ZERO:Matrix3 = new Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0); + public static const IDENTITY:Matrix3 = new Matrix3(); + + public var a:Number; + public var b:Number; + public var c:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix3(a:Number = 1, b:Number = 0, c:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1) { + this.a = a; + this.b = b; + this.c = c; + + this.e = e; + this.f = f; + this.g = g; + + this.i = i; + this.j = j; + this.k = k; + } + + /** + * + * @return + */ + public function toIdentity():Matrix3 { + a = f = k = 1; + b = c = e = g = i = j = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + var det:Number = 1/(-cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk); + + a = (ff*kk - gg*jj)*det; + b = (cc*jj - bb*kk)*det; + c = (bb*gg - cc*ff)*det; + e = (gg*ii - ee*kk)*det; + f = (aa*kk - cc*ii)*det; + g = (cc*ee - aa*gg)*det; + i = (ee*jj - ff*ii)*det; + j = (bb*ii - aa*jj)*det; + k = (aa*ff - bb*ee)*det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prependTransposed(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.b + cc*m.c; + b = aa*m.e + bb*m.f + cc*m.g; + c = aa*m.i + bb*m.j + cc*m.k; + e = ee*m.a + ff*m.b + gg*m.c; + f = ee*m.e + ff*m.f + gg*m.g; + g = ee*m.i + ff*m.j + gg*m.k; + i = ii*m.a + jj*m.b + kk*m.c; + j = ii*m.e + jj*m.f + kk*m.g; + k = ii*m.i + jj*m.j + kk*m.k; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix3):Matrix3 { + a += m.a; + b += m.b; + c += m.c; + e += m.e; + f += m.f; + g += m.g; + i += m.i; + j += m.j; + k += m.k; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix3):Matrix3 { + a -= m.a; + b -= m.b; + c -= m.c; + e -= m.e; + f -= m.f; + g -= m.g; + i -= m.i; + j -= m.j; + k -= m.k; + + return this; + } + + /** + * + * @return + */ + public function transpose():Matrix3 { + var tmp:Number = b; + b = e; + e = tmp; + tmp = c; + c = i; + i = tmp; + tmp = g; + g = j; + j = tmp; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector3To3D(vin:Vector3, vout:Vector3D):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * + * @param matrix + */ + public function createSkewSymmetric(v:Vector3):Matrix3 { + a = f = k = 0; + b = -v.z; + c = v.y; + e = v.z; + g = -v.x; + i = -v.y; + j = v.x; + return this; + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(i:int, axis:Vector3):void { + switch (i) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + } + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix3):Matrix3 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @return + */ + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):Matrix3 { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + + return this; + } + + /** + * Метод формирует матрицу поворота на заданный угол относительно заданной оси. + * + * @param axis нормализованный вектор, задающий ось, относительно которой выполняется поворот + * @param angle угол поворота в радианах + */ + public function fromAxisAngle(axis:Vector3, angle:Number):void { + var c1:Number = Math.cos(angle); + var s:Number = Math.sin(angle); + var t:Number = 1 - c1; + var x:Number = axis.x; + var y:Number = axis.y; + var z:Number = axis.z; + + a = t*x*x + c1; + b = t*x*y - z*s; + c = t*x*z + y*s; + + e = t*x*y + z*s; + f = t*y*y + c1; + g = t*y*z - x*s; + + i = t*x*z - y*s; + j = t*y*z + x*s; + k = t*z*z + c1; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix3 { + return new Matrix3(a, b, c, e, f, g, i, j, k); + } + + public function toString():String { + return "[Matrix3 (" + a + ", " + b + ", " + c + "), (" + e + ", " + f + ", " + g + "), (" + i + ", " + j + ", " + k + ")]"; + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/types/Matrix4.as b/0.0.5.0/src/alternativa/physics/types/Matrix4.as new file mode 100644 index 0000000..25aa01c --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/types/Matrix4.as @@ -0,0 +1,444 @@ +package alternativa.physics.types { + import __AS3__.vec.Vector; + + /** + * + */ + public class Matrix4 { + + public var a:Number; + public var b:Number; + public var c:Number; + public var d:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + public var h:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + public var l:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix4(a:Number = 1, b:Number = 0, c:Number = 0, d:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, h:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1, l:Number = 0) { + this.a = a; + this.b = b; + this.c = c; + this.d = h; + + this.e = e; + this.f = f; + this.g = g; + this.h = h; + + this.i = i; + this.j = j; + this.k = k; + this.l = l; + } + + /** + * + * @return + */ + public function toIdentity():Matrix4 { + a = f = k = 1; + b = c = e = g = i = j = d = h = l = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + var det:Number = -cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk; + + a = (-gg*jj + ff*kk)/det; + b = (cc*jj - bb*kk)/det; + c = (-cc*ff + bb*gg)/det; + d = (dd*gg*jj - cc*hh*jj - dd*ff*kk + bb*hh*kk + cc*ff*ll - bb*gg*ll)/det; + e = (gg*ii - ee*kk)/det; + f = (-cc*ii + aa*kk)/det; + g = (cc*ee - aa*gg)/det; + h = (cc*hh*ii - dd*gg*ii + dd*ee*kk - aa*hh*kk - cc*ee*ll + aa*gg*ll)/det; + i = (-ff*ii + ee*jj)/det; + j = (bb*ii - aa*jj)/det; + k = (-bb*ee + aa*ff)/det; + l = (dd*ff*ii - bb*hh*ii - dd*ee*jj + aa*hh*jj + bb*ee*ll - aa*ff*ll)/det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + d = m.a*dd + m.b*hh + m.c*ll + m.d; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + h = m.e*dd + m.f*hh + m.g*ll + m.h; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + l = m.i*dd + m.j*hh + m.k*ll + m.l; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + d = aa*m.d + bb*m.h + cc*m.l + dd; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + h = ee*m.d + ff*m.h + gg*m.l + hh; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + l = ii*m.d + jj*m.h + kk*m.l + ll; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix4):Matrix4 { + a += m.a; + b += m.b; + c += m.c; + d += m.d; + e += m.e; + f += m.f; + g += m.g; + h += m.h; + i += m.i; + j += m.j; + k += m.k; + l += m.l; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix4):Matrix4 { + a -= m.a; + b -= m.b; + c -= m.c; + d -= m.d; + e -= m.e; + f -= m.f; + g -= m.g; + h -= m.h; + i -= m.i; + j -= m.j; + k -= m.k; + l -= m.l; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectors(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + * @param len + */ + public function transformVectorsN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverse(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverseN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(idx:int, axis:Vector3):void { + switch (idx) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + case 3: + axis.x = d; + axis.y = h; + axis.z = l; + return; + } + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix4):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = m.d; + e = m.e; + f = m.f; + g = m.g; + h = m.h; + i = m.i; + j = m.j; + k = m.k; + l = m.l; + + return this; + } + + public function setFromMatrix3(m:Matrix3, offset:Vector3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = offset.x; + e = m.e; + f = m.f; + g = m.g; + h = offset.y; + i = m.i; + j = m.j; + k = m.k; + l = offset.z; + + return this; + } + + /** + * @param rotations + */ + public function getEulerAngles(rotations:Vector3):void { + if (-1 < i && i < 1) { + rotations.x = Math.atan2(j, k); + rotations.y = -Math.asin(i); + rotations.z = Math.atan2(e, a); + } else { + rotations.x = 0; + rotations.y = 0.5*((i <= -1) ? Math.PI : -Math.PI); + rotations.z = Math.atan2(-b, f); + } + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix4 { + return new Matrix4(a, b, c, d, e, f, g, h, i, j, k, l); + } + + public function toString():String { + return "[Matrix4 [" + a.toFixed(3) + " " + b.toFixed(3) + " " + c.toFixed(3) + " " + d.toFixed(3) + "] [" + e.toFixed(3) + " " + f.toFixed(3) + " " + g.toFixed(3) + " " + h.toFixed(3) + "] [" + i.toFixed(3) + " " + j.toFixed(3) + " " + k.toFixed(3) + " " + l.toFixed(3) + "]]"; + } + + } +} \ No newline at end of file diff --git a/0.0.5.0/src/alternativa/physics/types/Quaternion.as b/0.0.5.0/src/alternativa/physics/types/Quaternion.as new file mode 100644 index 0000000..7ad2af0 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/types/Quaternion.as @@ -0,0 +1,396 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + public class Quaternion { + + public var w:Number; + public var x:Number; + public var y:Number; + public var z:Number; + + public static function multiply(q1:Quaternion, q2:Quaternion, result:Quaternion):void { + result.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z; + result.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y; + result.y = q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z; + result.z = q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x; + } + + public static function createFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngle(axis, angle); + return q; + } + + public static function createFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngleComponents(x, y, z, angle); + return q; + } + + public function Quaternion(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0) { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + } + + public function reset(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0):Quaternion { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + return this; + } + + public function normalize():Quaternion { + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * Умножает на указанный кватернион слева: this * q + * + * @param q множитель + */ + public function prepend(q:Quaternion):Quaternion { + var ww:Number = w*q.w - x*q.x - y*q.y - z*q.z; + var xx:Number = w*q.x + x*q.w + y*q.z - z*q.y; + var yy:Number = w*q.y + y*q.w + z*q.x - x*q.z; + var zz:Number = w*q.z + z*q.w + x*q.y - y*q.x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Умножает на указанный кватернион справа: q * this + * + * @param q множитель + */ + public function append(q:Quaternion):Quaternion { + var ww:Number = q.w*w - q.x*x - q.y*y - q.z*z; + var xx:Number = q.w*x + q.x*w + q.y*z - q.z*y; + var yy:Number = q.w*y + q.y*w + q.z*x - q.x*z; + var zz:Number = q.w*z + q.z*w + q.x*y - q.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * + * @param vector + */ + public function rotateByVector(v:Vector3):Quaternion { + var ww:Number = -v.x*x - v.y*y - v.z*z; + var xx:Number = v.x*w + v.y*z - v.z*y; + var yy:Number = v.y*w + v.z*x - v.x*z; + var zz:Number = v.z*w + v.x*y - v.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Добавляет вращение, приданное вектором угловой скорости за указанное время. + * + * @param v + * @param scale + */ + public function addScaledVector(v:Vector3, scale:Number):Quaternion { + var vx:Number = v.x*scale; + var vy:Number = v.y*scale; + var vz:Number = v.z*scale; + var ww:Number = -x*vx - y*vy - z*vz; + var xx:Number = vx*w + vy*z - vz*y; + var yy:Number = vy*w + vz*x - vx*z; + var zz:Number = vz*w + vx*y - vy*x; + w += 0.5*ww; + x += 0.5*xx; + y += 0.5*yy; + z += 0.5*zz; + // inlined normalize + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param m + * @return + */ + public function toMatrix3(m:Matrix3):Quaternion { + var xx2:Number = 2*x*x; + var yy2:Number = 2*y*y; + var zz2:Number = 2*z*z; + var xy2:Number = 2*x*y; + var yz2:Number = 2*y*z; + var zx2:Number = 2*z*x; + var wx2:Number = 2*w*x; + var wy2:Number = 2*w*y; + var wz2:Number = 2*w*z; + + m.a = 1 - yy2 - zz2; + m.b = xy2 - wz2; + m.c = zx2 + wy2; + + m.e = xy2 + wz2; + m.f = 1 - xx2 - zz2; + m.g = yz2 - wx2; + + m.i = zx2 - wy2; + m.j = yz2 + wx2; + m.k = 1 - xx2 - yy2; + return this; + } + + /** + * + * @return + */ + public function length():Number { + return Math.sqrt(w*w + x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function lengthSqr():Number { + return w*w + x*x + y*y + z*z; + } + + /** + * + * @param axis + * @param angle + * @return + */ + public function setFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z); + x = axis.x*k; + y = axis.y*k; + z = axis.z*k; + return this; + } + + /** + * + * @param x + * @param y + * @param z + * @param angle + */ + public function setFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(x*x + y*y + z*z); + this.x = x*k; + this.y = y*k; + this.z = z*k; + return this; + } + + /** + * + * @param vector + */ + public function toAxisVector(v:Vector3 = null):Vector3 { + if (w < -1 || w > 1) { + normalize(); + } + if (v == null) { + v = new Vector3(); + } + if (w > -1 && w < 1) { + if (w == 0) { + v.x = x; + v.y = y; + v.z = z; + } else { + var angle:Number = 2*Math.acos(w); + var coeff:Number = 1/Math.sqrt(1 - w*w); + v.x = x*coeff*angle; + v.y = y*coeff*angle; + v.z = z*coeff*angle; + } + } else { + v.x = 0; + v.y = 0; + v.z = 0; + } + return v; + } + + /** + * + * @param rotations + */ + public function getEulerAngles(angles:Vector3):Vector3 { + var qi2:Number = 2*x*x; + var qj2:Number = 2*y*y; + var qk2:Number = 2*z*z; + var qij:Number = 2*x*y; + var qjk:Number = 2*y*z; + var qki:Number = 2*z*x; + var qri:Number = 2*w*x; + var qrj:Number = 2*w*y; + var qrk:Number = 2*w*z; + + var aa:Number = 1 - qj2 - qk2; + var bb:Number = qij - qrk; + var ee:Number = qij + qrk; + var ff:Number = 1 - qi2 - qk2; + var ii:Number = qki - qrj; + var jj:Number = qjk + qri; + var kk:Number = 1 - qi2 - qj2; + + if (-1 < ii && ii < 1) { + if (angles == null) { + angles = new Vector3(Math.atan2(jj, kk), -Math.asin(ii), Math.atan2(ee, aa)); + } else { + angles.x = Math.atan2(jj, kk); + angles.y = -Math.asin(ii); + angles.z = Math.atan2(ee, aa); + } + } else { + if (angles == null) { + angles = new Vector3(0, 0.5*((ii <= -1) ? Math.PI : -Math.PI), Math.atan2(-bb, ff)); + } else { + angles.x = 0; + angles.y = 0.5*((ii <= -1) ? Math.PI : -Math.PI); + angles.z = Math.atan2(-bb, ff); + } + } + return angles; + } + + /** + * + */ + public function conjugate():void { + x = -x; + y = -y; + z = -z; + } + + /** + * Выполняет линейную интерполяцию. + * + * @param q1 начало отрезка + * @param q2 конец отрезка + * @param t время, обычно задаётся в интервале [0, 1] + */ + public function nlerp(q1:Quaternion, q2:Quaternion, t:Number):Quaternion { + var d:Number = 1 - t; + w = q1.w*d + q2.w*t; + x = q1.x*d + q2.x*t; + y = q1.y*d + q2.y*t; + z = q1.z*d + q2.z*t; + // inlined normalize + d = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param q + * @return + */ + public function subtract(q:Quaternion):Quaternion { + w -= q.w; + x -= q.x; + y -= q.y; + z -= q.z; + return this; + } + + /** + * + * @param q1 + * @param q2 + * @return + */ + public function diff(q1:Quaternion, q2:Quaternion):Quaternion { + w = q2.w - q1.w; + x = q2.x - q1.x; + y = q2.y - q1.y; + z = q2.z - q1.z; + return this; + } + + /** + * + * @param q + * @return + */ + public function copy(q:Quaternion):Quaternion { + w = q.w; + x = q.x; + y = q.y; + z = q.z; + return this; + } + + /** + * @param reslult + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + result.w = w; + return result; + } + + /** + * + * @return + */ + public function clone():Quaternion { + return new Quaternion(w, x, y, z); + } + + /** + * + * @return + */ + public function toString():String { + return "[" + w + ", " + x + ", " + y + ", " + z + "]"; + } + } +} diff --git a/0.0.5.0/src/alternativa/physics/types/Vector3.as b/0.0.5.0/src/alternativa/physics/types/Vector3.as new file mode 100644 index 0000000..4f0e7e4 --- /dev/null +++ b/0.0.5.0/src/alternativa/physics/types/Vector3.as @@ -0,0 +1,335 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + /** + * + */ + public class Vector3 extends Vector3D { + + public static const ZERO:Vector3 = new Vector3(0, 0, 0); + public static const X_AXIS:Vector3 = new Vector3(1, 0, 0); + public static const Y_AXIS:Vector3 = new Vector3(0, 1, 0); + public static const Z_AXIS:Vector3 = new Vector3(0, 0, 1); + + /** + * + * @param x + * @param y + * @param z + */ + public function Vector3(x:Number = 0, y:Number = 0, z:Number = 0) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * + * @return + */ + public function vLength():Number { + return Math.sqrt(x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function vLengthSqr():Number { + return x*x + y*y + z*z; + } + + /** + * + * @param length + * @return + */ + public function vSetLength(length:Number):Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = length; + } else { + var k:Number = length/Math.sqrt(x*x + y*y + z*z); + x *= k; + y *= k; + z *= k; + } + return this; + } + + /** + * + * @return + */ + public function vNormalize():Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = 1; + } else { + d = Math.sqrt(d); + x /= d; + y /= d; + z /= d; + } + return this; + } + + /** + * Сложение координат. + * + * @param v точка, координаты которой прибавляются к собственным + */ + public function vAdd(v:Vector3):Vector3 { + x += v.x; + y += v.y; + z += v.z; + return this; + } + + /** + * + * @param k + * @param v + * @return + */ + public function vAddScaled(k:Number, v:Vector3):Vector3 { + x += k*v.x; + y += k*v.y; + z += k*v.z; + return this; + } + + /** + * Вычитание координат. + * + * @param v точка, координаты которой вычитаются из собственных + */ + public function vSubtract(v:Vector3):Vector3 { + x -= v.x; + y -= v.y; + z -= v.z; + return this; + } + + /** + * Вычисляет сумму векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + * @return this + */ + public function vSum(a:Vector3, b:Vector3):Vector3 { + x = a.x + b.x; + y = a.y + b.y; + z = a.z + b.z; + return this; + } + + /** + * Вычисление разности векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + */ + public function vDiff(a:Vector3, b:Vector3):Vector3 { + x = a.x - b.x; + y = a.y - b.y; + z = a.z - b.z; + return this; + } + + /** + * Умножение на скаляр. + * + * @param k число, на которое умножаются координаты + */ + public function vScale(k:Number):Vector3 { + x *= k; + y *= k; + z *= k; + return this; + } + + /** + * Инвертирование вектора. + */ + public function vReverse():Vector3 { + x = -x; + y = -y; + z = -z; + return this; + } + + /** + * + * @param v + * @return + */ + public function vDot(v:Vector3):Number { + return x*v.x + y*v.y + z*v.z; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross(v:Vector3):Vector3 { + var xx:Number = y*v.z - z*v.y; + var yy:Number = z*v.x - x*v.z; + var zz:Number = x*v.y - y*v.x; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross2(a:Vector3, b:Vector3):Vector3 { + x = a.y*b.z - a.z*b.y; + y = a.z*b.x - a.x*b.z; + z = a.x*b.y - a.y*b.x; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy3(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * + * @param matrix + */ + public function vTransformBy3Tr(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz + m.d; + y = m.e*xx + m.f*yy + m.g*zz + m.h; + z = m.i*xx + m.j*yy + m.k*zz + m.l; + return this; + } + + /** + * + * @param m + * @return + */ + public function vTransformInverseBy4(m:Matrix4):Vector3 { + var xx:Number = x - m.d; + var yy:Number = y - m.h; + var zz:Number = z - m.l; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * + * @param m + */ + public function vDeltaTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * Установка координат. + */ + public function vReset(x:Number = 0, y:Number = 0, z:Number = 0):Vector3 { + this.x = x; + this.y = y; + this.z = z; + return this; + } + + /** + * Копирование координат точки. + * + * @param v точка, координаты которой копируются + */ + public function vCopy(v:Vector3):Vector3 { + x = v.x; + y = v.y; + z = v.z; + return this; + } + + /** + * Клонирование точки. + * + * @return клонированная точка + */ + public function vClone():Vector3 { + return new Vector3(x, y, z); + } + + /** + * Копирует координаты вектора в экземпляр класса Vector3D. + * + * @param result экземпляр, в который копируются координаты + * @return переданный параметр + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + return result; + } + + /** + * @param source + * @return + */ + public function copyFromVector3D(source:Vector3D):Vector3 { + x = source.x; + y = source.y; + z = source.z; + return this; + } + + override public function toString():String { + return "Vector3(" + x + "," + y + ", " + z + ")"; + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/.actionScriptProperties b/0.0.5.Optimized/.actionScriptProperties new file mode 100644 index 0000000..f73adc8 --- /dev/null +++ b/0.0.5.Optimized/.actionScriptProperties @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/0.0.5.Optimized/.flexLibProperties b/0.0.5.Optimized/.flexLibProperties new file mode 100644 index 0000000..a9fd6b3 --- /dev/null +++ b/0.0.5.Optimized/.flexLibProperties @@ -0,0 +1,52 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/0.0.5.Optimized/.project b/0.0.5.Optimized/.project new file mode 100644 index 0000000..1f1425f --- /dev/null +++ b/0.0.5.Optimized/.project @@ -0,0 +1,19 @@ + + + AlternativaPhysics + + + + + + com.adobe.flexbuilder.project.flexbuilder + + + + + + org.maven.ide.eclipse.maven2Nature + com.adobe.flexbuilder.project.flexlibnature + com.adobe.flexbuilder.project.actionscriptnature + + diff --git a/0.0.5.Optimized/.settings/.svn/all-wcprops b/0.0.5.Optimized/.settings/.svn/all-wcprops new file mode 100644 index 0000000..88f3bea --- /dev/null +++ b/0.0.5.Optimized/.settings/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 101 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/.settings +END +org.eclipse.core.resources.prefs +K 25 +svn:wc:ra_dav:version-url +V 134 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/.settings/org.eclipse.core.resources.prefs +END diff --git a/0.0.5.Optimized/.settings/.svn/entries b/0.0.5.Optimized/.settings/.svn/entries new file mode 100644 index 0000000..574aa90 --- /dev/null +++ b/0.0.5.Optimized/.settings/.svn/entries @@ -0,0 +1,40 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/.settings +http://svndev.alternativaplatform.com + + + +2009-04-01T12:25:29.638016Z +10301 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +org.eclipse.core.resources.prefs +file + + + + +2010-10-28T04:32:38.000000Z +4f70b476f3e5075e399505021df2f89b +2009-04-01T12:25:29.638016Z +10301 +mike + diff --git a/0.0.5.Optimized/.settings/.svn/format b/0.0.5.Optimized/.settings/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.5.Optimized/.settings/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.5.Optimized/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base b/0.0.5.Optimized/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base new file mode 100644 index 0000000..b3d0d49 --- /dev/null +++ b/0.0.5.Optimized/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base @@ -0,0 +1,3 @@ +#Fri Mar 20 00:19:33 YEKT 2009 +eclipse.preferences.version=1 +encoding/=UTF-8 diff --git a/0.0.5.Optimized/.settings/org.eclipse.core.resources.prefs b/0.0.5.Optimized/.settings/org.eclipse.core.resources.prefs new file mode 100644 index 0000000..b3d0d49 --- /dev/null +++ b/0.0.5.Optimized/.settings/org.eclipse.core.resources.prefs @@ -0,0 +1,3 @@ +#Fri Mar 20 00:19:33 YEKT 2009 +eclipse.preferences.version=1 +encoding/=UTF-8 diff --git a/0.0.5.Optimized/.svn/all-wcprops b/0.0.5.Optimized/.svn/all-wcprops new file mode 100644 index 0000000..5ba6628 --- /dev/null +++ b/0.0.5.Optimized/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 91 +/!svn/ver/19860/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized +END +.flexLibProperties +K 25 +svn:wc:ra_dav:version-url +V 110 +/!svn/ver/19850/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/.flexLibProperties +END +.project +K 25 +svn:wc:ra_dav:version-url +V 100 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/.project +END +pom.xml +K 25 +svn:wc:ra_dav:version-url +V 99 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/pom.xml +END +.actionScriptProperties +K 25 +svn:wc:ra_dav:version-url +V 115 +/!svn/ver/19809/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/.actionScriptProperties +END diff --git a/0.0.5.Optimized/.svn/dir-prop-base b/0.0.5.Optimized/.svn/dir-prop-base new file mode 100644 index 0000000..8eda4b3 --- /dev/null +++ b/0.0.5.Optimized/.svn/dir-prop-base @@ -0,0 +1,5 @@ +K 13 +svn:mergeinfo +V 85 +/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D:13892-14098 +END diff --git a/0.0.5.Optimized/.svn/entries b/0.0.5.Optimized/.svn/entries new file mode 100644 index 0000000..2c342c1 --- /dev/null +++ b/0.0.5.Optimized/.svn/entries @@ -0,0 +1,85 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized +http://svndev.alternativaplatform.com + + + +2009-09-15T05:34:09.526646Z +19860 +mike +has-props + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +META-INF +dir + +.flexLibProperties +file + + + + +2010-10-28T04:32:38.000000Z +6c3b24c32d738d6707eb66c8958d0dde +2009-09-14T12:08:58.450611Z +19850 +mike + +.project +file + + + + +2010-10-28T04:32:38.000000Z +1e90cc68b52b93173b2b5de88eb5c3a3 +2009-04-20T17:15:55.260110Z +11541 +mike + +src +dir + +pom.xml +file + + + + +2010-10-28T04:32:38.000000Z +3a5f9823247a9630f614b08b93a555eb +2009-09-10T20:48:14.856790Z +19727 +mike + +.actionScriptProperties +file + + + + +2010-10-28T04:32:38.000000Z +b8656f882fff2baedd57a92db86fcf8c +2009-09-13T12:13:24.307076Z +19809 +mike + +.settings +dir + diff --git a/0.0.5.Optimized/.svn/format b/0.0.5.Optimized/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.5.Optimized/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.5.Optimized/.svn/text-base/.actionScriptProperties.svn-base b/0.0.5.Optimized/.svn/text-base/.actionScriptProperties.svn-base new file mode 100644 index 0000000..f73adc8 --- /dev/null +++ b/0.0.5.Optimized/.svn/text-base/.actionScriptProperties.svn-base @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/0.0.5.Optimized/.svn/text-base/.flexLibProperties.svn-base b/0.0.5.Optimized/.svn/text-base/.flexLibProperties.svn-base new file mode 100644 index 0000000..a9fd6b3 --- /dev/null +++ b/0.0.5.Optimized/.svn/text-base/.flexLibProperties.svn-base @@ -0,0 +1,52 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/0.0.5.Optimized/.svn/text-base/.project.svn-base b/0.0.5.Optimized/.svn/text-base/.project.svn-base new file mode 100644 index 0000000..1f1425f --- /dev/null +++ b/0.0.5.Optimized/.svn/text-base/.project.svn-base @@ -0,0 +1,19 @@ + + + AlternativaPhysics + + + + + + com.adobe.flexbuilder.project.flexbuilder + + + + + + org.maven.ide.eclipse.maven2Nature + com.adobe.flexbuilder.project.flexlibnature + com.adobe.flexbuilder.project.actionscriptnature + + diff --git a/0.0.5.Optimized/.svn/text-base/pom.xml.svn-base b/0.0.5.Optimized/.svn/text-base/pom.xml.svn-base new file mode 100644 index 0000000..4a8301f --- /dev/null +++ b/0.0.5.Optimized/.svn/text-base/pom.xml.svn-base @@ -0,0 +1,17 @@ + + 4.0.0 + platform.clients.fp10.libraries + AlternativaPhysics + swc + 0.0.6.0-SNAPSHOT + + platform.tools.maven + FlashBasePom + 1.0 + + + scm:svn:http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/trunk + + + + \ No newline at end of file diff --git a/0.0.5.Optimized/META-INF/.svn/all-wcprops b/0.0.5.Optimized/META-INF/.svn/all-wcprops new file mode 100644 index 0000000..1bd13a5 --- /dev/null +++ b/0.0.5.Optimized/META-INF/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 100 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/META-INF +END +MANIFEST.MF +K 25 +svn:wc:ra_dav:version-url +V 112 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/META-INF/MANIFEST.MF +END diff --git a/0.0.5.Optimized/META-INF/.svn/entries b/0.0.5.Optimized/META-INF/.svn/entries new file mode 100644 index 0000000..bc80b26 --- /dev/null +++ b/0.0.5.Optimized/META-INF/.svn/entries @@ -0,0 +1,40 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/META-INF +http://svndev.alternativaplatform.com + + + +2009-04-20T17:15:55.260110Z +11541 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +MANIFEST.MF +file + + + + +2010-10-28T04:32:37.000000Z +7b64dceb50a6905850ff00a0bfbe77eb +2009-04-20T17:15:55.260110Z +11541 +mike + diff --git a/0.0.5.Optimized/META-INF/.svn/format b/0.0.5.Optimized/META-INF/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.5.Optimized/META-INF/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.5.Optimized/META-INF/.svn/text-base/MANIFEST.MF.svn-base b/0.0.5.Optimized/META-INF/.svn/text-base/MANIFEST.MF.svn-base new file mode 100644 index 0000000..5066a55 --- /dev/null +++ b/0.0.5.Optimized/META-INF/.svn/text-base/MANIFEST.MF.svn-base @@ -0,0 +1 @@ +Bundle-Name: platform.clients.fp10.libraries.AlternativaPhysics diff --git a/0.0.5.Optimized/META-INF/MANIFEST.MF b/0.0.5.Optimized/META-INF/MANIFEST.MF new file mode 100644 index 0000000..5066a55 --- /dev/null +++ b/0.0.5.Optimized/META-INF/MANIFEST.MF @@ -0,0 +1 @@ +Bundle-Name: platform.clients.fp10.libraries.AlternativaPhysics diff --git a/0.0.5.Optimized/pom.xml b/0.0.5.Optimized/pom.xml new file mode 100644 index 0000000..4a8301f --- /dev/null +++ b/0.0.5.Optimized/pom.xml @@ -0,0 +1,17 @@ + + 4.0.0 + platform.clients.fp10.libraries + AlternativaPhysics + swc + 0.0.6.0-SNAPSHOT + + platform.tools.maven + FlashBasePom + 1.0 + + + scm:svn:http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/trunk + + + + \ No newline at end of file diff --git a/0.0.5.Optimized/src/.svn/all-wcprops b/0.0.5.Optimized/src/.svn/all-wcprops new file mode 100644 index 0000000..8adcec3 --- /dev/null +++ b/0.0.5.Optimized/src/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 95 +/!svn/ver/19860/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src +END diff --git a/0.0.5.Optimized/src/.svn/entries b/0.0.5.Optimized/src/.svn/entries new file mode 100644 index 0000000..3b97b21 --- /dev/null +++ b/0.0.5.Optimized/src/.svn/entries @@ -0,0 +1,31 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src +http://svndev.alternativaplatform.com + + + +2009-09-15T05:34:09.526646Z +19860 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +alternativa +dir + diff --git a/0.0.5.Optimized/src/.svn/format b/0.0.5.Optimized/src/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.5.Optimized/src/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.5.Optimized/src/alternativa/.svn/all-wcprops b/0.0.5.Optimized/src/alternativa/.svn/all-wcprops new file mode 100644 index 0000000..48c2f32 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 107 +/!svn/ver/19860/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa +END diff --git a/0.0.5.Optimized/src/alternativa/.svn/entries b/0.0.5.Optimized/src/alternativa/.svn/entries new file mode 100644 index 0000000..588050b --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/.svn/entries @@ -0,0 +1,31 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa +http://svndev.alternativaplatform.com + + + +2009-09-15T05:34:09.526646Z +19860 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +physics +dir + diff --git a/0.0.5.Optimized/src/alternativa/.svn/format b/0.0.5.Optimized/src/alternativa/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.5.Optimized/src/alternativa/physics/.svn/all-wcprops b/0.0.5.Optimized/src/alternativa/physics/.svn/all-wcprops new file mode 100644 index 0000000..8482491 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 115 +/!svn/ver/19860/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics +END +altphysics.as +K 25 +svn:wc:ra_dav:version-url +V 129 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/altphysics.as +END diff --git a/0.0.5.Optimized/src/alternativa/physics/.svn/entries b/0.0.5.Optimized/src/alternativa/physics/.svn/entries new file mode 100644 index 0000000..50dcc7b --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/.svn/entries @@ -0,0 +1,49 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics +http://svndev.alternativaplatform.com + + + +2009-09-15T05:34:09.526646Z +19860 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +types +dir + +altphysics.as +file + + + + +2010-10-28T04:32:38.000000Z +04fbe40a27502dbbd0dba02a76b2df50 +2009-04-20T20:09:14.715403Z +11579 +mike + +collision +dir + +rigid +dir + diff --git a/0.0.5.Optimized/src/alternativa/physics/.svn/format b/0.0.5.Optimized/src/alternativa/physics/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.5.Optimized/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base new file mode 100644 index 0000000..2715307 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base @@ -0,0 +1,3 @@ +package alternativa.physics { + public namespace altphysics = "http://alternativaplatform.com/en/altphysics"; +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/altphysics.as b/0.0.5.Optimized/src/alternativa/physics/altphysics.as new file mode 100644 index 0000000..2715307 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/altphysics.as @@ -0,0 +1,3 @@ +package alternativa.physics { + public namespace altphysics = "http://alternativaplatform.com/en/altphysics"; +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/.svn/all-wcprops b/0.0.5.Optimized/src/alternativa/physics/collision/.svn/all-wcprops new file mode 100644 index 0000000..58f6297 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/.svn/all-wcprops @@ -0,0 +1,113 @@ +K 25 +svn:wc:ra_dav:version-url +V 125 +/!svn/ver/19860/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/collision +END +BoxSphereCollider.as +K 25 +svn:wc:ra_dav:version-url +V 146 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/collision/BoxSphereCollider.as +END +BoxBoxCollider.as +K 25 +svn:wc:ra_dav:version-url +V 143 +/!svn/ver/19860/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/collision/BoxBoxCollider.as +END +CollisionKdTree2D.as +K 25 +svn:wc:ra_dav:version-url +V 146 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/collision/CollisionKdTree2D.as +END +CollisionKdNode.as +K 25 +svn:wc:ra_dav:version-url +V 144 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/collision/CollisionKdNode.as +END +KdTreeCollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 152 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/collision/KdTreeCollisionDetector.as +END +BoxRectCollider.as +K 25 +svn:wc:ra_dav:version-url +V 144 +/!svn/ver/19860/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/collision/BoxRectCollider.as +END +SpherePlaneCollider.as +K 25 +svn:wc:ra_dav:version-url +V 148 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/collision/SpherePlaneCollider.as +END +ICollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 148 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/collision/ICollisionPredicate.as +END +BoxPlaneCollider.as +K 25 +svn:wc:ra_dav:version-url +V 145 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/collision/BoxPlaneCollider.as +END +CollisionKdTree.as +K 25 +svn:wc:ra_dav:version-url +V 144 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/collision/CollisionKdTree.as +END +BoxTriangleCollider.as +K 25 +svn:wc:ra_dav:version-url +V 148 +/!svn/ver/19860/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/collision/BoxTriangleCollider.as +END +ICollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 147 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/collision/ICollisionDetector.as +END +IRayCollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 151 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/collision/IRayCollisionPredicate.as +END +IBodyCollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 152 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/collision/IBodyCollisionPredicate.as +END +BruteForceCollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 156 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/collision/BruteForceCollisionDetector.as +END +ICollider.as +K 25 +svn:wc:ra_dav:version-url +V 138 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/collision/ICollider.as +END +BoxCollider.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/19860/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/collision/BoxCollider.as +END +SphereSphereCollider.as +K 25 +svn:wc:ra_dav:version-url +V 149 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/collision/SphereSphereCollider.as +END diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/.svn/entries b/0.0.5.Optimized/src/alternativa/physics/collision/.svn/entries new file mode 100644 index 0000000..98f7a58 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/.svn/entries @@ -0,0 +1,250 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/collision +http://svndev.alternativaplatform.com + + + +2009-09-15T05:34:09.526646Z +19860 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +BoxSphereCollider.as +file + + + + +2010-10-28T04:32:38.000000Z +8bfabb1784a5eb6f6b5a76cbd30d9e88 +2009-09-09T15:57:45.246374Z +19648 +mike + +BoxBoxCollider.as +file + + + + +2010-10-28T04:32:38.000000Z +169ad19c54fc6b17524b553cfad80e44 +2009-09-15T05:34:09.526646Z +19860 +mike + +CollisionKdNode.as +file + + + + +2010-10-28T04:32:38.000000Z +1d52aba28ce259ca1f9916d70619263d +2009-07-20T03:48:26.517738Z +16522 +mike + +CollisionKdTree2D.as +file + + + + +2010-10-28T04:32:38.000000Z +d8211949ef77817543bc2882480da12f +2009-07-20T03:48:26.517738Z +16522 +mike + +SpherePlaneCollider.as +file + + + + +2010-10-28T04:32:38.000000Z +9416a2154f699ed3330ebf270ac760f9 +2009-09-09T15:57:45.246374Z +19648 +mike + +BoxRectCollider.as +file + + + + +2010-10-28T04:32:38.000000Z +709c935cfe3736242305f66c2c53684c +2009-09-15T05:34:09.526646Z +19860 +mike + +KdTreeCollisionDetector.as +file + + + + +2010-10-28T04:32:38.000000Z +ed2340f19df5496ea226eed8a196014e +2009-09-10T19:17:30.639579Z +19708 +mike + +ICollisionPredicate.as +file + + + + +2010-10-28T04:32:38.000000Z +866471ea582e95e46e94eb7ae437c827 +2009-04-26T15:39:06.673206Z +11995 +mike + +BoxPlaneCollider.as +file + + + + +2010-10-28T04:32:38.000000Z +827b78750c00050de610aa5b17ef33bd +2009-09-09T15:57:45.246374Z +19648 +mike + +primitives +dir + +CollisionKdTree.as +file + + + + +2010-10-28T04:32:38.000000Z +45bb93cc65be7d8a55015c4ac75b1242 +2009-07-20T03:48:26.517738Z +16522 +mike + +BoxTriangleCollider.as +file + + + + +2010-10-28T04:32:38.000000Z +43e341cda3486a0a036458cff5202279 +2009-09-15T05:34:09.526646Z +19860 +mike + +ICollisionDetector.as +file + + + + +2010-10-28T04:32:38.000000Z +c415c22df9a1b1064c0501d5d525a5cb +2009-07-13T10:34:12.822563Z +16296 +mike + +IRayCollisionPredicate.as +file + + + + +2010-10-28T04:32:38.000000Z +f7443fb728cf4e6157dcfb4f39665889 +2009-04-26T15:39:06.673206Z +11995 +mike + +types +dir + +IBodyCollisionPredicate.as +file + + + + +2010-10-28T04:32:38.000000Z +fce7e70f95b8ef96709157dcdc8433dd +2009-06-29T14:07:24.833358Z +15390 +mike + +ICollider.as +file + + + + +2010-10-28T04:32:38.000000Z +4eedf93d1111e060a54aadf29b86c63f +2009-09-09T15:57:45.246374Z +19648 +mike + +BruteForceCollisionDetector.as +file + + + + +2010-10-28T04:32:38.000000Z +a313af2ee5d9c84ec2551058b1b4c763 +2009-07-13T10:34:12.822563Z +16296 +mike + +BoxCollider.as +file + + + + +2010-10-28T04:32:38.000000Z +eedf135991f73c141baabd079d63332b +2009-09-15T05:34:09.526646Z +19860 +mike + +SphereSphereCollider.as +file + + + + +2010-10-28T04:32:38.000000Z +2e720e302145d54e8391df879f68d6d8 +2009-09-09T15:57:45.246374Z +19648 +mike + diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/.svn/format b/0.0.5.Optimized/src/alternativa/physics/collision/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/BoxBoxCollider.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/BoxBoxCollider.as.svn-base new file mode 100644 index 0000000..d3a7de4 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/BoxBoxCollider.as.svn-base @@ -0,0 +1,568 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Расчитывает точки контакта двух боксов. Нормаль контакта направляется в сторону бокса с меньшим ID. + */ + public class BoxBoxCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + + private var toBox1:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var colAxis:Vector3 = new Vector3(); + private var matrix:Matrix4 = new Matrix4(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + private var tmpPoints:Vector. = new Vector.(8, true); + private var pcount:int; + + /** + * + */ + public function BoxBoxCollider() { + for (var i:int = 0; i < 8; i++) { + tmpPoints[i] = new ContactPoint(); + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box1:CollisionBox = CollisionBox(prim1); + var box2:CollisionBox = CollisionBox(prim2); + + if (bestAxisIndex < 6) { + // Контакт грань-(грань|ребро|вершина) + findFaceContactPoints(box1, box2, toBox1, bestAxisIndex, contact); + } else { + // Контакт ребро-ребро + bestAxisIndex -= 6; + findEdgesIntersection(box1, box2, toBox1, int(bestAxisIndex/3), bestAxisIndex%3, contact); + } + contact.body1 = box1.body; + contact.body2 = box2.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + var box1:CollisionBox = CollisionBox(prim1); + var box2:CollisionBox = CollisionBox(prim2); + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + minOverlap = 1e10; + + // Вектор из центра второго бокса в центр первого + toBox1.x = transform1.d - transform2.d; + toBox1.y = transform1.h - transform2.h; + toBox1.z = transform1.l - transform2.l; + + // Проверка пересечения по основным осям + axis10.x = transform1.a; + axis10.y = transform1.e; + axis10.z = transform1.i; + if (!testMainAxis(box1, box2, axis10, 0, toBox1)) return false; + axis11.x = transform1.b; + axis11.y = transform1.f; + axis11.z = transform1.j; + if (!testMainAxis(box1, box2, axis11, 1, toBox1)) return false; + axis12.x = transform1.c; + axis12.y = transform1.g; + axis12.z = transform1.k; + if (!testMainAxis(box1, box2, axis12, 2, toBox1)) return false; + + axis20.x = transform2.a; + axis20.y = transform2.e; + axis20.z = transform2.i; + if (!testMainAxis(box1, box2, axis20, 3, toBox1)) return false; + axis21.x = transform2.b; + axis21.y = transform2.f; + axis21.z = transform2.j; + if (!testMainAxis(box1, box2, axis21, 4, toBox1)) return false; + axis22.x = transform2.c; + axis22.y = transform2.g; + axis22.z = transform2.k; + if (!testMainAxis(box1, box2, axis22, 5, toBox1)) return false; + + // Проверка производных осей + if (!testDerivedAxis(box1, box2, axis10, axis20, 6, toBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis21, 7, toBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis22, 8, toBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis11, axis20, 9, toBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis21, 10, toBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis22, 11, toBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis12, axis20, 12, toBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis21, 13, toBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis22, 14, toBox1)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта грани одного бокса с гранью/ребром/вершиной другого. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param toBox1 вектор, направленный из центра второго бокса в центр первого + * @param faceAxisIdx индекс оси первого бокса, перпендикулярной грани, с которой произошло столкновение + * @param contactInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box1:CollisionBox, box2:CollisionBox, toBox1:Vector3, faceAxisIdx:int, contactInfo:Contact):void { + var swapNormal:Boolean = false; + if (faceAxisIdx > 2) { + // Столкновение с гранью второго бокса. Для дальнейших расчётов боксы меняются местами, + // но нормаль контакта всё равно должна быть направлена в сторону первоначального box1 + var tmpBox:CollisionBox = box1; + box1 = box2; + box2 = tmpBox; + toBox1.x = -toBox1.x; + toBox1.y = -toBox1.y; + toBox1.z = -toBox1.z; + faceAxisIdx -= 3; + swapNormal = true; + } + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + transform1.getAxis(faceAxisIdx, colAxis); + var negativeFace:Boolean = colAxis.x*toBox1.x + colAxis.y*toBox1.y + colAxis.z*toBox1.z > 0; + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + // Ищем ось второго бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var maxDot:Number = 0; + var incidentAxisDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + transform2.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisDot = dot; + incidentAxisIdx = axisIdx; + } + } + // Получаем список вершин грани второго бокса, переводим их в систему координат первого бокса и выполняем обрезку + // по грани первого бокса. Таким образом получается список потенциальных точек контакта. + transform2.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box2.hs, incidentAxisIdx, incidentAxisDot < 0, points1); + + // TODO: Рассчитать результирующую матрицу и конвертировать точки один раз + transform2.transformVectorsN(points1, points2, 4); + transform1.transformVectorsInverseN(points2, points1, 4); + + var pnum:int = clip(box1.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points1[i]; + if ((pen = getPointBoxPenetration(box1.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + var cp:ContactPoint = tmpPoints[pcount++]; + var cpPos:Vector3 = cp.pos; + + cpPos.x = transform1.a*v.x + transform1.b*v.y + transform1.c*v.z + transform1.d; + cpPos.y = transform1.e*v.x + transform1.f*v.y + transform1.g*v.z + transform1.h; + cpPos.z = transform1.i*v.x + transform1.j*v.y + transform1.k*v.z + transform1.l; + + cp.r1.x = cpPos.x - transform1.d; + cp.r1.y = cpPos.y - transform1.h; + cp.r1.z = cpPos.z - transform1.l; + + cp.r2.x = cpPos.x - transform2.d; + cp.r2.y = cpPos.y - transform2.h; + cp.r2.z = cpPos.z - transform2.l; + + cp.penetration = pen; + } + } + if (swapNormal) { + contactInfo.normal.x = -colAxis.x; + contactInfo.normal.y = -colAxis.y; + contactInfo.normal.x = -colAxis.x; + } else { + contactInfo.normal.x = colAxis.x; + contactInfo.normal.y = colAxis.y; + contactInfo.normal.x = colAxis.x; + } + + if (pcount > 4) { + reducePoints(); + } + for (i = 0; i < pcount; i++) { + ContactPoint(contactInfo.points[i]).copyFrom(tmpPoints[i]); + } + contactInfo.pcount = pcount; + } + + /** + * + * @param contactInfo + */ + private function reducePoints():void { + var i:int; + var minIdx:int; + var cp1:ContactPoint; + var cp2:ContactPoint; + while (pcount > 4) { + var minLen:Number = 1e10; + var p1:Vector3 = ContactPoint(tmpPoints[int(pcount - 1)]).pos; + var p2:Vector3; + for (i = 0; i < pcount; i++) { + p2 = ContactPoint(tmpPoints[i]).pos; + var dx:Number = p2.x - p1.x; + var dy:Number = p2.y - p1.y; + var dz:Number = p2.z - p1.z; + var len:Number = dx*dx + dy*dy + dz*dz; + if (len < minLen) { + minLen = len; + minIdx = i; + } + p1 = p2; + } + cp1 = tmpPoints[minIdx == 0 ? int(pcount - 1) : int(minIdx - 1)]; + cp2 = tmpPoints[minIdx]; + p1 = cp1.pos; + p2 = cp2.pos; + p2.x = 0.5*(p1.x + p2.x); + p2.y = 0.5*(p1.y + p2.y); + p2.z = 0.5*(p1.z + p2.z); + cp2.penetration = 0.5*(cp1.penetration + cp2.penetration); + if (minIdx > 0) { + for (i = minIdx; i < pcount; i++) { + tmpPoints[int(i - 1)] = tmpPoints[i]; + } + tmpPoints[int(pcount - 1)] = cp1; + } + pcount--; + } + } + + /** + * + * @param hs + * @param p + * @param faceAxisIdx + * @param negativeFace + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, negativeFace:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (negativeFace) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (negativeFace) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (negativeFace) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clip(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param toBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param contactInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box1:CollisionBox, box2:CollisionBox, toBox1:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):void { + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + transform1.getAxis(axisIdx1, axis10); + transform2.getAxis(axisIdx2, axis20); + + var normalX:Number = axis10.y*axis20.z - axis10.z*axis20.y; + var normalY:Number = axis10.z*axis20.x - axis10.x*axis20.z; + var normalZ:Number = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(normalX*normalX + normalY*normalY + normalZ*normalZ); + normalX *= k; + normalY *= k; + normalZ *= k; + // Разворот нормали в сторону первого бокса + if (normalX*toBox1.x + normalY*toBox1.y + normalZ*toBox1.z < 0) { + normalX = -normalX; + normalY = -normalY; + normalZ = -normalZ; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + var x1:Number = box1.hs.x; + var y1:Number = box1.hs.y; + var z1:Number = box1.hs.z; + var x2:Number = box2.hs.x; + var y2:Number = box2.hs.y; + var z2:Number = box2.hs.z; + // x + if (axisIdx1 == 0) { + x1 = 0; + halfLen1 = box1.hs.x; + } else { + if (normalX*transform1.a + normalY*transform1.e + normalZ*transform1.i > 0) { + x1 = -x1; + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = box2.hs.x; + } else { + if (normalX*transform2.a + normalY*transform2.e + normalZ*transform2.i < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + y1 = 0; + halfLen1 = box1.hs.y; + } else { + if (normalX*transform1.b + normalY*transform1.f + normalZ*transform1.j > 0) { + y1 = -y1; + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = box2.hs.y; + } else { + if (normalX*transform2.b + normalY*transform2.f + normalZ*transform2.j < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + z1 = 0; + halfLen1 = box1.hs.z; + } else { + if (normalX*transform1.c + normalY*transform1.g + normalZ*transform1.k > 0) { + z1 = -z1; + } + } + if (axisIdx2 == 2) { + z2 = 0; + halfLen2 = box2.hs.z; + } else { + if (normalX*transform2.c + normalY*transform2.g + normalZ*transform2.k < 0) { + z2 = -z2; + } + } + // Получаем глобальные координаты средних точек рёбер + var xx:Number = x1; + var yy:Number = y1; + var zz:Number = z1; + x1 = transform1.a*xx + transform1.b*yy + transform1.c*zz + transform1.d; + y1 = transform1.e*xx + transform1.f*yy + transform1.g*zz + transform1.h; + z1 = transform1.i*xx + transform1.j*yy + transform1.k*zz + transform1.l; + xx = x2; + yy = y2; + zz = z2; + x2 = transform2.a*xx + transform2.b*yy + transform2.c*zz + transform2.d; + y2 = transform2.e*xx + transform2.f*yy + transform2.g*zz + transform2.h; + z2 = transform2.i*xx + transform2.j*yy + transform2.k*zz + transform2.l; + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + xx = x2 - x1; + yy = y2 - y1; + zz = z2 - z1; + var c1:Number = axis10.x*xx + axis10.y*yy + axis10.z*zz; + var c2:Number = axis20.x*xx + axis20.y*yy + axis20.z*zz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.normal.x = normalX; + contact.normal.y = normalY; + contact.normal.z = normalZ; + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cpPos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cpPos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cpPos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + + cp.r1.x = cpPos.x - transform1.d; + cp.r1.y = cpPos.y - transform1.h; + cp.r1.z = cpPos.z - transform1.l; + + cp.r2.x = cpPos.x - transform2.d; + cp.r2.y = cpPos.y - transform2.h; + cp.r2.z = cpPos.z - transform2.l; + + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение боксов вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box1 + * @param box2 + * @param axis + * @param axisIndex + * @param toBox1 + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box1 + * @param box2 + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox1 + * @return + */ + private function testDerivedAxis(box1:CollisionBox, box2:CollisionBox, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия двух боксов вдоль заданной оси. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param axis ось + * @param toBox1 вектор, соединяющий центр второго бокса с центром первого + * @return величина перекрытия боксов вдоль оси + */ + public function overlapOnAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, toBox1:Vector3):Number { + var m:Matrix4 = box1.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box1.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box1.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box1.hs.z; + if (d < 0) d = -d; + projection += d; + + m = box2.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box2.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box2.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box2.hs.z; + if (d < 0) d = -d; + projection += d; + + d = toBox1.x*axis.x + toBox1.y*axis.y + toBox1.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} + import alternativa.physics.types.Vector3; + + class CollisionPointTmp { + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + } diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/BoxCollider.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/BoxCollider.as.svn-base new file mode 100644 index 0000000..bf3a2c7 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/BoxCollider.as.svn-base @@ -0,0 +1,387 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + public class BoxCollider implements ICollider { + + public function BoxCollider() { + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + return false; + } + + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + protected function getFaceVertsByAxis(hs:Vector3, axisIdx:int, negativeFace:Boolean, result:Vector.):void { + var v:Vector3; + switch (axisIdx) { + case 0: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + } + break; + case 1: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + } + break; + case 2: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + } else { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } + break; + } + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + v = result[num++]; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + v = result[num++]; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + v = result[num++]; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + v = result[num++]; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + v = result[num++]; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + v = result[num++]; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/BoxPlaneCollider.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/BoxPlaneCollider.as.svn-base new file mode 100644 index 0000000..9817aac --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/BoxPlaneCollider.as.svn-base @@ -0,0 +1,87 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + public class BoxPlaneCollider implements ICollider { + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var normal:Vector3 = new Vector3(); + + public function BoxPlaneCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var box:RigidBox = body1 as RigidBox; +// var plane:RigidPlane; +// if (box == null) { +// box = body2 as RigidBox; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные координаты вершин бокса +// var sx:Number = box.halfSize.x; +// var sy:Number = box.halfSize.y; +// var sz:Number = box.halfSize.z; +// (verts1[0] as Vector3).reset(-sx, -sy, -sz); +// (verts1[1] as Vector3).reset(sx, -sy, -sz); +// (verts1[2] as Vector3).reset(sx, sy, -sz); +// (verts1[3] as Vector3).reset(-sx, sy, -sz); +// (verts1[4] as Vector3).reset(-sx, -sy, sz); +// (verts1[5] as Vector3).reset(sx, -sy, sz); +// (verts1[6] as Vector3).reset(sx, sy, sz); +// (verts1[7] as Vector3).reset(-sx, sy, sz); +// +// box.transform.transformVectors(verts1, verts2); +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// // Проверяем наличие столкновений с каждой вершиной +// collisionInfo.pcount = 0; +// for (var i:int = 0; i < 8; i++) { +// // Вершина добавляется в список точек столкновения, если лежит под плоскостью +// var dist:Number = (verts2[i] as Vector3).dot(normal); +// if (dist < offset) { +// var cp:ContactPoint; +// if (collisionInfo.pcount == collisionInfo.points.length) { +// cp = new ContactPoint(); +// collisionInfo.points[collisionInfo.pcount] = cp; +// } else { +// cp = collisionInfo.points[collisionInfo.pcount]; +// } +// cp.pos.copy(verts2[i]); +// cp.r1.diff(cp.pos, box.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); +// cp.penetration = offset - dist; +// collisionInfo.pcount++; +// } +// } +// if (collisionInfo.pcount > 0) { +// collisionInfo.body1 = box; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// return true; +// } + return false; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/BoxRectCollider.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/BoxRectCollider.as.svn-base new file mode 100644 index 0000000..de99234 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/BoxRectCollider.as.svn-base @@ -0,0 +1,522 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + /** + * + */ + public class BoxRectCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + + private var vectorToBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxRectCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, rect, vectorToBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + findEdgesIntersection(box, rect, vectorToBox, int(bestAxisIndex/2), bestAxisIndex%2, contact); + } + contact.body1 = box.body; + contact.body2 = rect.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + // Вектор из центра прямоугольника в центр бокса + vectorToBox.x = boxTransform.d - rectTransform.d; + vectorToBox.y = boxTransform.h - rectTransform.h; + vectorToBox.z = boxTransform.l - rectTransform.l; + + // Проверка пересечения по нормали прямоугольника + rectTransform.getAxis(2, axis22); + if (!testMainAxis(box, rect, axis22, 0, vectorToBox)) return false; + + // Проверка пересечения по основным осям бокса + boxTransform.getAxis(0, axis10); + if (!testMainAxis(box, rect, axis10, 1, vectorToBox)) return false; + boxTransform.getAxis(1, axis11); + if (!testMainAxis(box, rect, axis11, 2, vectorToBox)) return false; + boxTransform.getAxis(2, axis12); + if (!testMainAxis(box, rect, axis12, 3, vectorToBox)) return false; + + // Получаем направляющие рёбер прямоугольника + rectTransform.getAxis(0, axis20); + rectTransform.getAxis(1, axis21); + + // Проверка производных осей + if (!testDerivedAxis(box, rect, axis10, axis20, 4, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis10, axis21, 5, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis11, axis20, 6, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis11, axis21, 7, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis12, axis20, 8, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis12, axis21, 9, vectorToBox)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта бокса с прямоугольником. + * + * @param box бокс + * @param rect прямоугольник + * @param vectorToBox вектор, направленный из центра прямоугольника в центр бокса + * @param faceAxisIdx индекс оси, идентифицирующей полскость столкновения (грань бокса или полскость прямоугольника) + * @param colInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var pnum:int; + var i:int; + var v:Vector3; + var cp:ContactPoint; + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + var colAxis:Vector3 = contact.normal; + if (faceAxisIdx == 0) { + // Столкновение с плоскостью прямоугольника + + // Проверим положение бокса относительно плоскости прямоугольника + colAxis.x = rectTransform.c; + colAxis.y = rectTransform.g; + colAxis.z = rectTransform.k; + +// var offset:Number = colAxis.x*rectTransform.d + colAxis.y*rectTransform.h + colAxis.z*rectTransform.l; +// if (bbPos.vDot(colAxis) < offset) return false; + + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisIdx = axisIdx; + incidentAxisDot = dot; + } + } + // Получаем список вершин грани бокса, переводим их в систему координат прямоугольника и выполняем обрезку + // по прямоугольнику. Таким образом получается список потенциальных точек контакта. + boxTransform.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box.hs, incidentAxisIdx, incidentAxisDot > 0, points1); + boxTransform.transformVectorsN(points1, points2, 4); + rectTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByRect(rect.hs); + // Проверяем каждую потенциальную точку на принадлежность нижней полуплоскости прямоугольника и добавляем такие точки в список контактов + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if (v.z < epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = -v.z; + var cpPos:Vector3 = cp.pos; + cpPos.x = rectTransform.a*v.x + rectTransform.b*v.y + rectTransform.c*v.z + rectTransform.d; + cpPos.y = rectTransform.e*v.x + rectTransform.f*v.y + rectTransform.g*v.z + rectTransform.h; + cpPos.z = rectTransform.i*v.x + rectTransform.j*v.y + rectTransform.k*v.z + rectTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } else { + // Столкновение с гранью бокса + faceAxisIdx--; + boxTransform.getAxis(faceAxisIdx, colAxis); + var negativeFace:Boolean = colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z > 0; + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (rectTransform.c*colAxis.x + rectTransform.g*colAxis.y + rectTransform.k*colAxis.z < 0) return false; + + getFaceVertsByAxis(rect.hs, 2, false, points1); + rectTransform.transformVectorsN(points1, points2, 4); + boxTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if ((pen = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = pen; + cpPos = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } + return true; + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * + * @param hs + * @return + */ + private function clipByRect(hs:Vector3):int { + var pnum:int = 4; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box первый бокс + * @param rect второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param colInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):void { + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + boxTransform.getAxis(axisIdx1, axis10); + rectTransform.getAxis(axisIdx2, axis20); + var colAxis:Vector3 = contact.normal; + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону бокса + if (colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + var vx:Number = box.hs.x; + var vy:Number = box.hs.y; + var vz:Number = box.hs.z; + var x2:Number = rect.hs.x; + var y2:Number = rect.hs.y; + var z2:Number = rect.hs.z; + // x + if (axisIdx1 == 0) { + vx = 0; + halfLen1 = box.hs.x; + } else { + if (boxTransform.a*colAxis.x + boxTransform.e*colAxis.y + boxTransform.i*colAxis.z > 0) { + vx = -vx; + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = rect.hs.x; + } else { + if (rectTransform.a*colAxis.x + rectTransform.e*colAxis.y + rectTransform.i*colAxis.z < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + vy = 0; + halfLen1 = box.hs.y; + } else { + if (boxTransform.b*colAxis.x + boxTransform.f*colAxis.y + boxTransform.j*colAxis.z > 0) { + vy = -vy; + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = rect.hs.y; + } else { + if (rectTransform.b*colAxis.x + rectTransform.f*colAxis.y + rectTransform.j*colAxis.z < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + vz = 0; + halfLen1 = box.hs.z; + } else { + if (boxTransform.c*colAxis.x + boxTransform.g*colAxis.y + boxTransform.k*colAxis.z > 0) { + vz = -vz; + } + } + // Получаем глобальные координаты средних точек рёбер + + var x1:Number = boxTransform.a*vx + boxTransform.b*vy + boxTransform.c*vz + boxTransform.d; + var y1:Number = boxTransform.e*vx + boxTransform.f*vy + boxTransform.g*vz + boxTransform.h; + var z1:Number = boxTransform.i*vx + boxTransform.j*vy + boxTransform.k*vz + boxTransform.l; + vx = x2; + vy = y2; + vz = z2; + x2 = rectTransform.a*vx + rectTransform.b*vy + rectTransform.c*vz + rectTransform.d; + y2 = rectTransform.e*vx + rectTransform.f*vy + rectTransform.g*vz + rectTransform.h; + z2 = rectTransform.i*vx + rectTransform.j*vy + rectTransform.k*vz + rectTransform.l; + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + vx = x2 - x1; + vy = y2 - y1; + vz = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cpPos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cpPos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cpPos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var v:Vector3 = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + + /** + * Проверяет пересечение вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box + * @param rect + * @param axis + * @param axisIndex + * @param vectorToBox + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param rect + * @param axis1 + * @param axis2 + * @param axisIndex + * @param vectorToBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, rect:CollisionRect, axis1:Vector3, axis2:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия вдоль заданной оси. + * + * @param box бокс + * @param rect прямоугольник + * @param axis ось + * @param vectorToBox вектор, соединяющий центр прямоугольника с центром бокса + * @return величина перекрытия вдоль оси + */ + public function overlapOnAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, vectorToBox:Vector3):Number { + var m:Matrix4 = box.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box.hs.z; + if (d < 0) d = -d; + projection += d; + + m = rect.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*rect.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*rect.hs.y; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox.x*axis.x + vectorToBox.y*axis.y + vectorToBox.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/BoxSphereCollider.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/BoxSphereCollider.as.svn-base new file mode 100644 index 0000000..27ae562 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/BoxSphereCollider.as.svn-base @@ -0,0 +1,113 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + use namespace altphysics; + /** + * + */ + public class BoxSphereCollider implements ICollider { + + private var center:Vector3 = new Vector3(); + private var closestPt:Vector3 = new Vector3(); + + private var bPos:Vector3 = new Vector3(); + private var sPos:Vector3 = new Vector3(); + + /** + * + */ + public function BoxSphereCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + // Зафиксированно столкновение + contact.body1 = sphere.body; + contact.body2 = box.body; + contact.normal.vCopy(closestPt).vTransformBy4(box.transform).vSubtract(sPos).vNormalize().vReverse(); + contact.pcount = 1; + + var cp:ContactPoint = contact.points[0]; + cp.penetration = sphere.r - Math.sqrt(distSqr); + cp.pos.vCopy(contact.normal).vScale(-sphere.r).vAdd(sPos); + cp.r1.vDiff(cp.pos, sPos); + cp.r2.vDiff(cp.pos, bPos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/BoxTriangleCollider.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/BoxTriangleCollider.as.svn-base new file mode 100644 index 0000000..23f1874 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/BoxTriangleCollider.as.svn-base @@ -0,0 +1,682 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionTriangle; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + + /** + * + */ + public class BoxTriangleCollider extends BoxCollider { + + public var epsilon:Number = 0.001; + + private var bestAxisIndex:int; + private var minOverlap:Number; + private var toBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var colNormal:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxTriangleCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, tri, toBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + findEdgesIntersection(box, tri, toBox, bestAxisIndex%3, int(bestAxisIndex/3), contact); + } + + contact.body1 = box.body; + contact.body2 = tri.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + toBox.x = boxTransform.d - triTransform.d; + toBox.y = boxTransform.h - triTransform.h; + toBox.z = boxTransform.l - triTransform.l; + + minOverlap = 1e308; + + // Сначала проверяется нормаль треугольника + axis.x = triTransform.c; + axis.y = triTransform.g; + axis.z = triTransform.k; + if (!testMainAxis(box, tri, axis, 0, toBox)) return false; + + // Проверка основных осей бокса + axis10.x = boxTransform.a; + axis10.y = boxTransform.e; + axis10.z = boxTransform.i; + if (!testMainAxis(box, tri, axis10, 1, toBox)) return false; + axis11.x = boxTransform.b; + axis11.y = boxTransform.f; + axis11.z = boxTransform.j; + if (!testMainAxis(box, tri, axis11, 2, toBox)) return false; + axis12.x = boxTransform.c; + axis12.y = boxTransform.g; + axis12.z = boxTransform.k; + if (!testMainAxis(box, tri, axis12, 3, toBox)) return false; + + // Проверка производных осей + // TODO: заменить вычисления векторных произведений инлайнами + var v:Vector3 = tri.e0; + axis20.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis20.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis20.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis20, 4, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis20, 5, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis20, 6, toBox)) return false; + + v = tri.e1; + axis21.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis21.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis21.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis21, 7, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis21, 8, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis21, 9, toBox)) return false; + + v = tri.e2; + axis22.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis22.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis22.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis22, 10, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis22, 11, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis22, 12, toBox)) return false; + + return true; + } + + /** + * Тестирует пересечение примитивов вдоль заданной оси. + * + * @param box бокс + * @param tri треугольник + * @param axis ось + * @param axisIndex индекс оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return true, если примитивы имеют перекрытие проекций вдоль указанной оси, иначе false + */ + private function testMainAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, axisIndex:int, toBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param tri + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, tri:CollisionTriangle, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Рассчитывает величину перекрытия проекций бокса и треугольника на заданную ось. + * + * @param box бокс + * @param tri треугольник + * @param axis единичный направляющий вектор оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return величина перекрытия. Положительное значение указывает на наличие перекрытия, нулевое или отрицательное значение указывает на отсутствие перекрытия. + */ + private function overlapOnAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, toBox:Vector3):Number { + var t:Matrix4 = box.transform; + var projection:Number = (t.a*axis.x + t.e*axis.y + t.i*axis.z)*box.hs.x; + if (projection < 0) projection = -projection; + var d:Number = (t.b*axis.x + t.f*axis.y + t.j*axis.z)*box.hs.y; + projection += d < 0 ? -d : d; + d = (t.c*axis.x + t.g*axis.y + t.k*axis.z)*box.hs.z; + projection += d < 0 ? -d : d; + + var vectorProjection:Number = toBox.x*axis.x + toBox.y*axis.y + toBox.z*axis.z; + // Для оптимизации ось преобразуется в систему треугольника, а не его вершины в мировую систему + t = tri.transform; + var ax:Number = t.a*axis.x + t.e*axis.y + t.i*axis.z; + var ay:Number = t.b*axis.x + t.f*axis.y + t.j*axis.z; + var az:Number = t.c*axis.x + t.g*axis.y + t.k*axis.z; + var max:Number = 0; + if (vectorProjection < 0) { + vectorProjection = -vectorProjection; + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d < max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d < max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d < max) max = d; + max = -max; + } else { + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d > max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d > max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d > max) max = d; + } + + return projection + max - vectorProjection; + } + + /** + * Определяет точки контакта бокса и треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function findFaceContactPoints(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIndex:int, contact:Contact):Boolean { + if (faceAxisIndex == 0) { + // Столкновение с плоскостью треугольника + return getBoxToTriContact(box, tri, toBox, contact); + } else { + // Столкновение с гранью бокса + return getTriToBoxContact(box, tri, toBox, faceAxisIndex, contact); + } + } + + /** + * Определяет точки контакта бокса с плоскостью треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getBoxToTriContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + colNormal.x = triTransform.c; + colNormal.y = triTransform.g; + colNormal.z = triTransform.k; + + var over:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + if (!over) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var incAxisDot:Number = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colNormal.x + axis.y*colNormal.y + axis.z*colNormal.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incAxisDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Обрезка грани + var negativeFace:Boolean = incAxisDot > 0; + getFaceVertsByAxis(box.hs, incFaceAxisIdx, negativeFace, points1); + boxTransform.transformVectorsN(points1, points2, 4); + triTransform.transformVectorsInverseN(points2, points1, 4); + var pnum:int = clipByTriangle(tri); + // Среди конечного списка точек определяются лежащие под плоскостью треугольника + var cp:ContactPoint; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points2[i]; + if ((over && v.z < 0) || (!over && v.z > 0)) { + cp = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z + triTransform.d; + cpPos.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z + triTransform.h; + cpPos.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z + triTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = over ? -v.z : v.z; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Оперделяет точки контакта треугольника с гранью бокса. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getTriToBoxContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + faceAxisIdx--; + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + boxTransform.getAxis(faceAxisIdx, colNormal); + var negativeFace:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + if (!negativeFace) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + var v:Vector3 = points1[0]; + v.x = tri.v0.x; + v.y = tri.v0.y; + v.z = tri.v0.z; + + v = points1[1]; + v.x = tri.v1.x; + v.y = tri.v1.y; + v.z = tri.v1.z; + + v = points1[2]; + v.x = tri.v2.x; + v.y = tri.v2.y; + v.z = tri.v2.z; + + triTransform.transformVectorsN(points1, points2, 3); + boxTransform.transformVectorsInverseN(points2, points1, 3); + + var pnum:int = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность боксу и добавляем такие точки в список контактов + var penetration:Number; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + v = points1[i]; + penetration = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace); + if (penetration > -epsilon) { + var cp:ContactPoint = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = penetration; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Вычисляет величину проникновения точки в бокс. + * + * @param hs вектор половинных размеров бокса + * @param p точка в системе координат бокса + * @param axisIndex индекс оси + * @param negativeFace если true, проверяется нижняя грань + * @return величина проникновения точки в бокс + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, negativeFace:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (negativeFace) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (negativeFace) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (negativeFace) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, вершины которой заданы в списке points1. Результат сохраняется в этом же списке. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 3; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Обрезает грань прямоугольника сторонами треугольника. Обрезка выполняется в системе координат и в проекции на + * плоскость треугольника. Входные вершины грани в количестве четырёх штук должны находиться в списке points1. + * Конечные вершины сохраняются в списке points2. + * + * @param tri треугольник + * @return количество врешин в конечном списке + */ + private function clipByTriangle(tri:CollisionTriangle):int { + var vnum:int = 4; + vnum = clipByLine(tri.v0, tri.e0, points1, vnum, points2); + if (vnum == 0) return 0; + vnum = clipByLine(tri.v1, tri.e1, points2, vnum, points1); + if (vnum == 0) return 0; + return clipByLine(tri.v2, tri.e2, points1, vnum, points2);; + } + + /** + * Обрезает полигон указанной прямой. + * + * @param linePoint точка на прямой + * @param lineDir единичный направляющий вектор прямой + * @param verticesIn список вершин исходного полигона + * @param vnum количество вершин исходного полигона + * @param verticesOut список, куда будут записаны вершины конечного полигона + * @return количество вершин конечного полигона + */ + private function clipByLine(linePoint:Vector3, lineDir:Vector3, verticesIn:Vector., vnum:int, verticesOut:Vector.):int { + var nx:Number = -lineDir.y; + var ny:Number = lineDir.x; + var offset:Number = linePoint.x*nx + linePoint.y*ny; + var v1:Vector3 = verticesIn[int(vnum - 1)]; + var offset1:Number = v1.x*nx + v1.y*ny; + var t:Number; + var v:Vector3; + var num:int = 0; + for (var i:int = 0; i < vnum; i++) { + var v2:Vector3 = verticesIn[i]; + var offset2:Number = v2.x*nx + v2.y*ny; + if (offset1 < offset) { + // Первая точка ребра во внешней полуплоскости + if (offset2 > offset) { + // Вторая точка ребра во внутренней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } else { + // Первая точка ребра во внутренней полуплоскости. Добавляем её в конечный список. + v = verticesOut[num]; + v.x = v1.x; + v.y = v1.y; + v.z = v1.z; + num++; + if (offset2 < offset) { + // Вторая точка ребра во внешней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } + v1 = v2; + offset1 = offset2; + } + return num; + } + + /** + * + * @param box + * @param tri + * @param toBox + * @param boxAxisIdx + * @param triAxisIdx + * @param contact + */ + private function findEdgesIntersection(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, boxAxisIdx:int, triAxisIdx:int, contact:Contact):void { + // Определение точки и направляющего вектора ребра треугольника + var tmpx1:Number; + var tmpy1:Number; + var tmpz1:Number; + var tmpx2:Number; + var tmpy2:Number; + var tmpz2:Number; + switch (triAxisIdx) { + case 0: + tmpx1 = tri.e0.x; + tmpy1 = tri.e0.y; + tmpz1 = tri.e0.z; + tmpx2 = tri.v0.x; + tmpy2 = tri.v0.y; + tmpz2 = tri.v0.z; + break; + case 1: + tmpx1 = tri.e1.x; + tmpy1 = tri.e1.y; + tmpz1 = tri.e1.z; + tmpx2 = tri.v1.x; + tmpy2 = tri.v1.y; + tmpz2 = tri.v1.z; + break; + case 2: + tmpx1 = tri.e2.x; + tmpy1 = tri.e2.y; + tmpz1 = tri.e2.z; + tmpx2 = tri.v2.x; + tmpy2 = tri.v2.y; + tmpz2 = tri.v2.z; + break; + } + var triTransform:Matrix4 = tri.transform; + axis20.x = triTransform.a*tmpx1 + triTransform.b*tmpy1 + triTransform.c*tmpz1; + axis20.y = triTransform.e*tmpx1 + triTransform.f*tmpy1 + triTransform.g*tmpz1; + axis20.z = triTransform.i*tmpx1 + triTransform.j*tmpy1 + triTransform.k*tmpz1; + var x2:Number = triTransform.a*tmpx2 + triTransform.b*tmpy2 + triTransform.c*tmpz2 + triTransform.d; + var y2:Number = triTransform.e*tmpx2 + triTransform.f*tmpy2 + triTransform.g*tmpz2 + triTransform.h; + var z2:Number = triTransform.i*tmpx2 + triTransform.j*tmpy2 + triTransform.k*tmpz2 + triTransform.l; + + // Определение нормали контакта, точки и направляющего вектора ребра бокса + var boxTransform:Matrix4 = box.transform; + boxTransform.getAxis(boxAxisIdx, axis10); + + // Нормаль контакта + var v:Vector3 = contact.normal; + v.x = axis10.y*axis20.z - axis10.z*axis20.y; + v.y = axis10.z*axis20.x - axis10.x*axis20.z; + v.z = axis10.x*axis20.y - axis10.y*axis20.x; + k = 1/Math.sqrt(v.x*v.x + v.y*v.y + v.z*v.z); + v.x *= k; + v.y *= k; + v.z *= k; + // Разворот нормали в сторону бокса + if (v.x*toBox.x + v.y*toBox.y + v.z*toBox.z < 0) { + v.x = -v.x; + v.y = -v.y; + v.z = -v.z; + } + + var boxHalfLen:Number; + tmpx1 = box.hs.x; + tmpy1 = box.hs.y; + tmpz1 = box.hs.z; + // X + if (boxAxisIdx == 0) { + tmpx1 = 0; + boxHalfLen = box.hs.x; + } else { + if (boxTransform.a*v.x + boxTransform.e*v.y + boxTransform.i*v.z > 0) { + tmpx1 = -tmpx1; + } + } + // Y + if (boxAxisIdx == 1) { + tmpy1 = 0; + boxHalfLen = box.hs.y; + } else { + if (boxTransform.b*v.x + boxTransform.f*v.y + boxTransform.j*v.z > 0) { + tmpy1 = -tmpy1; + } + } + // Z + if (boxAxisIdx == 2) { + tmpz1 = 0; + boxHalfLen = box.hs.z; + } else { + if (boxTransform.c*v.x + boxTransform.g*v.y + boxTransform.k*v.z > 0) { + tmpz1 = -tmpz1; + } + } + var x1:Number = boxTransform.a*tmpx1 + boxTransform.b*tmpy1 + boxTransform.c*tmpz1 + boxTransform.d; + var y1:Number = boxTransform.e*tmpx1 + boxTransform.f*tmpy1 + boxTransform.g*tmpz1 + boxTransform.h; + var z1:Number = boxTransform.i*tmpx1 + boxTransform.j*tmpy1 + boxTransform.k*tmpz1 + boxTransform.l; + + // Находим точку пересечения рёбер, решая систему уравнений + // axis10 - направляющий вектор ребра бокса + // x1, y1, z1 - средняя точка ребра бокса + // axis20 - направляющий вектор ребра треугольника + // x2, y2, z2 - начальная точка ребра треугольника + var k:Number = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + var vx:Number = x2 - x1; + var vy:Number = y2 - y1; + var vz:Number = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + + // Запись данных о контакте + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + v = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + v.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + v.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + v.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = v.x - boxTransform.d; + r.y = v.y - boxTransform.h; + r.z = v.z - boxTransform.l; + r = cp.r2; + r.x = v.x - triTransform.d; + r.y = v.y - triTransform.h; + r.z = v.z - triTransform.l; + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/BruteForceCollisionDetector.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/BruteForceCollisionDetector.as.svn-base new file mode 100644 index 0000000..85d84fa --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/BruteForceCollisionDetector.as.svn-base @@ -0,0 +1,63 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + public class BruteForceCollisionDetector implements ICollisionDetector { + public function BruteForceCollisionDetector() { + } + + public function addPrimitive(primitive:CollisionPrimitive, isStatic:Boolean=true):Boolean { + return false; + } + + public function removePrimitive(primitive:CollisionPrimitive, isStatic:Boolean=true):Boolean { + return false; + } + + public function init():void { + } + + public function getAllCollisions(contacts:Vector.):int { + return 0; + } + + public function intersectRay(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, intersection:RayIntersection):Boolean { + return false; + } + + public function intersectRayWithStatic(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean { + return false; + } + + public function testBodyPrimitiveCollision(body:Body, primitive:CollisionPrimitive):Boolean { + return false; + } + + /** + * + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + return false; + } + + /** + * + * @param prim1 + * @param prim2 + * @return + */ + public function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base new file mode 100644 index 0000000..e24922c --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base @@ -0,0 +1,18 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.types.BoundBox; + + public class CollisionKdNode { + public var indices:Vector.; + public var splitIndices:Vector.; + public var boundBox:BoundBox; + public var parent:CollisionKdNode; + public var splitTree:CollisionKdTree2D; + + public var axis:int = -1; // 0 - x, 1 - y, 2 - z + public var coord:Number; + public var positiveNode:CollisionKdNode; + public var negativeNode:CollisionKdNode; + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base new file mode 100644 index 0000000..c046f17 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base @@ -0,0 +1,280 @@ +package alternativa.physics.collision { + + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + + /** + * @author mike + */ + public class CollisionKdTree { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + public var rootNode:CollisionKdNode; + public var staticChildren:Vector. = new Vector.(); + public var numStaticChildren:int; + public var staticBoundBoxes:Vector. = new Vector.(); + + private var splitAxis:int; + private var splitCoord:Number; + private var splitCost:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + + /** + * @param primitive + */ + public function addStaticPrimitive(primitive:CollisionPrimitive):void { + staticChildren[numStaticChildren++] = primitive; + } + + /** + * @param primitive + * @return + */ + public function removeStaticPrimitive(primitive:CollisionPrimitive):Boolean { + var idx:int = staticChildren.indexOf(primitive); + if (idx < 0) return false; + staticChildren.splice(idx, 1); + numStaticChildren--; + return true; + } + + /** + * @param boundBox + */ + public function createTree(boundBox:BoundBox = null):void { + if (numStaticChildren == 0) return; + // Создаём корневую ноду + rootNode = new CollisionKdNode(); + rootNode.indices = new Vector.(); + // Расчитываем баунды объектов и рутовой ноды + var rootNodeBoundBox:BoundBox = rootNode.boundBox = (boundBox != null) ? boundBox : new BoundBox(); + for (var i:int = 0; i < numStaticChildren; ++i) { + var child:CollisionPrimitive = staticChildren[i]; + var childBoundBox:BoundBox = staticBoundBoxes[i] = child.calculateAABB(); + rootNodeBoundBox.addBoundBox(childBoundBox); + rootNode.indices[i] = i; + } + staticBoundBoxes.length = numStaticChildren; + // Разделяем рутовую ноду + splitNode(rootNode); + + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + var indices:Vector. = node.indices; + var numPrimitives:int = indices.length; + if (numPrimitives <= minPrimitivesPerNode) return; + + // Подготовка баунда с погрешностями + var nodeBoundBox:BoundBox = node.boundBox; + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + // Собираем опорные координаты + var i:int; + var j:int; + var numSplitCoordsX:int = 0 + var numSplitCoordsY:int = 0; + var numSplitCoordsZ:int = 0; + for (i = 0; i < numPrimitives; ++i) { + var boundBox:BoundBox = staticBoundBoxes[indices[i]]; + + if (boundBox.maxX - boundBox.minX <= doubleThreshold) { + if (boundBox.minX <= nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.minX; + else if (boundBox.maxX >= nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.maxX; + else splitCoordsX[numSplitCoordsX++] = (boundBox.minX + boundBox.maxX)*0.5; + } else { + if (boundBox.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = boundBox.minX; + if (boundBox.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = boundBox.maxX; + } + + if (boundBox.maxY - boundBox.minY <= doubleThreshold) { + if (boundBox.minY <= nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.minY; + else if (boundBox.maxY >= nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.maxY; + else splitCoordsY[numSplitCoordsY++] = (boundBox.minY + boundBox.maxY)*0.5; + } else { + if (boundBox.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = boundBox.minY; + if (boundBox.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = boundBox.maxY; + } + + if (boundBox.maxZ - boundBox.minZ <= doubleThreshold) { + if (boundBox.minZ <= nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.minZ; + else if (boundBox.maxZ >= nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.maxZ; + else splitCoordsZ[numSplitCoordsZ++] = (boundBox.minZ + boundBox.maxZ)*0.5; + } else { + if (boundBox.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.minZ; + if (boundBox.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numPrimitives; ++i) { + boundBox = staticBoundBoxes[indices[i]]; + var min:Number = axisX ? boundBox.minX : (axisY ? boundBox.minY : boundBox.minZ); + var max:Number = axisX ? boundBox.maxX : (axisY ? boundBox.maxY : boundBox.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(indices[i]); + indices[i] = -1; + } else { + if (node.splitIndices == null) node.splitIndices = new Vector.(); + node.splitIndices.push(indices[i]); + indices[i] = -1; + } + } else { + if (min >= coordMin) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(indices[i]); + indices[i] = -1; + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numPrimitives; ++i) { + if (indices[i] >= 0) indices[j++] = indices[i]; + } + if (j > 0) indices.length = j; + else node.indices = null; + + if (node.splitIndices != null) { + node.splitTree = new CollisionKdTree2D(this, node); + node.splitTree.createTree(); + } + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + /** + * + */ + public function traceTree():void { + traceNode("", rootNode); + } + + /** + * @param str + * @param node + */ + private function traceNode(str:String, node:CollisionKdNode):void { + if (node == null) return; + trace(str, node.axis == -1 ? "end" : ((node.axis == 0) ? "X" : ((node.axis == 1) ? "Y" : "Z")), "splitCoord=" + splitCoord, "bound", node.boundBox, "objs:", node.indices); + traceNode(str + "-", node.negativeNode); + traceNode(str + "+", node.positiveNode); + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base new file mode 100644 index 0000000..78aaa95 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base @@ -0,0 +1,226 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.types.BoundBox; + + /** + * + */ + public class CollisionKdTree2D { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + + public var parentTree:CollisionKdTree; + public var parentNode:CollisionKdNode; + public var rootNode:CollisionKdNode; + + private var splitAxis:int; + private var splitCost:Number; + private var splitCoord:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + /** + * + * @param parentTree + * @param parentNode + */ + public function CollisionKdTree2D(parentTree:CollisionKdTree, parentNode:CollisionKdNode) { + this.parentTree = parentTree; + this.parentNode = parentNode; + } + + /** + * + */ + public function createTree():void { + rootNode = new CollisionKdNode(); + rootNode.boundBox = parentNode.boundBox.clone(); + rootNode.indices = new Vector.(); + var numObjects:int = parentNode.splitIndices.length; + for (var i:int = 0; i < numObjects; ++i) rootNode.indices[i] = parentNode.splitIndices[i]; + + splitNode(rootNode); + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + if (node.indices.length <= minPrimitivesPerNode) return; + + var objects:Vector. = node.indices; + var i:int; + var j:int; + + var nodeBoundBox:BoundBox = node.boundBox; + + // Подготовка баунда с погрешностями + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + // Собираем опорные координаты + var numSplitCoordsX:int; + var numSplitCoordsY:int; + var numSplitCoordsZ:int; + var numObjects:int = objects.length; + for (i = 0; i < numObjects; ++i) { + var bb:BoundBox = staticBoundBoxes[objects[i]]; + + if (parentNode.axis != 0) { + if (bb.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = bb.minX; + if (bb.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = bb.maxX; + } + + if (parentNode.axis != 1) { + if (bb.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = bb.minY; + if (bb.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = bb.maxY; + } + + if (parentNode.axis != 2) { + if (bb.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = bb.minZ; + if (bb.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = bb.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + if (parentNode.axis != 0) checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + if (parentNode.axis != 1) checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + if (parentNode.axis != 2) checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numObjects; ++i) { + bb = staticBoundBoxes[objects[i]]; + var min:Number = axisX ? bb.minX : (axisY ? bb.minY : bb.minZ); + var max:Number = axisX ? bb.maxX : (axisY ? bb.maxY : bb.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(objects[i]); + objects[i] = -1; + } + } else { + if (min >= coordMin) { + if (max > coordMax) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(objects[i]); + objects[i] = -1; + } + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numObjects; ++i) { + if (objects[i] >= 0) objects[j++] = objects[i]; + } + if (j > 0) objects.length = j; + else node.indices = null; + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base new file mode 100644 index 0000000..9cfdca5 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Body; + + public interface IBodyCollisionPredicate { + function considerBodies(body1:Body, body2:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base new file mode 100644 index 0000000..220e717 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base @@ -0,0 +1,30 @@ +package alternativa.physics.collision { + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + + /** + * Интерфейс определителя столкновений между двумя примитивами. + */ + public interface ICollider { + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base new file mode 100644 index 0000000..af43dfb --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base @@ -0,0 +1,92 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + /** + * Интерфейс детектора столкновений. + */ + public interface ICollisionDetector { + + /** + * Добавляет физический примитив в коллайдер. + * + * @param primitive добавляемый примитив + * @param isStatic указывает тип примитива: статический или динамический + * @return true если примитив был успешно добавлен, иначе false + */ + function addPrimitive(primitive:CollisionPrimitive, isStatic:Boolean = true):Boolean; + + /** + * Удаляет физический примитив из коллайдера. + * + * @param primitive удаляемый примитив + * @param isStatic указывает тип примитива: статический или динамический + * @return true если примитив был успшено удалён, иначе false + */ + function removePrimitive(primitive:CollisionPrimitive, isStatic:Boolean = true):Boolean; + + /** + * Выполняет инициализацию детектора после обновления списка примитивов. + */ + function init():void; + + /** + * Получает все столкновения в текущей конфигурации физической геометрии. + * + * @param contacts список контактов, в кторые будет записана информация о столкновении + * @return количество найденных столкновений + */ + function getAllCollisions(contacts:Vector.):int; + + /** + * Тестирует луч на пересечение с физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param dir направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRay(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * Тестирует луч на пересечение со статической физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param dir направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRayWithStatic(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @return + */ + function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base new file mode 100644 index 0000000..bb1b756 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base @@ -0,0 +1,9 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + + public interface ICollisionPredicate { + + function considerCollision(primitive:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base new file mode 100644 index 0000000..0f3d929 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Body; + + public interface IRayCollisionPredicate { + function considerBody(body:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base new file mode 100644 index 0000000..7b0b78e --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base @@ -0,0 +1,484 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Детектор, хранящий статическую геометрию в kD-дереве и использующий дерево для ускорения тестов на пересечения. + */ + public class KdTreeCollisionDetector implements ICollisionDetector { + + altphysics var tree:CollisionKdTree; + altphysics var dynamicPrimitives:Vector.; + 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.(); + + 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():void { + tree.createTree(); +// tree.traceTree(); + } + + /** + * + * @param contacts + * @return + */ + public function getAllCollisions(contacts:Vector.):int { + var colNum:int = 0; + for (var i:int = 0; i < dynamicPrimitivesNum; i++) { + var primitive:CollisionPrimitive = dynamicPrimitives[i]; + primitive.calculateAABB(); + if (primitive.body != null && primitive.body.frozen) continue; + colNum += getPrimitiveNodeCollisions(tree.rootNode, primitive, contacts, colNum); + + // Столкновения динамических примитивов между собой + // TODO: Попробовать различные оптимизации (сортировка по баундам, встраивание в дерево) + for (var j:int = i + 1; j < dynamicPrimitivesNum; j++) { + if (collide(primitive, dynamicPrimitives[j], contacts[colNum])) colNum++; + } + } + + return colNum; + } + + /** + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + public function collide(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 = (contact.points[0] as ContactPoint).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:Vector., startIndex:int):int { + var colNum:int = 0; + if (node.indices != null) { + // Поиск столкновений со статическими примитивами узла + var primitives:Vector. = tree.staticChildren; + var indices:Vector. = node.indices; + for (var i:int = indices.length - 1; i >= 0; i--) + if (collide(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; + } + + 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., indices:Vector., 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; + } \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/SpherePlaneCollider.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/SpherePlaneCollider.as.svn-base new file mode 100644 index 0000000..77eb1b0 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/SpherePlaneCollider.as.svn-base @@ -0,0 +1,67 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + /** + * + */ + public class SpherePlaneCollider implements ICollider { + + private var normal:Vector3 = new Vector3(); + + /** + * + */ + public function SpherePlaneCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var sphere:RigidSphere = body1 as RigidSphere; +// var plane:RigidPlane; +// if (sphere == null) { +// sphere = body2 as RigidSphere; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// +// var dist:Number = sphere.state.pos.dot(normal) - offset; +// if (dist > sphere.r) return false; +// +// collisionInfo.body1 = sphere; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// collisionInfo.pcount = 1; +// +// var cp:ContactPoint = collisionInfo.points[0]; +// cp.penetration = sphere.r - dist; +// cp.pos.copy(normal).reverse().scale(sphere.r).add(sphere.state.pos); +// cp.r1.diff(cp.pos, sphere.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/SphereSphereCollider.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/SphereSphereCollider.as.svn-base new file mode 100644 index 0000000..25971d5 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/.svn/text-base/SphereSphereCollider.as.svn-base @@ -0,0 +1,69 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + public class SphereSphereCollider implements ICollider { + + private var p1:Vector3 = new Vector3(); + private var p2:Vector3 = new Vector3(); + + public function SphereSphereCollider() { + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var s1:CollisionSphere; + var s2:CollisionSphere; + if (prim1.body != null) { + s1 = prim1 as CollisionSphere; + s2 = prim2 as CollisionSphere; + } else { + s1 = prim2 as CollisionSphere; + s2 = prim1 as CollisionSphere; + } + + s1.transform.getAxis(3, p1); + s2.transform.getAxis(3, p2); + var dx:Number = p1.x - p2.x; + var dy:Number = p1.y - p2.y; + var dz:Number = p1.z - p2.z; + var len:Number = dx*dx + dy*dy + dz*dz; + var sum:Number = s1.r + s2.r; + if (len > sum*sum) return false; + len = Math.sqrt(len); + dx /= len; + dy /= len; + dz /= len; + + contact.body1 = s1.body; + contact.body2 = s2.body; + contact.normal.x = dx; + contact.normal.y = dy; + contact.normal.z = dz; + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = sum - len; + cp.pos.x = p1.x - dx*s1.r; + cp.pos.y = p1.y - dy*s1.r; + cp.pos.z = p1.z - dz*s1.r; + cp.r1.vDiff(cp.pos, p1); + cp.r2.vDiff(cp.pos, p2); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/BoxBoxCollider.as b/0.0.5.Optimized/src/alternativa/physics/collision/BoxBoxCollider.as new file mode 100644 index 0000000..d3a7de4 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/BoxBoxCollider.as @@ -0,0 +1,568 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Расчитывает точки контакта двух боксов. Нормаль контакта направляется в сторону бокса с меньшим ID. + */ + public class BoxBoxCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + + private var toBox1:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var colAxis:Vector3 = new Vector3(); + private var matrix:Matrix4 = new Matrix4(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + private var tmpPoints:Vector. = new Vector.(8, true); + private var pcount:int; + + /** + * + */ + public function BoxBoxCollider() { + for (var i:int = 0; i < 8; i++) { + tmpPoints[i] = new ContactPoint(); + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box1:CollisionBox = CollisionBox(prim1); + var box2:CollisionBox = CollisionBox(prim2); + + if (bestAxisIndex < 6) { + // Контакт грань-(грань|ребро|вершина) + findFaceContactPoints(box1, box2, toBox1, bestAxisIndex, contact); + } else { + // Контакт ребро-ребро + bestAxisIndex -= 6; + findEdgesIntersection(box1, box2, toBox1, int(bestAxisIndex/3), bestAxisIndex%3, contact); + } + contact.body1 = box1.body; + contact.body2 = box2.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + var box1:CollisionBox = CollisionBox(prim1); + var box2:CollisionBox = CollisionBox(prim2); + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + minOverlap = 1e10; + + // Вектор из центра второго бокса в центр первого + toBox1.x = transform1.d - transform2.d; + toBox1.y = transform1.h - transform2.h; + toBox1.z = transform1.l - transform2.l; + + // Проверка пересечения по основным осям + axis10.x = transform1.a; + axis10.y = transform1.e; + axis10.z = transform1.i; + if (!testMainAxis(box1, box2, axis10, 0, toBox1)) return false; + axis11.x = transform1.b; + axis11.y = transform1.f; + axis11.z = transform1.j; + if (!testMainAxis(box1, box2, axis11, 1, toBox1)) return false; + axis12.x = transform1.c; + axis12.y = transform1.g; + axis12.z = transform1.k; + if (!testMainAxis(box1, box2, axis12, 2, toBox1)) return false; + + axis20.x = transform2.a; + axis20.y = transform2.e; + axis20.z = transform2.i; + if (!testMainAxis(box1, box2, axis20, 3, toBox1)) return false; + axis21.x = transform2.b; + axis21.y = transform2.f; + axis21.z = transform2.j; + if (!testMainAxis(box1, box2, axis21, 4, toBox1)) return false; + axis22.x = transform2.c; + axis22.y = transform2.g; + axis22.z = transform2.k; + if (!testMainAxis(box1, box2, axis22, 5, toBox1)) return false; + + // Проверка производных осей + if (!testDerivedAxis(box1, box2, axis10, axis20, 6, toBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis21, 7, toBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis22, 8, toBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis11, axis20, 9, toBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis21, 10, toBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis22, 11, toBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis12, axis20, 12, toBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis21, 13, toBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis22, 14, toBox1)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта грани одного бокса с гранью/ребром/вершиной другого. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param toBox1 вектор, направленный из центра второго бокса в центр первого + * @param faceAxisIdx индекс оси первого бокса, перпендикулярной грани, с которой произошло столкновение + * @param contactInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box1:CollisionBox, box2:CollisionBox, toBox1:Vector3, faceAxisIdx:int, contactInfo:Contact):void { + var swapNormal:Boolean = false; + if (faceAxisIdx > 2) { + // Столкновение с гранью второго бокса. Для дальнейших расчётов боксы меняются местами, + // но нормаль контакта всё равно должна быть направлена в сторону первоначального box1 + var tmpBox:CollisionBox = box1; + box1 = box2; + box2 = tmpBox; + toBox1.x = -toBox1.x; + toBox1.y = -toBox1.y; + toBox1.z = -toBox1.z; + faceAxisIdx -= 3; + swapNormal = true; + } + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + transform1.getAxis(faceAxisIdx, colAxis); + var negativeFace:Boolean = colAxis.x*toBox1.x + colAxis.y*toBox1.y + colAxis.z*toBox1.z > 0; + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + // Ищем ось второго бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var maxDot:Number = 0; + var incidentAxisDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + transform2.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisDot = dot; + incidentAxisIdx = axisIdx; + } + } + // Получаем список вершин грани второго бокса, переводим их в систему координат первого бокса и выполняем обрезку + // по грани первого бокса. Таким образом получается список потенциальных точек контакта. + transform2.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box2.hs, incidentAxisIdx, incidentAxisDot < 0, points1); + + // TODO: Рассчитать результирующую матрицу и конвертировать точки один раз + transform2.transformVectorsN(points1, points2, 4); + transform1.transformVectorsInverseN(points2, points1, 4); + + var pnum:int = clip(box1.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points1[i]; + if ((pen = getPointBoxPenetration(box1.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + var cp:ContactPoint = tmpPoints[pcount++]; + var cpPos:Vector3 = cp.pos; + + cpPos.x = transform1.a*v.x + transform1.b*v.y + transform1.c*v.z + transform1.d; + cpPos.y = transform1.e*v.x + transform1.f*v.y + transform1.g*v.z + transform1.h; + cpPos.z = transform1.i*v.x + transform1.j*v.y + transform1.k*v.z + transform1.l; + + cp.r1.x = cpPos.x - transform1.d; + cp.r1.y = cpPos.y - transform1.h; + cp.r1.z = cpPos.z - transform1.l; + + cp.r2.x = cpPos.x - transform2.d; + cp.r2.y = cpPos.y - transform2.h; + cp.r2.z = cpPos.z - transform2.l; + + cp.penetration = pen; + } + } + if (swapNormal) { + contactInfo.normal.x = -colAxis.x; + contactInfo.normal.y = -colAxis.y; + contactInfo.normal.x = -colAxis.x; + } else { + contactInfo.normal.x = colAxis.x; + contactInfo.normal.y = colAxis.y; + contactInfo.normal.x = colAxis.x; + } + + if (pcount > 4) { + reducePoints(); + } + for (i = 0; i < pcount; i++) { + ContactPoint(contactInfo.points[i]).copyFrom(tmpPoints[i]); + } + contactInfo.pcount = pcount; + } + + /** + * + * @param contactInfo + */ + private function reducePoints():void { + var i:int; + var minIdx:int; + var cp1:ContactPoint; + var cp2:ContactPoint; + while (pcount > 4) { + var minLen:Number = 1e10; + var p1:Vector3 = ContactPoint(tmpPoints[int(pcount - 1)]).pos; + var p2:Vector3; + for (i = 0; i < pcount; i++) { + p2 = ContactPoint(tmpPoints[i]).pos; + var dx:Number = p2.x - p1.x; + var dy:Number = p2.y - p1.y; + var dz:Number = p2.z - p1.z; + var len:Number = dx*dx + dy*dy + dz*dz; + if (len < minLen) { + minLen = len; + minIdx = i; + } + p1 = p2; + } + cp1 = tmpPoints[minIdx == 0 ? int(pcount - 1) : int(minIdx - 1)]; + cp2 = tmpPoints[minIdx]; + p1 = cp1.pos; + p2 = cp2.pos; + p2.x = 0.5*(p1.x + p2.x); + p2.y = 0.5*(p1.y + p2.y); + p2.z = 0.5*(p1.z + p2.z); + cp2.penetration = 0.5*(cp1.penetration + cp2.penetration); + if (minIdx > 0) { + for (i = minIdx; i < pcount; i++) { + tmpPoints[int(i - 1)] = tmpPoints[i]; + } + tmpPoints[int(pcount - 1)] = cp1; + } + pcount--; + } + } + + /** + * + * @param hs + * @param p + * @param faceAxisIdx + * @param negativeFace + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, negativeFace:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (negativeFace) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (negativeFace) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (negativeFace) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clip(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param toBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param contactInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box1:CollisionBox, box2:CollisionBox, toBox1:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):void { + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + transform1.getAxis(axisIdx1, axis10); + transform2.getAxis(axisIdx2, axis20); + + var normalX:Number = axis10.y*axis20.z - axis10.z*axis20.y; + var normalY:Number = axis10.z*axis20.x - axis10.x*axis20.z; + var normalZ:Number = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(normalX*normalX + normalY*normalY + normalZ*normalZ); + normalX *= k; + normalY *= k; + normalZ *= k; + // Разворот нормали в сторону первого бокса + if (normalX*toBox1.x + normalY*toBox1.y + normalZ*toBox1.z < 0) { + normalX = -normalX; + normalY = -normalY; + normalZ = -normalZ; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + var x1:Number = box1.hs.x; + var y1:Number = box1.hs.y; + var z1:Number = box1.hs.z; + var x2:Number = box2.hs.x; + var y2:Number = box2.hs.y; + var z2:Number = box2.hs.z; + // x + if (axisIdx1 == 0) { + x1 = 0; + halfLen1 = box1.hs.x; + } else { + if (normalX*transform1.a + normalY*transform1.e + normalZ*transform1.i > 0) { + x1 = -x1; + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = box2.hs.x; + } else { + if (normalX*transform2.a + normalY*transform2.e + normalZ*transform2.i < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + y1 = 0; + halfLen1 = box1.hs.y; + } else { + if (normalX*transform1.b + normalY*transform1.f + normalZ*transform1.j > 0) { + y1 = -y1; + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = box2.hs.y; + } else { + if (normalX*transform2.b + normalY*transform2.f + normalZ*transform2.j < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + z1 = 0; + halfLen1 = box1.hs.z; + } else { + if (normalX*transform1.c + normalY*transform1.g + normalZ*transform1.k > 0) { + z1 = -z1; + } + } + if (axisIdx2 == 2) { + z2 = 0; + halfLen2 = box2.hs.z; + } else { + if (normalX*transform2.c + normalY*transform2.g + normalZ*transform2.k < 0) { + z2 = -z2; + } + } + // Получаем глобальные координаты средних точек рёбер + var xx:Number = x1; + var yy:Number = y1; + var zz:Number = z1; + x1 = transform1.a*xx + transform1.b*yy + transform1.c*zz + transform1.d; + y1 = transform1.e*xx + transform1.f*yy + transform1.g*zz + transform1.h; + z1 = transform1.i*xx + transform1.j*yy + transform1.k*zz + transform1.l; + xx = x2; + yy = y2; + zz = z2; + x2 = transform2.a*xx + transform2.b*yy + transform2.c*zz + transform2.d; + y2 = transform2.e*xx + transform2.f*yy + transform2.g*zz + transform2.h; + z2 = transform2.i*xx + transform2.j*yy + transform2.k*zz + transform2.l; + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + xx = x2 - x1; + yy = y2 - y1; + zz = z2 - z1; + var c1:Number = axis10.x*xx + axis10.y*yy + axis10.z*zz; + var c2:Number = axis20.x*xx + axis20.y*yy + axis20.z*zz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.normal.x = normalX; + contact.normal.y = normalY; + contact.normal.z = normalZ; + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cpPos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cpPos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cpPos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + + cp.r1.x = cpPos.x - transform1.d; + cp.r1.y = cpPos.y - transform1.h; + cp.r1.z = cpPos.z - transform1.l; + + cp.r2.x = cpPos.x - transform2.d; + cp.r2.y = cpPos.y - transform2.h; + cp.r2.z = cpPos.z - transform2.l; + + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение боксов вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box1 + * @param box2 + * @param axis + * @param axisIndex + * @param toBox1 + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box1 + * @param box2 + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox1 + * @return + */ + private function testDerivedAxis(box1:CollisionBox, box2:CollisionBox, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия двух боксов вдоль заданной оси. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param axis ось + * @param toBox1 вектор, соединяющий центр второго бокса с центром первого + * @return величина перекрытия боксов вдоль оси + */ + public function overlapOnAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, toBox1:Vector3):Number { + var m:Matrix4 = box1.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box1.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box1.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box1.hs.z; + if (d < 0) d = -d; + projection += d; + + m = box2.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box2.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box2.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box2.hs.z; + if (d < 0) d = -d; + projection += d; + + d = toBox1.x*axis.x + toBox1.y*axis.y + toBox1.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} + import alternativa.physics.types.Vector3; + + class CollisionPointTmp { + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + } diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/BoxCollider.as b/0.0.5.Optimized/src/alternativa/physics/collision/BoxCollider.as new file mode 100644 index 0000000..bf3a2c7 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/BoxCollider.as @@ -0,0 +1,387 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + public class BoxCollider implements ICollider { + + public function BoxCollider() { + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + return false; + } + + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + protected function getFaceVertsByAxis(hs:Vector3, axisIdx:int, negativeFace:Boolean, result:Vector.):void { + var v:Vector3; + switch (axisIdx) { + case 0: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + } + break; + case 1: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + } + break; + case 2: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + } else { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } + break; + } + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + v = result[num++]; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + v = result[num++]; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + v = result[num++]; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + v = result[num++]; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + v = result[num++]; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + v = result[num++]; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/BoxPlaneCollider.as b/0.0.5.Optimized/src/alternativa/physics/collision/BoxPlaneCollider.as new file mode 100644 index 0000000..9817aac --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/BoxPlaneCollider.as @@ -0,0 +1,87 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + public class BoxPlaneCollider implements ICollider { + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var normal:Vector3 = new Vector3(); + + public function BoxPlaneCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var box:RigidBox = body1 as RigidBox; +// var plane:RigidPlane; +// if (box == null) { +// box = body2 as RigidBox; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные координаты вершин бокса +// var sx:Number = box.halfSize.x; +// var sy:Number = box.halfSize.y; +// var sz:Number = box.halfSize.z; +// (verts1[0] as Vector3).reset(-sx, -sy, -sz); +// (verts1[1] as Vector3).reset(sx, -sy, -sz); +// (verts1[2] as Vector3).reset(sx, sy, -sz); +// (verts1[3] as Vector3).reset(-sx, sy, -sz); +// (verts1[4] as Vector3).reset(-sx, -sy, sz); +// (verts1[5] as Vector3).reset(sx, -sy, sz); +// (verts1[6] as Vector3).reset(sx, sy, sz); +// (verts1[7] as Vector3).reset(-sx, sy, sz); +// +// box.transform.transformVectors(verts1, verts2); +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// // Проверяем наличие столкновений с каждой вершиной +// collisionInfo.pcount = 0; +// for (var i:int = 0; i < 8; i++) { +// // Вершина добавляется в список точек столкновения, если лежит под плоскостью +// var dist:Number = (verts2[i] as Vector3).dot(normal); +// if (dist < offset) { +// var cp:ContactPoint; +// if (collisionInfo.pcount == collisionInfo.points.length) { +// cp = new ContactPoint(); +// collisionInfo.points[collisionInfo.pcount] = cp; +// } else { +// cp = collisionInfo.points[collisionInfo.pcount]; +// } +// cp.pos.copy(verts2[i]); +// cp.r1.diff(cp.pos, box.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); +// cp.penetration = offset - dist; +// collisionInfo.pcount++; +// } +// } +// if (collisionInfo.pcount > 0) { +// collisionInfo.body1 = box; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// return true; +// } + return false; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/BoxRectCollider.as b/0.0.5.Optimized/src/alternativa/physics/collision/BoxRectCollider.as new file mode 100644 index 0000000..de99234 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/BoxRectCollider.as @@ -0,0 +1,522 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + /** + * + */ + public class BoxRectCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + + private var vectorToBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxRectCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, rect, vectorToBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + findEdgesIntersection(box, rect, vectorToBox, int(bestAxisIndex/2), bestAxisIndex%2, contact); + } + contact.body1 = box.body; + contact.body2 = rect.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + // Вектор из центра прямоугольника в центр бокса + vectorToBox.x = boxTransform.d - rectTransform.d; + vectorToBox.y = boxTransform.h - rectTransform.h; + vectorToBox.z = boxTransform.l - rectTransform.l; + + // Проверка пересечения по нормали прямоугольника + rectTransform.getAxis(2, axis22); + if (!testMainAxis(box, rect, axis22, 0, vectorToBox)) return false; + + // Проверка пересечения по основным осям бокса + boxTransform.getAxis(0, axis10); + if (!testMainAxis(box, rect, axis10, 1, vectorToBox)) return false; + boxTransform.getAxis(1, axis11); + if (!testMainAxis(box, rect, axis11, 2, vectorToBox)) return false; + boxTransform.getAxis(2, axis12); + if (!testMainAxis(box, rect, axis12, 3, vectorToBox)) return false; + + // Получаем направляющие рёбер прямоугольника + rectTransform.getAxis(0, axis20); + rectTransform.getAxis(1, axis21); + + // Проверка производных осей + if (!testDerivedAxis(box, rect, axis10, axis20, 4, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis10, axis21, 5, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis11, axis20, 6, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis11, axis21, 7, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis12, axis20, 8, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis12, axis21, 9, vectorToBox)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта бокса с прямоугольником. + * + * @param box бокс + * @param rect прямоугольник + * @param vectorToBox вектор, направленный из центра прямоугольника в центр бокса + * @param faceAxisIdx индекс оси, идентифицирующей полскость столкновения (грань бокса или полскость прямоугольника) + * @param colInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var pnum:int; + var i:int; + var v:Vector3; + var cp:ContactPoint; + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + var colAxis:Vector3 = contact.normal; + if (faceAxisIdx == 0) { + // Столкновение с плоскостью прямоугольника + + // Проверим положение бокса относительно плоскости прямоугольника + colAxis.x = rectTransform.c; + colAxis.y = rectTransform.g; + colAxis.z = rectTransform.k; + +// var offset:Number = colAxis.x*rectTransform.d + colAxis.y*rectTransform.h + colAxis.z*rectTransform.l; +// if (bbPos.vDot(colAxis) < offset) return false; + + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisIdx = axisIdx; + incidentAxisDot = dot; + } + } + // Получаем список вершин грани бокса, переводим их в систему координат прямоугольника и выполняем обрезку + // по прямоугольнику. Таким образом получается список потенциальных точек контакта. + boxTransform.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box.hs, incidentAxisIdx, incidentAxisDot > 0, points1); + boxTransform.transformVectorsN(points1, points2, 4); + rectTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByRect(rect.hs); + // Проверяем каждую потенциальную точку на принадлежность нижней полуплоскости прямоугольника и добавляем такие точки в список контактов + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if (v.z < epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = -v.z; + var cpPos:Vector3 = cp.pos; + cpPos.x = rectTransform.a*v.x + rectTransform.b*v.y + rectTransform.c*v.z + rectTransform.d; + cpPos.y = rectTransform.e*v.x + rectTransform.f*v.y + rectTransform.g*v.z + rectTransform.h; + cpPos.z = rectTransform.i*v.x + rectTransform.j*v.y + rectTransform.k*v.z + rectTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } else { + // Столкновение с гранью бокса + faceAxisIdx--; + boxTransform.getAxis(faceAxisIdx, colAxis); + var negativeFace:Boolean = colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z > 0; + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (rectTransform.c*colAxis.x + rectTransform.g*colAxis.y + rectTransform.k*colAxis.z < 0) return false; + + getFaceVertsByAxis(rect.hs, 2, false, points1); + rectTransform.transformVectorsN(points1, points2, 4); + boxTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if ((pen = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = pen; + cpPos = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } + return true; + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * + * @param hs + * @return + */ + private function clipByRect(hs:Vector3):int { + var pnum:int = 4; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box первый бокс + * @param rect второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param colInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):void { + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + boxTransform.getAxis(axisIdx1, axis10); + rectTransform.getAxis(axisIdx2, axis20); + var colAxis:Vector3 = contact.normal; + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону бокса + if (colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + var vx:Number = box.hs.x; + var vy:Number = box.hs.y; + var vz:Number = box.hs.z; + var x2:Number = rect.hs.x; + var y2:Number = rect.hs.y; + var z2:Number = rect.hs.z; + // x + if (axisIdx1 == 0) { + vx = 0; + halfLen1 = box.hs.x; + } else { + if (boxTransform.a*colAxis.x + boxTransform.e*colAxis.y + boxTransform.i*colAxis.z > 0) { + vx = -vx; + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = rect.hs.x; + } else { + if (rectTransform.a*colAxis.x + rectTransform.e*colAxis.y + rectTransform.i*colAxis.z < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + vy = 0; + halfLen1 = box.hs.y; + } else { + if (boxTransform.b*colAxis.x + boxTransform.f*colAxis.y + boxTransform.j*colAxis.z > 0) { + vy = -vy; + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = rect.hs.y; + } else { + if (rectTransform.b*colAxis.x + rectTransform.f*colAxis.y + rectTransform.j*colAxis.z < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + vz = 0; + halfLen1 = box.hs.z; + } else { + if (boxTransform.c*colAxis.x + boxTransform.g*colAxis.y + boxTransform.k*colAxis.z > 0) { + vz = -vz; + } + } + // Получаем глобальные координаты средних точек рёбер + + var x1:Number = boxTransform.a*vx + boxTransform.b*vy + boxTransform.c*vz + boxTransform.d; + var y1:Number = boxTransform.e*vx + boxTransform.f*vy + boxTransform.g*vz + boxTransform.h; + var z1:Number = boxTransform.i*vx + boxTransform.j*vy + boxTransform.k*vz + boxTransform.l; + vx = x2; + vy = y2; + vz = z2; + x2 = rectTransform.a*vx + rectTransform.b*vy + rectTransform.c*vz + rectTransform.d; + y2 = rectTransform.e*vx + rectTransform.f*vy + rectTransform.g*vz + rectTransform.h; + z2 = rectTransform.i*vx + rectTransform.j*vy + rectTransform.k*vz + rectTransform.l; + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + vx = x2 - x1; + vy = y2 - y1; + vz = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cpPos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cpPos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cpPos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var v:Vector3 = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + + /** + * Проверяет пересечение вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box + * @param rect + * @param axis + * @param axisIndex + * @param vectorToBox + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param rect + * @param axis1 + * @param axis2 + * @param axisIndex + * @param vectorToBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, rect:CollisionRect, axis1:Vector3, axis2:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия вдоль заданной оси. + * + * @param box бокс + * @param rect прямоугольник + * @param axis ось + * @param vectorToBox вектор, соединяющий центр прямоугольника с центром бокса + * @return величина перекрытия вдоль оси + */ + public function overlapOnAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, vectorToBox:Vector3):Number { + var m:Matrix4 = box.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box.hs.z; + if (d < 0) d = -d; + projection += d; + + m = rect.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*rect.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*rect.hs.y; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox.x*axis.x + vectorToBox.y*axis.y + vectorToBox.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/BoxSphereCollider.as b/0.0.5.Optimized/src/alternativa/physics/collision/BoxSphereCollider.as new file mode 100644 index 0000000..27ae562 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/BoxSphereCollider.as @@ -0,0 +1,113 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + use namespace altphysics; + /** + * + */ + public class BoxSphereCollider implements ICollider { + + private var center:Vector3 = new Vector3(); + private var closestPt:Vector3 = new Vector3(); + + private var bPos:Vector3 = new Vector3(); + private var sPos:Vector3 = new Vector3(); + + /** + * + */ + public function BoxSphereCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + // Зафиксированно столкновение + contact.body1 = sphere.body; + contact.body2 = box.body; + contact.normal.vCopy(closestPt).vTransformBy4(box.transform).vSubtract(sPos).vNormalize().vReverse(); + contact.pcount = 1; + + var cp:ContactPoint = contact.points[0]; + cp.penetration = sphere.r - Math.sqrt(distSqr); + cp.pos.vCopy(contact.normal).vScale(-sphere.r).vAdd(sPos); + cp.r1.vDiff(cp.pos, sPos); + cp.r2.vDiff(cp.pos, bPos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/BoxTriangleCollider.as b/0.0.5.Optimized/src/alternativa/physics/collision/BoxTriangleCollider.as new file mode 100644 index 0000000..23f1874 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/BoxTriangleCollider.as @@ -0,0 +1,682 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionTriangle; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + + /** + * + */ + public class BoxTriangleCollider extends BoxCollider { + + public var epsilon:Number = 0.001; + + private var bestAxisIndex:int; + private var minOverlap:Number; + private var toBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var colNormal:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxTriangleCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, tri, toBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + findEdgesIntersection(box, tri, toBox, bestAxisIndex%3, int(bestAxisIndex/3), contact); + } + + contact.body1 = box.body; + contact.body2 = tri.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + toBox.x = boxTransform.d - triTransform.d; + toBox.y = boxTransform.h - triTransform.h; + toBox.z = boxTransform.l - triTransform.l; + + minOverlap = 1e308; + + // Сначала проверяется нормаль треугольника + axis.x = triTransform.c; + axis.y = triTransform.g; + axis.z = triTransform.k; + if (!testMainAxis(box, tri, axis, 0, toBox)) return false; + + // Проверка основных осей бокса + axis10.x = boxTransform.a; + axis10.y = boxTransform.e; + axis10.z = boxTransform.i; + if (!testMainAxis(box, tri, axis10, 1, toBox)) return false; + axis11.x = boxTransform.b; + axis11.y = boxTransform.f; + axis11.z = boxTransform.j; + if (!testMainAxis(box, tri, axis11, 2, toBox)) return false; + axis12.x = boxTransform.c; + axis12.y = boxTransform.g; + axis12.z = boxTransform.k; + if (!testMainAxis(box, tri, axis12, 3, toBox)) return false; + + // Проверка производных осей + // TODO: заменить вычисления векторных произведений инлайнами + var v:Vector3 = tri.e0; + axis20.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis20.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis20.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis20, 4, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis20, 5, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis20, 6, toBox)) return false; + + v = tri.e1; + axis21.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis21.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis21.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis21, 7, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis21, 8, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis21, 9, toBox)) return false; + + v = tri.e2; + axis22.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis22.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis22.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis22, 10, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis22, 11, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis22, 12, toBox)) return false; + + return true; + } + + /** + * Тестирует пересечение примитивов вдоль заданной оси. + * + * @param box бокс + * @param tri треугольник + * @param axis ось + * @param axisIndex индекс оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return true, если примитивы имеют перекрытие проекций вдоль указанной оси, иначе false + */ + private function testMainAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, axisIndex:int, toBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param tri + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, tri:CollisionTriangle, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Рассчитывает величину перекрытия проекций бокса и треугольника на заданную ось. + * + * @param box бокс + * @param tri треугольник + * @param axis единичный направляющий вектор оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return величина перекрытия. Положительное значение указывает на наличие перекрытия, нулевое или отрицательное значение указывает на отсутствие перекрытия. + */ + private function overlapOnAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, toBox:Vector3):Number { + var t:Matrix4 = box.transform; + var projection:Number = (t.a*axis.x + t.e*axis.y + t.i*axis.z)*box.hs.x; + if (projection < 0) projection = -projection; + var d:Number = (t.b*axis.x + t.f*axis.y + t.j*axis.z)*box.hs.y; + projection += d < 0 ? -d : d; + d = (t.c*axis.x + t.g*axis.y + t.k*axis.z)*box.hs.z; + projection += d < 0 ? -d : d; + + var vectorProjection:Number = toBox.x*axis.x + toBox.y*axis.y + toBox.z*axis.z; + // Для оптимизации ось преобразуется в систему треугольника, а не его вершины в мировую систему + t = tri.transform; + var ax:Number = t.a*axis.x + t.e*axis.y + t.i*axis.z; + var ay:Number = t.b*axis.x + t.f*axis.y + t.j*axis.z; + var az:Number = t.c*axis.x + t.g*axis.y + t.k*axis.z; + var max:Number = 0; + if (vectorProjection < 0) { + vectorProjection = -vectorProjection; + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d < max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d < max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d < max) max = d; + max = -max; + } else { + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d > max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d > max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d > max) max = d; + } + + return projection + max - vectorProjection; + } + + /** + * Определяет точки контакта бокса и треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function findFaceContactPoints(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIndex:int, contact:Contact):Boolean { + if (faceAxisIndex == 0) { + // Столкновение с плоскостью треугольника + return getBoxToTriContact(box, tri, toBox, contact); + } else { + // Столкновение с гранью бокса + return getTriToBoxContact(box, tri, toBox, faceAxisIndex, contact); + } + } + + /** + * Определяет точки контакта бокса с плоскостью треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getBoxToTriContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + colNormal.x = triTransform.c; + colNormal.y = triTransform.g; + colNormal.z = triTransform.k; + + var over:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + if (!over) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var incAxisDot:Number = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colNormal.x + axis.y*colNormal.y + axis.z*colNormal.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incAxisDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Обрезка грани + var negativeFace:Boolean = incAxisDot > 0; + getFaceVertsByAxis(box.hs, incFaceAxisIdx, negativeFace, points1); + boxTransform.transformVectorsN(points1, points2, 4); + triTransform.transformVectorsInverseN(points2, points1, 4); + var pnum:int = clipByTriangle(tri); + // Среди конечного списка точек определяются лежащие под плоскостью треугольника + var cp:ContactPoint; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points2[i]; + if ((over && v.z < 0) || (!over && v.z > 0)) { + cp = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z + triTransform.d; + cpPos.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z + triTransform.h; + cpPos.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z + triTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = over ? -v.z : v.z; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Оперделяет точки контакта треугольника с гранью бокса. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getTriToBoxContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + faceAxisIdx--; + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + boxTransform.getAxis(faceAxisIdx, colNormal); + var negativeFace:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + if (!negativeFace) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + var v:Vector3 = points1[0]; + v.x = tri.v0.x; + v.y = tri.v0.y; + v.z = tri.v0.z; + + v = points1[1]; + v.x = tri.v1.x; + v.y = tri.v1.y; + v.z = tri.v1.z; + + v = points1[2]; + v.x = tri.v2.x; + v.y = tri.v2.y; + v.z = tri.v2.z; + + triTransform.transformVectorsN(points1, points2, 3); + boxTransform.transformVectorsInverseN(points2, points1, 3); + + var pnum:int = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность боксу и добавляем такие точки в список контактов + var penetration:Number; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + v = points1[i]; + penetration = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace); + if (penetration > -epsilon) { + var cp:ContactPoint = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = penetration; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Вычисляет величину проникновения точки в бокс. + * + * @param hs вектор половинных размеров бокса + * @param p точка в системе координат бокса + * @param axisIndex индекс оси + * @param negativeFace если true, проверяется нижняя грань + * @return величина проникновения точки в бокс + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, negativeFace:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (negativeFace) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (negativeFace) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (negativeFace) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, вершины которой заданы в списке points1. Результат сохраняется в этом же списке. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 3; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Обрезает грань прямоугольника сторонами треугольника. Обрезка выполняется в системе координат и в проекции на + * плоскость треугольника. Входные вершины грани в количестве четырёх штук должны находиться в списке points1. + * Конечные вершины сохраняются в списке points2. + * + * @param tri треугольник + * @return количество врешин в конечном списке + */ + private function clipByTriangle(tri:CollisionTriangle):int { + var vnum:int = 4; + vnum = clipByLine(tri.v0, tri.e0, points1, vnum, points2); + if (vnum == 0) return 0; + vnum = clipByLine(tri.v1, tri.e1, points2, vnum, points1); + if (vnum == 0) return 0; + return clipByLine(tri.v2, tri.e2, points1, vnum, points2);; + } + + /** + * Обрезает полигон указанной прямой. + * + * @param linePoint точка на прямой + * @param lineDir единичный направляющий вектор прямой + * @param verticesIn список вершин исходного полигона + * @param vnum количество вершин исходного полигона + * @param verticesOut список, куда будут записаны вершины конечного полигона + * @return количество вершин конечного полигона + */ + private function clipByLine(linePoint:Vector3, lineDir:Vector3, verticesIn:Vector., vnum:int, verticesOut:Vector.):int { + var nx:Number = -lineDir.y; + var ny:Number = lineDir.x; + var offset:Number = linePoint.x*nx + linePoint.y*ny; + var v1:Vector3 = verticesIn[int(vnum - 1)]; + var offset1:Number = v1.x*nx + v1.y*ny; + var t:Number; + var v:Vector3; + var num:int = 0; + for (var i:int = 0; i < vnum; i++) { + var v2:Vector3 = verticesIn[i]; + var offset2:Number = v2.x*nx + v2.y*ny; + if (offset1 < offset) { + // Первая точка ребра во внешней полуплоскости + if (offset2 > offset) { + // Вторая точка ребра во внутренней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } else { + // Первая точка ребра во внутренней полуплоскости. Добавляем её в конечный список. + v = verticesOut[num]; + v.x = v1.x; + v.y = v1.y; + v.z = v1.z; + num++; + if (offset2 < offset) { + // Вторая точка ребра во внешней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } + v1 = v2; + offset1 = offset2; + } + return num; + } + + /** + * + * @param box + * @param tri + * @param toBox + * @param boxAxisIdx + * @param triAxisIdx + * @param contact + */ + private function findEdgesIntersection(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, boxAxisIdx:int, triAxisIdx:int, contact:Contact):void { + // Определение точки и направляющего вектора ребра треугольника + var tmpx1:Number; + var tmpy1:Number; + var tmpz1:Number; + var tmpx2:Number; + var tmpy2:Number; + var tmpz2:Number; + switch (triAxisIdx) { + case 0: + tmpx1 = tri.e0.x; + tmpy1 = tri.e0.y; + tmpz1 = tri.e0.z; + tmpx2 = tri.v0.x; + tmpy2 = tri.v0.y; + tmpz2 = tri.v0.z; + break; + case 1: + tmpx1 = tri.e1.x; + tmpy1 = tri.e1.y; + tmpz1 = tri.e1.z; + tmpx2 = tri.v1.x; + tmpy2 = tri.v1.y; + tmpz2 = tri.v1.z; + break; + case 2: + tmpx1 = tri.e2.x; + tmpy1 = tri.e2.y; + tmpz1 = tri.e2.z; + tmpx2 = tri.v2.x; + tmpy2 = tri.v2.y; + tmpz2 = tri.v2.z; + break; + } + var triTransform:Matrix4 = tri.transform; + axis20.x = triTransform.a*tmpx1 + triTransform.b*tmpy1 + triTransform.c*tmpz1; + axis20.y = triTransform.e*tmpx1 + triTransform.f*tmpy1 + triTransform.g*tmpz1; + axis20.z = triTransform.i*tmpx1 + triTransform.j*tmpy1 + triTransform.k*tmpz1; + var x2:Number = triTransform.a*tmpx2 + triTransform.b*tmpy2 + triTransform.c*tmpz2 + triTransform.d; + var y2:Number = triTransform.e*tmpx2 + triTransform.f*tmpy2 + triTransform.g*tmpz2 + triTransform.h; + var z2:Number = triTransform.i*tmpx2 + triTransform.j*tmpy2 + triTransform.k*tmpz2 + triTransform.l; + + // Определение нормали контакта, точки и направляющего вектора ребра бокса + var boxTransform:Matrix4 = box.transform; + boxTransform.getAxis(boxAxisIdx, axis10); + + // Нормаль контакта + var v:Vector3 = contact.normal; + v.x = axis10.y*axis20.z - axis10.z*axis20.y; + v.y = axis10.z*axis20.x - axis10.x*axis20.z; + v.z = axis10.x*axis20.y - axis10.y*axis20.x; + k = 1/Math.sqrt(v.x*v.x + v.y*v.y + v.z*v.z); + v.x *= k; + v.y *= k; + v.z *= k; + // Разворот нормали в сторону бокса + if (v.x*toBox.x + v.y*toBox.y + v.z*toBox.z < 0) { + v.x = -v.x; + v.y = -v.y; + v.z = -v.z; + } + + var boxHalfLen:Number; + tmpx1 = box.hs.x; + tmpy1 = box.hs.y; + tmpz1 = box.hs.z; + // X + if (boxAxisIdx == 0) { + tmpx1 = 0; + boxHalfLen = box.hs.x; + } else { + if (boxTransform.a*v.x + boxTransform.e*v.y + boxTransform.i*v.z > 0) { + tmpx1 = -tmpx1; + } + } + // Y + if (boxAxisIdx == 1) { + tmpy1 = 0; + boxHalfLen = box.hs.y; + } else { + if (boxTransform.b*v.x + boxTransform.f*v.y + boxTransform.j*v.z > 0) { + tmpy1 = -tmpy1; + } + } + // Z + if (boxAxisIdx == 2) { + tmpz1 = 0; + boxHalfLen = box.hs.z; + } else { + if (boxTransform.c*v.x + boxTransform.g*v.y + boxTransform.k*v.z > 0) { + tmpz1 = -tmpz1; + } + } + var x1:Number = boxTransform.a*tmpx1 + boxTransform.b*tmpy1 + boxTransform.c*tmpz1 + boxTransform.d; + var y1:Number = boxTransform.e*tmpx1 + boxTransform.f*tmpy1 + boxTransform.g*tmpz1 + boxTransform.h; + var z1:Number = boxTransform.i*tmpx1 + boxTransform.j*tmpy1 + boxTransform.k*tmpz1 + boxTransform.l; + + // Находим точку пересечения рёбер, решая систему уравнений + // axis10 - направляющий вектор ребра бокса + // x1, y1, z1 - средняя точка ребра бокса + // axis20 - направляющий вектор ребра треугольника + // x2, y2, z2 - начальная точка ребра треугольника + var k:Number = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + var vx:Number = x2 - x1; + var vy:Number = y2 - y1; + var vz:Number = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + + // Запись данных о контакте + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + v = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + v.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + v.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + v.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = v.x - boxTransform.d; + r.y = v.y - boxTransform.h; + r.z = v.z - boxTransform.l; + r = cp.r2; + r.x = v.x - triTransform.d; + r.y = v.y - triTransform.h; + r.z = v.z - triTransform.l; + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/BruteForceCollisionDetector.as b/0.0.5.Optimized/src/alternativa/physics/collision/BruteForceCollisionDetector.as new file mode 100644 index 0000000..85d84fa --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/BruteForceCollisionDetector.as @@ -0,0 +1,63 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + public class BruteForceCollisionDetector implements ICollisionDetector { + public function BruteForceCollisionDetector() { + } + + public function addPrimitive(primitive:CollisionPrimitive, isStatic:Boolean=true):Boolean { + return false; + } + + public function removePrimitive(primitive:CollisionPrimitive, isStatic:Boolean=true):Boolean { + return false; + } + + public function init():void { + } + + public function getAllCollisions(contacts:Vector.):int { + return 0; + } + + public function intersectRay(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, intersection:RayIntersection):Boolean { + return false; + } + + public function intersectRayWithStatic(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean { + return false; + } + + public function testBodyPrimitiveCollision(body:Body, primitive:CollisionPrimitive):Boolean { + return false; + } + + /** + * + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + return false; + } + + /** + * + * @param prim1 + * @param prim2 + * @return + */ + public function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/CollisionKdNode.as b/0.0.5.Optimized/src/alternativa/physics/collision/CollisionKdNode.as new file mode 100644 index 0000000..e24922c --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/CollisionKdNode.as @@ -0,0 +1,18 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.types.BoundBox; + + public class CollisionKdNode { + public var indices:Vector.; + public var splitIndices:Vector.; + public var boundBox:BoundBox; + public var parent:CollisionKdNode; + public var splitTree:CollisionKdTree2D; + + public var axis:int = -1; // 0 - x, 1 - y, 2 - z + public var coord:Number; + public var positiveNode:CollisionKdNode; + public var negativeNode:CollisionKdNode; + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/CollisionKdTree.as b/0.0.5.Optimized/src/alternativa/physics/collision/CollisionKdTree.as new file mode 100644 index 0000000..c046f17 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/CollisionKdTree.as @@ -0,0 +1,280 @@ +package alternativa.physics.collision { + + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + + /** + * @author mike + */ + public class CollisionKdTree { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + public var rootNode:CollisionKdNode; + public var staticChildren:Vector. = new Vector.(); + public var numStaticChildren:int; + public var staticBoundBoxes:Vector. = new Vector.(); + + private var splitAxis:int; + private var splitCoord:Number; + private var splitCost:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + + /** + * @param primitive + */ + public function addStaticPrimitive(primitive:CollisionPrimitive):void { + staticChildren[numStaticChildren++] = primitive; + } + + /** + * @param primitive + * @return + */ + public function removeStaticPrimitive(primitive:CollisionPrimitive):Boolean { + var idx:int = staticChildren.indexOf(primitive); + if (idx < 0) return false; + staticChildren.splice(idx, 1); + numStaticChildren--; + return true; + } + + /** + * @param boundBox + */ + public function createTree(boundBox:BoundBox = null):void { + if (numStaticChildren == 0) return; + // Создаём корневую ноду + rootNode = new CollisionKdNode(); + rootNode.indices = new Vector.(); + // Расчитываем баунды объектов и рутовой ноды + var rootNodeBoundBox:BoundBox = rootNode.boundBox = (boundBox != null) ? boundBox : new BoundBox(); + for (var i:int = 0; i < numStaticChildren; ++i) { + var child:CollisionPrimitive = staticChildren[i]; + var childBoundBox:BoundBox = staticBoundBoxes[i] = child.calculateAABB(); + rootNodeBoundBox.addBoundBox(childBoundBox); + rootNode.indices[i] = i; + } + staticBoundBoxes.length = numStaticChildren; + // Разделяем рутовую ноду + splitNode(rootNode); + + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + var indices:Vector. = node.indices; + var numPrimitives:int = indices.length; + if (numPrimitives <= minPrimitivesPerNode) return; + + // Подготовка баунда с погрешностями + var nodeBoundBox:BoundBox = node.boundBox; + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + // Собираем опорные координаты + var i:int; + var j:int; + var numSplitCoordsX:int = 0 + var numSplitCoordsY:int = 0; + var numSplitCoordsZ:int = 0; + for (i = 0; i < numPrimitives; ++i) { + var boundBox:BoundBox = staticBoundBoxes[indices[i]]; + + if (boundBox.maxX - boundBox.minX <= doubleThreshold) { + if (boundBox.minX <= nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.minX; + else if (boundBox.maxX >= nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.maxX; + else splitCoordsX[numSplitCoordsX++] = (boundBox.minX + boundBox.maxX)*0.5; + } else { + if (boundBox.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = boundBox.minX; + if (boundBox.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = boundBox.maxX; + } + + if (boundBox.maxY - boundBox.minY <= doubleThreshold) { + if (boundBox.minY <= nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.minY; + else if (boundBox.maxY >= nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.maxY; + else splitCoordsY[numSplitCoordsY++] = (boundBox.minY + boundBox.maxY)*0.5; + } else { + if (boundBox.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = boundBox.minY; + if (boundBox.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = boundBox.maxY; + } + + if (boundBox.maxZ - boundBox.minZ <= doubleThreshold) { + if (boundBox.minZ <= nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.minZ; + else if (boundBox.maxZ >= nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.maxZ; + else splitCoordsZ[numSplitCoordsZ++] = (boundBox.minZ + boundBox.maxZ)*0.5; + } else { + if (boundBox.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.minZ; + if (boundBox.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numPrimitives; ++i) { + boundBox = staticBoundBoxes[indices[i]]; + var min:Number = axisX ? boundBox.minX : (axisY ? boundBox.minY : boundBox.minZ); + var max:Number = axisX ? boundBox.maxX : (axisY ? boundBox.maxY : boundBox.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(indices[i]); + indices[i] = -1; + } else { + if (node.splitIndices == null) node.splitIndices = new Vector.(); + node.splitIndices.push(indices[i]); + indices[i] = -1; + } + } else { + if (min >= coordMin) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(indices[i]); + indices[i] = -1; + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numPrimitives; ++i) { + if (indices[i] >= 0) indices[j++] = indices[i]; + } + if (j > 0) indices.length = j; + else node.indices = null; + + if (node.splitIndices != null) { + node.splitTree = new CollisionKdTree2D(this, node); + node.splitTree.createTree(); + } + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + /** + * + */ + public function traceTree():void { + traceNode("", rootNode); + } + + /** + * @param str + * @param node + */ + private function traceNode(str:String, node:CollisionKdNode):void { + if (node == null) return; + trace(str, node.axis == -1 ? "end" : ((node.axis == 0) ? "X" : ((node.axis == 1) ? "Y" : "Z")), "splitCoord=" + splitCoord, "bound", node.boundBox, "objs:", node.indices); + traceNode(str + "-", node.negativeNode); + traceNode(str + "+", node.positiveNode); + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/CollisionKdTree2D.as b/0.0.5.Optimized/src/alternativa/physics/collision/CollisionKdTree2D.as new file mode 100644 index 0000000..78aaa95 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/CollisionKdTree2D.as @@ -0,0 +1,226 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.types.BoundBox; + + /** + * + */ + public class CollisionKdTree2D { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + + public var parentTree:CollisionKdTree; + public var parentNode:CollisionKdNode; + public var rootNode:CollisionKdNode; + + private var splitAxis:int; + private var splitCost:Number; + private var splitCoord:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + /** + * + * @param parentTree + * @param parentNode + */ + public function CollisionKdTree2D(parentTree:CollisionKdTree, parentNode:CollisionKdNode) { + this.parentTree = parentTree; + this.parentNode = parentNode; + } + + /** + * + */ + public function createTree():void { + rootNode = new CollisionKdNode(); + rootNode.boundBox = parentNode.boundBox.clone(); + rootNode.indices = new Vector.(); + var numObjects:int = parentNode.splitIndices.length; + for (var i:int = 0; i < numObjects; ++i) rootNode.indices[i] = parentNode.splitIndices[i]; + + splitNode(rootNode); + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + if (node.indices.length <= minPrimitivesPerNode) return; + + var objects:Vector. = node.indices; + var i:int; + var j:int; + + var nodeBoundBox:BoundBox = node.boundBox; + + // Подготовка баунда с погрешностями + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + // Собираем опорные координаты + var numSplitCoordsX:int; + var numSplitCoordsY:int; + var numSplitCoordsZ:int; + var numObjects:int = objects.length; + for (i = 0; i < numObjects; ++i) { + var bb:BoundBox = staticBoundBoxes[objects[i]]; + + if (parentNode.axis != 0) { + if (bb.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = bb.minX; + if (bb.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = bb.maxX; + } + + if (parentNode.axis != 1) { + if (bb.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = bb.minY; + if (bb.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = bb.maxY; + } + + if (parentNode.axis != 2) { + if (bb.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = bb.minZ; + if (bb.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = bb.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + if (parentNode.axis != 0) checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + if (parentNode.axis != 1) checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + if (parentNode.axis != 2) checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numObjects; ++i) { + bb = staticBoundBoxes[objects[i]]; + var min:Number = axisX ? bb.minX : (axisY ? bb.minY : bb.minZ); + var max:Number = axisX ? bb.maxX : (axisY ? bb.maxY : bb.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(objects[i]); + objects[i] = -1; + } + } else { + if (min >= coordMin) { + if (max > coordMax) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(objects[i]); + objects[i] = -1; + } + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numObjects; ++i) { + if (objects[i] >= 0) objects[j++] = objects[i]; + } + if (j > 0) objects.length = j; + else node.indices = null; + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/IBodyCollisionPredicate.as b/0.0.5.Optimized/src/alternativa/physics/collision/IBodyCollisionPredicate.as new file mode 100644 index 0000000..9cfdca5 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/IBodyCollisionPredicate.as @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Body; + + public interface IBodyCollisionPredicate { + function considerBodies(body1:Body, body2:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/ICollider.as b/0.0.5.Optimized/src/alternativa/physics/collision/ICollider.as new file mode 100644 index 0000000..220e717 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/ICollider.as @@ -0,0 +1,30 @@ +package alternativa.physics.collision { + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + + /** + * Интерфейс определителя столкновений между двумя примитивами. + */ + public interface ICollider { + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/ICollisionDetector.as b/0.0.5.Optimized/src/alternativa/physics/collision/ICollisionDetector.as new file mode 100644 index 0000000..af43dfb --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/ICollisionDetector.as @@ -0,0 +1,92 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + /** + * Интерфейс детектора столкновений. + */ + public interface ICollisionDetector { + + /** + * Добавляет физический примитив в коллайдер. + * + * @param primitive добавляемый примитив + * @param isStatic указывает тип примитива: статический или динамический + * @return true если примитив был успешно добавлен, иначе false + */ + function addPrimitive(primitive:CollisionPrimitive, isStatic:Boolean = true):Boolean; + + /** + * Удаляет физический примитив из коллайдера. + * + * @param primitive удаляемый примитив + * @param isStatic указывает тип примитива: статический или динамический + * @return true если примитив был успшено удалён, иначе false + */ + function removePrimitive(primitive:CollisionPrimitive, isStatic:Boolean = true):Boolean; + + /** + * Выполняет инициализацию детектора после обновления списка примитивов. + */ + function init():void; + + /** + * Получает все столкновения в текущей конфигурации физической геометрии. + * + * @param contacts список контактов, в кторые будет записана информация о столкновении + * @return количество найденных столкновений + */ + function getAllCollisions(contacts:Vector.):int; + + /** + * Тестирует луч на пересечение с физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param dir направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRay(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * Тестирует луч на пересечение со статической физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param dir направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRayWithStatic(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @return + */ + function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/ICollisionPredicate.as b/0.0.5.Optimized/src/alternativa/physics/collision/ICollisionPredicate.as new file mode 100644 index 0000000..bb1b756 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/ICollisionPredicate.as @@ -0,0 +1,9 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + + public interface ICollisionPredicate { + + function considerCollision(primitive:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/IRayCollisionPredicate.as b/0.0.5.Optimized/src/alternativa/physics/collision/IRayCollisionPredicate.as new file mode 100644 index 0000000..0f3d929 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/IRayCollisionPredicate.as @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Body; + + public interface IRayCollisionPredicate { + function considerBody(body:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/KdTreeCollisionDetector.as b/0.0.5.Optimized/src/alternativa/physics/collision/KdTreeCollisionDetector.as new file mode 100644 index 0000000..7b0b78e --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/KdTreeCollisionDetector.as @@ -0,0 +1,484 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Детектор, хранящий статическую геометрию в kD-дереве и использующий дерево для ускорения тестов на пересечения. + */ + public class KdTreeCollisionDetector implements ICollisionDetector { + + altphysics var tree:CollisionKdTree; + altphysics var dynamicPrimitives:Vector.; + 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.(); + + 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():void { + tree.createTree(); +// tree.traceTree(); + } + + /** + * + * @param contacts + * @return + */ + public function getAllCollisions(contacts:Vector.):int { + var colNum:int = 0; + for (var i:int = 0; i < dynamicPrimitivesNum; i++) { + var primitive:CollisionPrimitive = dynamicPrimitives[i]; + primitive.calculateAABB(); + if (primitive.body != null && primitive.body.frozen) continue; + colNum += getPrimitiveNodeCollisions(tree.rootNode, primitive, contacts, colNum); + + // Столкновения динамических примитивов между собой + // TODO: Попробовать различные оптимизации (сортировка по баундам, встраивание в дерево) + for (var j:int = i + 1; j < dynamicPrimitivesNum; j++) { + if (collide(primitive, dynamicPrimitives[j], contacts[colNum])) colNum++; + } + } + + return colNum; + } + + /** + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + public function collide(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 = (contact.points[0] as ContactPoint).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:Vector., startIndex:int):int { + var colNum:int = 0; + if (node.indices != null) { + // Поиск столкновений со статическими примитивами узла + var primitives:Vector. = tree.staticChildren; + var indices:Vector. = node.indices; + for (var i:int = indices.length - 1; i >= 0; i--) + if (collide(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; + } + + 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., indices:Vector., 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; + } \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/SpherePlaneCollider.as b/0.0.5.Optimized/src/alternativa/physics/collision/SpherePlaneCollider.as new file mode 100644 index 0000000..77eb1b0 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/SpherePlaneCollider.as @@ -0,0 +1,67 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + /** + * + */ + public class SpherePlaneCollider implements ICollider { + + private var normal:Vector3 = new Vector3(); + + /** + * + */ + public function SpherePlaneCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var sphere:RigidSphere = body1 as RigidSphere; +// var plane:RigidPlane; +// if (sphere == null) { +// sphere = body2 as RigidSphere; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// +// var dist:Number = sphere.state.pos.dot(normal) - offset; +// if (dist > sphere.r) return false; +// +// collisionInfo.body1 = sphere; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// collisionInfo.pcount = 1; +// +// var cp:ContactPoint = collisionInfo.points[0]; +// cp.penetration = sphere.r - dist; +// cp.pos.copy(normal).reverse().scale(sphere.r).add(sphere.state.pos); +// cp.r1.diff(cp.pos, sphere.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/SphereSphereCollider.as b/0.0.5.Optimized/src/alternativa/physics/collision/SphereSphereCollider.as new file mode 100644 index 0000000..25971d5 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/SphereSphereCollider.as @@ -0,0 +1,69 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + public class SphereSphereCollider implements ICollider { + + private var p1:Vector3 = new Vector3(); + private var p2:Vector3 = new Vector3(); + + public function SphereSphereCollider() { + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var s1:CollisionSphere; + var s2:CollisionSphere; + if (prim1.body != null) { + s1 = prim1 as CollisionSphere; + s2 = prim2 as CollisionSphere; + } else { + s1 = prim2 as CollisionSphere; + s2 = prim1 as CollisionSphere; + } + + s1.transform.getAxis(3, p1); + s2.transform.getAxis(3, p2); + var dx:Number = p1.x - p2.x; + var dy:Number = p1.y - p2.y; + var dz:Number = p1.z - p2.z; + var len:Number = dx*dx + dy*dy + dz*dz; + var sum:Number = s1.r + s2.r; + if (len > sum*sum) return false; + len = Math.sqrt(len); + dx /= len; + dy /= len; + dz /= len; + + contact.body1 = s1.body; + contact.body2 = s2.body; + contact.normal.x = dx; + contact.normal.y = dy; + contact.normal.z = dz; + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = sum - len; + cp.pos.x = p1.x - dx*s1.r; + cp.pos.y = p1.y - dy*s1.r; + cp.pos.z = p1.z - dz*s1.r; + cp.r1.vDiff(cp.pos, p1); + cp.r2.vDiff(cp.pos, p2); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/primitives/.svn/all-wcprops b/0.0.5.Optimized/src/alternativa/physics/collision/primitives/.svn/all-wcprops new file mode 100644 index 0000000..d02e058 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/primitives/.svn/all-wcprops @@ -0,0 +1,35 @@ +K 25 +svn:wc:ra_dav:version-url +V 136 +/!svn/ver/19853/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/collision/primitives +END +CollisionTriangle.as +K 25 +svn:wc:ra_dav:version-url +V 157 +/!svn/ver/19853/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/collision/primitives/CollisionTriangle.as +END +CollisionSphere.as +K 25 +svn:wc:ra_dav:version-url +V 155 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/collision/primitives/CollisionSphere.as +END +CollisionBox.as +K 25 +svn:wc:ra_dav:version-url +V 152 +/!svn/ver/19853/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/collision/primitives/CollisionBox.as +END +CollisionPrimitive.as +K 25 +svn:wc:ra_dav:version-url +V 158 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/collision/primitives/CollisionPrimitive.as +END +CollisionRect.as +K 25 +svn:wc:ra_dav:version-url +V 153 +/!svn/ver/19853/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/collision/primitives/CollisionRect.as +END diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/primitives/.svn/entries b/0.0.5.Optimized/src/alternativa/physics/collision/primitives/.svn/entries new file mode 100644 index 0000000..0f572ca --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/primitives/.svn/entries @@ -0,0 +1,88 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/collision/primitives +http://svndev.alternativaplatform.com + + + +2009-09-14T13:27:15.443685Z +19853 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +CollisionTriangle.as +file + + + + +2010-10-28T04:32:38.000000Z +9567a440e5f4552480eec32af2b1541a +2009-09-14T13:27:15.443685Z +19853 +mike + +CollisionSphere.as +file + + + + +2010-10-28T04:32:38.000000Z +d0d8b961b5e74673c357cd98e0963f4a +2009-09-09T12:16:47.747177Z +19632 +mike + +CollisionBox.as +file + + + + +2010-10-28T04:32:38.000000Z +6949185b7324dde6fc0a48547d56fb92 +2009-09-14T13:27:15.443685Z +19853 +mike + +CollisionPrimitive.as +file + + + + +2010-10-28T04:32:38.000000Z +709fc587a0f9d3cb6a67dce3f3e4c92e +2009-09-11T09:22:20.813670Z +19776 +mike + +CollisionRect.as +file + + + + +2010-10-28T04:32:38.000000Z +ace6d006988f2caaf2577db09794910c +2009-09-14T13:27:15.443685Z +19853 +mike + diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/primitives/.svn/format b/0.0.5.Optimized/src/alternativa/physics/collision/primitives/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/primitives/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base new file mode 100644 index 0000000..ff6bbf5 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base @@ -0,0 +1,178 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + /** + * Ориентированный бокс. + */ + public class CollisionBox extends CollisionPrimitive { + + // Половинные размеры вдоль каждой из осей + public var hs:Vector3 = new Vector3(); + + /** + * @param hs + * @param collisionGroup + */ + public function CollisionBox(hs:Vector3, collisionGroup:int) { + super(BOX, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + var t:Matrix4 = transform; + aabb.maxX = hs.x*(t.a < 0 ? -t.a : t.a) + hs.y*(t.b < 0 ? -t.b : t.b) + hs.z*(t.c < 0 ? -t.c : t.c); + aabb.minX = -aabb.maxX; + + aabb.maxY = hs.x*(t.e < 0 ? -t.e : t.e) + hs.y*(t.f < 0 ? -t.f : t.f) + hs.z*(t.g < 0 ? -t.g : t.g); + aabb.minY = -aabb.maxY; + + aabb.maxZ = hs.x*(t.i < 0 ? -t.i : t.i) + hs.y*(t.j < 0 ? -t.j : t.j) + hs.z*(t.k < 0 ? -t.k : t.k); + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var box:CollisionBox = source as CollisionBox; + if (box == null) return this; + super.copyFrom(box); + hs.vCopy(box.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionBox(hs, collisionGroup); + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + var tMin:Number = -1; + var tMax:Number = 1e308; + var t1:Number; + var t2:Number; + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + // X + if (vx < epsilon && vx > -epsilon) { + if (ox < -hs.x || ox > hs.x) return -1; + } else { + t1 = (-hs.x - ox)/vx; + t2 = (hs.x - ox)/vx; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.x = -1; + normal.y = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.x = 1; + normal.y = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Y + if (vy < epsilon && vy > -epsilon) { + if (oy < -hs.y || oy > hs.y) return -1; + } else { + t1 = (-hs.y - oy)/vy; + t2 = (hs.y - oy)/vy; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.y = -1; + normal.x = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.y = 1; + normal.x = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Z + if (vz < epsilon && vz > -epsilon) { + if (oz < -hs.z || oz > hs.z) return -1; + } else { + t1 = (-hs.z - oz)/vz; + t2 = (hs.z - oz)/vz; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.z = -1; + normal.x = normal.y = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.z = 1; + normal.x = normal.y = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Перевод нормали в мировую систему координат + // Inlined normal.vDeltaTransformBy4(transform); + vx = normal.x; + vy = normal.y; + vz = normal.z; + normal.x = transform.a*vx + transform.b*vy + transform.c*vz; + normal.y = transform.e*vx + transform.f*vy + transform.g*vz; + normal.z = transform.i*vx + transform.j*vy + transform.k*vz; + + return tMin; + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionPrimitive.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionPrimitive.as.svn-base new file mode 100644 index 0000000..5a8b5a9 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionPrimitive.as.svn-base @@ -0,0 +1,143 @@ +package alternativa.physics.collision.primitives { + + import alternativa.physics.collision.ICollisionPredicate; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + + /** + * Базовый класс для примитивов, использующихся детектором столкновений. + */ + public class CollisionPrimitive { + // Константы типов примитива + public static const BOX:int = 1; + public static const PLANE:int = 2; + public static const SPHERE:int = 3; + public static const RECT:int = 4; + public static const TRIANGLE:int = 5; + + // Тип примитива + public var type:int; + // Группы примитива. Каждая группа определяется установленным битом. Столкновения проверяются только для примитивов, + // имеющих хотя бы одну общую группу. + public var collisionGroup:int; + // Предикат примитива, вызывающийся детектором при нахождении столкновения. В зависимости от возвращённого + // предикатом результата столкновение либо регистрируется, либо игнорируется. + // Nullable + public var postCollisionPredicate:ICollisionPredicate; + // Тело, владеющее примитивом. Поле сделано открытым для быстрого доступа на чтение. Установка значения должна выполняться вызовом метода setBody(). + // Nullable + public var body:Body; + // Трансформация примитива в системе координат тела, если оно указано. Не допускается масштабирование матрицы. + public var localTransform:Matrix4; + // Полная трансформация примитива. Не допускается масштабирование матрицы. + public var transform:Matrix4 = new Matrix4(); + // AABB в мировой системе координат. Расчитывается системой вызовом функции calculateBoundBox(). + public var aabb:BoundBox = new BoundBox(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param type тип примитива + * @param collisionGroup группа примитива + */ + public function CollisionPrimitive(type:int, collisionGroup:int) { + this.type = type; + this.collisionGroup = collisionGroup; + } + + /** + * Устанавливает тело, владеющее примитивом. + * + * @param body тело, которое владеет примитивом. Если указано значение null, матрица локальной трансформации удаляется. + * @param localTransform трансформация примитива в системе координат тела. Указание значения null равносильно + * заданию единичной матрицы, однако в первом случае не будет дополнительного умножения матриц при вычислении полной трансформации примитива. + */ + public function setBody(body:Body, localTransform:Matrix4 = null):void { + if (this.body == body) return; + this.body = body; + if (body == null) { + this.localTransform = null; + } else { + if (localTransform != null) { + if (this.localTransform == null) { + this.localTransform = new Matrix4(); + } + this.localTransform.copy(localTransform); + } else { + this.localTransform = null; + } + } + } + + /** + * Рассчитывает AABB примитива. Наследники должны переопределять этот метод, реализуя в нём корректный рассчёт. + * + * @return ссылка на свой AABB + */ + public function calculateAABB():BoundBox { + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + return -1; + } + + /** + * Клонирует примитив. Переопределять не рекомендуется. Вместо этого переопределяются методы createPrimitive() и copyFrom(). + * + * @return клон примитива + */ + public function clone():CollisionPrimitive { + var p:CollisionPrimitive = createPrimitive(); + return p.copyFrom(this); + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + if (source == null) { + throw new ArgumentError("Parameter source cannot be null"); + } + type = source.type; + transform.copy(source.transform); + collisionGroup = source.collisionGroup; + setBody(source.body, source.localTransform); + aabb.copyFrom(source.aabb); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + public function toString():String { + return "[CollisionPrimitive type=" + type + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + protected function createPrimitive():CollisionPrimitive { + return new CollisionPrimitive(type, collisionGroup); + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base new file mode 100644 index 0000000..bc4b34c --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base @@ -0,0 +1,123 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + /** + * Ориентированный прямоугольник. Задаётся половинами размеров вдоль осей X (ширина) и Y (длина) локальной системы + * координат примитива. Таким образом, прямоугольник лежит в плоскости XY, его стороны параллельны осям этой + * плоскости, а нормаль направлена вдоль локальной оси Z. + * + * Прямоугольник может быть одно- или двусторонним. В случае одностороннего прямоугольника, + * столкновения с внутренней стороны не регистрируются. + */ + public class CollisionRect extends CollisionPrimitive { + // Половинные размеры прямоугольника вдоль осей X и Y. Нормаль направлена вдоль оси Z. + public var hs:Vector3 = new Vector3(); + // Флаг указывает, является примитив одно- или двусторонним + public var twoSided:Boolean = true; + + // Малое значение. Используется для указания фиктивной высоты примитива и в функции определения пересечения с сегментом. + private static const EPSILON:Number = 0.005; + + /** + * Создаёт новый экземпляр примитива. + * + * @param hs половинные размерв прямоугольника вдоль осей X и Y. Значение z игнорируется. + * @param collisionGroup группа примитива + */ + public function CollisionRect(hs:Vector3, collisionGroup:int) { + super(RECT, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * Расчитывает ограничивающий бокс прямоугольника. Для избежания проблем высота примитива принимается равной + * не нулю, а малому значению. + * + * @return + */ + override public function calculateAABB():BoundBox { + // Баунд бокс прямоугольника имеет минимальную высоту, отличную от нуля во избежание проблем с построением kd-дерева + var t:Matrix4 = transform; + aabb.maxX = hs.x*(t.a < 0 ? -t.a : t.a) + hs.y*(t.b < 0 ? -t.b : t.b) + EPSILON*(t.c < 0 ? -t.c : t.c); + aabb.minX = -aabb.maxX; + + aabb.maxY = hs.x*(t.e < 0 ? -t.e : t.e) + hs.y*(t.f < 0 ? -t.f : t.f) + EPSILON*(t.g < 0 ? -t.g : t.g); + aabb.minY = -aabb.maxY; + + aabb.maxZ = hs.x*(t.i < 0 ? -t.i : t.i) + hs.y*(t.j < 0 ? -t.j : t.j) + EPSILON*(t.k < 0 ? -t.k : t.k); + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var rect:CollisionRect = source as CollisionRect; + if (rect == null) return this; + super.copyFrom(rect); + hs.vCopy(rect.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionRect(hs, collisionGroup); + } + + /** + * @param origin + * @param vector + * @param collisionGroup + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + + // Проверка параллельности сегмента и плоскости примитива + if (vz > -threshold && vz < threshold) return -1; + var t:Number = -oz/vz; + if (t < 0) return -1; + // Проверка вхождения точки пересечения в прямоугольник + ox += vx*t; + oy += vy*t; + oz = 0; + if (ox < (-hs.x - threshold) || ox > (hs.x + threshold) || oy < (-hs.y - threshold) || oy > (hs.y + threshold)) return -1; + + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + return t; + } + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base new file mode 100644 index 0000000..dc2328b --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base @@ -0,0 +1,80 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + + /** + * Сфера. + */ + public class CollisionSphere extends CollisionPrimitive { + + // Радиус сферы + public var r:Number = 0; + + /** + * + * @param r + * @param collisionGroup + */ + public function CollisionSphere(r:Number, collisionGroup:int) { + super(SPHERE, collisionGroup); + this.r = r; + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + aabb.maxX = transform.d + r; + aabb.minX = transform.d - r; + + aabb.maxY = transform.h + r; + aabb.minY = transform.h - r; + + aabb.maxZ = transform.l + r; + aabb.minZ = transform.l - r; + + return aabb; + } + + /** + * @param origin + * @param vector + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var px:Number = origin.x - transform.d; + var py:Number = origin.y - transform.h; + var pz:Number = origin.z - transform.l; + var k:Number = vector.x*px + vector.y*py + vector.z*pz; + if (k > 0) return -1; + var a:Number = vector.x*vector.x + vector.y*vector.y + vector.z*vector.z; + var D:Number = k*k - a*(px*px + py*py + pz*pz - r*r); + if (D < 0) return -1; + return -(k + Math.sqrt(D))/a; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var sphere:CollisionSphere = source as CollisionSphere; + if (sphere == null) return this; + super.copyFrom(sphere); + r = sphere.r; + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionSphere(r, collisionGroup); + } + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base new file mode 100644 index 0000000..995d3cb --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base @@ -0,0 +1,243 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + /** + * Примитив треугольник. + */ + public class CollisionTriangle extends CollisionPrimitive { + + public var v0:Vector3 = new Vector3(); + public var v1:Vector3 = new Vector3(); + public var v2:Vector3 = new Vector3(); + + public var e0:Vector3 = new Vector3(); + public var e1:Vector3 = new Vector3(); + public var e2:Vector3 = new Vector3(); + +// public var len0:Number; +// public var len1:Number; +// public var len2:Number; +// + /** + * + * @param v0 + * @param v1 + * @param v2 + * @param collisionGroup + */ + public function CollisionTriangle(v0:Vector3, v1:Vector3, v2:Vector3, collisionGroup:int) { + super(TRIANGLE, collisionGroup); + initVertices(v0, v1, v2); + } + + /** + * Рассчитывает AABB примитива. + * + * @return ссылка на свой AABB + */ + override public function calculateAABB():BoundBox { + var epsilon:Number = 0.005; + var a:Number; + var b:Number; + var eps_c:Number = epsilon*transform.c; + var eps_g:Number = epsilon*transform.g; + var eps_k:Number = epsilon*transform.k; + + // Вершина 0 + // Ось X + a = v0.x*transform.a + v0.y*transform.b; + aabb.minX = aabb.maxX = a + eps_c; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v0.x*transform.e + v0.y*transform.f; + aabb.minY = aabb.maxY = a + eps_g; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v0.x*transform.i + v0.y*transform.j; + aabb.minZ = aabb.maxZ = a + eps_k; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 1 + // Ось X + a = v1.x*transform.a + v1.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v1.x*transform.e + v1.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v1.x*transform.i + v1.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 2 + // Ось X + a = v2.x*transform.a + v2.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v2.x*transform.e + v2.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v2.x*transform.i + v2.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + aabb.minX += transform.d; + aabb.maxX += transform.d; + + aabb.minY += transform.h; + aabb.maxY += transform.h; + + aabb.minZ += transform.l; + aabb.maxZ += transform.l; + + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + // Луч трансформируется в систему координат примитива, затем проверяется пересечение + + var vz:Number = vector.x*transform.c + vector.y*transform.g + vector.z*transform.k; + // Если луч параллелен плоскости птимитива, то пересечения нет + if (vz < epsilon && vz > -epsilon) return -1; + + var tx:Number = origin.x - transform.d; + var ty:Number = origin.y - transform.h; + var tz:Number = origin.z - transform.l; + + var oz:Number = tx*transform.c + ty*transform.g + tz*transform.k; + var t:Number = -oz/vz; + if (t < 0) return -1; + + var ox:Number = tx*transform.a + ty*transform.e + tz*transform.i; + var oy:Number = tx*transform.b + ty*transform.f + tz*transform.j; + + tx = ox + t*(vector.x*transform.a + vector.y*transform.e + vector.z*transform.i); + ty = oy + t*(vector.x*transform.b + vector.y*transform.f + vector.z*transform.j); + tz = oz + t*vz; + + // Проверка вхождения точки в треугольник + if ((e0.x*(ty - v0.y) - e0.y*(tx - v0.x) < 0) || (e1.x*(ty - v1.y) - e1.y*(tx - v1.x) < 0) || (e2.x*(ty - v2.y) - e2.y*(tx - v2.x) < 0)) return -1; + + // Запись нормали + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + + return t; + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + super.copyFrom(source); + var tri:CollisionTriangle = source as CollisionTriangle; + if (tri != null) { + v0.vCopy(tri.v0); + v1.vCopy(tri.v1); + v2.vCopy(tri.v2); + + e0.vCopy(tri.e0); + e1.vCopy(tri.e1); + e2.vCopy(tri.e2); + +// len0 = tri.len0; +// len1 = tri.len1; +// len2 = tri.len2; + } + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionTriangle v0=" + v0 + ", v1=" + v1 + ", v2=" + v2 + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionTriangle(v0, v1, v2, collisionGroup); + } + + /** + * + * @param v0 + * @param v1 + * @param v2 + */ + private function initVertices(v0:Vector3, v1:Vector3, v2:Vector3):void { + this.v0.vCopy(v0); + this.v1.vCopy(v1); + this.v2.vCopy(v2); + + e0.vDiff(v1, v0); +// len0 = e0.vLength(); + e0.vNormalize(); + + e1.vDiff(v2, v1); +// len1 = e1.vLength(); + e1.vNormalize(); + + e2.vDiff(v0, v2); +// len2 = e2.vLength(); + e2.vNormalize(); + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/primitives/CollisionBox.as b/0.0.5.Optimized/src/alternativa/physics/collision/primitives/CollisionBox.as new file mode 100644 index 0000000..ff6bbf5 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/primitives/CollisionBox.as @@ -0,0 +1,178 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + /** + * Ориентированный бокс. + */ + public class CollisionBox extends CollisionPrimitive { + + // Половинные размеры вдоль каждой из осей + public var hs:Vector3 = new Vector3(); + + /** + * @param hs + * @param collisionGroup + */ + public function CollisionBox(hs:Vector3, collisionGroup:int) { + super(BOX, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + var t:Matrix4 = transform; + aabb.maxX = hs.x*(t.a < 0 ? -t.a : t.a) + hs.y*(t.b < 0 ? -t.b : t.b) + hs.z*(t.c < 0 ? -t.c : t.c); + aabb.minX = -aabb.maxX; + + aabb.maxY = hs.x*(t.e < 0 ? -t.e : t.e) + hs.y*(t.f < 0 ? -t.f : t.f) + hs.z*(t.g < 0 ? -t.g : t.g); + aabb.minY = -aabb.maxY; + + aabb.maxZ = hs.x*(t.i < 0 ? -t.i : t.i) + hs.y*(t.j < 0 ? -t.j : t.j) + hs.z*(t.k < 0 ? -t.k : t.k); + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var box:CollisionBox = source as CollisionBox; + if (box == null) return this; + super.copyFrom(box); + hs.vCopy(box.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionBox(hs, collisionGroup); + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + var tMin:Number = -1; + var tMax:Number = 1e308; + var t1:Number; + var t2:Number; + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + // X + if (vx < epsilon && vx > -epsilon) { + if (ox < -hs.x || ox > hs.x) return -1; + } else { + t1 = (-hs.x - ox)/vx; + t2 = (hs.x - ox)/vx; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.x = -1; + normal.y = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.x = 1; + normal.y = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Y + if (vy < epsilon && vy > -epsilon) { + if (oy < -hs.y || oy > hs.y) return -1; + } else { + t1 = (-hs.y - oy)/vy; + t2 = (hs.y - oy)/vy; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.y = -1; + normal.x = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.y = 1; + normal.x = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Z + if (vz < epsilon && vz > -epsilon) { + if (oz < -hs.z || oz > hs.z) return -1; + } else { + t1 = (-hs.z - oz)/vz; + t2 = (hs.z - oz)/vz; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.z = -1; + normal.x = normal.y = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.z = 1; + normal.x = normal.y = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Перевод нормали в мировую систему координат + // Inlined normal.vDeltaTransformBy4(transform); + vx = normal.x; + vy = normal.y; + vz = normal.z; + normal.x = transform.a*vx + transform.b*vy + transform.c*vz; + normal.y = transform.e*vx + transform.f*vy + transform.g*vz; + normal.z = transform.i*vx + transform.j*vy + transform.k*vz; + + return tMin; + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/primitives/CollisionPrimitive.as b/0.0.5.Optimized/src/alternativa/physics/collision/primitives/CollisionPrimitive.as new file mode 100644 index 0000000..5a8b5a9 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/primitives/CollisionPrimitive.as @@ -0,0 +1,143 @@ +package alternativa.physics.collision.primitives { + + import alternativa.physics.collision.ICollisionPredicate; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + + /** + * Базовый класс для примитивов, использующихся детектором столкновений. + */ + public class CollisionPrimitive { + // Константы типов примитива + public static const BOX:int = 1; + public static const PLANE:int = 2; + public static const SPHERE:int = 3; + public static const RECT:int = 4; + public static const TRIANGLE:int = 5; + + // Тип примитива + public var type:int; + // Группы примитива. Каждая группа определяется установленным битом. Столкновения проверяются только для примитивов, + // имеющих хотя бы одну общую группу. + public var collisionGroup:int; + // Предикат примитива, вызывающийся детектором при нахождении столкновения. В зависимости от возвращённого + // предикатом результата столкновение либо регистрируется, либо игнорируется. + // Nullable + public var postCollisionPredicate:ICollisionPredicate; + // Тело, владеющее примитивом. Поле сделано открытым для быстрого доступа на чтение. Установка значения должна выполняться вызовом метода setBody(). + // Nullable + public var body:Body; + // Трансформация примитива в системе координат тела, если оно указано. Не допускается масштабирование матрицы. + public var localTransform:Matrix4; + // Полная трансформация примитива. Не допускается масштабирование матрицы. + public var transform:Matrix4 = new Matrix4(); + // AABB в мировой системе координат. Расчитывается системой вызовом функции calculateBoundBox(). + public var aabb:BoundBox = new BoundBox(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param type тип примитива + * @param collisionGroup группа примитива + */ + public function CollisionPrimitive(type:int, collisionGroup:int) { + this.type = type; + this.collisionGroup = collisionGroup; + } + + /** + * Устанавливает тело, владеющее примитивом. + * + * @param body тело, которое владеет примитивом. Если указано значение null, матрица локальной трансформации удаляется. + * @param localTransform трансформация примитива в системе координат тела. Указание значения null равносильно + * заданию единичной матрицы, однако в первом случае не будет дополнительного умножения матриц при вычислении полной трансформации примитива. + */ + public function setBody(body:Body, localTransform:Matrix4 = null):void { + if (this.body == body) return; + this.body = body; + if (body == null) { + this.localTransform = null; + } else { + if (localTransform != null) { + if (this.localTransform == null) { + this.localTransform = new Matrix4(); + } + this.localTransform.copy(localTransform); + } else { + this.localTransform = null; + } + } + } + + /** + * Рассчитывает AABB примитива. Наследники должны переопределять этот метод, реализуя в нём корректный рассчёт. + * + * @return ссылка на свой AABB + */ + public function calculateAABB():BoundBox { + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + return -1; + } + + /** + * Клонирует примитив. Переопределять не рекомендуется. Вместо этого переопределяются методы createPrimitive() и copyFrom(). + * + * @return клон примитива + */ + public function clone():CollisionPrimitive { + var p:CollisionPrimitive = createPrimitive(); + return p.copyFrom(this); + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + if (source == null) { + throw new ArgumentError("Parameter source cannot be null"); + } + type = source.type; + transform.copy(source.transform); + collisionGroup = source.collisionGroup; + setBody(source.body, source.localTransform); + aabb.copyFrom(source.aabb); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + public function toString():String { + return "[CollisionPrimitive type=" + type + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + protected function createPrimitive():CollisionPrimitive { + return new CollisionPrimitive(type, collisionGroup); + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/primitives/CollisionRect.as b/0.0.5.Optimized/src/alternativa/physics/collision/primitives/CollisionRect.as new file mode 100644 index 0000000..bc4b34c --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/primitives/CollisionRect.as @@ -0,0 +1,123 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + /** + * Ориентированный прямоугольник. Задаётся половинами размеров вдоль осей X (ширина) и Y (длина) локальной системы + * координат примитива. Таким образом, прямоугольник лежит в плоскости XY, его стороны параллельны осям этой + * плоскости, а нормаль направлена вдоль локальной оси Z. + * + * Прямоугольник может быть одно- или двусторонним. В случае одностороннего прямоугольника, + * столкновения с внутренней стороны не регистрируются. + */ + public class CollisionRect extends CollisionPrimitive { + // Половинные размеры прямоугольника вдоль осей X и Y. Нормаль направлена вдоль оси Z. + public var hs:Vector3 = new Vector3(); + // Флаг указывает, является примитив одно- или двусторонним + public var twoSided:Boolean = true; + + // Малое значение. Используется для указания фиктивной высоты примитива и в функции определения пересечения с сегментом. + private static const EPSILON:Number = 0.005; + + /** + * Создаёт новый экземпляр примитива. + * + * @param hs половинные размерв прямоугольника вдоль осей X и Y. Значение z игнорируется. + * @param collisionGroup группа примитива + */ + public function CollisionRect(hs:Vector3, collisionGroup:int) { + super(RECT, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * Расчитывает ограничивающий бокс прямоугольника. Для избежания проблем высота примитива принимается равной + * не нулю, а малому значению. + * + * @return + */ + override public function calculateAABB():BoundBox { + // Баунд бокс прямоугольника имеет минимальную высоту, отличную от нуля во избежание проблем с построением kd-дерева + var t:Matrix4 = transform; + aabb.maxX = hs.x*(t.a < 0 ? -t.a : t.a) + hs.y*(t.b < 0 ? -t.b : t.b) + EPSILON*(t.c < 0 ? -t.c : t.c); + aabb.minX = -aabb.maxX; + + aabb.maxY = hs.x*(t.e < 0 ? -t.e : t.e) + hs.y*(t.f < 0 ? -t.f : t.f) + EPSILON*(t.g < 0 ? -t.g : t.g); + aabb.minY = -aabb.maxY; + + aabb.maxZ = hs.x*(t.i < 0 ? -t.i : t.i) + hs.y*(t.j < 0 ? -t.j : t.j) + EPSILON*(t.k < 0 ? -t.k : t.k); + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var rect:CollisionRect = source as CollisionRect; + if (rect == null) return this; + super.copyFrom(rect); + hs.vCopy(rect.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionRect(hs, collisionGroup); + } + + /** + * @param origin + * @param vector + * @param collisionGroup + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + + // Проверка параллельности сегмента и плоскости примитива + if (vz > -threshold && vz < threshold) return -1; + var t:Number = -oz/vz; + if (t < 0) return -1; + // Проверка вхождения точки пересечения в прямоугольник + ox += vx*t; + oy += vy*t; + oz = 0; + if (ox < (-hs.x - threshold) || ox > (hs.x + threshold) || oy < (-hs.y - threshold) || oy > (hs.y + threshold)) return -1; + + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + return t; + } + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/primitives/CollisionSphere.as b/0.0.5.Optimized/src/alternativa/physics/collision/primitives/CollisionSphere.as new file mode 100644 index 0000000..dc2328b --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/primitives/CollisionSphere.as @@ -0,0 +1,80 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + + /** + * Сфера. + */ + public class CollisionSphere extends CollisionPrimitive { + + // Радиус сферы + public var r:Number = 0; + + /** + * + * @param r + * @param collisionGroup + */ + public function CollisionSphere(r:Number, collisionGroup:int) { + super(SPHERE, collisionGroup); + this.r = r; + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + aabb.maxX = transform.d + r; + aabb.minX = transform.d - r; + + aabb.maxY = transform.h + r; + aabb.minY = transform.h - r; + + aabb.maxZ = transform.l + r; + aabb.minZ = transform.l - r; + + return aabb; + } + + /** + * @param origin + * @param vector + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var px:Number = origin.x - transform.d; + var py:Number = origin.y - transform.h; + var pz:Number = origin.z - transform.l; + var k:Number = vector.x*px + vector.y*py + vector.z*pz; + if (k > 0) return -1; + var a:Number = vector.x*vector.x + vector.y*vector.y + vector.z*vector.z; + var D:Number = k*k - a*(px*px + py*py + pz*pz - r*r); + if (D < 0) return -1; + return -(k + Math.sqrt(D))/a; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var sphere:CollisionSphere = source as CollisionSphere; + if (sphere == null) return this; + super.copyFrom(sphere); + r = sphere.r; + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionSphere(r, collisionGroup); + } + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/primitives/CollisionTriangle.as b/0.0.5.Optimized/src/alternativa/physics/collision/primitives/CollisionTriangle.as new file mode 100644 index 0000000..995d3cb --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/primitives/CollisionTriangle.as @@ -0,0 +1,243 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + /** + * Примитив треугольник. + */ + public class CollisionTriangle extends CollisionPrimitive { + + public var v0:Vector3 = new Vector3(); + public var v1:Vector3 = new Vector3(); + public var v2:Vector3 = new Vector3(); + + public var e0:Vector3 = new Vector3(); + public var e1:Vector3 = new Vector3(); + public var e2:Vector3 = new Vector3(); + +// public var len0:Number; +// public var len1:Number; +// public var len2:Number; +// + /** + * + * @param v0 + * @param v1 + * @param v2 + * @param collisionGroup + */ + public function CollisionTriangle(v0:Vector3, v1:Vector3, v2:Vector3, collisionGroup:int) { + super(TRIANGLE, collisionGroup); + initVertices(v0, v1, v2); + } + + /** + * Рассчитывает AABB примитива. + * + * @return ссылка на свой AABB + */ + override public function calculateAABB():BoundBox { + var epsilon:Number = 0.005; + var a:Number; + var b:Number; + var eps_c:Number = epsilon*transform.c; + var eps_g:Number = epsilon*transform.g; + var eps_k:Number = epsilon*transform.k; + + // Вершина 0 + // Ось X + a = v0.x*transform.a + v0.y*transform.b; + aabb.minX = aabb.maxX = a + eps_c; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v0.x*transform.e + v0.y*transform.f; + aabb.minY = aabb.maxY = a + eps_g; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v0.x*transform.i + v0.y*transform.j; + aabb.minZ = aabb.maxZ = a + eps_k; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 1 + // Ось X + a = v1.x*transform.a + v1.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v1.x*transform.e + v1.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v1.x*transform.i + v1.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 2 + // Ось X + a = v2.x*transform.a + v2.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v2.x*transform.e + v2.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v2.x*transform.i + v2.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + aabb.minX += transform.d; + aabb.maxX += transform.d; + + aabb.minY += transform.h; + aabb.maxY += transform.h; + + aabb.minZ += transform.l; + aabb.maxZ += transform.l; + + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + // Луч трансформируется в систему координат примитива, затем проверяется пересечение + + var vz:Number = vector.x*transform.c + vector.y*transform.g + vector.z*transform.k; + // Если луч параллелен плоскости птимитива, то пересечения нет + if (vz < epsilon && vz > -epsilon) return -1; + + var tx:Number = origin.x - transform.d; + var ty:Number = origin.y - transform.h; + var tz:Number = origin.z - transform.l; + + var oz:Number = tx*transform.c + ty*transform.g + tz*transform.k; + var t:Number = -oz/vz; + if (t < 0) return -1; + + var ox:Number = tx*transform.a + ty*transform.e + tz*transform.i; + var oy:Number = tx*transform.b + ty*transform.f + tz*transform.j; + + tx = ox + t*(vector.x*transform.a + vector.y*transform.e + vector.z*transform.i); + ty = oy + t*(vector.x*transform.b + vector.y*transform.f + vector.z*transform.j); + tz = oz + t*vz; + + // Проверка вхождения точки в треугольник + if ((e0.x*(ty - v0.y) - e0.y*(tx - v0.x) < 0) || (e1.x*(ty - v1.y) - e1.y*(tx - v1.x) < 0) || (e2.x*(ty - v2.y) - e2.y*(tx - v2.x) < 0)) return -1; + + // Запись нормали + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + + return t; + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + super.copyFrom(source); + var tri:CollisionTriangle = source as CollisionTriangle; + if (tri != null) { + v0.vCopy(tri.v0); + v1.vCopy(tri.v1); + v2.vCopy(tri.v2); + + e0.vCopy(tri.e0); + e1.vCopy(tri.e1); + e2.vCopy(tri.e2); + +// len0 = tri.len0; +// len1 = tri.len1; +// len2 = tri.len2; + } + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionTriangle v0=" + v0 + ", v1=" + v1 + ", v2=" + v2 + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionTriangle(v0, v1, v2, collisionGroup); + } + + /** + * + * @param v0 + * @param v1 + * @param v2 + */ + private function initVertices(v0:Vector3, v1:Vector3, v2:Vector3):void { + this.v0.vCopy(v0); + this.v1.vCopy(v1); + this.v2.vCopy(v2); + + e0.vDiff(v1, v0); +// len0 = e0.vLength(); + e0.vNormalize(); + + e1.vDiff(v2, v1); +// len1 = e1.vLength(); + e1.vNormalize(); + + e2.vDiff(v0, v2); +// len2 = e2.vLength(); + e2.vNormalize(); + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/types/.svn/all-wcprops b/0.0.5.Optimized/src/alternativa/physics/collision/types/.svn/all-wcprops new file mode 100644 index 0000000..a12bdd2 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/types/.svn/all-wcprops @@ -0,0 +1,23 @@ +K 25 +svn:wc:ra_dav:version-url +V 131 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/collision/types +END +BoundBox.as +K 25 +svn:wc:ra_dav:version-url +V 143 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/collision/types/BoundBox.as +END +RayIntersection.as +K 25 +svn:wc:ra_dav:version-url +V 150 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/collision/types/RayIntersection.as +END +Ray.as +K 25 +svn:wc:ra_dav:version-url +V 138 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/collision/types/Ray.as +END diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/types/.svn/entries b/0.0.5.Optimized/src/alternativa/physics/collision/types/.svn/entries new file mode 100644 index 0000000..f4dd164 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/types/.svn/entries @@ -0,0 +1,64 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/collision/types +http://svndev.alternativaplatform.com + + + +2009-09-09T14:02:48.134744Z +19633 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +BoundBox.as +file + + + + +2010-10-28T04:32:38.000000Z +d92d4ab3dde698feb674fb9488f59678 +2009-09-09T14:02:48.134744Z +19633 +mike + +RayIntersection.as +file + + + + +2010-10-28T04:32:38.000000Z +a0a4cf3dd94468fdf44cd06734927c94 +2009-06-08T06:46:18.310732Z +14099 +mike + +Ray.as +file + + + + +2010-10-28T04:32:38.000000Z +55eeb1cf79a416bb863bee95f089dbcd +2009-04-17T17:39:21.367569Z +11341 +mike + diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/types/.svn/format b/0.0.5.Optimized/src/alternativa/physics/collision/types/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/types/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base new file mode 100644 index 0000000..b4d5278 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base @@ -0,0 +1,75 @@ +package alternativa.physics.collision.types { + + /** + * Ограничивающий параллелепипед. + */ + public class BoundBox { + + public var minX:Number = 1e308; + public var minY:Number = 1e308; + public var minZ:Number = 1e308; + public var maxX:Number = -1e308; + public var maxY:Number = -1e308; + public var maxZ:Number = -1e308; + + public function setSize(minX:Number, minY:Number, minZ:Number, maxX:Number, maxY:Number, maxZ:Number):void { + this.minX = minX; + this.minY = minY; + this.minZ = minZ; + this.maxX = maxX; + this.maxY = maxY; + this.maxZ = maxZ; + } + + public function addBoundBox(boundBox:BoundBox):void { + minX = (boundBox.minX < minX) ? boundBox.minX : minX; + minY = (boundBox.minY < minY) ? boundBox.minY : minY; + minZ = (boundBox.minZ < minZ) ? boundBox.minZ : minZ; + maxX = (boundBox.maxX > maxX) ? boundBox.maxX : maxX; + maxY = (boundBox.maxY > maxY) ? boundBox.maxY : maxY; + maxZ = (boundBox.maxZ > maxZ) ? boundBox.maxZ : maxZ; + } + + public function addPoint(x:Number, y:Number, z:Number):void { + if (x < minX) minX = x; + if (x > maxX) maxX = x; + if (y < minY) minY = y; + if (y > maxY) maxY = y; + if (z < minZ) minZ = z; + if (z > maxZ) maxZ = z; + } + + public function infinity():void { + minX = 1e308; + minY = 1e308; + minZ = 1e308; + maxX = -1e308; + maxY = -1e308; + maxZ = -1e308; + } + + public function intersects(bb:BoundBox, epsilon:Number):Boolean { + return !(minX > bb.maxX + epsilon || maxX < bb.minX - epsilon || minY > bb.maxY + epsilon || maxY < bb.minY - epsilon || minZ > bb.maxZ + epsilon || maxZ < bb.minZ - epsilon); + } + + public function copyFrom(boundBox:BoundBox):void { + minX = boundBox.minX; + minY = boundBox.minY; + minZ = boundBox.minZ; + maxX = boundBox.maxX; + maxY = boundBox.maxY; + maxZ = boundBox.maxZ; + } + + public function clone():BoundBox { + var clone:BoundBox = new BoundBox(); + clone.copyFrom(this); + return clone; + } + + public function toString():String { + return "BoundBox [" + minX + ", " + minY + ", " + minZ + " : " + maxX + ", " + maxY + ", " + maxZ + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base new file mode 100644 index 0000000..f80620a --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base @@ -0,0 +1,13 @@ +package alternativa.physics.collision.types { + import alternativa.physics.types.Vector3; + + /** + * Структура, описывающая луч и сегмент прямой в пространстве. + */ + public class Ray { + // Начало луча. Начальная точка сегмента на прямой луча. + public var origin:Vector3 = new Vector3(); + // Направляющий вектор луча. Длина вектора задаёт длину сегмента на луче. Конечная точка сегмента вычисляется как origin + dir. + public var dir:Vector3 = new Vector3(); + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base new file mode 100644 index 0000000..8a89f75 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base @@ -0,0 +1,25 @@ +package alternativa.physics.collision.types { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.types.Vector3; + + /** + * Структура описывает точку пересечения луча с физической геометрией. + */ + public class RayIntersection { + // Физический примитив, с которым пересекается луч + public var primitive:CollisionPrimitive; + // Положенеи точки пересечения + public var pos:Vector3 = new Vector3(); + // Нормаль поверхности физического примитива в точке пересечения + public var normal:Vector3 = new Vector3(); + // Время пересечения (|pos - ray.origin|/|ray.dir|) + public var t:Number = 0; + + public function copy(source:RayIntersection):void { + primitive = source.primitive; + pos.vCopy(source.pos); + normal.vCopy(source.normal); + t = source.t; + } + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/types/BoundBox.as b/0.0.5.Optimized/src/alternativa/physics/collision/types/BoundBox.as new file mode 100644 index 0000000..b4d5278 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/types/BoundBox.as @@ -0,0 +1,75 @@ +package alternativa.physics.collision.types { + + /** + * Ограничивающий параллелепипед. + */ + public class BoundBox { + + public var minX:Number = 1e308; + public var minY:Number = 1e308; + public var minZ:Number = 1e308; + public var maxX:Number = -1e308; + public var maxY:Number = -1e308; + public var maxZ:Number = -1e308; + + public function setSize(minX:Number, minY:Number, minZ:Number, maxX:Number, maxY:Number, maxZ:Number):void { + this.minX = minX; + this.minY = minY; + this.minZ = minZ; + this.maxX = maxX; + this.maxY = maxY; + this.maxZ = maxZ; + } + + public function addBoundBox(boundBox:BoundBox):void { + minX = (boundBox.minX < minX) ? boundBox.minX : minX; + minY = (boundBox.minY < minY) ? boundBox.minY : minY; + minZ = (boundBox.minZ < minZ) ? boundBox.minZ : minZ; + maxX = (boundBox.maxX > maxX) ? boundBox.maxX : maxX; + maxY = (boundBox.maxY > maxY) ? boundBox.maxY : maxY; + maxZ = (boundBox.maxZ > maxZ) ? boundBox.maxZ : maxZ; + } + + public function addPoint(x:Number, y:Number, z:Number):void { + if (x < minX) minX = x; + if (x > maxX) maxX = x; + if (y < minY) minY = y; + if (y > maxY) maxY = y; + if (z < minZ) minZ = z; + if (z > maxZ) maxZ = z; + } + + public function infinity():void { + minX = 1e308; + minY = 1e308; + minZ = 1e308; + maxX = -1e308; + maxY = -1e308; + maxZ = -1e308; + } + + public function intersects(bb:BoundBox, epsilon:Number):Boolean { + return !(minX > bb.maxX + epsilon || maxX < bb.minX - epsilon || minY > bb.maxY + epsilon || maxY < bb.minY - epsilon || minZ > bb.maxZ + epsilon || maxZ < bb.minZ - epsilon); + } + + public function copyFrom(boundBox:BoundBox):void { + minX = boundBox.minX; + minY = boundBox.minY; + minZ = boundBox.minZ; + maxX = boundBox.maxX; + maxY = boundBox.maxY; + maxZ = boundBox.maxZ; + } + + public function clone():BoundBox { + var clone:BoundBox = new BoundBox(); + clone.copyFrom(this); + return clone; + } + + public function toString():String { + return "BoundBox [" + minX + ", " + minY + ", " + minZ + " : " + maxX + ", " + maxY + ", " + maxZ + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/types/Ray.as b/0.0.5.Optimized/src/alternativa/physics/collision/types/Ray.as new file mode 100644 index 0000000..f80620a --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/types/Ray.as @@ -0,0 +1,13 @@ +package alternativa.physics.collision.types { + import alternativa.physics.types.Vector3; + + /** + * Структура, описывающая луч и сегмент прямой в пространстве. + */ + public class Ray { + // Начало луча. Начальная точка сегмента на прямой луча. + public var origin:Vector3 = new Vector3(); + // Направляющий вектор луча. Длина вектора задаёт длину сегмента на луче. Конечная точка сегмента вычисляется как origin + dir. + public var dir:Vector3 = new Vector3(); + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/collision/types/RayIntersection.as b/0.0.5.Optimized/src/alternativa/physics/collision/types/RayIntersection.as new file mode 100644 index 0000000..8a89f75 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/collision/types/RayIntersection.as @@ -0,0 +1,25 @@ +package alternativa.physics.collision.types { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.types.Vector3; + + /** + * Структура описывает точку пересечения луча с физической геометрией. + */ + public class RayIntersection { + // Физический примитив, с которым пересекается луч + public var primitive:CollisionPrimitive; + // Положенеи точки пересечения + public var pos:Vector3 = new Vector3(); + // Нормаль поверхности физического примитива в точке пересечения + public var normal:Vector3 = new Vector3(); + // Время пересечения (|pos - ray.origin|/|ray.dir|) + public var t:Number = 0; + + public function copy(source:RayIntersection):void { + primitive = source.primitive; + pos.vCopy(source.pos); + normal.vCopy(source.normal); + t = source.t; + } + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/rigid/.svn/all-wcprops b/0.0.5.Optimized/src/alternativa/physics/rigid/.svn/all-wcprops new file mode 100644 index 0000000..2ce58bc --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/rigid/.svn/all-wcprops @@ -0,0 +1,47 @@ +K 25 +svn:wc:ra_dav:version-url +V 121 +/!svn/ver/19850/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/rigid +END +Body.as +K 25 +svn:wc:ra_dav:version-url +V 129 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/rigid/Body.as +END +BodyState.as +K 25 +svn:wc:ra_dav:version-url +V 134 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/rigid/BodyState.as +END +PhysicsUtils.as +K 25 +svn:wc:ra_dav:version-url +V 137 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/rigid/PhysicsUtils.as +END +ContactPoint.as +K 25 +svn:wc:ra_dav:version-url +V 137 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/rigid/ContactPoint.as +END +RigidWorld.as +K 25 +svn:wc:ra_dav:version-url +V 135 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/rigid/RigidWorld.as +END +Contact.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/rigid/Contact.as +END +BodyMaterial.as +K 25 +svn:wc:ra_dav:version-url +V 137 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/rigid/BodyMaterial.as +END diff --git a/0.0.5.Optimized/src/alternativa/physics/rigid/.svn/entries b/0.0.5.Optimized/src/alternativa/physics/rigid/.svn/entries new file mode 100644 index 0000000..840ed76 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/rigid/.svn/entries @@ -0,0 +1,118 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/rigid +http://svndev.alternativaplatform.com + + + +2009-09-14T12:08:58.450611Z +19850 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Body.as +file + + + + +2010-10-28T04:32:38.000000Z +23bad2eff16ce73dccb56a7a601cc3c0 +2009-06-29T14:07:24.833358Z +15390 +mike + +BodyState.as +file + + + + +2010-10-28T04:32:38.000000Z +8a089205beccac13a05eccfb9cfc6387 +2009-06-08T06:46:18.310732Z +14099 +mike + +PhysicsUtils.as +file + + + + +2010-10-28T04:32:38.000000Z +6d88004ddab2ae19e0d15f5b1b7c016a +2009-04-26T21:40:17.813142Z +12023 +mike + +constraints +dir + +ContactPoint.as +file + + + + +2010-10-28T04:32:38.000000Z +a94e73e3bfc85be2a79d6cc939512d91 +2009-06-08T06:46:18.310732Z +14099 +mike + +RigidWorld.as +file + + + + +2010-10-28T04:32:38.000000Z +b0b528a79a6fce55e5d800a15dc99534 +2009-07-03T12:54:42.818772Z +15744 +mike + +primitives +dir + +Contact.as +file + + + + +2010-10-28T04:32:38.000000Z +d65ad3d488d4a06addcbfa4aaf958f10 +2009-04-13T03:04:08.898183Z +11018 +mike + +BodyMaterial.as +file + + + + +2010-10-28T04:32:38.000000Z +49c7a55b5b26fdb03a60f24bac460f53 +2009-04-07T08:34:06.342994Z +10587 +mike + diff --git a/0.0.5.Optimized/src/alternativa/physics/rigid/.svn/format b/0.0.5.Optimized/src/alternativa/physics/rigid/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/rigid/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.5.Optimized/src/alternativa/physics/rigid/.svn/text-base/Body.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/rigid/.svn/text-base/Body.as.svn-base new file mode 100644 index 0000000..7c08ce9 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/rigid/.svn/text-base/Body.as.svn-base @@ -0,0 +1,340 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.IBodyCollisionPredicate; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Quaternion; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class Body { + + public static var linDamping:Number = 0.997; + public static var rotDamping:Number = 0.997; + + public var name:String; + public var movable:Boolean = true; + public var canFreeze:Boolean = false; + + public var freezeCounter:int; + public var frozen:Boolean = false; + public var aabb:BoundBox = new BoundBox(); + public var postCollisionPredicate:IBodyCollisionPredicate; + + altphysics var id:int; + // Мир, в котором находится тело + altphysics var world:RigidWorld; + // Текущее и предыдущее состояние тела. Промежуточное состояние вычисляется линейной интерполяцией. + altphysics var state:BodyState = new BodyState(); + altphysics var prevState:BodyState = new BodyState(); + // Линейное и угловое ускорение тела на текущем шаге симуляции + altphysics var accel:Vector3 = new Vector3(); + altphysics var angleAccel:Vector3 = new Vector3(); + // Материал тела + altphysics var material:BodyMaterial = new BodyMaterial(); + + altphysics var invMass:Number = 1; + altphysics var invInertia:Matrix3 = new Matrix3(); + altphysics var invInertiaWorld:Matrix3 = new Matrix3(); + altphysics var baseMatrix:Matrix3 = new Matrix3(); + + altphysics const MAX_CONTACTS:int = 20; + altphysics var contacts:Vector. = new Vector.(MAX_CONTACTS); + altphysics var contactsNum:int; + + altphysics var collisionPrimitives:Vector.; + altphysics var collisionPrimitivesNum:int; + + // Аккумулятор сил + altphysics var forceAccum:Vector3 = new Vector3(); + // Аккумулятор моментов + altphysics var torqueAccum:Vector3 = new Vector3(); + + // Внутренние переменные для избежания создания экземпляров + private static var _r:Vector3 = new Vector3(); + private static var _f:Vector3 = new Vector3(); + + /** + * + * @param invMass + * @param invInertia + */ + public function Body(invMass:Number, invInertia:Matrix3) { + this.invMass = invMass; + this.invInertia.copy(invInertia); + } + + /** + * @param primitive + * @param localTransform + */ + public function addCollisionPrimitive(primitive:CollisionPrimitive, localTransform:Matrix4 = null):void { + if (primitive == null) throw new ArgumentError("Primitive cannot be null"); + if (collisionPrimitives == null) collisionPrimitives = new Vector.(); + collisionPrimitives[collisionPrimitivesNum++] = primitive; + primitive.setBody(this, localTransform); + } + + /** + * @param t + * @param result + */ + public function interpolate(t:Number, pos:Vector3, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.w = prevState.orientation.w*t1 + state.orientation.w*t; + orientation.x = prevState.orientation.x*t1 + state.orientation.x*t; + orientation.y = prevState.orientation.y*t1 + state.orientation.y*t; + orientation.z = prevState.orientation.z*t1 + state.orientation.z*t; + } + + /** + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + state.pos.vCopy(pos); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setPositionXYZ(x:Number, y:Number, z:Number):void { + state.pos.vReset(x, y, z); + } + + /** + * + * @param vel + */ + public function setVelocity(vel:Vector3):void { + state.velocity.vCopy(vel); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setVelocityXYZ(x:Number, y:Number, z:Number):void { + state.velocity.vReset(x, y, z); + } + + /** + * + * @param rot + */ + public function setRotation(rot:Vector3):void { + state.rotation.vCopy(rot); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setRotationXYZ(x:Number, y:Number, z:Number):void { + state.rotation.vReset(x, y, z); + } + + /** + * + * @param q + */ + public function setOrientation(q:Quaternion):void { + state.orientation.copy(q); + } + + /** + * @param r + * @param dir + * @param magnitude + */ + public function applyRelPosWorldImpulse(r:Vector3, dir:Vector3, magnitude:Number):void { + var d:Number = magnitude*invMass; + // Линейная часть + state.velocity.x += d*dir.x; + state.velocity.y += d*dir.y; + state.velocity.z += d*dir.z; + + // Вращательная часть + var rx:Number = r.x; + var ry:Number = r.y; + var rz:Number = r.z; + _r.x = (ry*dir.z - rz*dir.y)*magnitude; + _r.y = (rz*dir.x - rx*dir.z)*magnitude; + _r.z = (rx*dir.y - ry*dir.x)*magnitude; + _r.vTransformBy3(invInertiaWorld); + state.rotation.x += _r.x; + state.rotation.y += _r.y; + state.rotation.z += _r.z; + } + + /** + * @param f + */ + public function addForce(f:Vector3):void { + forceAccum.vAdd(f); + } + + /** + * @param pos + * @param f + */ + public function addWorldForce(pos:Vector3, force:Vector3):void { + forceAccum.vAdd(force); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(force)); + } + + /** + * @param pos + * @param f + */ + public function addWorldForceScaled(pos:Vector3, force:Vector3, scale:Number):void { + _f.x = scale*force.x, _f.y = scale*force.y, _f.z = scale*force.z; + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(_f)); + } + + /** + * @param pos + * @param f + */ + public function addLocalForce(pos:Vector3, force:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(pos, _r); + // Трансформируем вектор силы в мировую систему + baseMatrix.transformVector(force, _f); + // Добавляем силу и момент + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vCross(_f)); + } + + /** + * + * @param localPos + * @param worldForce + */ + public function addWorldForceAtLocalPoint(localPos:Vector3, worldForce:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(localPos, _r); + // Добавляем силу и момент + forceAccum.vAdd(worldForce); + torqueAccum.vAdd(_r.vCross(worldForce)); + } + + /** + * @param dt + */ + public function addExternalForces(dt:Number):void { + } + + /** + * @param t + */ + public function addTorque(t:Vector3):void { + torqueAccum.vAdd(t); + } + + /** + * + */ + altphysics function clearAccumulators():void { + forceAccum.vReset(); + torqueAccum.vReset(); + } + + /** + * + */ + altphysics function calcAccelerations():void { + accel.x = forceAccum.x*invMass; + accel.y = forceAccum.y*invMass; + accel.z = forceAccum.z*invMass; + invInertiaWorld.transformVector(torqueAccum, angleAccel); + } + + /** + * Вычисляет производные данные. + */ + altphysics function calcDerivedData():void { + // Вычисление базисной матрицы и обратного тензора инерции в мировых координатах + state.orientation.toMatrix3(baseMatrix); + invInertiaWorld.copy(invInertia).append(baseMatrix).prependTransposed(baseMatrix); + if (collisionPrimitives != null) { + aabb.infinity(); + for (var i:int = 0; i < collisionPrimitivesNum; i++) { + var primitive:CollisionPrimitive = collisionPrimitives[i]; + primitive.transform.setFromMatrix3(baseMatrix, state.pos); + if (primitive.localTransform != null) primitive.transform.prepend(primitive.localTransform); + primitive.calculateAABB(); + aabb.addBoundBox(primitive.aabb); + } + } + } + + /** + * + */ + altphysics function saveState():void { + prevState.copy(state); + } + + /** + * + */ + altphysics function restoreState():void { + state.copy(prevState); + } + + /** + * @param dt + */ + altphysics function integrateVelocity(dt:Number):void { + // v = v + a*t + state.velocity.x += accel.x*dt; + state.velocity.y += accel.y*dt; + state.velocity.z += accel.z*dt; + // rot = rot + eps*t + state.rotation.x += angleAccel.x*dt; + state.rotation.y += angleAccel.y*dt; + state.rotation.z += angleAccel.z*dt; + + state.velocity.x *= linDamping; + state.velocity.y *= linDamping; + state.velocity.z *= linDamping; + + state.rotation.x *= rotDamping; + state.rotation.y *= rotDamping; + state.rotation.z *= rotDamping; + } + + /** + * + */ + altphysics function integratePosition(dt:Number):void { + // pos = pos + v*t + state.pos.x += state.velocity.x*dt; + state.pos.y += state.velocity.y*dt; + state.pos.z += state.velocity.z*dt; + // q = q + 0.5*rot*q + state.orientation.addScaledVector(state.rotation, dt); + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/rigid/.svn/text-base/BodyMaterial.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/rigid/.svn/text-base/BodyMaterial.as.svn-base new file mode 100644 index 0000000..61d4382 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/rigid/.svn/text-base/BodyMaterial.as.svn-base @@ -0,0 +1,11 @@ +package alternativa.physics.rigid { + + public class BodyMaterial { + + public var restitution:Number = 0; + public var friction:Number = 0.3; +// public var dynamicFriction:Number = 0.2; +// public var dynamicFriction:Number = 0.2; + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/rigid/.svn/text-base/BodyState.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/rigid/.svn/text-base/BodyState.as.svn-base new file mode 100644 index 0000000..408b0ea --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/rigid/.svn/text-base/BodyState.as.svn-base @@ -0,0 +1,39 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Quaternion; + import alternativa.physics.types.Vector3; + + /** + * Класс описывает состояние твёрдого тела. + */ + public class BodyState { + /** + * Положение тела. + */ + public var pos:Vector3 = new Vector3(); + /** + * Ориентация тела. + */ + public var orientation:Quaternion = new Quaternion(); + /** + * Скорость тела. + */ + public var velocity:Vector3 = new Vector3(); + /** + * Угловая скорость тела. + */ + public var rotation:Vector3 = new Vector3(); + + /** + * Копирует значение указанного объекта. + * + * @param state + */ + public function copy(state:BodyState):void { + pos.vCopy(state.pos); + orientation.copy(state.orientation); + velocity.vCopy(state.velocity); + rotation.vCopy(state.rotation); + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/rigid/.svn/text-base/Contact.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/rigid/.svn/text-base/Contact.as.svn-base new file mode 100644 index 0000000..b73fc55 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/rigid/.svn/text-base/Contact.as.svn-base @@ -0,0 +1,53 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.types.Vector3; + + /** + * Информация о контакте между двумя телами. Содержит нормаль, множество точек контакта и прочие величины. + */ + public class Contact { + +// private static var pool:Vector. = new Vector.(); +// +// public static function create():ContactInfo { +// if (pool.length > 0) { +// return pool.pop(); +// } +// return new ContactInfo(); +// } +// +// public static function destroy(collInfo:ContactInfo):void { +// collInfo.body1 = collInfo.body2 = null; +// pool.push(collInfo); +// } + + // Максимальное количество точек контакта + private const N:int = 8; + + public var body1:Body; + public var body2:Body; + // Взаимный коэффициент отскока + public var restitution:Number; + // Взаимный коэффициент трения + public var friction:Number; + // Нормаль контакта. Направлена от второго тела к первому. + public var normal:Vector3 = new Vector3(); + // Список точек контакта + public var points:Vector. = new Vector.(N, true); + // Количество точек контакта + public var pcount:int; + // Максимальная глубина пересечения тел + public var maxPenetration:Number = 0; + + public var satisfied:Boolean; + + /** + * + */ + public function Contact() { + for (var i:int = 0; i < N; i++) points[i] = new ContactPoint(); + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/rigid/.svn/text-base/ContactPoint.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/rigid/.svn/text-base/ContactPoint.as.svn-base new file mode 100644 index 0000000..c36f958 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/rigid/.svn/text-base/ContactPoint.as.svn-base @@ -0,0 +1,52 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Vector3; + + public class ContactPoint { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + + // Величины, расчитываемые перед началом фазы решения контактов + + // Требуемая проекция конечной скорости на нормаль для упругого контакта + public var normalVel:Number; + // Минимальная скорость разделения неупругого контакта + public var minSepVel:Number; + // Изменение проекции скорости на единицу нормального импульса + public var velByUnitImpulseN:Number; + + public var angularInertia1:Number; + public var angularInertia2:Number; + + // Радиус-вектор точки контакта относительно центра первого тела + public var r1:Vector3 = new Vector3(); + // Радиус-вектор точки контакта относительно центра второго тела + public var r2:Vector3 = new Vector3(); + + // Величины, накапливаемые во время фазы решения контактов + + // Накопленный импульс, применяемый для получения требуемой относительной скорости в точке контакта + public var accumImpulseN:Number; + // Накопленный импульс, применяемый для разделения тел в точке контакта. Не создаёт момента. +// public var accumSepImpulse:Number; + + public var satisfied:Boolean; + + /** + * + * @param cp + */ + public function copyFrom(cp:ContactPoint):void { + pos.vCopy(cp.pos); + penetration = cp.penetration; + feature1 = cp.feature1; + feature2 = cp.feature2; + r1.vCopy(cp.r1); + r2.vCopy(cp.r2); + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/rigid/.svn/text-base/PhysicsUtils.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/rigid/.svn/text-base/PhysicsUtils.as.svn-base new file mode 100644 index 0000000..a9aaa99 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/rigid/.svn/text-base/PhysicsUtils.as.svn-base @@ -0,0 +1,49 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + public class PhysicsUtils { + + /** + * @param mass + * @param halfSize + * @param result + */ + public static function getBoxInvInertia(mass:Number, halfSize:Vector3, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + result.a = 3/(mass*(yy + zz)); + result.f = 3/(mass*(zz + xx)); + result.k = 3/(mass*(xx + yy)); + } + + /** + * @param mass + * @param r + * @param h + * @param result + */ + public static function getCylinderInvInertia(mass:Number, r:Number, h:Number, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + + result.a = result.f = 1/(mass*(h*h/12 + r*r/4)); + result.k = 2/(mass*r*r); + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/rigid/.svn/text-base/RigidWorld.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/rigid/.svn/text-base/RigidWorld.as.svn-base new file mode 100644 index 0000000..696580b --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/rigid/.svn/text-base/RigidWorld.as.svn-base @@ -0,0 +1,653 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.ICollisionDetector; + import alternativa.physics.collision.KdTreeCollisionDetector; + import alternativa.physics.rigid.constraints.Constraint; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Класс реализует физическую симуляцию поведения твёрдых тел. + */ + public class RigidWorld { + + private static var lastBodyId:int; + + // Максимальное количество контактов + altphysics const MAX_CONTACTS:int = 1000; + + ////////////////////////////////////////////////////////////////// + // Настроечные параметры симуляции. Могут быть изменены в любой + // момент времени без нарушения корректной работы симуляции. + ////////////////////////////////////////////////////////////////// + // Количество шагов, за которое пересекающиеся тела должны разделиться + public var penResolutionSteps:int = 10; + // Величина допустимой глубины пересечения + public var allowedPenetration:Number = 0.1; + // Максимальная скорость, добавляемая с целью разделения тел + public var maxPenResolutionSpeed:Number = 0.5; + // Количество итераций для обработки упругих контактов + public var collisionIterations:int = 5; + // Количество итераций для обработки неупругих контактов + public var contactIterations:int = 5; + // Флаг использования предсказания состояний + public var usePrediction:Boolean = false; + + public var freezeSteps:int = 10; + public var linSpeedFreezeLimit:Number = 1; + public var angSpeedFreezeLimit:Number = 0.01; + + // Переменные для процедуры разделения тел путём непосредственного их перемещенеия. + // !!!!!!!!!!!!!!!!!!!!!!!! + // !!! Экспериментально !!! + // !!!!!!!!!!!!!!!!!!!!!!!! + public var staticSeparationIterations:int = 10; + public var staticSeparationSteps:int = 10; + public var maxAngleMove:Number = 10; + public var useStaticSeparation:Boolean = false; + + // Вектор гравитации + altphysics var _gravity:Vector3 = new Vector3(0, 0, -9.8); + // Использующийся детектор столкновений + altphysics var collisionDetector:ICollisionDetector; + // Список тел, участвующих в симуляции + altphysics var bodies:Vector. = new Vector.(); + // Количество тел, участвующих в симуляции + altphysics var bodiesNum:int; + // Список контактов на текущем шаге симуляции + altphysics var contacts:Vector. = new Vector.(MAX_CONTACTS, true); + // Количество контактов на текущем шаге симуляции + altphysics var contactsNum:int; + // Список ограничений + altphysics var constraints:Vector. = new Vector.(); + // Количество ограничений + altphysics var constraintsNum:int; + // Временная метка. Число прошедших шагов с начала симуляции. + altphysics var timeStamp:uint; + + // Временные переменные для избежания создания экземпляров + private var _r:Vector3 = new Vector3(); + private var _t:Vector3 = new Vector3(); + private var _v:Vector3 = new Vector3(); + private var _v1:Vector3 = new Vector3(); + private var _v2:Vector3 = new Vector3(); + + /** + * + */ + public function RigidWorld() { + for (var i:int = 0; i < MAX_CONTACTS; i++) contacts[i] = new Contact(); + collisionDetector = new KdTreeCollisionDetector(); + } + + /** + * Вектор гравитации. + */ + public function get gravity():Vector3 { + return _gravity.vClone(); + } + + /** + * @private + */ + public function set gravity(value:Vector3):void { + _gravity.vCopy(value); + } + + /** + * Добавляет тело в симуляцию. + * @param body + * @return + */ + public function addBody(body:Body, isStatic:Boolean):Boolean { + var i:int = bodies.indexOf(body); + if (i > -1) return false; + bodies[bodiesNum++] = body; + body.world = this; + body.id = lastBodyId++; +// if (body.collisionPrimitives != null) { +// for (i = 0; i < body.collisionPrimitivesNum; i++) collisionDetector.addPrimitive(body.collisionPrimitives[i], isStatic); +// } + return true; + } + + /** + * Удаляет тело из симуляции. + * @param body + * @return + */ + public function removeBody(body:Body, isStatic:Boolean):Boolean { + var i:int = bodies.indexOf(body); + if (i == -1) return false; + bodies.splice(i, 1); + body.world = null; +// if (body.collisionPrimitives != null) { +// for (i = 0; i < body.collisionPrimitivesNum; i++) collisionDetector.removePrimitive(body.collisionPrimitives[i], isStatic); +// } + bodiesNum--; + return true; + } + + /** + * Добавляет ограничение. + * @param c + */ + public function addConstraint(c:Constraint):void { + constraints[constraintsNum++] = c; + c.world = this; + } + + /** + * Удаляет ограничение. + * @param c + */ + public function removeConstraint(c:Constraint):Boolean { + var idx:int = constraints.indexOf(c); + if (idx < 0) return false; + constraints.splice(idx, 1) + constraintsNum--; + c.world = null; + return true; + } + + /** + * Применяет к телам действующте на них силы. Аккумуляторы сил и моментов тел очищаются после завершения шага + * симуляции, поэтому на момент вызова метода могут уже содержать некоторые значения. + * + * @param dt промежуток времени, в течении которого действуют силы + */ + private function applyForces(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i]; + body.addExternalForces(dt); + body.calcAccelerations(); + // Ускорение свободного падения применяется только к подвижным телам во избежание некорректного изменения + // фиктивной скорости неподвижных тел. + if (body.movable && !body.frozen) { + body.accel.x += _gravity.x; + body.accel.y += _gravity.y; + body.accel.z += _gravity.z; + } + } + } + + /** + * Определяет все столкновения на текущем шаге симуляции и заполняет список получившихся контактов. + * + * @param dt длительность шага симуляции + */ + private function detectCollisions(dt:Number):void { + var i:int; + var j:int; + var body:Body; + + for (i = 0; i < bodiesNum; i++) { + body = bodies[i]; + if (body.frozen) continue; + body.contactsNum = 0; + body.saveState(); + // При включённом режиме предсказания состояние тел интегрируется на один шаг вперёд + if (usePrediction) { + body.integrateVelocity(dt); + body.integratePosition(dt); + } + body.calcDerivedData(); + } + + contactsNum = collisionDetector.getAllCollisions(contacts); + + // Расчёт относительных векторов точки контакта вынесен сюда из-за необходимости учитывать + // положение тел в предсказанном состоянии + for (i = 0; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + var b1:Body = contact.body1; + var b2:Body = contact.body2; + for (j = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.r1.x = cp.pos.x - b1.state.pos.x; + cp.r1.y = cp.pos.y - b1.state.pos.y; + cp.r1.z = cp.pos.z - b1.state.pos.z; + if (b2 != null) { + cp.r2.x = cp.pos.x - b2.state.pos.x; + cp.r2.y = cp.pos.y - b2.state.pos.y; + cp.r2.z = cp.pos.z - b2.state.pos.z; + } + } + } + + // Восстановление состояния тел + if (usePrediction) { + for (i = 0; i < bodiesNum; i++) { + body = bodies[i]; + if (body.frozen) continue; + body.restoreState(); + body.calcDerivedData(); + } + } + } + + /** + * Подготваливает полученные из детектора столкновений контакты, расчитывая значения, не меняющиеся + * в ходе шага симуляции. + */ + private function preProcessContacts(dt:Number):void { + var i:int; + for (i = 0; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + var b1:Body = contact.body1; + var b2:Body = contact.body2; + // Столкнувшиеся тела размораживаются + if (b1.frozen) { + b1.frozen = false; + b1.freezeCounter = 0; + } + if (b2 != null && b2.frozen) { + b2.frozen = false; + b2.freezeCounter = 0; + } + contact.restitution = b1.material.restitution; + if (b2 != null && b2.material.restitution < contact.restitution) contact.restitution = b2.material.restitution; + contact.friction = b1.material.friction; + if (b2 != null && b2.material.friction < contact.friction) contact.friction = b2.material.friction; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.accumImpulseN = 0; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + cp.velByUnitImpulseN = 0; + if (b1.movable) { + cp.angularInertia1 = _v.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1).vDot(contact.normal); + cp.velByUnitImpulseN += b1.invMass + cp.angularInertia1; + } + if (b2 != null && b2.movable) { + cp.angularInertia2 = _v.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2).vDot(contact.normal); + cp.velByUnitImpulseN += b2.invMass + cp.angularInertia2; + } + // Расчёт требуемой конечной скорости для упругого контакта + calcSepVelocity(b1, b2, cp, _v); + cp.normalVel = _v.vDot(contact.normal); + if (cp.normalVel < 0) cp.normalVel = - contact.restitution*cp.normalVel; + // Скорость разделения неупругого контакта + cp.minSepVel = cp.penetration > allowedPenetration ? (cp.penetration - allowedPenetration)/(penResolutionSteps*dt) : 0; + if (cp.minSepVel > maxPenResolutionSpeed) cp.minSepVel = maxPenResolutionSpeed; + } + } + for (i = 0; i < constraintsNum; i++) Constraint(constraints[i]).preProcess(dt); + } + + /** + * + * @param dt + * @param forceInelastic + */ + private function processContacts(dt:Number, forceInelastic:Boolean):void { + var iterNum:int = forceInelastic ? contactIterations : collisionIterations; + var i:int; + var forwardLoop:Boolean = false; + for (var iter:int = 0; iter < iterNum; iter++) { + forwardLoop = !forwardLoop; + for (i = 0; i < contactsNum; i++) resolveContact(contacts[i], forceInelastic, forwardLoop); + // Ограничения + for (i = 0; i < constraintsNum; i++) Constraint(constraints[i]).apply(dt); + } + + // Разделение контактов путём непосредственного изменения координат и ориентации. Экспериментально. +// if (forceInelastic && useStaticSeparation) performStaticSeparation(); + } + + /** + * + */ + private function resolveContact(contactInfo:Contact, forceInelastic:Boolean, forwardLoop:Boolean):void { + var b1:Body = contactInfo.body1; + var b2:Body = contactInfo.body2; + var normal:Vector3 = contactInfo.normal; + var i:int; + if (forwardLoop) { + for (i = 0; i < contactInfo.pcount; i++) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } else { + for (i = contactInfo.pcount - 1; i >= 0; i--) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } + } + + /** + * + * @param idx + * @param b1 + * @param b2 + * @param colInfo + * @param normal + * @param forceInelastic + */ + private function resolveContactPoint(idx:int, b1:Body, b2:Body, contact:Contact, normal:Vector3, forceInelastic:Boolean):void { + var cp:ContactPoint = contact.points[idx]; + if (!forceInelastic) cp.satisfied = true; + + var newVel:Number = 0; + calcSepVelocity(b1, b2, cp, _v); + var sepVel:Number = _v.vDot(contact.normal); + if (forceInelastic) { + var minSpeVel:Number = useStaticSeparation ? 0 : cp.minSepVel; + if (sepVel < minSpeVel) cp.satisfied = false; + else if (cp.satisfied) return; + newVel = minSpeVel; + } else { + newVel = cp.normalVel; + } + var deltaVel:Number = newVel - sepVel; + var impulse:Number = deltaVel/cp.velByUnitImpulseN; + var accumImpulse:Number = cp.accumImpulseN + impulse; + if (accumImpulse < 0) accumImpulse = 0; + var deltaImpulse:Number = accumImpulse - cp.accumImpulseN; + cp.accumImpulseN = accumImpulse; + // Применяем импульс к телам + if (b1.movable) b1.applyRelPosWorldImpulse(cp.r1, normal, deltaImpulse); + if (b2 != null && b2.movable) b2.applyRelPosWorldImpulse(cp.r2, normal, -deltaImpulse); + + // Учёт силы трения + calcSepVelocity(b1, b2, cp, _v); + // Расчитываем изменение касательной скорости на единицу касательного импульса + var tanSpeedByUnitImpulse:Number = 0; + _v.vAddScaled(-_v.vDot(contact.normal), contact.normal); + var tanSpeed:Number = _v.vLength(); + if (tanSpeed < 0.001) return; + _t.vCopy(_v).vNormalize().vReverse(); + // dV = b.invMass + ((invI * (r % t)) % r) * t + if (b1.movable) { + _v.vCross2(cp.r1, _t).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1); + tanSpeedByUnitImpulse += b1.invMass + _v.vDot(_t); + } + if (b2 != null && b2.movable) { + _v.vCross2(cp.r2, _t).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2); + tanSpeedByUnitImpulse += b2.invMass + _v.vDot(_t); + } + + var tanImpulse:Number = tanSpeed/tanSpeedByUnitImpulse; + var max:Number = contact.friction*cp.accumImpulseN; + if (max < 0) { + if (tanImpulse < max) tanImpulse = max; + } else { + if (tanImpulse > max) tanImpulse = max; + } + + // Применяем импульс к телам + if (b1.movable) b1.applyRelPosWorldImpulse(cp.r1, _t, tanImpulse); + if (b2 != null && b2.movable) b2.applyRelPosWorldImpulse(cp.r2, _t, -tanImpulse); + } + + /** + * + * @param cp + * @param normal + * @return + */ + private function calcSepVelocity(body1:Body, body2:Body, cp:ContactPoint, result:Vector3):void { + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 +// result.vCopy(body1.state.velocity).vAdd(_v1.vCross2(body1.state.rotation, cp.r1)); + var rot:Vector3 = body1.state.rotation; + var v:Vector3 = cp.r1; + var x:Number = rot.y*v.z - rot.z*v.y; + var y:Number = rot.z*v.x - rot.x*v.z; + var z:Number = rot.x*v.y - rot.y*v.x; + v = body1.state.velocity; + result.x = v.x + x; + result.y = v.y + y; + result.z = v.z + z; + // V2 = V2_c + w2%r2 + if (body2 != null) { +// result.vSubtract(body2.state.velocity).vSubtract(_v2.vCross2(body2.state.rotation, cp.r2)); + rot = body2.state.rotation; + v = cp.r2; + x = rot.y*v.z - rot.z*v.y; + y = rot.z*v.x - rot.x*v.z; + z = rot.x*v.y - rot.y*v.x; + v = body2.state.velocity; + result.x -= v.x + x; + result.y -= v.y + y; + result.z -= v.z + z; + } + } + + /** + * + * @param dt + */ + private function intergateVelocities(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + (bodies[i] as Body).integrateVelocity(dt); + } + } + + /** + * + * @param dt + */ + private function integratePositions(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i] as Body; + if (body.movable && !body.frozen) body.integratePosition(dt); + } + } + + /** + * + */ + private function performStaticSeparation():void { + var iterNum:int = staticSeparationIterations; +// iterNum = 100; + + // 1. В начале каждой итерации для всех контактов сбрасывается флаг satisfied с одновременным поиском наихудшего контакта, т.е. имеющего наибольшую величину пересечения тел. + // 2. Если найденный контакт имеет величину пересечения меньше предельно допустимой процедура прерывается, т.к. разделение контактов не требуется. + // 3. Если процедура продолжается, то выполняется разделение найденного контакта и установка его флага satisfied в true, после чего запускается внутренний цикл, состоящий + // из contactsNum - 1 итераций. На каждой итерации ищется наихудший контакт среди оставшихся (satisfied == false). Если такой контакт найден, то выполняется его разделение + // и переход к следующей итерации внутреннего цикла, иначе внутренний цикл прерывается и выполняется переход к следующей итерации внешнего цикла (шаг 1.). + for (var iter:int = 0; iter < iterNum; iter++) { + // Ищем контакт с максимальной величиной пересечения тел, одновременно сбрасывая флаги satisfied в false + var worstContact:Contact = contacts[0]; + var i:int; + for (i = 1; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + contact.satisfied = false; + if (contact.maxPenetration > worstContact.maxPenetration) worstContact = contact; + } + if (worstContact.maxPenetration <= allowedPenetration) return; + + resolveInterpenetration(worstContact); + // Внутренний цикл по оставшимся контактам + for (i = 1; i < contactsNum; i++) { + worstContact = getWorstContact(); + if (worstContact == null) break; + resolveInterpenetration(worstContact); + } + } + } + + /** + * + */ + private function getWorstContact():Contact { + var maxPen:Number = 0; + var worst:Contact = null; + for (var i:int = 0; i < contactsNum; i++) { + var c:Contact = contacts[i]; + if (!c.satisfied && c.maxPenetration > maxPen) { + worst = c; + maxPen = c.maxPenetration; + } + } + return maxPen > allowedPenetration ? worst : null; + } + + /** + * Разделяет указанный контакт, выполняя staticSeparationSteps итераций по списку точек контакта. + * 1. В начале каждой итерации ищется точка с наибольшим пересечением, одновременно сбрасываются флаги satisfied у точек. + * 2. Для найденной точки выполняется процедура разделения. + * 3. После выполняется pcount - 1 итерация по списку точек, каждый раз ищется наихудшая среди имеющих satisfied == false, для которой выполняется процедура разделения. + * Если наихудшая точка не найдена, то выполнется переход к следующей итерации внешнего цикла (шаг 1). + * + * В конце процедуры разделения обновляются значения пересечений для остальных точек контакта, а также для всех прочих контактов, относящихся к телам текущего. + * + * @param contact контакт для разделения + */ + private function resolveInterpenetration(contact:Contact):void { + contact.satisfied = true; + + for (var step:int = 0; step < staticSeparationSteps; step++) { + var worstCp:ContactPoint = contact.points[0]; + var cp:ContactPoint; + var i:int; + for (i = 1; i < contact.pcount; i++) { + cp = contact.points[i]; + cp.satisfied = false; + if (cp.penetration > worstCp.penetration) worstCp = cp; + } + if (worstCp.penetration <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + // Разделяем оставшиеся точки + var maxPen:Number = 0; + for (i = 1; i < contact.pcount; i++) { + // Поиск наихудшей точки + for (var j:int = 0; j < contact.pcount; j++) { + cp = contact.points[j]; + if (cp.satisfied) continue; + if (cp.penetration > maxPen) { + maxPen = cp.penetration; + worstCp = cp; + } + } + if (maxPen <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + } + } + } + + /** + * + * @param cp + * @param contact + */ + private function separateContactPoint(cp:ContactPoint, contact:Contact):void { + cp.satisfied = true; + + var b1:Body = contact.body1; + var b2:Body = contact.body2; + var totalMove:Number = cp.penetration - allowedPenetration; + var moveCoeff:Number = totalMove/cp.velByUnitImpulseN; + var linMove1:Number; + var angleMove1:Number; + if (b1.movable) { + linMove1 = b1.invMass*moveCoeff; + angleMove1 = cp.angularInertia1*moveCoeff; + if (angleMove1 > maxAngleMove) { + linMove1 += angleMove1 - maxAngleMove; + angleMove1 -= maxAngleMove; + } + b1.state.pos.vAddScaled(linMove1, contact.normal); + _v1.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vScale(angleMove1); + b1.state.orientation.addScaledVector(_v1, 1); + } + var linMove2:Number; + var angleMove2:Number; + if (b2 != null && b2.movable) { + linMove2 = b2.invMass*moveCoeff; + angleMove2 = cp.angularInertia2*moveCoeff; + if (angleMove2 > maxAngleMove) { + linMove2 += angleMove2 - maxAngleMove; + angleMove2 -= maxAngleMove; + } + b2.state.pos.vAddScaled(-linMove2, contact.normal); + _v2.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vScale(angleMove2); + _v2.vReverse(); + b2.state.orientation.addScaledVector(_v2, 1); + } + cp.penetration = allowedPenetration; + // Обновляем пересечения в других точках + var i:int; + for (i = 0; i < contact.pcount; i++) { + var cp1:ContactPoint = contact.points[i]; + if (cp1 == cp) continue; + var angularMove:Number; + if (b1.movable) { + angularMove = _v.vCross2(_v1, cp1.r1).vDot(contact.normal); + cp1.penetration -= linMove1 + angularMove; + } + if (b2 != null && b2.movable) { + angularMove = _v.vCross2(_v2, cp1.r2).vDot(contact.normal); + cp1.penetration -= linMove2 - angularMove; + } + // Обновление максимального значения для контакта + if (cp1.penetration > contact.maxPenetration) contact.maxPenetration = cp1.penetration; + } + // Обновляем пересечения для других контактов + var c:Contact; + var j:int; + if (b1.movable) { + for (i = 0; i < b1.contactsNum; i++) { + c = b1.contacts[i]; + if (c == contact) continue; + for (j = 0; j < c.pcount; j++) { + cp1 = c.points[j]; + if (b1 == c.body1) cp1.penetration -= linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r1).vDot(c.normal); + else cp1.penetration += linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r2).vDot(c.normal); + if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; + } + } + } + if (b2 != null && b2.movable) { + for (i = 0; i < b2.contactsNum; i++) { + c = b2.contacts[i]; + if (c == contact) continue; + for (j = 0; j < c.pcount; j++) { + cp1 = c.points[j]; + if (b2 == c.body1) cp1.penetration -= linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r1).vDot(c.normal); + else cp1.penetration += linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r2).vDot(c.normal); + if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; + } + } + } + } + + /** + * + */ + private function postPhysics():void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i]; + body.clearAccumulators(); + body.calcDerivedData(); + if (body.canFreeze) { + if (body.state.velocity.vLength() < linSpeedFreezeLimit && body.state.rotation.vLength() < angSpeedFreezeLimit) { + if (!body.frozen) { + body.freezeCounter++; + if (body.freezeCounter >= freezeSteps) body.frozen = true; + } + } else { + body.freezeCounter = 0; + body.frozen = false; + } + } + } + } + + /** + * + * @param dt + */ + public function runPhysics(dt:Number):void { + timeStamp++; + applyForces(dt); + detectCollisions(dt); + preProcessContacts(dt); + processContacts(dt, false); + intergateVelocities(dt); + processContacts(dt, true); + integratePositions(dt); + postPhysics(); + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/rigid/Body.as b/0.0.5.Optimized/src/alternativa/physics/rigid/Body.as new file mode 100644 index 0000000..7c08ce9 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/rigid/Body.as @@ -0,0 +1,340 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.IBodyCollisionPredicate; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Quaternion; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class Body { + + public static var linDamping:Number = 0.997; + public static var rotDamping:Number = 0.997; + + public var name:String; + public var movable:Boolean = true; + public var canFreeze:Boolean = false; + + public var freezeCounter:int; + public var frozen:Boolean = false; + public var aabb:BoundBox = new BoundBox(); + public var postCollisionPredicate:IBodyCollisionPredicate; + + altphysics var id:int; + // Мир, в котором находится тело + altphysics var world:RigidWorld; + // Текущее и предыдущее состояние тела. Промежуточное состояние вычисляется линейной интерполяцией. + altphysics var state:BodyState = new BodyState(); + altphysics var prevState:BodyState = new BodyState(); + // Линейное и угловое ускорение тела на текущем шаге симуляции + altphysics var accel:Vector3 = new Vector3(); + altphysics var angleAccel:Vector3 = new Vector3(); + // Материал тела + altphysics var material:BodyMaterial = new BodyMaterial(); + + altphysics var invMass:Number = 1; + altphysics var invInertia:Matrix3 = new Matrix3(); + altphysics var invInertiaWorld:Matrix3 = new Matrix3(); + altphysics var baseMatrix:Matrix3 = new Matrix3(); + + altphysics const MAX_CONTACTS:int = 20; + altphysics var contacts:Vector. = new Vector.(MAX_CONTACTS); + altphysics var contactsNum:int; + + altphysics var collisionPrimitives:Vector.; + altphysics var collisionPrimitivesNum:int; + + // Аккумулятор сил + altphysics var forceAccum:Vector3 = new Vector3(); + // Аккумулятор моментов + altphysics var torqueAccum:Vector3 = new Vector3(); + + // Внутренние переменные для избежания создания экземпляров + private static var _r:Vector3 = new Vector3(); + private static var _f:Vector3 = new Vector3(); + + /** + * + * @param invMass + * @param invInertia + */ + public function Body(invMass:Number, invInertia:Matrix3) { + this.invMass = invMass; + this.invInertia.copy(invInertia); + } + + /** + * @param primitive + * @param localTransform + */ + public function addCollisionPrimitive(primitive:CollisionPrimitive, localTransform:Matrix4 = null):void { + if (primitive == null) throw new ArgumentError("Primitive cannot be null"); + if (collisionPrimitives == null) collisionPrimitives = new Vector.(); + collisionPrimitives[collisionPrimitivesNum++] = primitive; + primitive.setBody(this, localTransform); + } + + /** + * @param t + * @param result + */ + public function interpolate(t:Number, pos:Vector3, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.w = prevState.orientation.w*t1 + state.orientation.w*t; + orientation.x = prevState.orientation.x*t1 + state.orientation.x*t; + orientation.y = prevState.orientation.y*t1 + state.orientation.y*t; + orientation.z = prevState.orientation.z*t1 + state.orientation.z*t; + } + + /** + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + state.pos.vCopy(pos); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setPositionXYZ(x:Number, y:Number, z:Number):void { + state.pos.vReset(x, y, z); + } + + /** + * + * @param vel + */ + public function setVelocity(vel:Vector3):void { + state.velocity.vCopy(vel); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setVelocityXYZ(x:Number, y:Number, z:Number):void { + state.velocity.vReset(x, y, z); + } + + /** + * + * @param rot + */ + public function setRotation(rot:Vector3):void { + state.rotation.vCopy(rot); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setRotationXYZ(x:Number, y:Number, z:Number):void { + state.rotation.vReset(x, y, z); + } + + /** + * + * @param q + */ + public function setOrientation(q:Quaternion):void { + state.orientation.copy(q); + } + + /** + * @param r + * @param dir + * @param magnitude + */ + public function applyRelPosWorldImpulse(r:Vector3, dir:Vector3, magnitude:Number):void { + var d:Number = magnitude*invMass; + // Линейная часть + state.velocity.x += d*dir.x; + state.velocity.y += d*dir.y; + state.velocity.z += d*dir.z; + + // Вращательная часть + var rx:Number = r.x; + var ry:Number = r.y; + var rz:Number = r.z; + _r.x = (ry*dir.z - rz*dir.y)*magnitude; + _r.y = (rz*dir.x - rx*dir.z)*magnitude; + _r.z = (rx*dir.y - ry*dir.x)*magnitude; + _r.vTransformBy3(invInertiaWorld); + state.rotation.x += _r.x; + state.rotation.y += _r.y; + state.rotation.z += _r.z; + } + + /** + * @param f + */ + public function addForce(f:Vector3):void { + forceAccum.vAdd(f); + } + + /** + * @param pos + * @param f + */ + public function addWorldForce(pos:Vector3, force:Vector3):void { + forceAccum.vAdd(force); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(force)); + } + + /** + * @param pos + * @param f + */ + public function addWorldForceScaled(pos:Vector3, force:Vector3, scale:Number):void { + _f.x = scale*force.x, _f.y = scale*force.y, _f.z = scale*force.z; + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(_f)); + } + + /** + * @param pos + * @param f + */ + public function addLocalForce(pos:Vector3, force:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(pos, _r); + // Трансформируем вектор силы в мировую систему + baseMatrix.transformVector(force, _f); + // Добавляем силу и момент + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vCross(_f)); + } + + /** + * + * @param localPos + * @param worldForce + */ + public function addWorldForceAtLocalPoint(localPos:Vector3, worldForce:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(localPos, _r); + // Добавляем силу и момент + forceAccum.vAdd(worldForce); + torqueAccum.vAdd(_r.vCross(worldForce)); + } + + /** + * @param dt + */ + public function addExternalForces(dt:Number):void { + } + + /** + * @param t + */ + public function addTorque(t:Vector3):void { + torqueAccum.vAdd(t); + } + + /** + * + */ + altphysics function clearAccumulators():void { + forceAccum.vReset(); + torqueAccum.vReset(); + } + + /** + * + */ + altphysics function calcAccelerations():void { + accel.x = forceAccum.x*invMass; + accel.y = forceAccum.y*invMass; + accel.z = forceAccum.z*invMass; + invInertiaWorld.transformVector(torqueAccum, angleAccel); + } + + /** + * Вычисляет производные данные. + */ + altphysics function calcDerivedData():void { + // Вычисление базисной матрицы и обратного тензора инерции в мировых координатах + state.orientation.toMatrix3(baseMatrix); + invInertiaWorld.copy(invInertia).append(baseMatrix).prependTransposed(baseMatrix); + if (collisionPrimitives != null) { + aabb.infinity(); + for (var i:int = 0; i < collisionPrimitivesNum; i++) { + var primitive:CollisionPrimitive = collisionPrimitives[i]; + primitive.transform.setFromMatrix3(baseMatrix, state.pos); + if (primitive.localTransform != null) primitive.transform.prepend(primitive.localTransform); + primitive.calculateAABB(); + aabb.addBoundBox(primitive.aabb); + } + } + } + + /** + * + */ + altphysics function saveState():void { + prevState.copy(state); + } + + /** + * + */ + altphysics function restoreState():void { + state.copy(prevState); + } + + /** + * @param dt + */ + altphysics function integrateVelocity(dt:Number):void { + // v = v + a*t + state.velocity.x += accel.x*dt; + state.velocity.y += accel.y*dt; + state.velocity.z += accel.z*dt; + // rot = rot + eps*t + state.rotation.x += angleAccel.x*dt; + state.rotation.y += angleAccel.y*dt; + state.rotation.z += angleAccel.z*dt; + + state.velocity.x *= linDamping; + state.velocity.y *= linDamping; + state.velocity.z *= linDamping; + + state.rotation.x *= rotDamping; + state.rotation.y *= rotDamping; + state.rotation.z *= rotDamping; + } + + /** + * + */ + altphysics function integratePosition(dt:Number):void { + // pos = pos + v*t + state.pos.x += state.velocity.x*dt; + state.pos.y += state.velocity.y*dt; + state.pos.z += state.velocity.z*dt; + // q = q + 0.5*rot*q + state.orientation.addScaledVector(state.rotation, dt); + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/rigid/BodyMaterial.as b/0.0.5.Optimized/src/alternativa/physics/rigid/BodyMaterial.as new file mode 100644 index 0000000..61d4382 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/rigid/BodyMaterial.as @@ -0,0 +1,11 @@ +package alternativa.physics.rigid { + + public class BodyMaterial { + + public var restitution:Number = 0; + public var friction:Number = 0.3; +// public var dynamicFriction:Number = 0.2; +// public var dynamicFriction:Number = 0.2; + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/rigid/BodyState.as b/0.0.5.Optimized/src/alternativa/physics/rigid/BodyState.as new file mode 100644 index 0000000..408b0ea --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/rigid/BodyState.as @@ -0,0 +1,39 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Quaternion; + import alternativa.physics.types.Vector3; + + /** + * Класс описывает состояние твёрдого тела. + */ + public class BodyState { + /** + * Положение тела. + */ + public var pos:Vector3 = new Vector3(); + /** + * Ориентация тела. + */ + public var orientation:Quaternion = new Quaternion(); + /** + * Скорость тела. + */ + public var velocity:Vector3 = new Vector3(); + /** + * Угловая скорость тела. + */ + public var rotation:Vector3 = new Vector3(); + + /** + * Копирует значение указанного объекта. + * + * @param state + */ + public function copy(state:BodyState):void { + pos.vCopy(state.pos); + orientation.copy(state.orientation); + velocity.vCopy(state.velocity); + rotation.vCopy(state.rotation); + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/rigid/Contact.as b/0.0.5.Optimized/src/alternativa/physics/rigid/Contact.as new file mode 100644 index 0000000..b73fc55 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/rigid/Contact.as @@ -0,0 +1,53 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.types.Vector3; + + /** + * Информация о контакте между двумя телами. Содержит нормаль, множество точек контакта и прочие величины. + */ + public class Contact { + +// private static var pool:Vector. = new Vector.(); +// +// public static function create():ContactInfo { +// if (pool.length > 0) { +// return pool.pop(); +// } +// return new ContactInfo(); +// } +// +// public static function destroy(collInfo:ContactInfo):void { +// collInfo.body1 = collInfo.body2 = null; +// pool.push(collInfo); +// } + + // Максимальное количество точек контакта + private const N:int = 8; + + public var body1:Body; + public var body2:Body; + // Взаимный коэффициент отскока + public var restitution:Number; + // Взаимный коэффициент трения + public var friction:Number; + // Нормаль контакта. Направлена от второго тела к первому. + public var normal:Vector3 = new Vector3(); + // Список точек контакта + public var points:Vector. = new Vector.(N, true); + // Количество точек контакта + public var pcount:int; + // Максимальная глубина пересечения тел + public var maxPenetration:Number = 0; + + public var satisfied:Boolean; + + /** + * + */ + public function Contact() { + for (var i:int = 0; i < N; i++) points[i] = new ContactPoint(); + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/rigid/ContactPoint.as b/0.0.5.Optimized/src/alternativa/physics/rigid/ContactPoint.as new file mode 100644 index 0000000..c36f958 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/rigid/ContactPoint.as @@ -0,0 +1,52 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Vector3; + + public class ContactPoint { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + + // Величины, расчитываемые перед началом фазы решения контактов + + // Требуемая проекция конечной скорости на нормаль для упругого контакта + public var normalVel:Number; + // Минимальная скорость разделения неупругого контакта + public var minSepVel:Number; + // Изменение проекции скорости на единицу нормального импульса + public var velByUnitImpulseN:Number; + + public var angularInertia1:Number; + public var angularInertia2:Number; + + // Радиус-вектор точки контакта относительно центра первого тела + public var r1:Vector3 = new Vector3(); + // Радиус-вектор точки контакта относительно центра второго тела + public var r2:Vector3 = new Vector3(); + + // Величины, накапливаемые во время фазы решения контактов + + // Накопленный импульс, применяемый для получения требуемой относительной скорости в точке контакта + public var accumImpulseN:Number; + // Накопленный импульс, применяемый для разделения тел в точке контакта. Не создаёт момента. +// public var accumSepImpulse:Number; + + public var satisfied:Boolean; + + /** + * + * @param cp + */ + public function copyFrom(cp:ContactPoint):void { + pos.vCopy(cp.pos); + penetration = cp.penetration; + feature1 = cp.feature1; + feature2 = cp.feature2; + r1.vCopy(cp.r1); + r2.vCopy(cp.r2); + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/rigid/PhysicsUtils.as b/0.0.5.Optimized/src/alternativa/physics/rigid/PhysicsUtils.as new file mode 100644 index 0000000..a9aaa99 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/rigid/PhysicsUtils.as @@ -0,0 +1,49 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + public class PhysicsUtils { + + /** + * @param mass + * @param halfSize + * @param result + */ + public static function getBoxInvInertia(mass:Number, halfSize:Vector3, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + result.a = 3/(mass*(yy + zz)); + result.f = 3/(mass*(zz + xx)); + result.k = 3/(mass*(xx + yy)); + } + + /** + * @param mass + * @param r + * @param h + * @param result + */ + public static function getCylinderInvInertia(mass:Number, r:Number, h:Number, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + + result.a = result.f = 1/(mass*(h*h/12 + r*r/4)); + result.k = 2/(mass*r*r); + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/rigid/RigidWorld.as b/0.0.5.Optimized/src/alternativa/physics/rigid/RigidWorld.as new file mode 100644 index 0000000..696580b --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/rigid/RigidWorld.as @@ -0,0 +1,653 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.ICollisionDetector; + import alternativa.physics.collision.KdTreeCollisionDetector; + import alternativa.physics.rigid.constraints.Constraint; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Класс реализует физическую симуляцию поведения твёрдых тел. + */ + public class RigidWorld { + + private static var lastBodyId:int; + + // Максимальное количество контактов + altphysics const MAX_CONTACTS:int = 1000; + + ////////////////////////////////////////////////////////////////// + // Настроечные параметры симуляции. Могут быть изменены в любой + // момент времени без нарушения корректной работы симуляции. + ////////////////////////////////////////////////////////////////// + // Количество шагов, за которое пересекающиеся тела должны разделиться + public var penResolutionSteps:int = 10; + // Величина допустимой глубины пересечения + public var allowedPenetration:Number = 0.1; + // Максимальная скорость, добавляемая с целью разделения тел + public var maxPenResolutionSpeed:Number = 0.5; + // Количество итераций для обработки упругих контактов + public var collisionIterations:int = 5; + // Количество итераций для обработки неупругих контактов + public var contactIterations:int = 5; + // Флаг использования предсказания состояний + public var usePrediction:Boolean = false; + + public var freezeSteps:int = 10; + public var linSpeedFreezeLimit:Number = 1; + public var angSpeedFreezeLimit:Number = 0.01; + + // Переменные для процедуры разделения тел путём непосредственного их перемещенеия. + // !!!!!!!!!!!!!!!!!!!!!!!! + // !!! Экспериментально !!! + // !!!!!!!!!!!!!!!!!!!!!!!! + public var staticSeparationIterations:int = 10; + public var staticSeparationSteps:int = 10; + public var maxAngleMove:Number = 10; + public var useStaticSeparation:Boolean = false; + + // Вектор гравитации + altphysics var _gravity:Vector3 = new Vector3(0, 0, -9.8); + // Использующийся детектор столкновений + altphysics var collisionDetector:ICollisionDetector; + // Список тел, участвующих в симуляции + altphysics var bodies:Vector. = new Vector.(); + // Количество тел, участвующих в симуляции + altphysics var bodiesNum:int; + // Список контактов на текущем шаге симуляции + altphysics var contacts:Vector. = new Vector.(MAX_CONTACTS, true); + // Количество контактов на текущем шаге симуляции + altphysics var contactsNum:int; + // Список ограничений + altphysics var constraints:Vector. = new Vector.(); + // Количество ограничений + altphysics var constraintsNum:int; + // Временная метка. Число прошедших шагов с начала симуляции. + altphysics var timeStamp:uint; + + // Временные переменные для избежания создания экземпляров + private var _r:Vector3 = new Vector3(); + private var _t:Vector3 = new Vector3(); + private var _v:Vector3 = new Vector3(); + private var _v1:Vector3 = new Vector3(); + private var _v2:Vector3 = new Vector3(); + + /** + * + */ + public function RigidWorld() { + for (var i:int = 0; i < MAX_CONTACTS; i++) contacts[i] = new Contact(); + collisionDetector = new KdTreeCollisionDetector(); + } + + /** + * Вектор гравитации. + */ + public function get gravity():Vector3 { + return _gravity.vClone(); + } + + /** + * @private + */ + public function set gravity(value:Vector3):void { + _gravity.vCopy(value); + } + + /** + * Добавляет тело в симуляцию. + * @param body + * @return + */ + public function addBody(body:Body, isStatic:Boolean):Boolean { + var i:int = bodies.indexOf(body); + if (i > -1) return false; + bodies[bodiesNum++] = body; + body.world = this; + body.id = lastBodyId++; +// if (body.collisionPrimitives != null) { +// for (i = 0; i < body.collisionPrimitivesNum; i++) collisionDetector.addPrimitive(body.collisionPrimitives[i], isStatic); +// } + return true; + } + + /** + * Удаляет тело из симуляции. + * @param body + * @return + */ + public function removeBody(body:Body, isStatic:Boolean):Boolean { + var i:int = bodies.indexOf(body); + if (i == -1) return false; + bodies.splice(i, 1); + body.world = null; +// if (body.collisionPrimitives != null) { +// for (i = 0; i < body.collisionPrimitivesNum; i++) collisionDetector.removePrimitive(body.collisionPrimitives[i], isStatic); +// } + bodiesNum--; + return true; + } + + /** + * Добавляет ограничение. + * @param c + */ + public function addConstraint(c:Constraint):void { + constraints[constraintsNum++] = c; + c.world = this; + } + + /** + * Удаляет ограничение. + * @param c + */ + public function removeConstraint(c:Constraint):Boolean { + var idx:int = constraints.indexOf(c); + if (idx < 0) return false; + constraints.splice(idx, 1) + constraintsNum--; + c.world = null; + return true; + } + + /** + * Применяет к телам действующте на них силы. Аккумуляторы сил и моментов тел очищаются после завершения шага + * симуляции, поэтому на момент вызова метода могут уже содержать некоторые значения. + * + * @param dt промежуток времени, в течении которого действуют силы + */ + private function applyForces(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i]; + body.addExternalForces(dt); + body.calcAccelerations(); + // Ускорение свободного падения применяется только к подвижным телам во избежание некорректного изменения + // фиктивной скорости неподвижных тел. + if (body.movable && !body.frozen) { + body.accel.x += _gravity.x; + body.accel.y += _gravity.y; + body.accel.z += _gravity.z; + } + } + } + + /** + * Определяет все столкновения на текущем шаге симуляции и заполняет список получившихся контактов. + * + * @param dt длительность шага симуляции + */ + private function detectCollisions(dt:Number):void { + var i:int; + var j:int; + var body:Body; + + for (i = 0; i < bodiesNum; i++) { + body = bodies[i]; + if (body.frozen) continue; + body.contactsNum = 0; + body.saveState(); + // При включённом режиме предсказания состояние тел интегрируется на один шаг вперёд + if (usePrediction) { + body.integrateVelocity(dt); + body.integratePosition(dt); + } + body.calcDerivedData(); + } + + contactsNum = collisionDetector.getAllCollisions(contacts); + + // Расчёт относительных векторов точки контакта вынесен сюда из-за необходимости учитывать + // положение тел в предсказанном состоянии + for (i = 0; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + var b1:Body = contact.body1; + var b2:Body = contact.body2; + for (j = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.r1.x = cp.pos.x - b1.state.pos.x; + cp.r1.y = cp.pos.y - b1.state.pos.y; + cp.r1.z = cp.pos.z - b1.state.pos.z; + if (b2 != null) { + cp.r2.x = cp.pos.x - b2.state.pos.x; + cp.r2.y = cp.pos.y - b2.state.pos.y; + cp.r2.z = cp.pos.z - b2.state.pos.z; + } + } + } + + // Восстановление состояния тел + if (usePrediction) { + for (i = 0; i < bodiesNum; i++) { + body = bodies[i]; + if (body.frozen) continue; + body.restoreState(); + body.calcDerivedData(); + } + } + } + + /** + * Подготваливает полученные из детектора столкновений контакты, расчитывая значения, не меняющиеся + * в ходе шага симуляции. + */ + private function preProcessContacts(dt:Number):void { + var i:int; + for (i = 0; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + var b1:Body = contact.body1; + var b2:Body = contact.body2; + // Столкнувшиеся тела размораживаются + if (b1.frozen) { + b1.frozen = false; + b1.freezeCounter = 0; + } + if (b2 != null && b2.frozen) { + b2.frozen = false; + b2.freezeCounter = 0; + } + contact.restitution = b1.material.restitution; + if (b2 != null && b2.material.restitution < contact.restitution) contact.restitution = b2.material.restitution; + contact.friction = b1.material.friction; + if (b2 != null && b2.material.friction < contact.friction) contact.friction = b2.material.friction; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.accumImpulseN = 0; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + cp.velByUnitImpulseN = 0; + if (b1.movable) { + cp.angularInertia1 = _v.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1).vDot(contact.normal); + cp.velByUnitImpulseN += b1.invMass + cp.angularInertia1; + } + if (b2 != null && b2.movable) { + cp.angularInertia2 = _v.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2).vDot(contact.normal); + cp.velByUnitImpulseN += b2.invMass + cp.angularInertia2; + } + // Расчёт требуемой конечной скорости для упругого контакта + calcSepVelocity(b1, b2, cp, _v); + cp.normalVel = _v.vDot(contact.normal); + if (cp.normalVel < 0) cp.normalVel = - contact.restitution*cp.normalVel; + // Скорость разделения неупругого контакта + cp.minSepVel = cp.penetration > allowedPenetration ? (cp.penetration - allowedPenetration)/(penResolutionSteps*dt) : 0; + if (cp.minSepVel > maxPenResolutionSpeed) cp.minSepVel = maxPenResolutionSpeed; + } + } + for (i = 0; i < constraintsNum; i++) Constraint(constraints[i]).preProcess(dt); + } + + /** + * + * @param dt + * @param forceInelastic + */ + private function processContacts(dt:Number, forceInelastic:Boolean):void { + var iterNum:int = forceInelastic ? contactIterations : collisionIterations; + var i:int; + var forwardLoop:Boolean = false; + for (var iter:int = 0; iter < iterNum; iter++) { + forwardLoop = !forwardLoop; + for (i = 0; i < contactsNum; i++) resolveContact(contacts[i], forceInelastic, forwardLoop); + // Ограничения + for (i = 0; i < constraintsNum; i++) Constraint(constraints[i]).apply(dt); + } + + // Разделение контактов путём непосредственного изменения координат и ориентации. Экспериментально. +// if (forceInelastic && useStaticSeparation) performStaticSeparation(); + } + + /** + * + */ + private function resolveContact(contactInfo:Contact, forceInelastic:Boolean, forwardLoop:Boolean):void { + var b1:Body = contactInfo.body1; + var b2:Body = contactInfo.body2; + var normal:Vector3 = contactInfo.normal; + var i:int; + if (forwardLoop) { + for (i = 0; i < contactInfo.pcount; i++) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } else { + for (i = contactInfo.pcount - 1; i >= 0; i--) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } + } + + /** + * + * @param idx + * @param b1 + * @param b2 + * @param colInfo + * @param normal + * @param forceInelastic + */ + private function resolveContactPoint(idx:int, b1:Body, b2:Body, contact:Contact, normal:Vector3, forceInelastic:Boolean):void { + var cp:ContactPoint = contact.points[idx]; + if (!forceInelastic) cp.satisfied = true; + + var newVel:Number = 0; + calcSepVelocity(b1, b2, cp, _v); + var sepVel:Number = _v.vDot(contact.normal); + if (forceInelastic) { + var minSpeVel:Number = useStaticSeparation ? 0 : cp.minSepVel; + if (sepVel < minSpeVel) cp.satisfied = false; + else if (cp.satisfied) return; + newVel = minSpeVel; + } else { + newVel = cp.normalVel; + } + var deltaVel:Number = newVel - sepVel; + var impulse:Number = deltaVel/cp.velByUnitImpulseN; + var accumImpulse:Number = cp.accumImpulseN + impulse; + if (accumImpulse < 0) accumImpulse = 0; + var deltaImpulse:Number = accumImpulse - cp.accumImpulseN; + cp.accumImpulseN = accumImpulse; + // Применяем импульс к телам + if (b1.movable) b1.applyRelPosWorldImpulse(cp.r1, normal, deltaImpulse); + if (b2 != null && b2.movable) b2.applyRelPosWorldImpulse(cp.r2, normal, -deltaImpulse); + + // Учёт силы трения + calcSepVelocity(b1, b2, cp, _v); + // Расчитываем изменение касательной скорости на единицу касательного импульса + var tanSpeedByUnitImpulse:Number = 0; + _v.vAddScaled(-_v.vDot(contact.normal), contact.normal); + var tanSpeed:Number = _v.vLength(); + if (tanSpeed < 0.001) return; + _t.vCopy(_v).vNormalize().vReverse(); + // dV = b.invMass + ((invI * (r % t)) % r) * t + if (b1.movable) { + _v.vCross2(cp.r1, _t).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1); + tanSpeedByUnitImpulse += b1.invMass + _v.vDot(_t); + } + if (b2 != null && b2.movable) { + _v.vCross2(cp.r2, _t).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2); + tanSpeedByUnitImpulse += b2.invMass + _v.vDot(_t); + } + + var tanImpulse:Number = tanSpeed/tanSpeedByUnitImpulse; + var max:Number = contact.friction*cp.accumImpulseN; + if (max < 0) { + if (tanImpulse < max) tanImpulse = max; + } else { + if (tanImpulse > max) tanImpulse = max; + } + + // Применяем импульс к телам + if (b1.movable) b1.applyRelPosWorldImpulse(cp.r1, _t, tanImpulse); + if (b2 != null && b2.movable) b2.applyRelPosWorldImpulse(cp.r2, _t, -tanImpulse); + } + + /** + * + * @param cp + * @param normal + * @return + */ + private function calcSepVelocity(body1:Body, body2:Body, cp:ContactPoint, result:Vector3):void { + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 +// result.vCopy(body1.state.velocity).vAdd(_v1.vCross2(body1.state.rotation, cp.r1)); + var rot:Vector3 = body1.state.rotation; + var v:Vector3 = cp.r1; + var x:Number = rot.y*v.z - rot.z*v.y; + var y:Number = rot.z*v.x - rot.x*v.z; + var z:Number = rot.x*v.y - rot.y*v.x; + v = body1.state.velocity; + result.x = v.x + x; + result.y = v.y + y; + result.z = v.z + z; + // V2 = V2_c + w2%r2 + if (body2 != null) { +// result.vSubtract(body2.state.velocity).vSubtract(_v2.vCross2(body2.state.rotation, cp.r2)); + rot = body2.state.rotation; + v = cp.r2; + x = rot.y*v.z - rot.z*v.y; + y = rot.z*v.x - rot.x*v.z; + z = rot.x*v.y - rot.y*v.x; + v = body2.state.velocity; + result.x -= v.x + x; + result.y -= v.y + y; + result.z -= v.z + z; + } + } + + /** + * + * @param dt + */ + private function intergateVelocities(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + (bodies[i] as Body).integrateVelocity(dt); + } + } + + /** + * + * @param dt + */ + private function integratePositions(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i] as Body; + if (body.movable && !body.frozen) body.integratePosition(dt); + } + } + + /** + * + */ + private function performStaticSeparation():void { + var iterNum:int = staticSeparationIterations; +// iterNum = 100; + + // 1. В начале каждой итерации для всех контактов сбрасывается флаг satisfied с одновременным поиском наихудшего контакта, т.е. имеющего наибольшую величину пересечения тел. + // 2. Если найденный контакт имеет величину пересечения меньше предельно допустимой процедура прерывается, т.к. разделение контактов не требуется. + // 3. Если процедура продолжается, то выполняется разделение найденного контакта и установка его флага satisfied в true, после чего запускается внутренний цикл, состоящий + // из contactsNum - 1 итераций. На каждой итерации ищется наихудший контакт среди оставшихся (satisfied == false). Если такой контакт найден, то выполняется его разделение + // и переход к следующей итерации внутреннего цикла, иначе внутренний цикл прерывается и выполняется переход к следующей итерации внешнего цикла (шаг 1.). + for (var iter:int = 0; iter < iterNum; iter++) { + // Ищем контакт с максимальной величиной пересечения тел, одновременно сбрасывая флаги satisfied в false + var worstContact:Contact = contacts[0]; + var i:int; + for (i = 1; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + contact.satisfied = false; + if (contact.maxPenetration > worstContact.maxPenetration) worstContact = contact; + } + if (worstContact.maxPenetration <= allowedPenetration) return; + + resolveInterpenetration(worstContact); + // Внутренний цикл по оставшимся контактам + for (i = 1; i < contactsNum; i++) { + worstContact = getWorstContact(); + if (worstContact == null) break; + resolveInterpenetration(worstContact); + } + } + } + + /** + * + */ + private function getWorstContact():Contact { + var maxPen:Number = 0; + var worst:Contact = null; + for (var i:int = 0; i < contactsNum; i++) { + var c:Contact = contacts[i]; + if (!c.satisfied && c.maxPenetration > maxPen) { + worst = c; + maxPen = c.maxPenetration; + } + } + return maxPen > allowedPenetration ? worst : null; + } + + /** + * Разделяет указанный контакт, выполняя staticSeparationSteps итераций по списку точек контакта. + * 1. В начале каждой итерации ищется точка с наибольшим пересечением, одновременно сбрасываются флаги satisfied у точек. + * 2. Для найденной точки выполняется процедура разделения. + * 3. После выполняется pcount - 1 итерация по списку точек, каждый раз ищется наихудшая среди имеющих satisfied == false, для которой выполняется процедура разделения. + * Если наихудшая точка не найдена, то выполнется переход к следующей итерации внешнего цикла (шаг 1). + * + * В конце процедуры разделения обновляются значения пересечений для остальных точек контакта, а также для всех прочих контактов, относящихся к телам текущего. + * + * @param contact контакт для разделения + */ + private function resolveInterpenetration(contact:Contact):void { + contact.satisfied = true; + + for (var step:int = 0; step < staticSeparationSteps; step++) { + var worstCp:ContactPoint = contact.points[0]; + var cp:ContactPoint; + var i:int; + for (i = 1; i < contact.pcount; i++) { + cp = contact.points[i]; + cp.satisfied = false; + if (cp.penetration > worstCp.penetration) worstCp = cp; + } + if (worstCp.penetration <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + // Разделяем оставшиеся точки + var maxPen:Number = 0; + for (i = 1; i < contact.pcount; i++) { + // Поиск наихудшей точки + for (var j:int = 0; j < contact.pcount; j++) { + cp = contact.points[j]; + if (cp.satisfied) continue; + if (cp.penetration > maxPen) { + maxPen = cp.penetration; + worstCp = cp; + } + } + if (maxPen <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + } + } + } + + /** + * + * @param cp + * @param contact + */ + private function separateContactPoint(cp:ContactPoint, contact:Contact):void { + cp.satisfied = true; + + var b1:Body = contact.body1; + var b2:Body = contact.body2; + var totalMove:Number = cp.penetration - allowedPenetration; + var moveCoeff:Number = totalMove/cp.velByUnitImpulseN; + var linMove1:Number; + var angleMove1:Number; + if (b1.movable) { + linMove1 = b1.invMass*moveCoeff; + angleMove1 = cp.angularInertia1*moveCoeff; + if (angleMove1 > maxAngleMove) { + linMove1 += angleMove1 - maxAngleMove; + angleMove1 -= maxAngleMove; + } + b1.state.pos.vAddScaled(linMove1, contact.normal); + _v1.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vScale(angleMove1); + b1.state.orientation.addScaledVector(_v1, 1); + } + var linMove2:Number; + var angleMove2:Number; + if (b2 != null && b2.movable) { + linMove2 = b2.invMass*moveCoeff; + angleMove2 = cp.angularInertia2*moveCoeff; + if (angleMove2 > maxAngleMove) { + linMove2 += angleMove2 - maxAngleMove; + angleMove2 -= maxAngleMove; + } + b2.state.pos.vAddScaled(-linMove2, contact.normal); + _v2.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vScale(angleMove2); + _v2.vReverse(); + b2.state.orientation.addScaledVector(_v2, 1); + } + cp.penetration = allowedPenetration; + // Обновляем пересечения в других точках + var i:int; + for (i = 0; i < contact.pcount; i++) { + var cp1:ContactPoint = contact.points[i]; + if (cp1 == cp) continue; + var angularMove:Number; + if (b1.movable) { + angularMove = _v.vCross2(_v1, cp1.r1).vDot(contact.normal); + cp1.penetration -= linMove1 + angularMove; + } + if (b2 != null && b2.movable) { + angularMove = _v.vCross2(_v2, cp1.r2).vDot(contact.normal); + cp1.penetration -= linMove2 - angularMove; + } + // Обновление максимального значения для контакта + if (cp1.penetration > contact.maxPenetration) contact.maxPenetration = cp1.penetration; + } + // Обновляем пересечения для других контактов + var c:Contact; + var j:int; + if (b1.movable) { + for (i = 0; i < b1.contactsNum; i++) { + c = b1.contacts[i]; + if (c == contact) continue; + for (j = 0; j < c.pcount; j++) { + cp1 = c.points[j]; + if (b1 == c.body1) cp1.penetration -= linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r1).vDot(c.normal); + else cp1.penetration += linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r2).vDot(c.normal); + if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; + } + } + } + if (b2 != null && b2.movable) { + for (i = 0; i < b2.contactsNum; i++) { + c = b2.contacts[i]; + if (c == contact) continue; + for (j = 0; j < c.pcount; j++) { + cp1 = c.points[j]; + if (b2 == c.body1) cp1.penetration -= linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r1).vDot(c.normal); + else cp1.penetration += linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r2).vDot(c.normal); + if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; + } + } + } + } + + /** + * + */ + private function postPhysics():void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i]; + body.clearAccumulators(); + body.calcDerivedData(); + if (body.canFreeze) { + if (body.state.velocity.vLength() < linSpeedFreezeLimit && body.state.rotation.vLength() < angSpeedFreezeLimit) { + if (!body.frozen) { + body.freezeCounter++; + if (body.freezeCounter >= freezeSteps) body.frozen = true; + } + } else { + body.freezeCounter = 0; + body.frozen = false; + } + } + } + } + + /** + * + * @param dt + */ + public function runPhysics(dt:Number):void { + timeStamp++; + applyForces(dt); + detectCollisions(dt); + preProcessContacts(dt); + processContacts(dt, false); + intergateVelocities(dt); + processContacts(dt, true); + integratePositions(dt); + postPhysics(); + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/rigid/constraints/.svn/all-wcprops b/0.0.5.Optimized/src/alternativa/physics/rigid/constraints/.svn/all-wcprops new file mode 100644 index 0000000..3d8f7fa --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/rigid/constraints/.svn/all-wcprops @@ -0,0 +1,17 @@ +K 25 +svn:wc:ra_dav:version-url +V 133 +/!svn/ver/19850/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/rigid/constraints +END +Constraint.as +K 25 +svn:wc:ra_dav:version-url +V 147 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/rigid/constraints/Constraint.as +END +MaxDistanceConstraint.as +K 25 +svn:wc:ra_dav:version-url +V 158 +/!svn/ver/19850/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/rigid/constraints/MaxDistanceConstraint.as +END diff --git a/0.0.5.Optimized/src/alternativa/physics/rigid/constraints/.svn/entries b/0.0.5.Optimized/src/alternativa/physics/rigid/constraints/.svn/entries new file mode 100644 index 0000000..32eb3b1 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/rigid/constraints/.svn/entries @@ -0,0 +1,52 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/rigid/constraints +http://svndev.alternativaplatform.com + + + +2009-09-14T12:08:58.450611Z +19850 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Constraint.as +file + + + + +2010-10-28T04:32:38.000000Z +757824f9eeb3c89243d104910e0d0529 +2009-04-23T13:43:38.903278Z +11779 +mike + +MaxDistanceConstraint.as +file + + + + +2010-10-28T04:32:38.000000Z +a200587160c8c15a80ec24c6d6d7aa14 +2009-09-14T12:08:58.450611Z +19850 +mike + diff --git a/0.0.5.Optimized/src/alternativa/physics/rigid/constraints/.svn/format b/0.0.5.Optimized/src/alternativa/physics/rigid/constraints/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/rigid/constraints/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.5.Optimized/src/alternativa/physics/rigid/constraints/.svn/text-base/Constraint.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/rigid/constraints/.svn/text-base/Constraint.as.svn-base new file mode 100644 index 0000000..c2cd912 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/rigid/constraints/.svn/text-base/Constraint.as.svn-base @@ -0,0 +1,35 @@ +package alternativa.physics.rigid.constraints { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.RigidWorld; + + use namespace altphysics; + + /** + * + */ + public class Constraint { + + altphysics var satisfied:Boolean; + altphysics var world:RigidWorld; + + /** + * + */ + public function Constraint() { + } + + /** + * @param dt + */ + public function preProcess(dt:Number):void { + } + + /** + * @param dt + */ + public function apply(dt:Number):void { + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/rigid/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/rigid/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base new file mode 100644 index 0000000..a59233b --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/rigid/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base @@ -0,0 +1,188 @@ +package alternativa.physics.rigid.constraints { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.BodyState; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class MaxDistanceConstraint extends Constraint { + // Первое тело ограничения + altphysics var body1:Body; + // Второе тело ограничения. Может быть null. + altphysics var body2:Body; + // Первая точка крепления в системе координат первого тела + altphysics var r1:Vector3; + // Вторая точка привязки в системе координат второго тела. В случае отсутствия второго тела, координаты задаются в мировой системе. + altphysics var r2:Vector3; + // Максимально допустимое расстояние между точками крепления + altphysics var maxDistance:Number; + // Радиус-вектор первой точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr1:Vector3 = new Vector3(); + // Радиус-вектор второй точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr2:Vector3 = new Vector3(); + // Минимальная скорость сближения для удовлетворения ограничения + private var minClosingVel:Number; + // Изменение относительной скорости на единицу импульса + private var velByUnitImpulseN:Number; + // Направление корректирующего импульса + private var impulseDirection:Vector3 = new Vector3(); + + // Вспомогательные переменные + private static var _v:Vector3 = new Vector3(); + private static var _v1:Vector3 = new Vector3(); + private static var _v2:Vector3 = new Vector3(); + + /** + * @param body1 + * @param body2 + * @param r1 + * @param r2 + * @param maxDistance + */ + public function MaxDistanceConstraint(body1:Body, body2:Body, r1:Vector3, r2:Vector3, maxDistance:Number) { + super(); + this.body1 = body1; + this.body2 = body2; + this.r1 = r1.vClone(); + this.r2 = r2.vClone(); + this.maxDistance = maxDistance; + } + + /** + * @param dt + */ + override public function preProcess(dt:Number):void { + // Вычислим расстояние между точками + var m:Matrix3 = body1.baseMatrix; + wr1.x = m.a*r1.x + m.b*r1.y + m.c*r1.z; + wr1.y = m.e*r1.x + m.f*r1.y + m.g*r1.z; + wr1.z = m.i*r1.x + m.j*r1.y + m.k*r1.z; + if (body2 != null) { + m = body2.baseMatrix; + wr2.x = m.a*r2.x + m.b*r2.y + m.c*r2.z; + wr2.y = m.e*r2.x + m.f*r2.y + m.g*r2.z; + wr2.z = m.i*r2.x + m.j*r2.y + m.k*r2.z; + } + else { + wr2.x = r2.x; + wr2.y = r2.y; + wr2.z = r2.z; + } + var p1:Vector3 = body1.state.pos; + impulseDirection.x = wr2.x - wr1.x - p1.x; + impulseDirection.y = wr2.y - wr1.y - p1.y; + impulseDirection.z = wr2.z - wr1.z - p1.z; + if (body2 != null) { + var p2:Vector3 = body2.state.pos; + impulseDirection.x += p2.x; + impulseDirection.y += p2.y; + impulseDirection.z += p2.z; + } + + var len:Number = Math.sqrt(impulseDirection.x*impulseDirection.x + impulseDirection.y*impulseDirection.y + impulseDirection.z*impulseDirection.z); + var delta:Number = len - maxDistance; + if (delta > 0) { + satisfied = false; + if (len < 0.001) { + impulseDirection.x = 1; + } else { + len = 1/len; + impulseDirection.x *= len; + impulseDirection.y *= len; + impulseDirection.z *= len; + } + minClosingVel = delta/(world.penResolutionSteps*dt); + if (minClosingVel > world.maxPenResolutionSpeed) { + minClosingVel = world.maxPenResolutionSpeed; + } + // Расчитываем изменение нормальной скорости на единицу нормального импульса + var x:Number; + var y:Number; + var z:Number; + // dV = b.invMass + ((invI * (r % n)) % r) * n + velByUnitImpulseN = 0; + if (body1.movable) { +// velByUnitImpulseN += body1.invMass + _v.vCross2(wr1, impulseDirection).vTransformBy3(body1.invInertiaWorld).vCross(wr1).vDot(impulseDirection); + _v.x = wr1.y*impulseDirection.z - wr1.z*impulseDirection.y; + _v.y = wr1.z*impulseDirection.x - wr1.x*impulseDirection.z; + _v.z = wr1.x*impulseDirection.y - wr1.y*impulseDirection.x; + + m = body1.invInertiaWorld; + x = m.a*_v.x + m.b*_v.y + m.c*_v.z; + y = m.e*_v.x + m.f*_v.y + m.g*_v.z; + z = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v1.x = y*wr1.z - z*wr1.y; + _v1.y = z*wr1.x - x*wr1.z; + _v1.z = x*wr1.y - y*wr1.x; + velByUnitImpulseN += body1.invMass + _v1.x*impulseDirection.x + _v1.y*impulseDirection.y + _v1.z*impulseDirection.z; + } + if (body2 != null && body2.movable) { +// velByUnitImpulseN += body2.invMass + _v.vCross2(wr2, impulseDirection).vTransformBy3(body2.invInertiaWorld).vCross(wr2).vDot(impulseDirection); + _v.x = wr2.y*impulseDirection.z - wr2.z*impulseDirection.y; + _v.y = wr2.z*impulseDirection.x - wr2.x*impulseDirection.z; + _v.z = wr2.x*impulseDirection.y - wr2.y*impulseDirection.x; + + m = body2.invInertiaWorld; + x = m.a*_v.x + m.b*_v.y + m.c*_v.z; + y = m.e*_v.x + m.f*_v.y + m.g*_v.z; + z = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v1.x = y*wr2.z - z*wr2.y; + _v1.y = z*wr2.x - x*wr2.z; + _v1.z = x*wr2.y - y*wr2.x; + velByUnitImpulseN += body2.invMass + _v1.x*impulseDirection.x + _v1.y*impulseDirection.y + _v1.z*impulseDirection.z; + } + } else { + satisfied = true; + } + } + + /** + * @param dt + */ + override public function apply(dt:Number):void { + if (satisfied) return; + // Расчитываем проекцию скорости относительной скорости + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 + var state:BodyState = body1.state; + var vel:Vector3 = state.velocity; + var rot:Vector3 = state.rotation; +// _v1.vCross2(state.rotation, wr1); + _v1.x = rot.y*wr1.z - rot.z*wr1.y; + _v1.y = rot.z*wr1.x - rot.x*wr1.z; + _v1.z = rot.x*wr1.y - rot.y*wr1.x; + _v.x = vel.x + _v1.x; + _v.y = vel.y + _v1.y; + _v.z = vel.z + _v1.z; + // V2 = V2_c + w2%r2 + if (body2 != null) { + state = body2.state; +// _v2.vCross2(state.rotation, wr2); + _v2.x = rot.y*wr2.z - rot.z*wr2.y; + _v2.y = rot.z*wr2.x - rot.x*wr2.z; + _v2.z = rot.x*wr2.y - rot.y*wr2.x; + _v.x -= vel.x - _v2.x; + _v.y -= vel.y - _v2.y; + _v.z -= vel.z - _v2.z; + } + var closingVel:Number = _v.x*impulseDirection.x + _v.y*impulseDirection.y + _v.z*impulseDirection.z; + if (closingVel > minClosingVel) return; + var impulse:Number = (minClosingVel - closingVel)/velByUnitImpulseN; + if (body1.movable) { + body1.applyRelPosWorldImpulse(wr1, impulseDirection, impulse); + } + if (body2 != null && body2.movable) { + body2.applyRelPosWorldImpulse(wr2, impulseDirection, -impulse); + } + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/rigid/constraints/Constraint.as b/0.0.5.Optimized/src/alternativa/physics/rigid/constraints/Constraint.as new file mode 100644 index 0000000..c2cd912 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/rigid/constraints/Constraint.as @@ -0,0 +1,35 @@ +package alternativa.physics.rigid.constraints { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.RigidWorld; + + use namespace altphysics; + + /** + * + */ + public class Constraint { + + altphysics var satisfied:Boolean; + altphysics var world:RigidWorld; + + /** + * + */ + public function Constraint() { + } + + /** + * @param dt + */ + public function preProcess(dt:Number):void { + } + + /** + * @param dt + */ + public function apply(dt:Number):void { + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/rigid/constraints/MaxDistanceConstraint.as b/0.0.5.Optimized/src/alternativa/physics/rigid/constraints/MaxDistanceConstraint.as new file mode 100644 index 0000000..a59233b --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/rigid/constraints/MaxDistanceConstraint.as @@ -0,0 +1,188 @@ +package alternativa.physics.rigid.constraints { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.BodyState; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class MaxDistanceConstraint extends Constraint { + // Первое тело ограничения + altphysics var body1:Body; + // Второе тело ограничения. Может быть null. + altphysics var body2:Body; + // Первая точка крепления в системе координат первого тела + altphysics var r1:Vector3; + // Вторая точка привязки в системе координат второго тела. В случае отсутствия второго тела, координаты задаются в мировой системе. + altphysics var r2:Vector3; + // Максимально допустимое расстояние между точками крепления + altphysics var maxDistance:Number; + // Радиус-вектор первой точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr1:Vector3 = new Vector3(); + // Радиус-вектор второй точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr2:Vector3 = new Vector3(); + // Минимальная скорость сближения для удовлетворения ограничения + private var minClosingVel:Number; + // Изменение относительной скорости на единицу импульса + private var velByUnitImpulseN:Number; + // Направление корректирующего импульса + private var impulseDirection:Vector3 = new Vector3(); + + // Вспомогательные переменные + private static var _v:Vector3 = new Vector3(); + private static var _v1:Vector3 = new Vector3(); + private static var _v2:Vector3 = new Vector3(); + + /** + * @param body1 + * @param body2 + * @param r1 + * @param r2 + * @param maxDistance + */ + public function MaxDistanceConstraint(body1:Body, body2:Body, r1:Vector3, r2:Vector3, maxDistance:Number) { + super(); + this.body1 = body1; + this.body2 = body2; + this.r1 = r1.vClone(); + this.r2 = r2.vClone(); + this.maxDistance = maxDistance; + } + + /** + * @param dt + */ + override public function preProcess(dt:Number):void { + // Вычислим расстояние между точками + var m:Matrix3 = body1.baseMatrix; + wr1.x = m.a*r1.x + m.b*r1.y + m.c*r1.z; + wr1.y = m.e*r1.x + m.f*r1.y + m.g*r1.z; + wr1.z = m.i*r1.x + m.j*r1.y + m.k*r1.z; + if (body2 != null) { + m = body2.baseMatrix; + wr2.x = m.a*r2.x + m.b*r2.y + m.c*r2.z; + wr2.y = m.e*r2.x + m.f*r2.y + m.g*r2.z; + wr2.z = m.i*r2.x + m.j*r2.y + m.k*r2.z; + } + else { + wr2.x = r2.x; + wr2.y = r2.y; + wr2.z = r2.z; + } + var p1:Vector3 = body1.state.pos; + impulseDirection.x = wr2.x - wr1.x - p1.x; + impulseDirection.y = wr2.y - wr1.y - p1.y; + impulseDirection.z = wr2.z - wr1.z - p1.z; + if (body2 != null) { + var p2:Vector3 = body2.state.pos; + impulseDirection.x += p2.x; + impulseDirection.y += p2.y; + impulseDirection.z += p2.z; + } + + var len:Number = Math.sqrt(impulseDirection.x*impulseDirection.x + impulseDirection.y*impulseDirection.y + impulseDirection.z*impulseDirection.z); + var delta:Number = len - maxDistance; + if (delta > 0) { + satisfied = false; + if (len < 0.001) { + impulseDirection.x = 1; + } else { + len = 1/len; + impulseDirection.x *= len; + impulseDirection.y *= len; + impulseDirection.z *= len; + } + minClosingVel = delta/(world.penResolutionSteps*dt); + if (minClosingVel > world.maxPenResolutionSpeed) { + minClosingVel = world.maxPenResolutionSpeed; + } + // Расчитываем изменение нормальной скорости на единицу нормального импульса + var x:Number; + var y:Number; + var z:Number; + // dV = b.invMass + ((invI * (r % n)) % r) * n + velByUnitImpulseN = 0; + if (body1.movable) { +// velByUnitImpulseN += body1.invMass + _v.vCross2(wr1, impulseDirection).vTransformBy3(body1.invInertiaWorld).vCross(wr1).vDot(impulseDirection); + _v.x = wr1.y*impulseDirection.z - wr1.z*impulseDirection.y; + _v.y = wr1.z*impulseDirection.x - wr1.x*impulseDirection.z; + _v.z = wr1.x*impulseDirection.y - wr1.y*impulseDirection.x; + + m = body1.invInertiaWorld; + x = m.a*_v.x + m.b*_v.y + m.c*_v.z; + y = m.e*_v.x + m.f*_v.y + m.g*_v.z; + z = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v1.x = y*wr1.z - z*wr1.y; + _v1.y = z*wr1.x - x*wr1.z; + _v1.z = x*wr1.y - y*wr1.x; + velByUnitImpulseN += body1.invMass + _v1.x*impulseDirection.x + _v1.y*impulseDirection.y + _v1.z*impulseDirection.z; + } + if (body2 != null && body2.movable) { +// velByUnitImpulseN += body2.invMass + _v.vCross2(wr2, impulseDirection).vTransformBy3(body2.invInertiaWorld).vCross(wr2).vDot(impulseDirection); + _v.x = wr2.y*impulseDirection.z - wr2.z*impulseDirection.y; + _v.y = wr2.z*impulseDirection.x - wr2.x*impulseDirection.z; + _v.z = wr2.x*impulseDirection.y - wr2.y*impulseDirection.x; + + m = body2.invInertiaWorld; + x = m.a*_v.x + m.b*_v.y + m.c*_v.z; + y = m.e*_v.x + m.f*_v.y + m.g*_v.z; + z = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v1.x = y*wr2.z - z*wr2.y; + _v1.y = z*wr2.x - x*wr2.z; + _v1.z = x*wr2.y - y*wr2.x; + velByUnitImpulseN += body2.invMass + _v1.x*impulseDirection.x + _v1.y*impulseDirection.y + _v1.z*impulseDirection.z; + } + } else { + satisfied = true; + } + } + + /** + * @param dt + */ + override public function apply(dt:Number):void { + if (satisfied) return; + // Расчитываем проекцию скорости относительной скорости + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 + var state:BodyState = body1.state; + var vel:Vector3 = state.velocity; + var rot:Vector3 = state.rotation; +// _v1.vCross2(state.rotation, wr1); + _v1.x = rot.y*wr1.z - rot.z*wr1.y; + _v1.y = rot.z*wr1.x - rot.x*wr1.z; + _v1.z = rot.x*wr1.y - rot.y*wr1.x; + _v.x = vel.x + _v1.x; + _v.y = vel.y + _v1.y; + _v.z = vel.z + _v1.z; + // V2 = V2_c + w2%r2 + if (body2 != null) { + state = body2.state; +// _v2.vCross2(state.rotation, wr2); + _v2.x = rot.y*wr2.z - rot.z*wr2.y; + _v2.y = rot.z*wr2.x - rot.x*wr2.z; + _v2.z = rot.x*wr2.y - rot.y*wr2.x; + _v.x -= vel.x - _v2.x; + _v.y -= vel.y - _v2.y; + _v.z -= vel.z - _v2.z; + } + var closingVel:Number = _v.x*impulseDirection.x + _v.y*impulseDirection.y + _v.z*impulseDirection.z; + if (closingVel > minClosingVel) return; + var impulse:Number = (minClosingVel - closingVel)/velByUnitImpulseN; + if (body1.movable) { + body1.applyRelPosWorldImpulse(wr1, impulseDirection, impulse); + } + if (body2 != null && body2.movable) { + body2.applyRelPosWorldImpulse(wr2, impulseDirection, -impulse); + } + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/rigid/primitives/.svn/all-wcprops b/0.0.5.Optimized/src/alternativa/physics/rigid/primitives/.svn/all-wcprops new file mode 100644 index 0000000..1b9250a --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/rigid/primitives/.svn/all-wcprops @@ -0,0 +1,35 @@ +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/rigid/primitives +END +RigidPlane.as +K 25 +svn:wc:ra_dav:version-url +V 146 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/rigid/primitives/RigidPlane.as +END +RigidSphere.as +K 25 +svn:wc:ra_dav:version-url +V 147 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/rigid/primitives/RigidSphere.as +END +RigidBox.as +K 25 +svn:wc:ra_dav:version-url +V 144 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/rigid/primitives/RigidBox.as +END +RigidCylinder.as +K 25 +svn:wc:ra_dav:version-url +V 149 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/rigid/primitives/RigidCylinder.as +END +RigidRect.as +K 25 +svn:wc:ra_dav:version-url +V 145 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/rigid/primitives/RigidRect.as +END diff --git a/0.0.5.Optimized/src/alternativa/physics/rigid/primitives/.svn/entries b/0.0.5.Optimized/src/alternativa/physics/rigid/primitives/.svn/entries new file mode 100644 index 0000000..f159779 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/rigid/primitives/.svn/entries @@ -0,0 +1,88 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/rigid/primitives +http://svndev.alternativaplatform.com + + + +2009-06-08T06:46:18.310732Z +14099 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +RigidPlane.as +file + + + + +2010-10-28T04:32:38.000000Z +c4713421e357ee1967dca9ab3ffa7290 +2009-06-08T06:46:18.310732Z +14099 +mike + +RigidSphere.as +file + + + + +2010-10-28T04:32:38.000000Z +79ef38eab05657ad9627bdac4ade7271 +2009-04-23T13:43:38.903278Z +11779 +mike + +RigidBox.as +file + + + + +2010-10-28T04:32:38.000000Z +da834a5d208ff30f3a74321f75301cd7 +2009-06-08T06:46:18.310732Z +14099 +mike + +RigidCylinder.as +file + + + + +2010-10-28T04:32:38.000000Z +61512d31f9e211dcb0864164aa6f5210 +2009-04-23T13:43:38.903278Z +11779 +mike + +RigidRect.as +file + + + + +2010-10-28T04:32:38.000000Z +4cff4e313a1f41d4ccfa6480ea78173e +2009-04-23T13:43:38.903278Z +11779 +mike + diff --git a/0.0.5.Optimized/src/alternativa/physics/rigid/primitives/.svn/format b/0.0.5.Optimized/src/alternativa/physics/rigid/primitives/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/rigid/primitives/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.5.Optimized/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidBox.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidBox.as.svn-base new file mode 100644 index 0000000..c01ff61 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidBox.as.svn-base @@ -0,0 +1,56 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class RigidBox extends Body { + + /** + * + * @param halfSize + * @param mass + */ + public function RigidBox(halfSize:Vector3, mass:Number) { + super(0, Matrix3.ZERO); + addCollisionPrimitive(new CollisionBox(halfSize, 1)); + setParams(halfSize, mass); + } + + /** + * @param halfSize + * @param mass + */ + public function setParams(halfSize:Vector3, mass:Number):void { + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + invInertia.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + invInertia.a = 3*invMass/(yy + zz); + invInertia.f = 3*invMass/(zz + xx); + invInertia.k = 3*invMass/(xx + yy); + } + var prim:CollisionBox = collisionPrimitives[0] as CollisionBox; + prim.hs.vCopy(halfSize); + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidCylinder.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidCylinder.as.svn-base new file mode 100644 index 0000000..0f9f2b3 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidCylinder.as.svn-base @@ -0,0 +1,32 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + + use namespace altphysics; + + public class RigidCylinder extends Body { + + altphysics var r:Number; + altphysics var h:Number; + + public function RigidCylinder(mass:Number, radius:Number, height:Number) { + super(0, Matrix3.ZERO); + setParams(mass, radius, height); + } + + public function setParams(mass:Number, radius:Number, height:Number):void { + r = radius; + h = height; + invInertiaWorld.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + invInertia.a = invInertia.f = mass*(h*h/12 + r*r/4); + invInertia.k = 0.5*mass*r*r; + invInertia.invert(); + } + } + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidPlane.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidPlane.as.svn-base new file mode 100644 index 0000000..26f74b2 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidPlane.as.svn-base @@ -0,0 +1,23 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + public class RigidPlane extends Body { + + public var normal:Vector3 = new Vector3(); + public var offset:Number; + + public function RigidPlane() { + super(0, Matrix3.ZERO); + this.normal.vCopy(Vector3.Z_AXIS); + this.offset = 0; + movable = false; + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidRect.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidRect.as.svn-base new file mode 100644 index 0000000..3c3913d --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidRect.as.svn-base @@ -0,0 +1,20 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + public class RigidRect extends Body { + + public function RigidRect(halfWidth:Number, halfLength:Number) { + super(0, Matrix3.ZERO); + movable = false; + addCollisionPrimitive(new CollisionRect(new Vector3(halfWidth, halfLength, 0), 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidSphere.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidSphere.as.svn-base new file mode 100644 index 0000000..9668716 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidSphere.as.svn-base @@ -0,0 +1,29 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + + use namespace altphysics; + + public class RigidSphere extends Body { + + public function RigidSphere(radius:Number, mass:Number) { + var m:Matrix3; + if (mass == Infinity) { + mass = 0; + m = Matrix3.ZERO; + } else { + mass = 1/mass; + var r2:Number = radius*radius; + m = new Matrix3(); + m.a = m.f = m.k = 2.5*mass/r2; + } + super(mass, m); + if (mass == 0) movable = false; + addCollisionPrimitive(new CollisionSphere(radius, 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/rigid/primitives/RigidBox.as b/0.0.5.Optimized/src/alternativa/physics/rigid/primitives/RigidBox.as new file mode 100644 index 0000000..c01ff61 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/rigid/primitives/RigidBox.as @@ -0,0 +1,56 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class RigidBox extends Body { + + /** + * + * @param halfSize + * @param mass + */ + public function RigidBox(halfSize:Vector3, mass:Number) { + super(0, Matrix3.ZERO); + addCollisionPrimitive(new CollisionBox(halfSize, 1)); + setParams(halfSize, mass); + } + + /** + * @param halfSize + * @param mass + */ + public function setParams(halfSize:Vector3, mass:Number):void { + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + invInertia.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + invInertia.a = 3*invMass/(yy + zz); + invInertia.f = 3*invMass/(zz + xx); + invInertia.k = 3*invMass/(xx + yy); + } + var prim:CollisionBox = collisionPrimitives[0] as CollisionBox; + prim.hs.vCopy(halfSize); + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/rigid/primitives/RigidCylinder.as b/0.0.5.Optimized/src/alternativa/physics/rigid/primitives/RigidCylinder.as new file mode 100644 index 0000000..0f9f2b3 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/rigid/primitives/RigidCylinder.as @@ -0,0 +1,32 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + + use namespace altphysics; + + public class RigidCylinder extends Body { + + altphysics var r:Number; + altphysics var h:Number; + + public function RigidCylinder(mass:Number, radius:Number, height:Number) { + super(0, Matrix3.ZERO); + setParams(mass, radius, height); + } + + public function setParams(mass:Number, radius:Number, height:Number):void { + r = radius; + h = height; + invInertiaWorld.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + invInertia.a = invInertia.f = mass*(h*h/12 + r*r/4); + invInertia.k = 0.5*mass*r*r; + invInertia.invert(); + } + } + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/rigid/primitives/RigidPlane.as b/0.0.5.Optimized/src/alternativa/physics/rigid/primitives/RigidPlane.as new file mode 100644 index 0000000..26f74b2 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/rigid/primitives/RigidPlane.as @@ -0,0 +1,23 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + public class RigidPlane extends Body { + + public var normal:Vector3 = new Vector3(); + public var offset:Number; + + public function RigidPlane() { + super(0, Matrix3.ZERO); + this.normal.vCopy(Vector3.Z_AXIS); + this.offset = 0; + movable = false; + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/rigid/primitives/RigidRect.as b/0.0.5.Optimized/src/alternativa/physics/rigid/primitives/RigidRect.as new file mode 100644 index 0000000..3c3913d --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/rigid/primitives/RigidRect.as @@ -0,0 +1,20 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + public class RigidRect extends Body { + + public function RigidRect(halfWidth:Number, halfLength:Number) { + super(0, Matrix3.ZERO); + movable = false; + addCollisionPrimitive(new CollisionRect(new Vector3(halfWidth, halfLength, 0), 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/rigid/primitives/RigidSphere.as b/0.0.5.Optimized/src/alternativa/physics/rigid/primitives/RigidSphere.as new file mode 100644 index 0000000..9668716 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/rigid/primitives/RigidSphere.as @@ -0,0 +1,29 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + + use namespace altphysics; + + public class RigidSphere extends Body { + + public function RigidSphere(radius:Number, mass:Number) { + var m:Matrix3; + if (mass == Infinity) { + mass = 0; + m = Matrix3.ZERO; + } else { + mass = 1/mass; + var r2:Number = radius*radius; + m = new Matrix3(); + m.a = m.f = m.k = 2.5*mass/r2; + } + super(mass, m); + if (mass == 0) movable = false; + addCollisionPrimitive(new CollisionSphere(radius, 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/types/.svn/all-wcprops b/0.0.5.Optimized/src/alternativa/physics/types/.svn/all-wcprops new file mode 100644 index 0000000..7257175 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/types/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 121 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/types +END +Quaternion.as +K 25 +svn:wc:ra_dav:version-url +V 135 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/types/Quaternion.as +END +Vector3.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/types/Vector3.as +END +Matrix3.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/types/Matrix3.as +END +Matrix4.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/19806/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/types/Matrix4.as +END diff --git a/0.0.5.Optimized/src/alternativa/physics/types/.svn/entries b/0.0.5.Optimized/src/alternativa/physics/types/.svn/entries new file mode 100644 index 0000000..ed8c499 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/types/.svn/entries @@ -0,0 +1,76 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized/src/alternativa/physics/types +http://svndev.alternativaplatform.com + + + +2009-09-11T09:22:20.813670Z +19776 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Quaternion.as +file + + + + +2010-10-28T04:32:38.000000Z +595f36fdfea8e82e752bb989e97e3a9a +2009-05-04T04:55:56.466789Z +12306 +mike + +Vector3.as +file + + + + +2010-10-28T04:32:38.000000Z +843b476caf4f3879350b906d5a80e815 +2009-09-09T14:41:32.057202Z +19641 +mike + +Matrix3.as +file + + + + +2010-10-28T04:32:38.000000Z +3fde68593cd3eb5a7046c441a76c9c5c +2009-06-25T09:18:57.805913Z +15117 +mike + +Matrix4.as +file + + + + +2010-10-28T04:32:38.000000Z +2056f21265c08fe8b5724d022ee9975a +2009-09-11T09:22:20.813670Z +19776 +mike + diff --git a/0.0.5.Optimized/src/alternativa/physics/types/.svn/format b/0.0.5.Optimized/src/alternativa/physics/types/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/types/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.5.Optimized/src/alternativa/physics/types/.svn/text-base/Matrix3.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/types/.svn/text-base/Matrix3.as.svn-base new file mode 100644 index 0000000..e07d0d1 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/types/.svn/text-base/Matrix3.as.svn-base @@ -0,0 +1,396 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + /** + * + */ + public class Matrix3 { + + public static const ZERO:Matrix3 = new Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0); + public static const IDENTITY:Matrix3 = new Matrix3(); + + public var a:Number; + public var b:Number; + public var c:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix3(a:Number = 1, b:Number = 0, c:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1) { + this.a = a; + this.b = b; + this.c = c; + + this.e = e; + this.f = f; + this.g = g; + + this.i = i; + this.j = j; + this.k = k; + } + + /** + * + * @return + */ + public function toIdentity():Matrix3 { + a = f = k = 1; + b = c = e = g = i = j = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + var det:Number = 1/(-cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk); + + a = (ff*kk - gg*jj)*det; + b = (cc*jj - bb*kk)*det; + c = (bb*gg - cc*ff)*det; + e = (gg*ii - ee*kk)*det; + f = (aa*kk - cc*ii)*det; + g = (cc*ee - aa*gg)*det; + i = (ee*jj - ff*ii)*det; + j = (bb*ii - aa*jj)*det; + k = (aa*ff - bb*ee)*det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prependTransposed(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.b + cc*m.c; + b = aa*m.e + bb*m.f + cc*m.g; + c = aa*m.i + bb*m.j + cc*m.k; + e = ee*m.a + ff*m.b + gg*m.c; + f = ee*m.e + ff*m.f + gg*m.g; + g = ee*m.i + ff*m.j + gg*m.k; + i = ii*m.a + jj*m.b + kk*m.c; + j = ii*m.e + jj*m.f + kk*m.g; + k = ii*m.i + jj*m.j + kk*m.k; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix3):Matrix3 { + a += m.a; + b += m.b; + c += m.c; + e += m.e; + f += m.f; + g += m.g; + i += m.i; + j += m.j; + k += m.k; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix3):Matrix3 { + a -= m.a; + b -= m.b; + c -= m.c; + e -= m.e; + f -= m.f; + g -= m.g; + i -= m.i; + j -= m.j; + k -= m.k; + + return this; + } + + /** + * + * @return + */ + public function transpose():Matrix3 { + var tmp:Number = b; + b = e; + e = tmp; + tmp = c; + c = i; + i = tmp; + tmp = g; + g = j; + j = tmp; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector3To3D(vin:Vector3, vout:Vector3D):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * + * @param matrix + */ + public function createSkewSymmetric(v:Vector3):Matrix3 { + a = f = k = 0; + b = -v.z; + c = v.y; + e = v.z; + g = -v.x; + i = -v.y; + j = v.x; + return this; + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(i:int, axis:Vector3):void { + switch (i) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + } + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix3):Matrix3 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @return + */ + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):Matrix3 { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + + return this; + } + + /** + * Метод формирует матрицу поворота на заданный угол относительно заданной оси. + * + * @param axis нормализованный вектор, задающий ось, относительно которой выполняется поворот + * @param angle угол поворота в радианах + */ + public function fromAxisAngle(axis:Vector3, angle:Number):void { + var c1:Number = Math.cos(angle); + var s:Number = Math.sin(angle); + var t:Number = 1 - c1; + var x:Number = axis.x; + var y:Number = axis.y; + var z:Number = axis.z; + + a = t*x*x + c1; + b = t*x*y - z*s; + c = t*x*z + y*s; + + e = t*x*y + z*s; + f = t*y*y + c1; + g = t*y*z - x*s; + + i = t*x*z - y*s; + j = t*y*z + x*s; + k = t*z*z + c1; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix3 { + return new Matrix3(a, b, c, e, f, g, i, j, k); + } + + public function toString():String { + return "[Matrix3 (" + a + ", " + b + ", " + c + "), (" + e + ", " + f + ", " + g + "), (" + i + ", " + j + ", " + k + ")]"; + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/types/.svn/text-base/Matrix4.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/types/.svn/text-base/Matrix4.as.svn-base new file mode 100644 index 0000000..d6af6c7 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/types/.svn/text-base/Matrix4.as.svn-base @@ -0,0 +1,458 @@ +package alternativa.physics.types { + import __AS3__.vec.Vector; + + /** + * + */ + public class Matrix4 { + + public var a:Number; + public var b:Number; + public var c:Number; + public var d:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + public var h:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + public var l:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix4(a:Number = 1, b:Number = 0, c:Number = 0, d:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, h:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1, l:Number = 0) { + this.a = a; + this.b = b; + this.c = c; + this.d = h; + + this.e = e; + this.f = f; + this.g = g; + this.h = h; + + this.i = i; + this.j = j; + this.k = k; + this.l = l; + } + + /** + * + * @return + */ + public function toIdentity():Matrix4 { + a = f = k = 1; + b = c = e = g = i = j = d = h = l = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + var det:Number = -cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk; + + a = (-gg*jj + ff*kk)/det; + b = (cc*jj - bb*kk)/det; + c = (-cc*ff + bb*gg)/det; + d = (dd*gg*jj - cc*hh*jj - dd*ff*kk + bb*hh*kk + cc*ff*ll - bb*gg*ll)/det; + e = (gg*ii - ee*kk)/det; + f = (-cc*ii + aa*kk)/det; + g = (cc*ee - aa*gg)/det; + h = (cc*hh*ii - dd*gg*ii + dd*ee*kk - aa*hh*kk - cc*ee*ll + aa*gg*ll)/det; + i = (-ff*ii + ee*jj)/det; + j = (bb*ii - aa*jj)/det; + k = (-bb*ee + aa*ff)/det; + l = (dd*ff*ii - bb*hh*ii - dd*ee*jj + aa*hh*jj + bb*ee*ll - aa*ff*ll)/det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + d = m.a*dd + m.b*hh + m.c*ll + m.d; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + h = m.e*dd + m.f*hh + m.g*ll + m.h; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + l = m.i*dd + m.j*hh + m.k*ll + m.l; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + d = aa*m.d + bb*m.h + cc*m.l + dd; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + h = ee*m.d + ff*m.h + gg*m.l + hh; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + l = ii*m.d + jj*m.h + kk*m.l + ll; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix4):Matrix4 { + a += m.a; + b += m.b; + c += m.c; + d += m.d; + e += m.e; + f += m.f; + g += m.g; + h += m.h; + i += m.i; + j += m.j; + k += m.k; + l += m.l; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix4):Matrix4 { + a -= m.a; + b -= m.b; + c -= m.c; + d -= m.d; + e -= m.e; + f -= m.f; + g -= m.g; + h -= m.h; + i -= m.i; + j -= m.j; + k -= m.k; + l -= m.l; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectors(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + * @param len + */ + public function transformVectorsN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverse(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverseN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(idx:int, axis:Vector3):void { + switch (idx) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + case 3: + axis.x = d; + axis.y = h; + axis.z = l; + return; + } + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix4):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = m.d; + e = m.e; + f = m.f; + g = m.g; + h = m.h; + i = m.i; + j = m.j; + k = m.k; + l = m.l; + + return this; + } + + public function setFromMatrix3(m:Matrix3, offset:Vector3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = offset.x; + e = m.e; + f = m.f; + g = m.g; + h = offset.y; + i = m.i; + j = m.j; + k = m.k; + l = offset.z; + + return this; + } + + public function setOrientationFromMatrix3(m:Matrix3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @param rotations + */ + public function getEulerAngles(rotations:Vector3):void { + if (-1 < i && i < 1) { + rotations.x = Math.atan2(j, k); + rotations.y = -Math.asin(i); + rotations.z = Math.atan2(e, a); + } else { + rotations.x = 0; + rotations.y = 0.5*((i <= -1) ? Math.PI : -Math.PI); + rotations.z = Math.atan2(-b, f); + } + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix4 { + return new Matrix4(a, b, c, d, e, f, g, h, i, j, k, l); + } + + public function toString():String { + return "[Matrix4 [" + a.toFixed(3) + " " + b.toFixed(3) + " " + c.toFixed(3) + " " + d.toFixed(3) + "] [" + e.toFixed(3) + " " + f.toFixed(3) + " " + g.toFixed(3) + " " + h.toFixed(3) + "] [" + i.toFixed(3) + " " + j.toFixed(3) + " " + k.toFixed(3) + " " + l.toFixed(3) + "]]"; + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/types/.svn/text-base/Quaternion.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/types/.svn/text-base/Quaternion.as.svn-base new file mode 100644 index 0000000..7ad2af0 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/types/.svn/text-base/Quaternion.as.svn-base @@ -0,0 +1,396 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + public class Quaternion { + + public var w:Number; + public var x:Number; + public var y:Number; + public var z:Number; + + public static function multiply(q1:Quaternion, q2:Quaternion, result:Quaternion):void { + result.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z; + result.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y; + result.y = q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z; + result.z = q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x; + } + + public static function createFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngle(axis, angle); + return q; + } + + public static function createFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngleComponents(x, y, z, angle); + return q; + } + + public function Quaternion(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0) { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + } + + public function reset(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0):Quaternion { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + return this; + } + + public function normalize():Quaternion { + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * Умножает на указанный кватернион слева: this * q + * + * @param q множитель + */ + public function prepend(q:Quaternion):Quaternion { + var ww:Number = w*q.w - x*q.x - y*q.y - z*q.z; + var xx:Number = w*q.x + x*q.w + y*q.z - z*q.y; + var yy:Number = w*q.y + y*q.w + z*q.x - x*q.z; + var zz:Number = w*q.z + z*q.w + x*q.y - y*q.x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Умножает на указанный кватернион справа: q * this + * + * @param q множитель + */ + public function append(q:Quaternion):Quaternion { + var ww:Number = q.w*w - q.x*x - q.y*y - q.z*z; + var xx:Number = q.w*x + q.x*w + q.y*z - q.z*y; + var yy:Number = q.w*y + q.y*w + q.z*x - q.x*z; + var zz:Number = q.w*z + q.z*w + q.x*y - q.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * + * @param vector + */ + public function rotateByVector(v:Vector3):Quaternion { + var ww:Number = -v.x*x - v.y*y - v.z*z; + var xx:Number = v.x*w + v.y*z - v.z*y; + var yy:Number = v.y*w + v.z*x - v.x*z; + var zz:Number = v.z*w + v.x*y - v.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Добавляет вращение, приданное вектором угловой скорости за указанное время. + * + * @param v + * @param scale + */ + public function addScaledVector(v:Vector3, scale:Number):Quaternion { + var vx:Number = v.x*scale; + var vy:Number = v.y*scale; + var vz:Number = v.z*scale; + var ww:Number = -x*vx - y*vy - z*vz; + var xx:Number = vx*w + vy*z - vz*y; + var yy:Number = vy*w + vz*x - vx*z; + var zz:Number = vz*w + vx*y - vy*x; + w += 0.5*ww; + x += 0.5*xx; + y += 0.5*yy; + z += 0.5*zz; + // inlined normalize + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param m + * @return + */ + public function toMatrix3(m:Matrix3):Quaternion { + var xx2:Number = 2*x*x; + var yy2:Number = 2*y*y; + var zz2:Number = 2*z*z; + var xy2:Number = 2*x*y; + var yz2:Number = 2*y*z; + var zx2:Number = 2*z*x; + var wx2:Number = 2*w*x; + var wy2:Number = 2*w*y; + var wz2:Number = 2*w*z; + + m.a = 1 - yy2 - zz2; + m.b = xy2 - wz2; + m.c = zx2 + wy2; + + m.e = xy2 + wz2; + m.f = 1 - xx2 - zz2; + m.g = yz2 - wx2; + + m.i = zx2 - wy2; + m.j = yz2 + wx2; + m.k = 1 - xx2 - yy2; + return this; + } + + /** + * + * @return + */ + public function length():Number { + return Math.sqrt(w*w + x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function lengthSqr():Number { + return w*w + x*x + y*y + z*z; + } + + /** + * + * @param axis + * @param angle + * @return + */ + public function setFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z); + x = axis.x*k; + y = axis.y*k; + z = axis.z*k; + return this; + } + + /** + * + * @param x + * @param y + * @param z + * @param angle + */ + public function setFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(x*x + y*y + z*z); + this.x = x*k; + this.y = y*k; + this.z = z*k; + return this; + } + + /** + * + * @param vector + */ + public function toAxisVector(v:Vector3 = null):Vector3 { + if (w < -1 || w > 1) { + normalize(); + } + if (v == null) { + v = new Vector3(); + } + if (w > -1 && w < 1) { + if (w == 0) { + v.x = x; + v.y = y; + v.z = z; + } else { + var angle:Number = 2*Math.acos(w); + var coeff:Number = 1/Math.sqrt(1 - w*w); + v.x = x*coeff*angle; + v.y = y*coeff*angle; + v.z = z*coeff*angle; + } + } else { + v.x = 0; + v.y = 0; + v.z = 0; + } + return v; + } + + /** + * + * @param rotations + */ + public function getEulerAngles(angles:Vector3):Vector3 { + var qi2:Number = 2*x*x; + var qj2:Number = 2*y*y; + var qk2:Number = 2*z*z; + var qij:Number = 2*x*y; + var qjk:Number = 2*y*z; + var qki:Number = 2*z*x; + var qri:Number = 2*w*x; + var qrj:Number = 2*w*y; + var qrk:Number = 2*w*z; + + var aa:Number = 1 - qj2 - qk2; + var bb:Number = qij - qrk; + var ee:Number = qij + qrk; + var ff:Number = 1 - qi2 - qk2; + var ii:Number = qki - qrj; + var jj:Number = qjk + qri; + var kk:Number = 1 - qi2 - qj2; + + if (-1 < ii && ii < 1) { + if (angles == null) { + angles = new Vector3(Math.atan2(jj, kk), -Math.asin(ii), Math.atan2(ee, aa)); + } else { + angles.x = Math.atan2(jj, kk); + angles.y = -Math.asin(ii); + angles.z = Math.atan2(ee, aa); + } + } else { + if (angles == null) { + angles = new Vector3(0, 0.5*((ii <= -1) ? Math.PI : -Math.PI), Math.atan2(-bb, ff)); + } else { + angles.x = 0; + angles.y = 0.5*((ii <= -1) ? Math.PI : -Math.PI); + angles.z = Math.atan2(-bb, ff); + } + } + return angles; + } + + /** + * + */ + public function conjugate():void { + x = -x; + y = -y; + z = -z; + } + + /** + * Выполняет линейную интерполяцию. + * + * @param q1 начало отрезка + * @param q2 конец отрезка + * @param t время, обычно задаётся в интервале [0, 1] + */ + public function nlerp(q1:Quaternion, q2:Quaternion, t:Number):Quaternion { + var d:Number = 1 - t; + w = q1.w*d + q2.w*t; + x = q1.x*d + q2.x*t; + y = q1.y*d + q2.y*t; + z = q1.z*d + q2.z*t; + // inlined normalize + d = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param q + * @return + */ + public function subtract(q:Quaternion):Quaternion { + w -= q.w; + x -= q.x; + y -= q.y; + z -= q.z; + return this; + } + + /** + * + * @param q1 + * @param q2 + * @return + */ + public function diff(q1:Quaternion, q2:Quaternion):Quaternion { + w = q2.w - q1.w; + x = q2.x - q1.x; + y = q2.y - q1.y; + z = q2.z - q1.z; + return this; + } + + /** + * + * @param q + * @return + */ + public function copy(q:Quaternion):Quaternion { + w = q.w; + x = q.x; + y = q.y; + z = q.z; + return this; + } + + /** + * @param reslult + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + result.w = w; + return result; + } + + /** + * + * @return + */ + public function clone():Quaternion { + return new Quaternion(w, x, y, z); + } + + /** + * + * @return + */ + public function toString():String { + return "[" + w + ", " + x + ", " + y + ", " + z + "]"; + } + } +} diff --git a/0.0.5.Optimized/src/alternativa/physics/types/.svn/text-base/Vector3.as.svn-base b/0.0.5.Optimized/src/alternativa/physics/types/.svn/text-base/Vector3.as.svn-base new file mode 100644 index 0000000..4f0e7e4 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/types/.svn/text-base/Vector3.as.svn-base @@ -0,0 +1,335 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + /** + * + */ + public class Vector3 extends Vector3D { + + public static const ZERO:Vector3 = new Vector3(0, 0, 0); + public static const X_AXIS:Vector3 = new Vector3(1, 0, 0); + public static const Y_AXIS:Vector3 = new Vector3(0, 1, 0); + public static const Z_AXIS:Vector3 = new Vector3(0, 0, 1); + + /** + * + * @param x + * @param y + * @param z + */ + public function Vector3(x:Number = 0, y:Number = 0, z:Number = 0) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * + * @return + */ + public function vLength():Number { + return Math.sqrt(x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function vLengthSqr():Number { + return x*x + y*y + z*z; + } + + /** + * + * @param length + * @return + */ + public function vSetLength(length:Number):Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = length; + } else { + var k:Number = length/Math.sqrt(x*x + y*y + z*z); + x *= k; + y *= k; + z *= k; + } + return this; + } + + /** + * + * @return + */ + public function vNormalize():Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = 1; + } else { + d = Math.sqrt(d); + x /= d; + y /= d; + z /= d; + } + return this; + } + + /** + * Сложение координат. + * + * @param v точка, координаты которой прибавляются к собственным + */ + public function vAdd(v:Vector3):Vector3 { + x += v.x; + y += v.y; + z += v.z; + return this; + } + + /** + * + * @param k + * @param v + * @return + */ + public function vAddScaled(k:Number, v:Vector3):Vector3 { + x += k*v.x; + y += k*v.y; + z += k*v.z; + return this; + } + + /** + * Вычитание координат. + * + * @param v точка, координаты которой вычитаются из собственных + */ + public function vSubtract(v:Vector3):Vector3 { + x -= v.x; + y -= v.y; + z -= v.z; + return this; + } + + /** + * Вычисляет сумму векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + * @return this + */ + public function vSum(a:Vector3, b:Vector3):Vector3 { + x = a.x + b.x; + y = a.y + b.y; + z = a.z + b.z; + return this; + } + + /** + * Вычисление разности векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + */ + public function vDiff(a:Vector3, b:Vector3):Vector3 { + x = a.x - b.x; + y = a.y - b.y; + z = a.z - b.z; + return this; + } + + /** + * Умножение на скаляр. + * + * @param k число, на которое умножаются координаты + */ + public function vScale(k:Number):Vector3 { + x *= k; + y *= k; + z *= k; + return this; + } + + /** + * Инвертирование вектора. + */ + public function vReverse():Vector3 { + x = -x; + y = -y; + z = -z; + return this; + } + + /** + * + * @param v + * @return + */ + public function vDot(v:Vector3):Number { + return x*v.x + y*v.y + z*v.z; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross(v:Vector3):Vector3 { + var xx:Number = y*v.z - z*v.y; + var yy:Number = z*v.x - x*v.z; + var zz:Number = x*v.y - y*v.x; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross2(a:Vector3, b:Vector3):Vector3 { + x = a.y*b.z - a.z*b.y; + y = a.z*b.x - a.x*b.z; + z = a.x*b.y - a.y*b.x; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy3(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * + * @param matrix + */ + public function vTransformBy3Tr(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz + m.d; + y = m.e*xx + m.f*yy + m.g*zz + m.h; + z = m.i*xx + m.j*yy + m.k*zz + m.l; + return this; + } + + /** + * + * @param m + * @return + */ + public function vTransformInverseBy4(m:Matrix4):Vector3 { + var xx:Number = x - m.d; + var yy:Number = y - m.h; + var zz:Number = z - m.l; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * + * @param m + */ + public function vDeltaTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * Установка координат. + */ + public function vReset(x:Number = 0, y:Number = 0, z:Number = 0):Vector3 { + this.x = x; + this.y = y; + this.z = z; + return this; + } + + /** + * Копирование координат точки. + * + * @param v точка, координаты которой копируются + */ + public function vCopy(v:Vector3):Vector3 { + x = v.x; + y = v.y; + z = v.z; + return this; + } + + /** + * Клонирование точки. + * + * @return клонированная точка + */ + public function vClone():Vector3 { + return new Vector3(x, y, z); + } + + /** + * Копирует координаты вектора в экземпляр класса Vector3D. + * + * @param result экземпляр, в который копируются координаты + * @return переданный параметр + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + return result; + } + + /** + * @param source + * @return + */ + public function copyFromVector3D(source:Vector3D):Vector3 { + x = source.x; + y = source.y; + z = source.z; + return this; + } + + override public function toString():String { + return "Vector3(" + x + "," + y + ", " + z + ")"; + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/types/Matrix3.as b/0.0.5.Optimized/src/alternativa/physics/types/Matrix3.as new file mode 100644 index 0000000..e07d0d1 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/types/Matrix3.as @@ -0,0 +1,396 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + /** + * + */ + public class Matrix3 { + + public static const ZERO:Matrix3 = new Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0); + public static const IDENTITY:Matrix3 = new Matrix3(); + + public var a:Number; + public var b:Number; + public var c:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix3(a:Number = 1, b:Number = 0, c:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1) { + this.a = a; + this.b = b; + this.c = c; + + this.e = e; + this.f = f; + this.g = g; + + this.i = i; + this.j = j; + this.k = k; + } + + /** + * + * @return + */ + public function toIdentity():Matrix3 { + a = f = k = 1; + b = c = e = g = i = j = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + var det:Number = 1/(-cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk); + + a = (ff*kk - gg*jj)*det; + b = (cc*jj - bb*kk)*det; + c = (bb*gg - cc*ff)*det; + e = (gg*ii - ee*kk)*det; + f = (aa*kk - cc*ii)*det; + g = (cc*ee - aa*gg)*det; + i = (ee*jj - ff*ii)*det; + j = (bb*ii - aa*jj)*det; + k = (aa*ff - bb*ee)*det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prependTransposed(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.b + cc*m.c; + b = aa*m.e + bb*m.f + cc*m.g; + c = aa*m.i + bb*m.j + cc*m.k; + e = ee*m.a + ff*m.b + gg*m.c; + f = ee*m.e + ff*m.f + gg*m.g; + g = ee*m.i + ff*m.j + gg*m.k; + i = ii*m.a + jj*m.b + kk*m.c; + j = ii*m.e + jj*m.f + kk*m.g; + k = ii*m.i + jj*m.j + kk*m.k; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix3):Matrix3 { + a += m.a; + b += m.b; + c += m.c; + e += m.e; + f += m.f; + g += m.g; + i += m.i; + j += m.j; + k += m.k; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix3):Matrix3 { + a -= m.a; + b -= m.b; + c -= m.c; + e -= m.e; + f -= m.f; + g -= m.g; + i -= m.i; + j -= m.j; + k -= m.k; + + return this; + } + + /** + * + * @return + */ + public function transpose():Matrix3 { + var tmp:Number = b; + b = e; + e = tmp; + tmp = c; + c = i; + i = tmp; + tmp = g; + g = j; + j = tmp; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector3To3D(vin:Vector3, vout:Vector3D):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * + * @param matrix + */ + public function createSkewSymmetric(v:Vector3):Matrix3 { + a = f = k = 0; + b = -v.z; + c = v.y; + e = v.z; + g = -v.x; + i = -v.y; + j = v.x; + return this; + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(i:int, axis:Vector3):void { + switch (i) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + } + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix3):Matrix3 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @return + */ + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):Matrix3 { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + + return this; + } + + /** + * Метод формирует матрицу поворота на заданный угол относительно заданной оси. + * + * @param axis нормализованный вектор, задающий ось, относительно которой выполняется поворот + * @param angle угол поворота в радианах + */ + public function fromAxisAngle(axis:Vector3, angle:Number):void { + var c1:Number = Math.cos(angle); + var s:Number = Math.sin(angle); + var t:Number = 1 - c1; + var x:Number = axis.x; + var y:Number = axis.y; + var z:Number = axis.z; + + a = t*x*x + c1; + b = t*x*y - z*s; + c = t*x*z + y*s; + + e = t*x*y + z*s; + f = t*y*y + c1; + g = t*y*z - x*s; + + i = t*x*z - y*s; + j = t*y*z + x*s; + k = t*z*z + c1; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix3 { + return new Matrix3(a, b, c, e, f, g, i, j, k); + } + + public function toString():String { + return "[Matrix3 (" + a + ", " + b + ", " + c + "), (" + e + ", " + f + ", " + g + "), (" + i + ", " + j + ", " + k + ")]"; + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/types/Matrix4.as b/0.0.5.Optimized/src/alternativa/physics/types/Matrix4.as new file mode 100644 index 0000000..d6af6c7 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/types/Matrix4.as @@ -0,0 +1,458 @@ +package alternativa.physics.types { + import __AS3__.vec.Vector; + + /** + * + */ + public class Matrix4 { + + public var a:Number; + public var b:Number; + public var c:Number; + public var d:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + public var h:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + public var l:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix4(a:Number = 1, b:Number = 0, c:Number = 0, d:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, h:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1, l:Number = 0) { + this.a = a; + this.b = b; + this.c = c; + this.d = h; + + this.e = e; + this.f = f; + this.g = g; + this.h = h; + + this.i = i; + this.j = j; + this.k = k; + this.l = l; + } + + /** + * + * @return + */ + public function toIdentity():Matrix4 { + a = f = k = 1; + b = c = e = g = i = j = d = h = l = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + var det:Number = -cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk; + + a = (-gg*jj + ff*kk)/det; + b = (cc*jj - bb*kk)/det; + c = (-cc*ff + bb*gg)/det; + d = (dd*gg*jj - cc*hh*jj - dd*ff*kk + bb*hh*kk + cc*ff*ll - bb*gg*ll)/det; + e = (gg*ii - ee*kk)/det; + f = (-cc*ii + aa*kk)/det; + g = (cc*ee - aa*gg)/det; + h = (cc*hh*ii - dd*gg*ii + dd*ee*kk - aa*hh*kk - cc*ee*ll + aa*gg*ll)/det; + i = (-ff*ii + ee*jj)/det; + j = (bb*ii - aa*jj)/det; + k = (-bb*ee + aa*ff)/det; + l = (dd*ff*ii - bb*hh*ii - dd*ee*jj + aa*hh*jj + bb*ee*ll - aa*ff*ll)/det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + d = m.a*dd + m.b*hh + m.c*ll + m.d; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + h = m.e*dd + m.f*hh + m.g*ll + m.h; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + l = m.i*dd + m.j*hh + m.k*ll + m.l; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + d = aa*m.d + bb*m.h + cc*m.l + dd; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + h = ee*m.d + ff*m.h + gg*m.l + hh; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + l = ii*m.d + jj*m.h + kk*m.l + ll; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix4):Matrix4 { + a += m.a; + b += m.b; + c += m.c; + d += m.d; + e += m.e; + f += m.f; + g += m.g; + h += m.h; + i += m.i; + j += m.j; + k += m.k; + l += m.l; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix4):Matrix4 { + a -= m.a; + b -= m.b; + c -= m.c; + d -= m.d; + e -= m.e; + f -= m.f; + g -= m.g; + h -= m.h; + i -= m.i; + j -= m.j; + k -= m.k; + l -= m.l; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectors(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + * @param len + */ + public function transformVectorsN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverse(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverseN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(idx:int, axis:Vector3):void { + switch (idx) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + case 3: + axis.x = d; + axis.y = h; + axis.z = l; + return; + } + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix4):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = m.d; + e = m.e; + f = m.f; + g = m.g; + h = m.h; + i = m.i; + j = m.j; + k = m.k; + l = m.l; + + return this; + } + + public function setFromMatrix3(m:Matrix3, offset:Vector3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = offset.x; + e = m.e; + f = m.f; + g = m.g; + h = offset.y; + i = m.i; + j = m.j; + k = m.k; + l = offset.z; + + return this; + } + + public function setOrientationFromMatrix3(m:Matrix3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @param rotations + */ + public function getEulerAngles(rotations:Vector3):void { + if (-1 < i && i < 1) { + rotations.x = Math.atan2(j, k); + rotations.y = -Math.asin(i); + rotations.z = Math.atan2(e, a); + } else { + rotations.x = 0; + rotations.y = 0.5*((i <= -1) ? Math.PI : -Math.PI); + rotations.z = Math.atan2(-b, f); + } + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix4 { + return new Matrix4(a, b, c, d, e, f, g, h, i, j, k, l); + } + + public function toString():String { + return "[Matrix4 [" + a.toFixed(3) + " " + b.toFixed(3) + " " + c.toFixed(3) + " " + d.toFixed(3) + "] [" + e.toFixed(3) + " " + f.toFixed(3) + " " + g.toFixed(3) + " " + h.toFixed(3) + "] [" + i.toFixed(3) + " " + j.toFixed(3) + " " + k.toFixed(3) + " " + l.toFixed(3) + "]]"; + } + + } +} \ No newline at end of file diff --git a/0.0.5.Optimized/src/alternativa/physics/types/Quaternion.as b/0.0.5.Optimized/src/alternativa/physics/types/Quaternion.as new file mode 100644 index 0000000..7ad2af0 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/types/Quaternion.as @@ -0,0 +1,396 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + public class Quaternion { + + public var w:Number; + public var x:Number; + public var y:Number; + public var z:Number; + + public static function multiply(q1:Quaternion, q2:Quaternion, result:Quaternion):void { + result.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z; + result.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y; + result.y = q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z; + result.z = q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x; + } + + public static function createFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngle(axis, angle); + return q; + } + + public static function createFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngleComponents(x, y, z, angle); + return q; + } + + public function Quaternion(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0) { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + } + + public function reset(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0):Quaternion { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + return this; + } + + public function normalize():Quaternion { + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * Умножает на указанный кватернион слева: this * q + * + * @param q множитель + */ + public function prepend(q:Quaternion):Quaternion { + var ww:Number = w*q.w - x*q.x - y*q.y - z*q.z; + var xx:Number = w*q.x + x*q.w + y*q.z - z*q.y; + var yy:Number = w*q.y + y*q.w + z*q.x - x*q.z; + var zz:Number = w*q.z + z*q.w + x*q.y - y*q.x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Умножает на указанный кватернион справа: q * this + * + * @param q множитель + */ + public function append(q:Quaternion):Quaternion { + var ww:Number = q.w*w - q.x*x - q.y*y - q.z*z; + var xx:Number = q.w*x + q.x*w + q.y*z - q.z*y; + var yy:Number = q.w*y + q.y*w + q.z*x - q.x*z; + var zz:Number = q.w*z + q.z*w + q.x*y - q.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * + * @param vector + */ + public function rotateByVector(v:Vector3):Quaternion { + var ww:Number = -v.x*x - v.y*y - v.z*z; + var xx:Number = v.x*w + v.y*z - v.z*y; + var yy:Number = v.y*w + v.z*x - v.x*z; + var zz:Number = v.z*w + v.x*y - v.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Добавляет вращение, приданное вектором угловой скорости за указанное время. + * + * @param v + * @param scale + */ + public function addScaledVector(v:Vector3, scale:Number):Quaternion { + var vx:Number = v.x*scale; + var vy:Number = v.y*scale; + var vz:Number = v.z*scale; + var ww:Number = -x*vx - y*vy - z*vz; + var xx:Number = vx*w + vy*z - vz*y; + var yy:Number = vy*w + vz*x - vx*z; + var zz:Number = vz*w + vx*y - vy*x; + w += 0.5*ww; + x += 0.5*xx; + y += 0.5*yy; + z += 0.5*zz; + // inlined normalize + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param m + * @return + */ + public function toMatrix3(m:Matrix3):Quaternion { + var xx2:Number = 2*x*x; + var yy2:Number = 2*y*y; + var zz2:Number = 2*z*z; + var xy2:Number = 2*x*y; + var yz2:Number = 2*y*z; + var zx2:Number = 2*z*x; + var wx2:Number = 2*w*x; + var wy2:Number = 2*w*y; + var wz2:Number = 2*w*z; + + m.a = 1 - yy2 - zz2; + m.b = xy2 - wz2; + m.c = zx2 + wy2; + + m.e = xy2 + wz2; + m.f = 1 - xx2 - zz2; + m.g = yz2 - wx2; + + m.i = zx2 - wy2; + m.j = yz2 + wx2; + m.k = 1 - xx2 - yy2; + return this; + } + + /** + * + * @return + */ + public function length():Number { + return Math.sqrt(w*w + x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function lengthSqr():Number { + return w*w + x*x + y*y + z*z; + } + + /** + * + * @param axis + * @param angle + * @return + */ + public function setFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z); + x = axis.x*k; + y = axis.y*k; + z = axis.z*k; + return this; + } + + /** + * + * @param x + * @param y + * @param z + * @param angle + */ + public function setFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(x*x + y*y + z*z); + this.x = x*k; + this.y = y*k; + this.z = z*k; + return this; + } + + /** + * + * @param vector + */ + public function toAxisVector(v:Vector3 = null):Vector3 { + if (w < -1 || w > 1) { + normalize(); + } + if (v == null) { + v = new Vector3(); + } + if (w > -1 && w < 1) { + if (w == 0) { + v.x = x; + v.y = y; + v.z = z; + } else { + var angle:Number = 2*Math.acos(w); + var coeff:Number = 1/Math.sqrt(1 - w*w); + v.x = x*coeff*angle; + v.y = y*coeff*angle; + v.z = z*coeff*angle; + } + } else { + v.x = 0; + v.y = 0; + v.z = 0; + } + return v; + } + + /** + * + * @param rotations + */ + public function getEulerAngles(angles:Vector3):Vector3 { + var qi2:Number = 2*x*x; + var qj2:Number = 2*y*y; + var qk2:Number = 2*z*z; + var qij:Number = 2*x*y; + var qjk:Number = 2*y*z; + var qki:Number = 2*z*x; + var qri:Number = 2*w*x; + var qrj:Number = 2*w*y; + var qrk:Number = 2*w*z; + + var aa:Number = 1 - qj2 - qk2; + var bb:Number = qij - qrk; + var ee:Number = qij + qrk; + var ff:Number = 1 - qi2 - qk2; + var ii:Number = qki - qrj; + var jj:Number = qjk + qri; + var kk:Number = 1 - qi2 - qj2; + + if (-1 < ii && ii < 1) { + if (angles == null) { + angles = new Vector3(Math.atan2(jj, kk), -Math.asin(ii), Math.atan2(ee, aa)); + } else { + angles.x = Math.atan2(jj, kk); + angles.y = -Math.asin(ii); + angles.z = Math.atan2(ee, aa); + } + } else { + if (angles == null) { + angles = new Vector3(0, 0.5*((ii <= -1) ? Math.PI : -Math.PI), Math.atan2(-bb, ff)); + } else { + angles.x = 0; + angles.y = 0.5*((ii <= -1) ? Math.PI : -Math.PI); + angles.z = Math.atan2(-bb, ff); + } + } + return angles; + } + + /** + * + */ + public function conjugate():void { + x = -x; + y = -y; + z = -z; + } + + /** + * Выполняет линейную интерполяцию. + * + * @param q1 начало отрезка + * @param q2 конец отрезка + * @param t время, обычно задаётся в интервале [0, 1] + */ + public function nlerp(q1:Quaternion, q2:Quaternion, t:Number):Quaternion { + var d:Number = 1 - t; + w = q1.w*d + q2.w*t; + x = q1.x*d + q2.x*t; + y = q1.y*d + q2.y*t; + z = q1.z*d + q2.z*t; + // inlined normalize + d = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param q + * @return + */ + public function subtract(q:Quaternion):Quaternion { + w -= q.w; + x -= q.x; + y -= q.y; + z -= q.z; + return this; + } + + /** + * + * @param q1 + * @param q2 + * @return + */ + public function diff(q1:Quaternion, q2:Quaternion):Quaternion { + w = q2.w - q1.w; + x = q2.x - q1.x; + y = q2.y - q1.y; + z = q2.z - q1.z; + return this; + } + + /** + * + * @param q + * @return + */ + public function copy(q:Quaternion):Quaternion { + w = q.w; + x = q.x; + y = q.y; + z = q.z; + return this; + } + + /** + * @param reslult + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + result.w = w; + return result; + } + + /** + * + * @return + */ + public function clone():Quaternion { + return new Quaternion(w, x, y, z); + } + + /** + * + * @return + */ + public function toString():String { + return "[" + w + ", " + x + ", " + y + ", " + z + "]"; + } + } +} diff --git a/0.0.5.Optimized/src/alternativa/physics/types/Vector3.as b/0.0.5.Optimized/src/alternativa/physics/types/Vector3.as new file mode 100644 index 0000000..4f0e7e4 --- /dev/null +++ b/0.0.5.Optimized/src/alternativa/physics/types/Vector3.as @@ -0,0 +1,335 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + /** + * + */ + public class Vector3 extends Vector3D { + + public static const ZERO:Vector3 = new Vector3(0, 0, 0); + public static const X_AXIS:Vector3 = new Vector3(1, 0, 0); + public static const Y_AXIS:Vector3 = new Vector3(0, 1, 0); + public static const Z_AXIS:Vector3 = new Vector3(0, 0, 1); + + /** + * + * @param x + * @param y + * @param z + */ + public function Vector3(x:Number = 0, y:Number = 0, z:Number = 0) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * + * @return + */ + public function vLength():Number { + return Math.sqrt(x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function vLengthSqr():Number { + return x*x + y*y + z*z; + } + + /** + * + * @param length + * @return + */ + public function vSetLength(length:Number):Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = length; + } else { + var k:Number = length/Math.sqrt(x*x + y*y + z*z); + x *= k; + y *= k; + z *= k; + } + return this; + } + + /** + * + * @return + */ + public function vNormalize():Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = 1; + } else { + d = Math.sqrt(d); + x /= d; + y /= d; + z /= d; + } + return this; + } + + /** + * Сложение координат. + * + * @param v точка, координаты которой прибавляются к собственным + */ + public function vAdd(v:Vector3):Vector3 { + x += v.x; + y += v.y; + z += v.z; + return this; + } + + /** + * + * @param k + * @param v + * @return + */ + public function vAddScaled(k:Number, v:Vector3):Vector3 { + x += k*v.x; + y += k*v.y; + z += k*v.z; + return this; + } + + /** + * Вычитание координат. + * + * @param v точка, координаты которой вычитаются из собственных + */ + public function vSubtract(v:Vector3):Vector3 { + x -= v.x; + y -= v.y; + z -= v.z; + return this; + } + + /** + * Вычисляет сумму векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + * @return this + */ + public function vSum(a:Vector3, b:Vector3):Vector3 { + x = a.x + b.x; + y = a.y + b.y; + z = a.z + b.z; + return this; + } + + /** + * Вычисление разности векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + */ + public function vDiff(a:Vector3, b:Vector3):Vector3 { + x = a.x - b.x; + y = a.y - b.y; + z = a.z - b.z; + return this; + } + + /** + * Умножение на скаляр. + * + * @param k число, на которое умножаются координаты + */ + public function vScale(k:Number):Vector3 { + x *= k; + y *= k; + z *= k; + return this; + } + + /** + * Инвертирование вектора. + */ + public function vReverse():Vector3 { + x = -x; + y = -y; + z = -z; + return this; + } + + /** + * + * @param v + * @return + */ + public function vDot(v:Vector3):Number { + return x*v.x + y*v.y + z*v.z; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross(v:Vector3):Vector3 { + var xx:Number = y*v.z - z*v.y; + var yy:Number = z*v.x - x*v.z; + var zz:Number = x*v.y - y*v.x; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross2(a:Vector3, b:Vector3):Vector3 { + x = a.y*b.z - a.z*b.y; + y = a.z*b.x - a.x*b.z; + z = a.x*b.y - a.y*b.x; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy3(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * + * @param matrix + */ + public function vTransformBy3Tr(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz + m.d; + y = m.e*xx + m.f*yy + m.g*zz + m.h; + z = m.i*xx + m.j*yy + m.k*zz + m.l; + return this; + } + + /** + * + * @param m + * @return + */ + public function vTransformInverseBy4(m:Matrix4):Vector3 { + var xx:Number = x - m.d; + var yy:Number = y - m.h; + var zz:Number = z - m.l; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * + * @param m + */ + public function vDeltaTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * Установка координат. + */ + public function vReset(x:Number = 0, y:Number = 0, z:Number = 0):Vector3 { + this.x = x; + this.y = y; + this.z = z; + return this; + } + + /** + * Копирование координат точки. + * + * @param v точка, координаты которой копируются + */ + public function vCopy(v:Vector3):Vector3 { + x = v.x; + y = v.y; + z = v.z; + return this; + } + + /** + * Клонирование точки. + * + * @return клонированная точка + */ + public function vClone():Vector3 { + return new Vector3(x, y, z); + } + + /** + * Копирует координаты вектора в экземпляр класса Vector3D. + * + * @param result экземпляр, в который копируются координаты + * @return переданный параметр + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + return result; + } + + /** + * @param source + * @return + */ + public function copyFromVector3D(source:Vector3D):Vector3 { + x = source.x; + y = source.y; + z = source.z; + return this; + } + + override public function toString():String { + return "Vector3(" + x + "," + y + ", " + z + ")"; + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/.actionScriptProperties b/0.0.6.0/.actionScriptProperties new file mode 100644 index 0000000..f73adc8 --- /dev/null +++ b/0.0.6.0/.actionScriptProperties @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/0.0.6.0/.flexLibProperties b/0.0.6.0/.flexLibProperties new file mode 100644 index 0000000..8306747 --- /dev/null +++ b/0.0.6.0/.flexLibProperties @@ -0,0 +1,52 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/0.0.6.0/.project b/0.0.6.0/.project new file mode 100644 index 0000000..1f1425f --- /dev/null +++ b/0.0.6.0/.project @@ -0,0 +1,19 @@ + + + AlternativaPhysics + + + + + + com.adobe.flexbuilder.project.flexbuilder + + + + + + org.maven.ide.eclipse.maven2Nature + com.adobe.flexbuilder.project.flexlibnature + com.adobe.flexbuilder.project.actionscriptnature + + diff --git a/0.0.6.0/.settings/.svn/all-wcprops b/0.0.6.0/.settings/.svn/all-wcprops new file mode 100644 index 0000000..f9b6fe7 --- /dev/null +++ b/0.0.6.0/.settings/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 89 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/.settings +END +org.eclipse.core.resources.prefs +K 25 +svn:wc:ra_dav:version-url +V 122 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/.settings/org.eclipse.core.resources.prefs +END diff --git a/0.0.6.0/.settings/.svn/entries b/0.0.6.0/.settings/.svn/entries new file mode 100644 index 0000000..ae3885d --- /dev/null +++ b/0.0.6.0/.settings/.svn/entries @@ -0,0 +1,40 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/.settings +http://svndev.alternativaplatform.com + + + +2009-04-01T12:25:29.638016Z +10301 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +org.eclipse.core.resources.prefs +file + + + + +2010-10-28T04:32:42.000000Z +4f70b476f3e5075e399505021df2f89b +2009-04-01T12:25:29.638016Z +10301 +mike + diff --git a/0.0.6.0/.settings/.svn/format b/0.0.6.0/.settings/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.6.0/.settings/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.6.0/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base b/0.0.6.0/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base new file mode 100644 index 0000000..b3d0d49 --- /dev/null +++ b/0.0.6.0/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base @@ -0,0 +1,3 @@ +#Fri Mar 20 00:19:33 YEKT 2009 +eclipse.preferences.version=1 +encoding/=UTF-8 diff --git a/0.0.6.0/.settings/org.eclipse.core.resources.prefs b/0.0.6.0/.settings/org.eclipse.core.resources.prefs new file mode 100644 index 0000000..b3d0d49 --- /dev/null +++ b/0.0.6.0/.settings/org.eclipse.core.resources.prefs @@ -0,0 +1,3 @@ +#Fri Mar 20 00:19:33 YEKT 2009 +eclipse.preferences.version=1 +encoding/=UTF-8 diff --git a/0.0.6.0/.svn/all-wcprops b/0.0.6.0/.svn/all-wcprops new file mode 100644 index 0000000..a936e28 --- /dev/null +++ b/0.0.6.0/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 79 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0 +END +.flexLibProperties +K 25 +svn:wc:ra_dav:version-url +V 98 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/.flexLibProperties +END +.project +K 25 +svn:wc:ra_dav:version-url +V 88 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/.project +END +pom.xml +K 25 +svn:wc:ra_dav:version-url +V 87 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/pom.xml +END +.actionScriptProperties +K 25 +svn:wc:ra_dav:version-url +V 103 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/.actionScriptProperties +END diff --git a/0.0.6.0/.svn/dir-prop-base b/0.0.6.0/.svn/dir-prop-base new file mode 100644 index 0000000..ed1c9d0 --- /dev/null +++ b/0.0.6.0/.svn/dir-prop-base @@ -0,0 +1,6 @@ +K 13 +svn:mergeinfo +V 174 +/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized:19806-19860 +/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D:13892-14098 +END diff --git a/0.0.6.0/.svn/entries b/0.0.6.0/.svn/entries new file mode 100644 index 0000000..e8afc61 --- /dev/null +++ b/0.0.6.0/.svn/entries @@ -0,0 +1,85 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0 +http://svndev.alternativaplatform.com + + + +2009-09-16T10:57:55.941680Z +20069 +mike +has-props + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +META-INF +dir + +.flexLibProperties +file + + + + +2010-10-28T04:32:42.000000Z +b5a236d4b6ecd69f57e47a72fa70a54b +2009-09-15T05:43:40.921290Z +19861 +mike + +.project +file + + + + +2010-10-28T04:32:42.000000Z +1e90cc68b52b93173b2b5de88eb5c3a3 +2009-04-20T17:15:55.260110Z +11541 +mike + +src +dir + +pom.xml +file + + + + +2010-10-28T04:32:42.000000Z +5955a875c3483996e84b8017582552a7 +2009-09-16T10:57:55.941680Z +20069 +mike + +.actionScriptProperties +file + + + + +2010-10-28T04:32:42.000000Z +b8656f882fff2baedd57a92db86fcf8c +2009-09-15T05:43:40.921290Z +19861 +mike + +.settings +dir + diff --git a/0.0.6.0/.svn/format b/0.0.6.0/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.6.0/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.6.0/.svn/text-base/.actionScriptProperties.svn-base b/0.0.6.0/.svn/text-base/.actionScriptProperties.svn-base new file mode 100644 index 0000000..f73adc8 --- /dev/null +++ b/0.0.6.0/.svn/text-base/.actionScriptProperties.svn-base @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/0.0.6.0/.svn/text-base/.flexLibProperties.svn-base b/0.0.6.0/.svn/text-base/.flexLibProperties.svn-base new file mode 100644 index 0000000..8306747 --- /dev/null +++ b/0.0.6.0/.svn/text-base/.flexLibProperties.svn-base @@ -0,0 +1,52 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/0.0.6.0/.svn/text-base/.project.svn-base b/0.0.6.0/.svn/text-base/.project.svn-base new file mode 100644 index 0000000..1f1425f --- /dev/null +++ b/0.0.6.0/.svn/text-base/.project.svn-base @@ -0,0 +1,19 @@ + + + AlternativaPhysics + + + + + + com.adobe.flexbuilder.project.flexbuilder + + + + + + org.maven.ide.eclipse.maven2Nature + com.adobe.flexbuilder.project.flexlibnature + com.adobe.flexbuilder.project.actionscriptnature + + diff --git a/0.0.6.0/.svn/text-base/pom.xml.svn-base b/0.0.6.0/.svn/text-base/pom.xml.svn-base new file mode 100644 index 0000000..416a587 --- /dev/null +++ b/0.0.6.0/.svn/text-base/pom.xml.svn-base @@ -0,0 +1,17 @@ + + 4.0.0 + platform.clients.fp10.libraries + AlternativaPhysics + swc + 0.0.6.0 + + platform.tools.maven + FlashBasePom + 1.0 + + + scm:svn:http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0 + + + + \ No newline at end of file diff --git a/0.0.6.0/META-INF/.svn/all-wcprops b/0.0.6.0/META-INF/.svn/all-wcprops new file mode 100644 index 0000000..8bcd855 --- /dev/null +++ b/0.0.6.0/META-INF/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 88 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/META-INF +END +MANIFEST.MF +K 25 +svn:wc:ra_dav:version-url +V 100 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/META-INF/MANIFEST.MF +END diff --git a/0.0.6.0/META-INF/.svn/entries b/0.0.6.0/META-INF/.svn/entries new file mode 100644 index 0000000..cd15126 --- /dev/null +++ b/0.0.6.0/META-INF/.svn/entries @@ -0,0 +1,40 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/META-INF +http://svndev.alternativaplatform.com + + + +2009-04-20T17:15:55.260110Z +11541 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +MANIFEST.MF +file + + + + +2010-10-28T04:32:41.000000Z +7b64dceb50a6905850ff00a0bfbe77eb +2009-04-20T17:15:55.260110Z +11541 +mike + diff --git a/0.0.6.0/META-INF/.svn/format b/0.0.6.0/META-INF/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.6.0/META-INF/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.6.0/META-INF/.svn/text-base/MANIFEST.MF.svn-base b/0.0.6.0/META-INF/.svn/text-base/MANIFEST.MF.svn-base new file mode 100644 index 0000000..5066a55 --- /dev/null +++ b/0.0.6.0/META-INF/.svn/text-base/MANIFEST.MF.svn-base @@ -0,0 +1 @@ +Bundle-Name: platform.clients.fp10.libraries.AlternativaPhysics diff --git a/0.0.6.0/META-INF/MANIFEST.MF b/0.0.6.0/META-INF/MANIFEST.MF new file mode 100644 index 0000000..5066a55 --- /dev/null +++ b/0.0.6.0/META-INF/MANIFEST.MF @@ -0,0 +1 @@ +Bundle-Name: platform.clients.fp10.libraries.AlternativaPhysics diff --git a/0.0.6.0/pom.xml b/0.0.6.0/pom.xml new file mode 100644 index 0000000..416a587 --- /dev/null +++ b/0.0.6.0/pom.xml @@ -0,0 +1,17 @@ + + 4.0.0 + platform.clients.fp10.libraries + AlternativaPhysics + swc + 0.0.6.0 + + platform.tools.maven + FlashBasePom + 1.0 + + + scm:svn:http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0 + + + + \ No newline at end of file diff --git a/0.0.6.0/src/.svn/all-wcprops b/0.0.6.0/src/.svn/all-wcprops new file mode 100644 index 0000000..d4bbafe --- /dev/null +++ b/0.0.6.0/src/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 83 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src +END diff --git a/0.0.6.0/src/.svn/entries b/0.0.6.0/src/.svn/entries new file mode 100644 index 0000000..d7d0367 --- /dev/null +++ b/0.0.6.0/src/.svn/entries @@ -0,0 +1,31 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src +http://svndev.alternativaplatform.com + + + +2009-09-15T13:40:35.203254Z +20014 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +alternativa +dir + diff --git a/0.0.6.0/src/.svn/format b/0.0.6.0/src/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.6.0/src/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.6.0/src/alternativa/.svn/all-wcprops b/0.0.6.0/src/alternativa/.svn/all-wcprops new file mode 100644 index 0000000..5ad66c6 --- /dev/null +++ b/0.0.6.0/src/alternativa/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 95 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa +END diff --git a/0.0.6.0/src/alternativa/.svn/entries b/0.0.6.0/src/alternativa/.svn/entries new file mode 100644 index 0000000..2ec8402 --- /dev/null +++ b/0.0.6.0/src/alternativa/.svn/entries @@ -0,0 +1,31 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa +http://svndev.alternativaplatform.com + + + +2009-09-15T13:40:35.203254Z +20014 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +physics +dir + diff --git a/0.0.6.0/src/alternativa/.svn/format b/0.0.6.0/src/alternativa/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.6.0/src/alternativa/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.6.0/src/alternativa/physics/.svn/all-wcprops b/0.0.6.0/src/alternativa/physics/.svn/all-wcprops new file mode 100644 index 0000000..4c0579e --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 103 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics +END +altphysics.as +K 25 +svn:wc:ra_dav:version-url +V 117 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/altphysics.as +END diff --git a/0.0.6.0/src/alternativa/physics/.svn/entries b/0.0.6.0/src/alternativa/physics/.svn/entries new file mode 100644 index 0000000..b588d3f --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/.svn/entries @@ -0,0 +1,49 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics +http://svndev.alternativaplatform.com + + + +2009-09-15T13:40:35.203254Z +20014 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +types +dir + +altphysics.as +file + + + + +2010-10-28T04:32:42.000000Z +04fbe40a27502dbbd0dba02a76b2df50 +2009-04-20T20:09:14.715403Z +11579 +mike + +collision +dir + +rigid +dir + diff --git a/0.0.6.0/src/alternativa/physics/.svn/format b/0.0.6.0/src/alternativa/physics/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.6.0/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base b/0.0.6.0/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base new file mode 100644 index 0000000..2715307 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base @@ -0,0 +1,3 @@ +package alternativa.physics { + public namespace altphysics = "http://alternativaplatform.com/en/altphysics"; +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/altphysics.as b/0.0.6.0/src/alternativa/physics/altphysics.as new file mode 100644 index 0000000..2715307 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/altphysics.as @@ -0,0 +1,3 @@ +package alternativa.physics { + public namespace altphysics = "http://alternativaplatform.com/en/altphysics"; +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/.svn/all-wcprops b/0.0.6.0/src/alternativa/physics/collision/.svn/all-wcprops new file mode 100644 index 0000000..0bba0b7 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/.svn/all-wcprops @@ -0,0 +1,113 @@ +K 25 +svn:wc:ra_dav:version-url +V 113 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/collision +END +BoxSphereCollider.as +K 25 +svn:wc:ra_dav:version-url +V 134 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/collision/BoxSphereCollider.as +END +BoxBoxCollider.as +K 25 +svn:wc:ra_dav:version-url +V 131 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/collision/BoxBoxCollider.as +END +CollisionKdTree2D.as +K 25 +svn:wc:ra_dav:version-url +V 134 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/collision/CollisionKdTree2D.as +END +CollisionKdNode.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/collision/CollisionKdNode.as +END +KdTreeCollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/collision/KdTreeCollisionDetector.as +END +BoxRectCollider.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/collision/BoxRectCollider.as +END +SpherePlaneCollider.as +K 25 +svn:wc:ra_dav:version-url +V 136 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/collision/SpherePlaneCollider.as +END +ICollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 136 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/collision/ICollisionPredicate.as +END +BoxPlaneCollider.as +K 25 +svn:wc:ra_dav:version-url +V 133 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/collision/BoxPlaneCollider.as +END +CollisionKdTree.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/collision/CollisionKdTree.as +END +BoxTriangleCollider.as +K 25 +svn:wc:ra_dav:version-url +V 136 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/collision/BoxTriangleCollider.as +END +ICollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 135 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/collision/ICollisionDetector.as +END +IRayCollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 139 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/collision/IRayCollisionPredicate.as +END +IBodyCollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/collision/IBodyCollisionPredicate.as +END +BruteForceCollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 144 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/collision/BruteForceCollisionDetector.as +END +ICollider.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/collision/ICollider.as +END +BoxCollider.as +K 25 +svn:wc:ra_dav:version-url +V 128 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/collision/BoxCollider.as +END +SphereSphereCollider.as +K 25 +svn:wc:ra_dav:version-url +V 137 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/collision/SphereSphereCollider.as +END diff --git a/0.0.6.0/src/alternativa/physics/collision/.svn/entries b/0.0.6.0/src/alternativa/physics/collision/.svn/entries new file mode 100644 index 0000000..40b3c59 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/.svn/entries @@ -0,0 +1,250 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/collision +http://svndev.alternativaplatform.com + + + +2009-09-15T13:40:35.203254Z +20014 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +BoxSphereCollider.as +file + + + + +2010-10-28T04:32:41.000000Z +8bfabb1784a5eb6f6b5a76cbd30d9e88 +2009-09-09T15:57:45.246374Z +19648 +mike + +BoxBoxCollider.as +file + + + + +2010-10-28T04:32:41.000000Z +9ac56e85e74c38d53f1c895734b1c2f4 +2009-09-15T13:40:35.203254Z +20014 +mike + +CollisionKdNode.as +file + + + + +2010-10-28T04:32:41.000000Z +1d52aba28ce259ca1f9916d70619263d +2009-07-20T03:48:26.517738Z +16522 +mike + +CollisionKdTree2D.as +file + + + + +2010-10-28T04:32:41.000000Z +d8211949ef77817543bc2882480da12f +2009-07-20T03:48:26.517738Z +16522 +mike + +SpherePlaneCollider.as +file + + + + +2010-10-28T04:32:41.000000Z +9416a2154f699ed3330ebf270ac760f9 +2009-09-09T15:57:45.246374Z +19648 +mike + +BoxRectCollider.as +file + + + + +2010-10-28T04:32:41.000000Z +709c935cfe3736242305f66c2c53684c +2009-09-15T05:43:40.921290Z +19861 +mike + +KdTreeCollisionDetector.as +file + + + + +2010-10-28T04:32:41.000000Z +ed2340f19df5496ea226eed8a196014e +2009-09-10T19:17:30.639579Z +19708 +mike + +ICollisionPredicate.as +file + + + + +2010-10-28T04:32:41.000000Z +866471ea582e95e46e94eb7ae437c827 +2009-04-26T15:39:06.673206Z +11995 +mike + +BoxPlaneCollider.as +file + + + + +2010-10-28T04:32:41.000000Z +827b78750c00050de610aa5b17ef33bd +2009-09-09T15:57:45.246374Z +19648 +mike + +primitives +dir + +CollisionKdTree.as +file + + + + +2010-10-28T04:32:41.000000Z +45bb93cc65be7d8a55015c4ac75b1242 +2009-07-20T03:48:26.517738Z +16522 +mike + +BoxTriangleCollider.as +file + + + + +2010-10-28T04:32:41.000000Z +43e341cda3486a0a036458cff5202279 +2009-09-15T05:43:40.921290Z +19861 +mike + +ICollisionDetector.as +file + + + + +2010-10-28T04:32:41.000000Z +c415c22df9a1b1064c0501d5d525a5cb +2009-07-13T10:34:12.822563Z +16296 +mike + +IRayCollisionPredicate.as +file + + + + +2010-10-28T04:32:41.000000Z +f7443fb728cf4e6157dcfb4f39665889 +2009-04-26T15:39:06.673206Z +11995 +mike + +types +dir + +IBodyCollisionPredicate.as +file + + + + +2010-10-28T04:32:41.000000Z +fce7e70f95b8ef96709157dcdc8433dd +2009-06-29T14:07:24.833358Z +15390 +mike + +ICollider.as +file + + + + +2010-10-28T04:32:41.000000Z +4eedf93d1111e060a54aadf29b86c63f +2009-09-09T15:57:45.246374Z +19648 +mike + +BruteForceCollisionDetector.as +file + + + + +2010-10-28T04:32:41.000000Z +a313af2ee5d9c84ec2551058b1b4c763 +2009-07-13T10:34:12.822563Z +16296 +mike + +BoxCollider.as +file + + + + +2010-10-28T04:32:41.000000Z +e87ebe413e1443fce9177fa29a6829e0 +2009-09-15T13:40:35.203254Z +20014 +mike + +SphereSphereCollider.as +file + + + + +2010-10-28T04:32:41.000000Z +2e720e302145d54e8391df879f68d6d8 +2009-09-09T15:57:45.246374Z +19648 +mike + diff --git a/0.0.6.0/src/alternativa/physics/collision/.svn/format b/0.0.6.0/src/alternativa/physics/collision/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/BoxBoxCollider.as.svn-base b/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/BoxBoxCollider.as.svn-base new file mode 100644 index 0000000..e8c4cef --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/BoxBoxCollider.as.svn-base @@ -0,0 +1,579 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Расчитывает точки контакта двух боксов. Нормаль контакта направляется в сторону бокса с меньшим ID. + */ + public class BoxBoxCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + private var vectorToBox1:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var bestAxisIndex:int; + private var minOverlap:Number; + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + private var tmpPoints:Vector. = new Vector.(8, true); + private var pcount:int; + + /** + * + */ + public function BoxBoxCollider() { + for (var i:int = 0; i < 8; i++) { + tmpPoints[i] = new ContactPoint(); + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + + if (bestAxisIndex < 6) { + // Контакт грань-(грань|ребро|вершина) + findFaceContactPoints(box1, box2, vectorToBox1, bestAxisIndex, contact); + } else { + // Контакт ребро-ребро + bestAxisIndex -= 6; + findEdgesIntersection(box1, box2, vectorToBox1, int(bestAxisIndex/3), bestAxisIndex%3, contact); + } + contact.body1 = box1.body; + contact.body2 = box2.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + // Вектор из центра второго бокса в центр первого + vectorToBox1.x = transform1.d - transform2.d; + vectorToBox1.y = transform1.h - transform2.h; + vectorToBox1.z = transform1.l - transform2.l; + + // Проверка пересечения по основным осям + axis10.x = transform1.a; + axis10.y = transform1.e; + axis10.z = transform1.i; + if (!testMainAxis(box1, box2, axis10, 0, vectorToBox1)) return false; + axis11.x = transform1.b; + axis11.y = transform1.f; + axis11.z = transform1.j; + if (!testMainAxis(box1, box2, axis11, 1, vectorToBox1)) return false; + axis12.x = transform1.c; + axis12.y = transform1.g; + axis12.z = transform1.k; + if (!testMainAxis(box1, box2, axis12, 2, vectorToBox1)) return false; + + axis20.x = transform2.a; + axis20.y = transform2.e; + axis20.z = transform2.i; + if (!testMainAxis(box1, box2, axis20, 3, vectorToBox1)) return false; + axis21.x = transform2.b; + axis21.y = transform2.f; + axis21.z = transform2.j; + if (!testMainAxis(box1, box2, axis21, 4, vectorToBox1)) return false; + axis22.x = transform2.c; + axis22.y = transform2.g; + axis22.z = transform2.k; + if (!testMainAxis(box1, box2, axis22, 5, vectorToBox1)) return false; + + // Проверка производных осей + if (!testDerivedAxis(box1, box2, axis10, axis20, 6, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis21, 7, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis22, 8, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis11, axis20, 9, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis21, 10, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis22, 11, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis12, axis20, 12, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis21, 13, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis22, 14, vectorToBox1)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта грани одного бокса с гранью/ребром/вершиной другого. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param faceAxisIdx индекс оси первого бокса, перпендикулярной грани, с которой произошло столкновение + * @param contactInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, faceAxisIdx:int, contact:Contact):void { + var swapNormal:Boolean = false; + if (faceAxisIdx > 2) { + // Столкновение с гранью второго бокса. Для дальнейших расчётов боксы меняются местами, + // но нормаль контакта всё равно должна быть направлена в сторону первоначального box1 + var tmpBox:CollisionBox = box1; + box1 = box2; + box2 = tmpBox; + vectorToBox1.x = -vectorToBox1.x; + vectorToBox1.y = -vectorToBox1.y; + vectorToBox1.z = -vectorToBox1.z; + faceAxisIdx -= 3; + swapNormal = true; + } + + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + var colAxis:Vector3 = contact.normal; + + transform1.getAxis(faceAxisIdx, colAxis); + var negativeFace:Boolean = colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z > 0; + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + // Ищем ось второго бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + transform2.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisDot = dot; + incidentAxisIdx = axisIdx; + } + } + // Получаем список вершин грани второго бокса, переводим их в систему координат первого бокса и выполняем обрезку + // по грани первого бокса. Таким образом получается список потенциальных точек контакта. + transform2.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box2.hs, incidentAxisIdx, incidentAxisDot < 0, points1); + + // TODO: Вычислить результирующую матрицу, затем преобразовать векторы за один заход + transform2.transformVectorsN(points1, points2, 4); + transform1.transformVectorsInverseN(points2, points1, 4); + + var pnum:int = clip(box1.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points1[i]; + if ((pen = getPointBoxPenetration(box1.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + var cp:ContactPoint = tmpPoints[pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = transform1.a*v.x + transform1.b*v.y + transform1.c*v.z + transform1.d; + cpPos.y = transform1.e*v.x + transform1.f*v.y + transform1.g*v.z + transform1.h; + cpPos.z = transform1.i*v.x + transform1.j*v.y + transform1.k*v.z + transform1.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + + cp.penetration = pen; + } + } + if (swapNormal) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (pcount > 4) { + reducePoints(); + } + for (i = 0; i < pcount; i++) { + ContactPoint(contact.points[i]).copyFrom(tmpPoints[i]); + } + contact.pcount = pcount; + } + + /** + * + * @param contactInfo + */ + private function reducePoints():void { + var i:int; + var minIdx:int; + var cp1:ContactPoint; + var cp2:ContactPoint; + while (pcount > 4) { + var minLen:Number = 1e10; + var p1:Vector3 = ContactPoint(tmpPoints[pcount - 1]).pos; + var p2:Vector3; + for (i = 0; i < pcount; i++) { + p2 = ContactPoint(tmpPoints[i]).pos; + var dx:Number = p2.x - p1.x; + var dy:Number = p2.y - p1.y; + var dz:Number = p2.z - p1.z; + var len:Number = dx*dx + dy*dy + dz*dz; + if (len < minLen) { + minLen = len; + minIdx = i; + } + p1 = p2; + } + cp1 = tmpPoints[minIdx == 0 ? (pcount - 1) : (minIdx - 1)]; + cp2 = tmpPoints[minIdx]; + p1 = cp1.pos; + p2 = cp2.pos; + p2.x = 0.5*(p1.x + p2.x); + p2.y = 0.5*(p1.y + p2.y); + p2.z = 0.5*(p1.z + p2.z); + cp2.penetration = 0.5*(cp1.penetration + cp2.penetration); + if (minIdx > 0) { + for (i = minIdx; i < pcount; i++) { + tmpPoints[i - 1] = tmpPoints[i]; + } + tmpPoints[pcount - 1] = cp1; + } + pcount--; + } + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clip(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param contactInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):void { + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + transform1.getAxis(axisIdx1, axis10); + transform2.getAxis(axisIdx2, axis20); + + var colAxis:Vector3 = contact.normal; + + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону первого бокса + if (colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + + var tx:Number = box1.hs.x; + var ty:Number = box1.hs.y; + var tz:Number = box1.hs.z; + + var x2:Number = box2.hs.x; + var y2:Number = box2.hs.y; + var z2:Number = box2.hs.z; + // x + if (axisIdx1 == 0) { + tx = 0; + halfLen1 = box1.hs.x; + } else { + if (colAxis.x*transform1.a + colAxis.y*transform1.e + colAxis.z*transform1.i > 0) { + tx = -tx; + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = box2.hs.x; + } else { + if (colAxis.x*transform2.a + colAxis.y*transform2.e + colAxis.z*transform2.i < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + ty = 0; + halfLen1 = box1.hs.y; + } else { + if (colAxis.x*transform1.b + colAxis.y*transform1.f + colAxis.z*transform1.j > 0) { + ty = -ty; + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = box2.hs.y; + } else { + if (colAxis.x*transform2.b + colAxis.y*transform2.f + colAxis.z*transform2.j < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + tz = 0; + halfLen1 = box1.hs.z; + } else { + if (colAxis.x*transform1.c + colAxis.y*transform1.g + colAxis.z*transform1.k > 0) { + tz = -tz; + } + } + if (axisIdx2 == 2) { + z2 = 0; + halfLen2 = box2.hs.z; + } else { + if (colAxis.x*transform2.c + colAxis.y*transform2.g + colAxis.z*transform2.k < 0) { + z2 = -z2; + } + } + // Получаем глобальные координаты средних точек рёбер + var x1:Number = transform1.a*tx + transform1.b*ty + transform1.c*tz + transform1.d; + var y1:Number = transform1.e*tx + transform1.f*ty + transform1.g*tz + transform1.h; + var z1:Number = transform1.i*tx + transform1.j*ty + transform1.k*tz + transform1.l; + tx = x2; + ty = y2; + tz = z2; + x2 = transform2.a*tx + transform2.b*ty + transform2.c*tz + transform2.d; + y2 = transform2.e*tx + transform2.f*ty + transform2.g*tz + transform2.h; + z2 = transform2.i*tx + transform2.j*ty + transform2.k*tz + transform2.l; + + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + tx = x2 - x1; + ty = y2 - y1; + tz = z2 - z1; + var c1:Number = axis10.x*tx + axis10.y*ty + axis10.z*tz; + var c2:Number = axis20.x*tx + axis20.y*ty + axis20.z*tz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cp.pos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cp.pos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение боксов вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box1 + * @param box2 + * @param axis + * @param axisIndex + * @param toBox1 + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box1 + * @param box2 + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox1 + * @return + */ + private function testDerivedAxis(box1:CollisionBox, box2:CollisionBox, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия двух боксов вдоль заданной оси. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param axis ось + * @param vectorToBox1 вектор, соединяющий центр второго бокса с центром первого + * @return величина перекрытия боксов вдоль оси + */ + public function overlapOnAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, vectorToBox1:Vector3):Number { + var m:Matrix4 = box1.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box1.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box1.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box1.hs.z; + if (d < 0) d = -d; + projection += d; + + m = box2.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box2.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box2.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box2.hs.z; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox1.x*axis.x + vectorToBox1.y*axis.y + vectorToBox1.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} + import alternativa.physics.types.Vector3; + + class CollisionPointTmp { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + } diff --git a/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/BoxCollider.as.svn-base b/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/BoxCollider.as.svn-base new file mode 100644 index 0000000..137256e --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/BoxCollider.as.svn-base @@ -0,0 +1,408 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + /** + * + */ + public class BoxCollider implements ICollider { + + /** + * + */ + public function BoxCollider() { + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + return false; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + protected function getFaceVertsByAxis(hs:Vector3, axisIdx:int, negativeFace:Boolean, result:Vector.):void { + var v:Vector3; + switch (axisIdx) { + case 0: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + } + break; + case 1: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + } + break; + case 2: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + } else { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } + break; + } + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + v = result[num++]; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + v = result[num++]; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + v = result[num++]; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + v = result[num++]; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + v = result[num++]; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + v = result[num++]; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/BoxPlaneCollider.as.svn-base b/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/BoxPlaneCollider.as.svn-base new file mode 100644 index 0000000..9817aac --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/BoxPlaneCollider.as.svn-base @@ -0,0 +1,87 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + public class BoxPlaneCollider implements ICollider { + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var normal:Vector3 = new Vector3(); + + public function BoxPlaneCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var box:RigidBox = body1 as RigidBox; +// var plane:RigidPlane; +// if (box == null) { +// box = body2 as RigidBox; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные координаты вершин бокса +// var sx:Number = box.halfSize.x; +// var sy:Number = box.halfSize.y; +// var sz:Number = box.halfSize.z; +// (verts1[0] as Vector3).reset(-sx, -sy, -sz); +// (verts1[1] as Vector3).reset(sx, -sy, -sz); +// (verts1[2] as Vector3).reset(sx, sy, -sz); +// (verts1[3] as Vector3).reset(-sx, sy, -sz); +// (verts1[4] as Vector3).reset(-sx, -sy, sz); +// (verts1[5] as Vector3).reset(sx, -sy, sz); +// (verts1[6] as Vector3).reset(sx, sy, sz); +// (verts1[7] as Vector3).reset(-sx, sy, sz); +// +// box.transform.transformVectors(verts1, verts2); +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// // Проверяем наличие столкновений с каждой вершиной +// collisionInfo.pcount = 0; +// for (var i:int = 0; i < 8; i++) { +// // Вершина добавляется в список точек столкновения, если лежит под плоскостью +// var dist:Number = (verts2[i] as Vector3).dot(normal); +// if (dist < offset) { +// var cp:ContactPoint; +// if (collisionInfo.pcount == collisionInfo.points.length) { +// cp = new ContactPoint(); +// collisionInfo.points[collisionInfo.pcount] = cp; +// } else { +// cp = collisionInfo.points[collisionInfo.pcount]; +// } +// cp.pos.copy(verts2[i]); +// cp.r1.diff(cp.pos, box.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); +// cp.penetration = offset - dist; +// collisionInfo.pcount++; +// } +// } +// if (collisionInfo.pcount > 0) { +// collisionInfo.body1 = box; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// return true; +// } + return false; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/BoxRectCollider.as.svn-base b/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/BoxRectCollider.as.svn-base new file mode 100644 index 0000000..de99234 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/BoxRectCollider.as.svn-base @@ -0,0 +1,522 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + /** + * + */ + public class BoxRectCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + + private var vectorToBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxRectCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, rect, vectorToBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + findEdgesIntersection(box, rect, vectorToBox, int(bestAxisIndex/2), bestAxisIndex%2, contact); + } + contact.body1 = box.body; + contact.body2 = rect.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + // Вектор из центра прямоугольника в центр бокса + vectorToBox.x = boxTransform.d - rectTransform.d; + vectorToBox.y = boxTransform.h - rectTransform.h; + vectorToBox.z = boxTransform.l - rectTransform.l; + + // Проверка пересечения по нормали прямоугольника + rectTransform.getAxis(2, axis22); + if (!testMainAxis(box, rect, axis22, 0, vectorToBox)) return false; + + // Проверка пересечения по основным осям бокса + boxTransform.getAxis(0, axis10); + if (!testMainAxis(box, rect, axis10, 1, vectorToBox)) return false; + boxTransform.getAxis(1, axis11); + if (!testMainAxis(box, rect, axis11, 2, vectorToBox)) return false; + boxTransform.getAxis(2, axis12); + if (!testMainAxis(box, rect, axis12, 3, vectorToBox)) return false; + + // Получаем направляющие рёбер прямоугольника + rectTransform.getAxis(0, axis20); + rectTransform.getAxis(1, axis21); + + // Проверка производных осей + if (!testDerivedAxis(box, rect, axis10, axis20, 4, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis10, axis21, 5, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis11, axis20, 6, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis11, axis21, 7, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis12, axis20, 8, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis12, axis21, 9, vectorToBox)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта бокса с прямоугольником. + * + * @param box бокс + * @param rect прямоугольник + * @param vectorToBox вектор, направленный из центра прямоугольника в центр бокса + * @param faceAxisIdx индекс оси, идентифицирующей полскость столкновения (грань бокса или полскость прямоугольника) + * @param colInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var pnum:int; + var i:int; + var v:Vector3; + var cp:ContactPoint; + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + var colAxis:Vector3 = contact.normal; + if (faceAxisIdx == 0) { + // Столкновение с плоскостью прямоугольника + + // Проверим положение бокса относительно плоскости прямоугольника + colAxis.x = rectTransform.c; + colAxis.y = rectTransform.g; + colAxis.z = rectTransform.k; + +// var offset:Number = colAxis.x*rectTransform.d + colAxis.y*rectTransform.h + colAxis.z*rectTransform.l; +// if (bbPos.vDot(colAxis) < offset) return false; + + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisIdx = axisIdx; + incidentAxisDot = dot; + } + } + // Получаем список вершин грани бокса, переводим их в систему координат прямоугольника и выполняем обрезку + // по прямоугольнику. Таким образом получается список потенциальных точек контакта. + boxTransform.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box.hs, incidentAxisIdx, incidentAxisDot > 0, points1); + boxTransform.transformVectorsN(points1, points2, 4); + rectTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByRect(rect.hs); + // Проверяем каждую потенциальную точку на принадлежность нижней полуплоскости прямоугольника и добавляем такие точки в список контактов + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if (v.z < epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = -v.z; + var cpPos:Vector3 = cp.pos; + cpPos.x = rectTransform.a*v.x + rectTransform.b*v.y + rectTransform.c*v.z + rectTransform.d; + cpPos.y = rectTransform.e*v.x + rectTransform.f*v.y + rectTransform.g*v.z + rectTransform.h; + cpPos.z = rectTransform.i*v.x + rectTransform.j*v.y + rectTransform.k*v.z + rectTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } else { + // Столкновение с гранью бокса + faceAxisIdx--; + boxTransform.getAxis(faceAxisIdx, colAxis); + var negativeFace:Boolean = colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z > 0; + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (rectTransform.c*colAxis.x + rectTransform.g*colAxis.y + rectTransform.k*colAxis.z < 0) return false; + + getFaceVertsByAxis(rect.hs, 2, false, points1); + rectTransform.transformVectorsN(points1, points2, 4); + boxTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if ((pen = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = pen; + cpPos = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } + return true; + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * + * @param hs + * @return + */ + private function clipByRect(hs:Vector3):int { + var pnum:int = 4; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box первый бокс + * @param rect второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param colInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):void { + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + boxTransform.getAxis(axisIdx1, axis10); + rectTransform.getAxis(axisIdx2, axis20); + var colAxis:Vector3 = contact.normal; + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону бокса + if (colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + var vx:Number = box.hs.x; + var vy:Number = box.hs.y; + var vz:Number = box.hs.z; + var x2:Number = rect.hs.x; + var y2:Number = rect.hs.y; + var z2:Number = rect.hs.z; + // x + if (axisIdx1 == 0) { + vx = 0; + halfLen1 = box.hs.x; + } else { + if (boxTransform.a*colAxis.x + boxTransform.e*colAxis.y + boxTransform.i*colAxis.z > 0) { + vx = -vx; + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = rect.hs.x; + } else { + if (rectTransform.a*colAxis.x + rectTransform.e*colAxis.y + rectTransform.i*colAxis.z < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + vy = 0; + halfLen1 = box.hs.y; + } else { + if (boxTransform.b*colAxis.x + boxTransform.f*colAxis.y + boxTransform.j*colAxis.z > 0) { + vy = -vy; + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = rect.hs.y; + } else { + if (rectTransform.b*colAxis.x + rectTransform.f*colAxis.y + rectTransform.j*colAxis.z < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + vz = 0; + halfLen1 = box.hs.z; + } else { + if (boxTransform.c*colAxis.x + boxTransform.g*colAxis.y + boxTransform.k*colAxis.z > 0) { + vz = -vz; + } + } + // Получаем глобальные координаты средних точек рёбер + + var x1:Number = boxTransform.a*vx + boxTransform.b*vy + boxTransform.c*vz + boxTransform.d; + var y1:Number = boxTransform.e*vx + boxTransform.f*vy + boxTransform.g*vz + boxTransform.h; + var z1:Number = boxTransform.i*vx + boxTransform.j*vy + boxTransform.k*vz + boxTransform.l; + vx = x2; + vy = y2; + vz = z2; + x2 = rectTransform.a*vx + rectTransform.b*vy + rectTransform.c*vz + rectTransform.d; + y2 = rectTransform.e*vx + rectTransform.f*vy + rectTransform.g*vz + rectTransform.h; + z2 = rectTransform.i*vx + rectTransform.j*vy + rectTransform.k*vz + rectTransform.l; + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + vx = x2 - x1; + vy = y2 - y1; + vz = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cpPos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cpPos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cpPos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var v:Vector3 = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + + /** + * Проверяет пересечение вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box + * @param rect + * @param axis + * @param axisIndex + * @param vectorToBox + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param rect + * @param axis1 + * @param axis2 + * @param axisIndex + * @param vectorToBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, rect:CollisionRect, axis1:Vector3, axis2:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия вдоль заданной оси. + * + * @param box бокс + * @param rect прямоугольник + * @param axis ось + * @param vectorToBox вектор, соединяющий центр прямоугольника с центром бокса + * @return величина перекрытия вдоль оси + */ + public function overlapOnAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, vectorToBox:Vector3):Number { + var m:Matrix4 = box.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box.hs.z; + if (d < 0) d = -d; + projection += d; + + m = rect.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*rect.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*rect.hs.y; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox.x*axis.x + vectorToBox.y*axis.y + vectorToBox.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/BoxSphereCollider.as.svn-base b/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/BoxSphereCollider.as.svn-base new file mode 100644 index 0000000..27ae562 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/BoxSphereCollider.as.svn-base @@ -0,0 +1,113 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + use namespace altphysics; + /** + * + */ + public class BoxSphereCollider implements ICollider { + + private var center:Vector3 = new Vector3(); + private var closestPt:Vector3 = new Vector3(); + + private var bPos:Vector3 = new Vector3(); + private var sPos:Vector3 = new Vector3(); + + /** + * + */ + public function BoxSphereCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + // Зафиксированно столкновение + contact.body1 = sphere.body; + contact.body2 = box.body; + contact.normal.vCopy(closestPt).vTransformBy4(box.transform).vSubtract(sPos).vNormalize().vReverse(); + contact.pcount = 1; + + var cp:ContactPoint = contact.points[0]; + cp.penetration = sphere.r - Math.sqrt(distSqr); + cp.pos.vCopy(contact.normal).vScale(-sphere.r).vAdd(sPos); + cp.r1.vDiff(cp.pos, sPos); + cp.r2.vDiff(cp.pos, bPos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/BoxTriangleCollider.as.svn-base b/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/BoxTriangleCollider.as.svn-base new file mode 100644 index 0000000..23f1874 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/BoxTriangleCollider.as.svn-base @@ -0,0 +1,682 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionTriangle; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + + /** + * + */ + public class BoxTriangleCollider extends BoxCollider { + + public var epsilon:Number = 0.001; + + private var bestAxisIndex:int; + private var minOverlap:Number; + private var toBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var colNormal:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxTriangleCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, tri, toBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + findEdgesIntersection(box, tri, toBox, bestAxisIndex%3, int(bestAxisIndex/3), contact); + } + + contact.body1 = box.body; + contact.body2 = tri.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + toBox.x = boxTransform.d - triTransform.d; + toBox.y = boxTransform.h - triTransform.h; + toBox.z = boxTransform.l - triTransform.l; + + minOverlap = 1e308; + + // Сначала проверяется нормаль треугольника + axis.x = triTransform.c; + axis.y = triTransform.g; + axis.z = triTransform.k; + if (!testMainAxis(box, tri, axis, 0, toBox)) return false; + + // Проверка основных осей бокса + axis10.x = boxTransform.a; + axis10.y = boxTransform.e; + axis10.z = boxTransform.i; + if (!testMainAxis(box, tri, axis10, 1, toBox)) return false; + axis11.x = boxTransform.b; + axis11.y = boxTransform.f; + axis11.z = boxTransform.j; + if (!testMainAxis(box, tri, axis11, 2, toBox)) return false; + axis12.x = boxTransform.c; + axis12.y = boxTransform.g; + axis12.z = boxTransform.k; + if (!testMainAxis(box, tri, axis12, 3, toBox)) return false; + + // Проверка производных осей + // TODO: заменить вычисления векторных произведений инлайнами + var v:Vector3 = tri.e0; + axis20.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis20.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis20.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis20, 4, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis20, 5, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis20, 6, toBox)) return false; + + v = tri.e1; + axis21.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis21.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis21.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis21, 7, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis21, 8, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis21, 9, toBox)) return false; + + v = tri.e2; + axis22.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis22.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis22.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis22, 10, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis22, 11, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis22, 12, toBox)) return false; + + return true; + } + + /** + * Тестирует пересечение примитивов вдоль заданной оси. + * + * @param box бокс + * @param tri треугольник + * @param axis ось + * @param axisIndex индекс оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return true, если примитивы имеют перекрытие проекций вдоль указанной оси, иначе false + */ + private function testMainAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, axisIndex:int, toBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param tri + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, tri:CollisionTriangle, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Рассчитывает величину перекрытия проекций бокса и треугольника на заданную ось. + * + * @param box бокс + * @param tri треугольник + * @param axis единичный направляющий вектор оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return величина перекрытия. Положительное значение указывает на наличие перекрытия, нулевое или отрицательное значение указывает на отсутствие перекрытия. + */ + private function overlapOnAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, toBox:Vector3):Number { + var t:Matrix4 = box.transform; + var projection:Number = (t.a*axis.x + t.e*axis.y + t.i*axis.z)*box.hs.x; + if (projection < 0) projection = -projection; + var d:Number = (t.b*axis.x + t.f*axis.y + t.j*axis.z)*box.hs.y; + projection += d < 0 ? -d : d; + d = (t.c*axis.x + t.g*axis.y + t.k*axis.z)*box.hs.z; + projection += d < 0 ? -d : d; + + var vectorProjection:Number = toBox.x*axis.x + toBox.y*axis.y + toBox.z*axis.z; + // Для оптимизации ось преобразуется в систему треугольника, а не его вершины в мировую систему + t = tri.transform; + var ax:Number = t.a*axis.x + t.e*axis.y + t.i*axis.z; + var ay:Number = t.b*axis.x + t.f*axis.y + t.j*axis.z; + var az:Number = t.c*axis.x + t.g*axis.y + t.k*axis.z; + var max:Number = 0; + if (vectorProjection < 0) { + vectorProjection = -vectorProjection; + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d < max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d < max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d < max) max = d; + max = -max; + } else { + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d > max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d > max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d > max) max = d; + } + + return projection + max - vectorProjection; + } + + /** + * Определяет точки контакта бокса и треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function findFaceContactPoints(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIndex:int, contact:Contact):Boolean { + if (faceAxisIndex == 0) { + // Столкновение с плоскостью треугольника + return getBoxToTriContact(box, tri, toBox, contact); + } else { + // Столкновение с гранью бокса + return getTriToBoxContact(box, tri, toBox, faceAxisIndex, contact); + } + } + + /** + * Определяет точки контакта бокса с плоскостью треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getBoxToTriContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + colNormal.x = triTransform.c; + colNormal.y = triTransform.g; + colNormal.z = triTransform.k; + + var over:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + if (!over) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var incAxisDot:Number = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colNormal.x + axis.y*colNormal.y + axis.z*colNormal.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incAxisDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Обрезка грани + var negativeFace:Boolean = incAxisDot > 0; + getFaceVertsByAxis(box.hs, incFaceAxisIdx, negativeFace, points1); + boxTransform.transformVectorsN(points1, points2, 4); + triTransform.transformVectorsInverseN(points2, points1, 4); + var pnum:int = clipByTriangle(tri); + // Среди конечного списка точек определяются лежащие под плоскостью треугольника + var cp:ContactPoint; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points2[i]; + if ((over && v.z < 0) || (!over && v.z > 0)) { + cp = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z + triTransform.d; + cpPos.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z + triTransform.h; + cpPos.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z + triTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = over ? -v.z : v.z; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Оперделяет точки контакта треугольника с гранью бокса. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getTriToBoxContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + faceAxisIdx--; + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + boxTransform.getAxis(faceAxisIdx, colNormal); + var negativeFace:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + if (!negativeFace) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + var v:Vector3 = points1[0]; + v.x = tri.v0.x; + v.y = tri.v0.y; + v.z = tri.v0.z; + + v = points1[1]; + v.x = tri.v1.x; + v.y = tri.v1.y; + v.z = tri.v1.z; + + v = points1[2]; + v.x = tri.v2.x; + v.y = tri.v2.y; + v.z = tri.v2.z; + + triTransform.transformVectorsN(points1, points2, 3); + boxTransform.transformVectorsInverseN(points2, points1, 3); + + var pnum:int = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность боксу и добавляем такие точки в список контактов + var penetration:Number; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + v = points1[i]; + penetration = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace); + if (penetration > -epsilon) { + var cp:ContactPoint = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = penetration; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Вычисляет величину проникновения точки в бокс. + * + * @param hs вектор половинных размеров бокса + * @param p точка в системе координат бокса + * @param axisIndex индекс оси + * @param negativeFace если true, проверяется нижняя грань + * @return величина проникновения точки в бокс + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, negativeFace:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (negativeFace) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (negativeFace) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (negativeFace) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, вершины которой заданы в списке points1. Результат сохраняется в этом же списке. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 3; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Обрезает грань прямоугольника сторонами треугольника. Обрезка выполняется в системе координат и в проекции на + * плоскость треугольника. Входные вершины грани в количестве четырёх штук должны находиться в списке points1. + * Конечные вершины сохраняются в списке points2. + * + * @param tri треугольник + * @return количество врешин в конечном списке + */ + private function clipByTriangle(tri:CollisionTriangle):int { + var vnum:int = 4; + vnum = clipByLine(tri.v0, tri.e0, points1, vnum, points2); + if (vnum == 0) return 0; + vnum = clipByLine(tri.v1, tri.e1, points2, vnum, points1); + if (vnum == 0) return 0; + return clipByLine(tri.v2, tri.e2, points1, vnum, points2);; + } + + /** + * Обрезает полигон указанной прямой. + * + * @param linePoint точка на прямой + * @param lineDir единичный направляющий вектор прямой + * @param verticesIn список вершин исходного полигона + * @param vnum количество вершин исходного полигона + * @param verticesOut список, куда будут записаны вершины конечного полигона + * @return количество вершин конечного полигона + */ + private function clipByLine(linePoint:Vector3, lineDir:Vector3, verticesIn:Vector., vnum:int, verticesOut:Vector.):int { + var nx:Number = -lineDir.y; + var ny:Number = lineDir.x; + var offset:Number = linePoint.x*nx + linePoint.y*ny; + var v1:Vector3 = verticesIn[int(vnum - 1)]; + var offset1:Number = v1.x*nx + v1.y*ny; + var t:Number; + var v:Vector3; + var num:int = 0; + for (var i:int = 0; i < vnum; i++) { + var v2:Vector3 = verticesIn[i]; + var offset2:Number = v2.x*nx + v2.y*ny; + if (offset1 < offset) { + // Первая точка ребра во внешней полуплоскости + if (offset2 > offset) { + // Вторая точка ребра во внутренней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } else { + // Первая точка ребра во внутренней полуплоскости. Добавляем её в конечный список. + v = verticesOut[num]; + v.x = v1.x; + v.y = v1.y; + v.z = v1.z; + num++; + if (offset2 < offset) { + // Вторая точка ребра во внешней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } + v1 = v2; + offset1 = offset2; + } + return num; + } + + /** + * + * @param box + * @param tri + * @param toBox + * @param boxAxisIdx + * @param triAxisIdx + * @param contact + */ + private function findEdgesIntersection(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, boxAxisIdx:int, triAxisIdx:int, contact:Contact):void { + // Определение точки и направляющего вектора ребра треугольника + var tmpx1:Number; + var tmpy1:Number; + var tmpz1:Number; + var tmpx2:Number; + var tmpy2:Number; + var tmpz2:Number; + switch (triAxisIdx) { + case 0: + tmpx1 = tri.e0.x; + tmpy1 = tri.e0.y; + tmpz1 = tri.e0.z; + tmpx2 = tri.v0.x; + tmpy2 = tri.v0.y; + tmpz2 = tri.v0.z; + break; + case 1: + tmpx1 = tri.e1.x; + tmpy1 = tri.e1.y; + tmpz1 = tri.e1.z; + tmpx2 = tri.v1.x; + tmpy2 = tri.v1.y; + tmpz2 = tri.v1.z; + break; + case 2: + tmpx1 = tri.e2.x; + tmpy1 = tri.e2.y; + tmpz1 = tri.e2.z; + tmpx2 = tri.v2.x; + tmpy2 = tri.v2.y; + tmpz2 = tri.v2.z; + break; + } + var triTransform:Matrix4 = tri.transform; + axis20.x = triTransform.a*tmpx1 + triTransform.b*tmpy1 + triTransform.c*tmpz1; + axis20.y = triTransform.e*tmpx1 + triTransform.f*tmpy1 + triTransform.g*tmpz1; + axis20.z = triTransform.i*tmpx1 + triTransform.j*tmpy1 + triTransform.k*tmpz1; + var x2:Number = triTransform.a*tmpx2 + triTransform.b*tmpy2 + triTransform.c*tmpz2 + triTransform.d; + var y2:Number = triTransform.e*tmpx2 + triTransform.f*tmpy2 + triTransform.g*tmpz2 + triTransform.h; + var z2:Number = triTransform.i*tmpx2 + triTransform.j*tmpy2 + triTransform.k*tmpz2 + triTransform.l; + + // Определение нормали контакта, точки и направляющего вектора ребра бокса + var boxTransform:Matrix4 = box.transform; + boxTransform.getAxis(boxAxisIdx, axis10); + + // Нормаль контакта + var v:Vector3 = contact.normal; + v.x = axis10.y*axis20.z - axis10.z*axis20.y; + v.y = axis10.z*axis20.x - axis10.x*axis20.z; + v.z = axis10.x*axis20.y - axis10.y*axis20.x; + k = 1/Math.sqrt(v.x*v.x + v.y*v.y + v.z*v.z); + v.x *= k; + v.y *= k; + v.z *= k; + // Разворот нормали в сторону бокса + if (v.x*toBox.x + v.y*toBox.y + v.z*toBox.z < 0) { + v.x = -v.x; + v.y = -v.y; + v.z = -v.z; + } + + var boxHalfLen:Number; + tmpx1 = box.hs.x; + tmpy1 = box.hs.y; + tmpz1 = box.hs.z; + // X + if (boxAxisIdx == 0) { + tmpx1 = 0; + boxHalfLen = box.hs.x; + } else { + if (boxTransform.a*v.x + boxTransform.e*v.y + boxTransform.i*v.z > 0) { + tmpx1 = -tmpx1; + } + } + // Y + if (boxAxisIdx == 1) { + tmpy1 = 0; + boxHalfLen = box.hs.y; + } else { + if (boxTransform.b*v.x + boxTransform.f*v.y + boxTransform.j*v.z > 0) { + tmpy1 = -tmpy1; + } + } + // Z + if (boxAxisIdx == 2) { + tmpz1 = 0; + boxHalfLen = box.hs.z; + } else { + if (boxTransform.c*v.x + boxTransform.g*v.y + boxTransform.k*v.z > 0) { + tmpz1 = -tmpz1; + } + } + var x1:Number = boxTransform.a*tmpx1 + boxTransform.b*tmpy1 + boxTransform.c*tmpz1 + boxTransform.d; + var y1:Number = boxTransform.e*tmpx1 + boxTransform.f*tmpy1 + boxTransform.g*tmpz1 + boxTransform.h; + var z1:Number = boxTransform.i*tmpx1 + boxTransform.j*tmpy1 + boxTransform.k*tmpz1 + boxTransform.l; + + // Находим точку пересечения рёбер, решая систему уравнений + // axis10 - направляющий вектор ребра бокса + // x1, y1, z1 - средняя точка ребра бокса + // axis20 - направляющий вектор ребра треугольника + // x2, y2, z2 - начальная точка ребра треугольника + var k:Number = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + var vx:Number = x2 - x1; + var vy:Number = y2 - y1; + var vz:Number = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + + // Запись данных о контакте + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + v = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + v.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + v.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + v.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = v.x - boxTransform.d; + r.y = v.y - boxTransform.h; + r.z = v.z - boxTransform.l; + r = cp.r2; + r.x = v.x - triTransform.d; + r.y = v.y - triTransform.h; + r.z = v.z - triTransform.l; + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/BruteForceCollisionDetector.as.svn-base b/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/BruteForceCollisionDetector.as.svn-base new file mode 100644 index 0000000..85d84fa --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/BruteForceCollisionDetector.as.svn-base @@ -0,0 +1,63 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + public class BruteForceCollisionDetector implements ICollisionDetector { + public function BruteForceCollisionDetector() { + } + + public function addPrimitive(primitive:CollisionPrimitive, isStatic:Boolean=true):Boolean { + return false; + } + + public function removePrimitive(primitive:CollisionPrimitive, isStatic:Boolean=true):Boolean { + return false; + } + + public function init():void { + } + + public function getAllCollisions(contacts:Vector.):int { + return 0; + } + + public function intersectRay(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, intersection:RayIntersection):Boolean { + return false; + } + + public function intersectRayWithStatic(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean { + return false; + } + + public function testBodyPrimitiveCollision(body:Body, primitive:CollisionPrimitive):Boolean { + return false; + } + + /** + * + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + return false; + } + + /** + * + * @param prim1 + * @param prim2 + * @return + */ + public function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base b/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base new file mode 100644 index 0000000..e24922c --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base @@ -0,0 +1,18 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.types.BoundBox; + + public class CollisionKdNode { + public var indices:Vector.; + public var splitIndices:Vector.; + public var boundBox:BoundBox; + public var parent:CollisionKdNode; + public var splitTree:CollisionKdTree2D; + + public var axis:int = -1; // 0 - x, 1 - y, 2 - z + public var coord:Number; + public var positiveNode:CollisionKdNode; + public var negativeNode:CollisionKdNode; + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base b/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base new file mode 100644 index 0000000..c046f17 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base @@ -0,0 +1,280 @@ +package alternativa.physics.collision { + + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + + /** + * @author mike + */ + public class CollisionKdTree { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + public var rootNode:CollisionKdNode; + public var staticChildren:Vector. = new Vector.(); + public var numStaticChildren:int; + public var staticBoundBoxes:Vector. = new Vector.(); + + private var splitAxis:int; + private var splitCoord:Number; + private var splitCost:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + + /** + * @param primitive + */ + public function addStaticPrimitive(primitive:CollisionPrimitive):void { + staticChildren[numStaticChildren++] = primitive; + } + + /** + * @param primitive + * @return + */ + public function removeStaticPrimitive(primitive:CollisionPrimitive):Boolean { + var idx:int = staticChildren.indexOf(primitive); + if (idx < 0) return false; + staticChildren.splice(idx, 1); + numStaticChildren--; + return true; + } + + /** + * @param boundBox + */ + public function createTree(boundBox:BoundBox = null):void { + if (numStaticChildren == 0) return; + // Создаём корневую ноду + rootNode = new CollisionKdNode(); + rootNode.indices = new Vector.(); + // Расчитываем баунды объектов и рутовой ноды + var rootNodeBoundBox:BoundBox = rootNode.boundBox = (boundBox != null) ? boundBox : new BoundBox(); + for (var i:int = 0; i < numStaticChildren; ++i) { + var child:CollisionPrimitive = staticChildren[i]; + var childBoundBox:BoundBox = staticBoundBoxes[i] = child.calculateAABB(); + rootNodeBoundBox.addBoundBox(childBoundBox); + rootNode.indices[i] = i; + } + staticBoundBoxes.length = numStaticChildren; + // Разделяем рутовую ноду + splitNode(rootNode); + + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + var indices:Vector. = node.indices; + var numPrimitives:int = indices.length; + if (numPrimitives <= minPrimitivesPerNode) return; + + // Подготовка баунда с погрешностями + var nodeBoundBox:BoundBox = node.boundBox; + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + // Собираем опорные координаты + var i:int; + var j:int; + var numSplitCoordsX:int = 0 + var numSplitCoordsY:int = 0; + var numSplitCoordsZ:int = 0; + for (i = 0; i < numPrimitives; ++i) { + var boundBox:BoundBox = staticBoundBoxes[indices[i]]; + + if (boundBox.maxX - boundBox.minX <= doubleThreshold) { + if (boundBox.minX <= nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.minX; + else if (boundBox.maxX >= nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.maxX; + else splitCoordsX[numSplitCoordsX++] = (boundBox.minX + boundBox.maxX)*0.5; + } else { + if (boundBox.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = boundBox.minX; + if (boundBox.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = boundBox.maxX; + } + + if (boundBox.maxY - boundBox.minY <= doubleThreshold) { + if (boundBox.minY <= nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.minY; + else if (boundBox.maxY >= nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.maxY; + else splitCoordsY[numSplitCoordsY++] = (boundBox.minY + boundBox.maxY)*0.5; + } else { + if (boundBox.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = boundBox.minY; + if (boundBox.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = boundBox.maxY; + } + + if (boundBox.maxZ - boundBox.minZ <= doubleThreshold) { + if (boundBox.minZ <= nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.minZ; + else if (boundBox.maxZ >= nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.maxZ; + else splitCoordsZ[numSplitCoordsZ++] = (boundBox.minZ + boundBox.maxZ)*0.5; + } else { + if (boundBox.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.minZ; + if (boundBox.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numPrimitives; ++i) { + boundBox = staticBoundBoxes[indices[i]]; + var min:Number = axisX ? boundBox.minX : (axisY ? boundBox.minY : boundBox.minZ); + var max:Number = axisX ? boundBox.maxX : (axisY ? boundBox.maxY : boundBox.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(indices[i]); + indices[i] = -1; + } else { + if (node.splitIndices == null) node.splitIndices = new Vector.(); + node.splitIndices.push(indices[i]); + indices[i] = -1; + } + } else { + if (min >= coordMin) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(indices[i]); + indices[i] = -1; + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numPrimitives; ++i) { + if (indices[i] >= 0) indices[j++] = indices[i]; + } + if (j > 0) indices.length = j; + else node.indices = null; + + if (node.splitIndices != null) { + node.splitTree = new CollisionKdTree2D(this, node); + node.splitTree.createTree(); + } + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + /** + * + */ + public function traceTree():void { + traceNode("", rootNode); + } + + /** + * @param str + * @param node + */ + private function traceNode(str:String, node:CollisionKdNode):void { + if (node == null) return; + trace(str, node.axis == -1 ? "end" : ((node.axis == 0) ? "X" : ((node.axis == 1) ? "Y" : "Z")), "splitCoord=" + splitCoord, "bound", node.boundBox, "objs:", node.indices); + traceNode(str + "-", node.negativeNode); + traceNode(str + "+", node.positiveNode); + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base b/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base new file mode 100644 index 0000000..78aaa95 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base @@ -0,0 +1,226 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.types.BoundBox; + + /** + * + */ + public class CollisionKdTree2D { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + + public var parentTree:CollisionKdTree; + public var parentNode:CollisionKdNode; + public var rootNode:CollisionKdNode; + + private var splitAxis:int; + private var splitCost:Number; + private var splitCoord:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + /** + * + * @param parentTree + * @param parentNode + */ + public function CollisionKdTree2D(parentTree:CollisionKdTree, parentNode:CollisionKdNode) { + this.parentTree = parentTree; + this.parentNode = parentNode; + } + + /** + * + */ + public function createTree():void { + rootNode = new CollisionKdNode(); + rootNode.boundBox = parentNode.boundBox.clone(); + rootNode.indices = new Vector.(); + var numObjects:int = parentNode.splitIndices.length; + for (var i:int = 0; i < numObjects; ++i) rootNode.indices[i] = parentNode.splitIndices[i]; + + splitNode(rootNode); + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + if (node.indices.length <= minPrimitivesPerNode) return; + + var objects:Vector. = node.indices; + var i:int; + var j:int; + + var nodeBoundBox:BoundBox = node.boundBox; + + // Подготовка баунда с погрешностями + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + // Собираем опорные координаты + var numSplitCoordsX:int; + var numSplitCoordsY:int; + var numSplitCoordsZ:int; + var numObjects:int = objects.length; + for (i = 0; i < numObjects; ++i) { + var bb:BoundBox = staticBoundBoxes[objects[i]]; + + if (parentNode.axis != 0) { + if (bb.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = bb.minX; + if (bb.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = bb.maxX; + } + + if (parentNode.axis != 1) { + if (bb.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = bb.minY; + if (bb.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = bb.maxY; + } + + if (parentNode.axis != 2) { + if (bb.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = bb.minZ; + if (bb.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = bb.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + if (parentNode.axis != 0) checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + if (parentNode.axis != 1) checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + if (parentNode.axis != 2) checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numObjects; ++i) { + bb = staticBoundBoxes[objects[i]]; + var min:Number = axisX ? bb.minX : (axisY ? bb.minY : bb.minZ); + var max:Number = axisX ? bb.maxX : (axisY ? bb.maxY : bb.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(objects[i]); + objects[i] = -1; + } + } else { + if (min >= coordMin) { + if (max > coordMax) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(objects[i]); + objects[i] = -1; + } + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numObjects; ++i) { + if (objects[i] >= 0) objects[j++] = objects[i]; + } + if (j > 0) objects.length = j; + else node.indices = null; + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base b/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base new file mode 100644 index 0000000..9cfdca5 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Body; + + public interface IBodyCollisionPredicate { + function considerBodies(body1:Body, body2:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base b/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base new file mode 100644 index 0000000..220e717 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base @@ -0,0 +1,30 @@ +package alternativa.physics.collision { + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + + /** + * Интерфейс определителя столкновений между двумя примитивами. + */ + public interface ICollider { + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base b/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base new file mode 100644 index 0000000..af43dfb --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base @@ -0,0 +1,92 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + /** + * Интерфейс детектора столкновений. + */ + public interface ICollisionDetector { + + /** + * Добавляет физический примитив в коллайдер. + * + * @param primitive добавляемый примитив + * @param isStatic указывает тип примитива: статический или динамический + * @return true если примитив был успешно добавлен, иначе false + */ + function addPrimitive(primitive:CollisionPrimitive, isStatic:Boolean = true):Boolean; + + /** + * Удаляет физический примитив из коллайдера. + * + * @param primitive удаляемый примитив + * @param isStatic указывает тип примитива: статический или динамический + * @return true если примитив был успшено удалён, иначе false + */ + function removePrimitive(primitive:CollisionPrimitive, isStatic:Boolean = true):Boolean; + + /** + * Выполняет инициализацию детектора после обновления списка примитивов. + */ + function init():void; + + /** + * Получает все столкновения в текущей конфигурации физической геометрии. + * + * @param contacts список контактов, в кторые будет записана информация о столкновении + * @return количество найденных столкновений + */ + function getAllCollisions(contacts:Vector.):int; + + /** + * Тестирует луч на пересечение с физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param dir направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRay(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * Тестирует луч на пересечение со статической физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param dir направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRayWithStatic(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @return + */ + function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base b/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base new file mode 100644 index 0000000..bb1b756 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base @@ -0,0 +1,9 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + + public interface ICollisionPredicate { + + function considerCollision(primitive:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base b/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base new file mode 100644 index 0000000..0f3d929 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Body; + + public interface IRayCollisionPredicate { + function considerBody(body:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base b/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base new file mode 100644 index 0000000..7b0b78e --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base @@ -0,0 +1,484 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Детектор, хранящий статическую геометрию в kD-дереве и использующий дерево для ускорения тестов на пересечения. + */ + public class KdTreeCollisionDetector implements ICollisionDetector { + + altphysics var tree:CollisionKdTree; + altphysics var dynamicPrimitives:Vector.; + 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.(); + + 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():void { + tree.createTree(); +// tree.traceTree(); + } + + /** + * + * @param contacts + * @return + */ + public function getAllCollisions(contacts:Vector.):int { + var colNum:int = 0; + for (var i:int = 0; i < dynamicPrimitivesNum; i++) { + var primitive:CollisionPrimitive = dynamicPrimitives[i]; + primitive.calculateAABB(); + if (primitive.body != null && primitive.body.frozen) continue; + colNum += getPrimitiveNodeCollisions(tree.rootNode, primitive, contacts, colNum); + + // Столкновения динамических примитивов между собой + // TODO: Попробовать различные оптимизации (сортировка по баундам, встраивание в дерево) + for (var j:int = i + 1; j < dynamicPrimitivesNum; j++) { + if (collide(primitive, dynamicPrimitives[j], contacts[colNum])) colNum++; + } + } + + return colNum; + } + + /** + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + public function collide(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 = (contact.points[0] as ContactPoint).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:Vector., startIndex:int):int { + var colNum:int = 0; + if (node.indices != null) { + // Поиск столкновений со статическими примитивами узла + var primitives:Vector. = tree.staticChildren; + var indices:Vector. = node.indices; + for (var i:int = indices.length - 1; i >= 0; i--) + if (collide(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; + } + + 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., indices:Vector., 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; + } \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/SpherePlaneCollider.as.svn-base b/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/SpherePlaneCollider.as.svn-base new file mode 100644 index 0000000..77eb1b0 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/SpherePlaneCollider.as.svn-base @@ -0,0 +1,67 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + /** + * + */ + public class SpherePlaneCollider implements ICollider { + + private var normal:Vector3 = new Vector3(); + + /** + * + */ + public function SpherePlaneCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var sphere:RigidSphere = body1 as RigidSphere; +// var plane:RigidPlane; +// if (sphere == null) { +// sphere = body2 as RigidSphere; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// +// var dist:Number = sphere.state.pos.dot(normal) - offset; +// if (dist > sphere.r) return false; +// +// collisionInfo.body1 = sphere; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// collisionInfo.pcount = 1; +// +// var cp:ContactPoint = collisionInfo.points[0]; +// cp.penetration = sphere.r - dist; +// cp.pos.copy(normal).reverse().scale(sphere.r).add(sphere.state.pos); +// cp.r1.diff(cp.pos, sphere.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/SphereSphereCollider.as.svn-base b/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/SphereSphereCollider.as.svn-base new file mode 100644 index 0000000..25971d5 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/.svn/text-base/SphereSphereCollider.as.svn-base @@ -0,0 +1,69 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + public class SphereSphereCollider implements ICollider { + + private var p1:Vector3 = new Vector3(); + private var p2:Vector3 = new Vector3(); + + public function SphereSphereCollider() { + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var s1:CollisionSphere; + var s2:CollisionSphere; + if (prim1.body != null) { + s1 = prim1 as CollisionSphere; + s2 = prim2 as CollisionSphere; + } else { + s1 = prim2 as CollisionSphere; + s2 = prim1 as CollisionSphere; + } + + s1.transform.getAxis(3, p1); + s2.transform.getAxis(3, p2); + var dx:Number = p1.x - p2.x; + var dy:Number = p1.y - p2.y; + var dz:Number = p1.z - p2.z; + var len:Number = dx*dx + dy*dy + dz*dz; + var sum:Number = s1.r + s2.r; + if (len > sum*sum) return false; + len = Math.sqrt(len); + dx /= len; + dy /= len; + dz /= len; + + contact.body1 = s1.body; + contact.body2 = s2.body; + contact.normal.x = dx; + contact.normal.y = dy; + contact.normal.z = dz; + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = sum - len; + cp.pos.x = p1.x - dx*s1.r; + cp.pos.y = p1.y - dy*s1.r; + cp.pos.z = p1.z - dz*s1.r; + cp.r1.vDiff(cp.pos, p1); + cp.r2.vDiff(cp.pos, p2); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/BoxBoxCollider.as b/0.0.6.0/src/alternativa/physics/collision/BoxBoxCollider.as new file mode 100644 index 0000000..e8c4cef --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/BoxBoxCollider.as @@ -0,0 +1,579 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Расчитывает точки контакта двух боксов. Нормаль контакта направляется в сторону бокса с меньшим ID. + */ + public class BoxBoxCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + private var vectorToBox1:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var bestAxisIndex:int; + private var minOverlap:Number; + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + private var tmpPoints:Vector. = new Vector.(8, true); + private var pcount:int; + + /** + * + */ + public function BoxBoxCollider() { + for (var i:int = 0; i < 8; i++) { + tmpPoints[i] = new ContactPoint(); + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + + if (bestAxisIndex < 6) { + // Контакт грань-(грань|ребро|вершина) + findFaceContactPoints(box1, box2, vectorToBox1, bestAxisIndex, contact); + } else { + // Контакт ребро-ребро + bestAxisIndex -= 6; + findEdgesIntersection(box1, box2, vectorToBox1, int(bestAxisIndex/3), bestAxisIndex%3, contact); + } + contact.body1 = box1.body; + contact.body2 = box2.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + // Вектор из центра второго бокса в центр первого + vectorToBox1.x = transform1.d - transform2.d; + vectorToBox1.y = transform1.h - transform2.h; + vectorToBox1.z = transform1.l - transform2.l; + + // Проверка пересечения по основным осям + axis10.x = transform1.a; + axis10.y = transform1.e; + axis10.z = transform1.i; + if (!testMainAxis(box1, box2, axis10, 0, vectorToBox1)) return false; + axis11.x = transform1.b; + axis11.y = transform1.f; + axis11.z = transform1.j; + if (!testMainAxis(box1, box2, axis11, 1, vectorToBox1)) return false; + axis12.x = transform1.c; + axis12.y = transform1.g; + axis12.z = transform1.k; + if (!testMainAxis(box1, box2, axis12, 2, vectorToBox1)) return false; + + axis20.x = transform2.a; + axis20.y = transform2.e; + axis20.z = transform2.i; + if (!testMainAxis(box1, box2, axis20, 3, vectorToBox1)) return false; + axis21.x = transform2.b; + axis21.y = transform2.f; + axis21.z = transform2.j; + if (!testMainAxis(box1, box2, axis21, 4, vectorToBox1)) return false; + axis22.x = transform2.c; + axis22.y = transform2.g; + axis22.z = transform2.k; + if (!testMainAxis(box1, box2, axis22, 5, vectorToBox1)) return false; + + // Проверка производных осей + if (!testDerivedAxis(box1, box2, axis10, axis20, 6, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis21, 7, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis22, 8, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis11, axis20, 9, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis21, 10, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis22, 11, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis12, axis20, 12, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis21, 13, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis22, 14, vectorToBox1)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта грани одного бокса с гранью/ребром/вершиной другого. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param faceAxisIdx индекс оси первого бокса, перпендикулярной грани, с которой произошло столкновение + * @param contactInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, faceAxisIdx:int, contact:Contact):void { + var swapNormal:Boolean = false; + if (faceAxisIdx > 2) { + // Столкновение с гранью второго бокса. Для дальнейших расчётов боксы меняются местами, + // но нормаль контакта всё равно должна быть направлена в сторону первоначального box1 + var tmpBox:CollisionBox = box1; + box1 = box2; + box2 = tmpBox; + vectorToBox1.x = -vectorToBox1.x; + vectorToBox1.y = -vectorToBox1.y; + vectorToBox1.z = -vectorToBox1.z; + faceAxisIdx -= 3; + swapNormal = true; + } + + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + var colAxis:Vector3 = contact.normal; + + transform1.getAxis(faceAxisIdx, colAxis); + var negativeFace:Boolean = colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z > 0; + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + // Ищем ось второго бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + transform2.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisDot = dot; + incidentAxisIdx = axisIdx; + } + } + // Получаем список вершин грани второго бокса, переводим их в систему координат первого бокса и выполняем обрезку + // по грани первого бокса. Таким образом получается список потенциальных точек контакта. + transform2.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box2.hs, incidentAxisIdx, incidentAxisDot < 0, points1); + + // TODO: Вычислить результирующую матрицу, затем преобразовать векторы за один заход + transform2.transformVectorsN(points1, points2, 4); + transform1.transformVectorsInverseN(points2, points1, 4); + + var pnum:int = clip(box1.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points1[i]; + if ((pen = getPointBoxPenetration(box1.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + var cp:ContactPoint = tmpPoints[pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = transform1.a*v.x + transform1.b*v.y + transform1.c*v.z + transform1.d; + cpPos.y = transform1.e*v.x + transform1.f*v.y + transform1.g*v.z + transform1.h; + cpPos.z = transform1.i*v.x + transform1.j*v.y + transform1.k*v.z + transform1.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + + cp.penetration = pen; + } + } + if (swapNormal) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (pcount > 4) { + reducePoints(); + } + for (i = 0; i < pcount; i++) { + ContactPoint(contact.points[i]).copyFrom(tmpPoints[i]); + } + contact.pcount = pcount; + } + + /** + * + * @param contactInfo + */ + private function reducePoints():void { + var i:int; + var minIdx:int; + var cp1:ContactPoint; + var cp2:ContactPoint; + while (pcount > 4) { + var minLen:Number = 1e10; + var p1:Vector3 = ContactPoint(tmpPoints[pcount - 1]).pos; + var p2:Vector3; + for (i = 0; i < pcount; i++) { + p2 = ContactPoint(tmpPoints[i]).pos; + var dx:Number = p2.x - p1.x; + var dy:Number = p2.y - p1.y; + var dz:Number = p2.z - p1.z; + var len:Number = dx*dx + dy*dy + dz*dz; + if (len < minLen) { + minLen = len; + minIdx = i; + } + p1 = p2; + } + cp1 = tmpPoints[minIdx == 0 ? (pcount - 1) : (minIdx - 1)]; + cp2 = tmpPoints[minIdx]; + p1 = cp1.pos; + p2 = cp2.pos; + p2.x = 0.5*(p1.x + p2.x); + p2.y = 0.5*(p1.y + p2.y); + p2.z = 0.5*(p1.z + p2.z); + cp2.penetration = 0.5*(cp1.penetration + cp2.penetration); + if (minIdx > 0) { + for (i = minIdx; i < pcount; i++) { + tmpPoints[i - 1] = tmpPoints[i]; + } + tmpPoints[pcount - 1] = cp1; + } + pcount--; + } + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clip(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param contactInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):void { + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + transform1.getAxis(axisIdx1, axis10); + transform2.getAxis(axisIdx2, axis20); + + var colAxis:Vector3 = contact.normal; + + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону первого бокса + if (colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + + var tx:Number = box1.hs.x; + var ty:Number = box1.hs.y; + var tz:Number = box1.hs.z; + + var x2:Number = box2.hs.x; + var y2:Number = box2.hs.y; + var z2:Number = box2.hs.z; + // x + if (axisIdx1 == 0) { + tx = 0; + halfLen1 = box1.hs.x; + } else { + if (colAxis.x*transform1.a + colAxis.y*transform1.e + colAxis.z*transform1.i > 0) { + tx = -tx; + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = box2.hs.x; + } else { + if (colAxis.x*transform2.a + colAxis.y*transform2.e + colAxis.z*transform2.i < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + ty = 0; + halfLen1 = box1.hs.y; + } else { + if (colAxis.x*transform1.b + colAxis.y*transform1.f + colAxis.z*transform1.j > 0) { + ty = -ty; + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = box2.hs.y; + } else { + if (colAxis.x*transform2.b + colAxis.y*transform2.f + colAxis.z*transform2.j < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + tz = 0; + halfLen1 = box1.hs.z; + } else { + if (colAxis.x*transform1.c + colAxis.y*transform1.g + colAxis.z*transform1.k > 0) { + tz = -tz; + } + } + if (axisIdx2 == 2) { + z2 = 0; + halfLen2 = box2.hs.z; + } else { + if (colAxis.x*transform2.c + colAxis.y*transform2.g + colAxis.z*transform2.k < 0) { + z2 = -z2; + } + } + // Получаем глобальные координаты средних точек рёбер + var x1:Number = transform1.a*tx + transform1.b*ty + transform1.c*tz + transform1.d; + var y1:Number = transform1.e*tx + transform1.f*ty + transform1.g*tz + transform1.h; + var z1:Number = transform1.i*tx + transform1.j*ty + transform1.k*tz + transform1.l; + tx = x2; + ty = y2; + tz = z2; + x2 = transform2.a*tx + transform2.b*ty + transform2.c*tz + transform2.d; + y2 = transform2.e*tx + transform2.f*ty + transform2.g*tz + transform2.h; + z2 = transform2.i*tx + transform2.j*ty + transform2.k*tz + transform2.l; + + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + tx = x2 - x1; + ty = y2 - y1; + tz = z2 - z1; + var c1:Number = axis10.x*tx + axis10.y*ty + axis10.z*tz; + var c2:Number = axis20.x*tx + axis20.y*ty + axis20.z*tz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cp.pos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cp.pos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение боксов вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box1 + * @param box2 + * @param axis + * @param axisIndex + * @param toBox1 + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box1 + * @param box2 + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox1 + * @return + */ + private function testDerivedAxis(box1:CollisionBox, box2:CollisionBox, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия двух боксов вдоль заданной оси. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param axis ось + * @param vectorToBox1 вектор, соединяющий центр второго бокса с центром первого + * @return величина перекрытия боксов вдоль оси + */ + public function overlapOnAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, vectorToBox1:Vector3):Number { + var m:Matrix4 = box1.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box1.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box1.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box1.hs.z; + if (d < 0) d = -d; + projection += d; + + m = box2.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box2.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box2.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box2.hs.z; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox1.x*axis.x + vectorToBox1.y*axis.y + vectorToBox1.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} + import alternativa.physics.types.Vector3; + + class CollisionPointTmp { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + } diff --git a/0.0.6.0/src/alternativa/physics/collision/BoxCollider.as b/0.0.6.0/src/alternativa/physics/collision/BoxCollider.as new file mode 100644 index 0000000..137256e --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/BoxCollider.as @@ -0,0 +1,408 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + /** + * + */ + public class BoxCollider implements ICollider { + + /** + * + */ + public function BoxCollider() { + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + return false; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + protected function getFaceVertsByAxis(hs:Vector3, axisIdx:int, negativeFace:Boolean, result:Vector.):void { + var v:Vector3; + switch (axisIdx) { + case 0: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + } + break; + case 1: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + } + break; + case 2: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + } else { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } + break; + } + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + v = result[num++]; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + v = result[num++]; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + v = result[num++]; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + v = result[num++]; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + v = result[num++]; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + v = result[num++]; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num++]; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/BoxPlaneCollider.as b/0.0.6.0/src/alternativa/physics/collision/BoxPlaneCollider.as new file mode 100644 index 0000000..9817aac --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/BoxPlaneCollider.as @@ -0,0 +1,87 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + public class BoxPlaneCollider implements ICollider { + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var normal:Vector3 = new Vector3(); + + public function BoxPlaneCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var box:RigidBox = body1 as RigidBox; +// var plane:RigidPlane; +// if (box == null) { +// box = body2 as RigidBox; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные координаты вершин бокса +// var sx:Number = box.halfSize.x; +// var sy:Number = box.halfSize.y; +// var sz:Number = box.halfSize.z; +// (verts1[0] as Vector3).reset(-sx, -sy, -sz); +// (verts1[1] as Vector3).reset(sx, -sy, -sz); +// (verts1[2] as Vector3).reset(sx, sy, -sz); +// (verts1[3] as Vector3).reset(-sx, sy, -sz); +// (verts1[4] as Vector3).reset(-sx, -sy, sz); +// (verts1[5] as Vector3).reset(sx, -sy, sz); +// (verts1[6] as Vector3).reset(sx, sy, sz); +// (verts1[7] as Vector3).reset(-sx, sy, sz); +// +// box.transform.transformVectors(verts1, verts2); +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// // Проверяем наличие столкновений с каждой вершиной +// collisionInfo.pcount = 0; +// for (var i:int = 0; i < 8; i++) { +// // Вершина добавляется в список точек столкновения, если лежит под плоскостью +// var dist:Number = (verts2[i] as Vector3).dot(normal); +// if (dist < offset) { +// var cp:ContactPoint; +// if (collisionInfo.pcount == collisionInfo.points.length) { +// cp = new ContactPoint(); +// collisionInfo.points[collisionInfo.pcount] = cp; +// } else { +// cp = collisionInfo.points[collisionInfo.pcount]; +// } +// cp.pos.copy(verts2[i]); +// cp.r1.diff(cp.pos, box.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); +// cp.penetration = offset - dist; +// collisionInfo.pcount++; +// } +// } +// if (collisionInfo.pcount > 0) { +// collisionInfo.body1 = box; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// return true; +// } + return false; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/BoxRectCollider.as b/0.0.6.0/src/alternativa/physics/collision/BoxRectCollider.as new file mode 100644 index 0000000..de99234 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/BoxRectCollider.as @@ -0,0 +1,522 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + /** + * + */ + public class BoxRectCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + + private var vectorToBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxRectCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, rect, vectorToBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + findEdgesIntersection(box, rect, vectorToBox, int(bestAxisIndex/2), bestAxisIndex%2, contact); + } + contact.body1 = box.body; + contact.body2 = rect.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + // Вектор из центра прямоугольника в центр бокса + vectorToBox.x = boxTransform.d - rectTransform.d; + vectorToBox.y = boxTransform.h - rectTransform.h; + vectorToBox.z = boxTransform.l - rectTransform.l; + + // Проверка пересечения по нормали прямоугольника + rectTransform.getAxis(2, axis22); + if (!testMainAxis(box, rect, axis22, 0, vectorToBox)) return false; + + // Проверка пересечения по основным осям бокса + boxTransform.getAxis(0, axis10); + if (!testMainAxis(box, rect, axis10, 1, vectorToBox)) return false; + boxTransform.getAxis(1, axis11); + if (!testMainAxis(box, rect, axis11, 2, vectorToBox)) return false; + boxTransform.getAxis(2, axis12); + if (!testMainAxis(box, rect, axis12, 3, vectorToBox)) return false; + + // Получаем направляющие рёбер прямоугольника + rectTransform.getAxis(0, axis20); + rectTransform.getAxis(1, axis21); + + // Проверка производных осей + if (!testDerivedAxis(box, rect, axis10, axis20, 4, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis10, axis21, 5, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis11, axis20, 6, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis11, axis21, 7, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis12, axis20, 8, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis12, axis21, 9, vectorToBox)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта бокса с прямоугольником. + * + * @param box бокс + * @param rect прямоугольник + * @param vectorToBox вектор, направленный из центра прямоугольника в центр бокса + * @param faceAxisIdx индекс оси, идентифицирующей полскость столкновения (грань бокса или полскость прямоугольника) + * @param colInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var pnum:int; + var i:int; + var v:Vector3; + var cp:ContactPoint; + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + var colAxis:Vector3 = contact.normal; + if (faceAxisIdx == 0) { + // Столкновение с плоскостью прямоугольника + + // Проверим положение бокса относительно плоскости прямоугольника + colAxis.x = rectTransform.c; + colAxis.y = rectTransform.g; + colAxis.z = rectTransform.k; + +// var offset:Number = colAxis.x*rectTransform.d + colAxis.y*rectTransform.h + colAxis.z*rectTransform.l; +// if (bbPos.vDot(colAxis) < offset) return false; + + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisIdx = axisIdx; + incidentAxisDot = dot; + } + } + // Получаем список вершин грани бокса, переводим их в систему координат прямоугольника и выполняем обрезку + // по прямоугольнику. Таким образом получается список потенциальных точек контакта. + boxTransform.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box.hs, incidentAxisIdx, incidentAxisDot > 0, points1); + boxTransform.transformVectorsN(points1, points2, 4); + rectTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByRect(rect.hs); + // Проверяем каждую потенциальную точку на принадлежность нижней полуплоскости прямоугольника и добавляем такие точки в список контактов + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if (v.z < epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = -v.z; + var cpPos:Vector3 = cp.pos; + cpPos.x = rectTransform.a*v.x + rectTransform.b*v.y + rectTransform.c*v.z + rectTransform.d; + cpPos.y = rectTransform.e*v.x + rectTransform.f*v.y + rectTransform.g*v.z + rectTransform.h; + cpPos.z = rectTransform.i*v.x + rectTransform.j*v.y + rectTransform.k*v.z + rectTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } else { + // Столкновение с гранью бокса + faceAxisIdx--; + boxTransform.getAxis(faceAxisIdx, colAxis); + var negativeFace:Boolean = colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z > 0; + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (rectTransform.c*colAxis.x + rectTransform.g*colAxis.y + rectTransform.k*colAxis.z < 0) return false; + + getFaceVertsByAxis(rect.hs, 2, false, points1); + rectTransform.transformVectorsN(points1, points2, 4); + boxTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if ((pen = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = pen; + cpPos = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } + return true; + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * + * @param hs + * @return + */ + private function clipByRect(hs:Vector3):int { + var pnum:int = 4; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box первый бокс + * @param rect второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param colInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):void { + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + boxTransform.getAxis(axisIdx1, axis10); + rectTransform.getAxis(axisIdx2, axis20); + var colAxis:Vector3 = contact.normal; + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону бокса + if (colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + var vx:Number = box.hs.x; + var vy:Number = box.hs.y; + var vz:Number = box.hs.z; + var x2:Number = rect.hs.x; + var y2:Number = rect.hs.y; + var z2:Number = rect.hs.z; + // x + if (axisIdx1 == 0) { + vx = 0; + halfLen1 = box.hs.x; + } else { + if (boxTransform.a*colAxis.x + boxTransform.e*colAxis.y + boxTransform.i*colAxis.z > 0) { + vx = -vx; + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = rect.hs.x; + } else { + if (rectTransform.a*colAxis.x + rectTransform.e*colAxis.y + rectTransform.i*colAxis.z < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + vy = 0; + halfLen1 = box.hs.y; + } else { + if (boxTransform.b*colAxis.x + boxTransform.f*colAxis.y + boxTransform.j*colAxis.z > 0) { + vy = -vy; + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = rect.hs.y; + } else { + if (rectTransform.b*colAxis.x + rectTransform.f*colAxis.y + rectTransform.j*colAxis.z < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + vz = 0; + halfLen1 = box.hs.z; + } else { + if (boxTransform.c*colAxis.x + boxTransform.g*colAxis.y + boxTransform.k*colAxis.z > 0) { + vz = -vz; + } + } + // Получаем глобальные координаты средних точек рёбер + + var x1:Number = boxTransform.a*vx + boxTransform.b*vy + boxTransform.c*vz + boxTransform.d; + var y1:Number = boxTransform.e*vx + boxTransform.f*vy + boxTransform.g*vz + boxTransform.h; + var z1:Number = boxTransform.i*vx + boxTransform.j*vy + boxTransform.k*vz + boxTransform.l; + vx = x2; + vy = y2; + vz = z2; + x2 = rectTransform.a*vx + rectTransform.b*vy + rectTransform.c*vz + rectTransform.d; + y2 = rectTransform.e*vx + rectTransform.f*vy + rectTransform.g*vz + rectTransform.h; + z2 = rectTransform.i*vx + rectTransform.j*vy + rectTransform.k*vz + rectTransform.l; + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + vx = x2 - x1; + vy = y2 - y1; + vz = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cpPos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cpPos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cpPos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var v:Vector3 = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + + /** + * Проверяет пересечение вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box + * @param rect + * @param axis + * @param axisIndex + * @param vectorToBox + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param rect + * @param axis1 + * @param axis2 + * @param axisIndex + * @param vectorToBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, rect:CollisionRect, axis1:Vector3, axis2:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия вдоль заданной оси. + * + * @param box бокс + * @param rect прямоугольник + * @param axis ось + * @param vectorToBox вектор, соединяющий центр прямоугольника с центром бокса + * @return величина перекрытия вдоль оси + */ + public function overlapOnAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, vectorToBox:Vector3):Number { + var m:Matrix4 = box.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box.hs.z; + if (d < 0) d = -d; + projection += d; + + m = rect.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*rect.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*rect.hs.y; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox.x*axis.x + vectorToBox.y*axis.y + vectorToBox.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/BoxSphereCollider.as b/0.0.6.0/src/alternativa/physics/collision/BoxSphereCollider.as new file mode 100644 index 0000000..27ae562 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/BoxSphereCollider.as @@ -0,0 +1,113 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + use namespace altphysics; + /** + * + */ + public class BoxSphereCollider implements ICollider { + + private var center:Vector3 = new Vector3(); + private var closestPt:Vector3 = new Vector3(); + + private var bPos:Vector3 = new Vector3(); + private var sPos:Vector3 = new Vector3(); + + /** + * + */ + public function BoxSphereCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + // Зафиксированно столкновение + contact.body1 = sphere.body; + contact.body2 = box.body; + contact.normal.vCopy(closestPt).vTransformBy4(box.transform).vSubtract(sPos).vNormalize().vReverse(); + contact.pcount = 1; + + var cp:ContactPoint = contact.points[0]; + cp.penetration = sphere.r - Math.sqrt(distSqr); + cp.pos.vCopy(contact.normal).vScale(-sphere.r).vAdd(sPos); + cp.r1.vDiff(cp.pos, sPos); + cp.r2.vDiff(cp.pos, bPos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/BoxTriangleCollider.as b/0.0.6.0/src/alternativa/physics/collision/BoxTriangleCollider.as new file mode 100644 index 0000000..23f1874 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/BoxTriangleCollider.as @@ -0,0 +1,682 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionTriangle; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + + /** + * + */ + public class BoxTriangleCollider extends BoxCollider { + + public var epsilon:Number = 0.001; + + private var bestAxisIndex:int; + private var minOverlap:Number; + private var toBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var colNormal:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxTriangleCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, tri, toBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + findEdgesIntersection(box, tri, toBox, bestAxisIndex%3, int(bestAxisIndex/3), contact); + } + + contact.body1 = box.body; + contact.body2 = tri.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + toBox.x = boxTransform.d - triTransform.d; + toBox.y = boxTransform.h - triTransform.h; + toBox.z = boxTransform.l - triTransform.l; + + minOverlap = 1e308; + + // Сначала проверяется нормаль треугольника + axis.x = triTransform.c; + axis.y = triTransform.g; + axis.z = triTransform.k; + if (!testMainAxis(box, tri, axis, 0, toBox)) return false; + + // Проверка основных осей бокса + axis10.x = boxTransform.a; + axis10.y = boxTransform.e; + axis10.z = boxTransform.i; + if (!testMainAxis(box, tri, axis10, 1, toBox)) return false; + axis11.x = boxTransform.b; + axis11.y = boxTransform.f; + axis11.z = boxTransform.j; + if (!testMainAxis(box, tri, axis11, 2, toBox)) return false; + axis12.x = boxTransform.c; + axis12.y = boxTransform.g; + axis12.z = boxTransform.k; + if (!testMainAxis(box, tri, axis12, 3, toBox)) return false; + + // Проверка производных осей + // TODO: заменить вычисления векторных произведений инлайнами + var v:Vector3 = tri.e0; + axis20.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis20.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis20.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis20, 4, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis20, 5, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis20, 6, toBox)) return false; + + v = tri.e1; + axis21.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis21.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis21.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis21, 7, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis21, 8, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis21, 9, toBox)) return false; + + v = tri.e2; + axis22.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis22.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis22.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis22, 10, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis22, 11, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis22, 12, toBox)) return false; + + return true; + } + + /** + * Тестирует пересечение примитивов вдоль заданной оси. + * + * @param box бокс + * @param tri треугольник + * @param axis ось + * @param axisIndex индекс оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return true, если примитивы имеют перекрытие проекций вдоль указанной оси, иначе false + */ + private function testMainAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, axisIndex:int, toBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param tri + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, tri:CollisionTriangle, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Рассчитывает величину перекрытия проекций бокса и треугольника на заданную ось. + * + * @param box бокс + * @param tri треугольник + * @param axis единичный направляющий вектор оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return величина перекрытия. Положительное значение указывает на наличие перекрытия, нулевое или отрицательное значение указывает на отсутствие перекрытия. + */ + private function overlapOnAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, toBox:Vector3):Number { + var t:Matrix4 = box.transform; + var projection:Number = (t.a*axis.x + t.e*axis.y + t.i*axis.z)*box.hs.x; + if (projection < 0) projection = -projection; + var d:Number = (t.b*axis.x + t.f*axis.y + t.j*axis.z)*box.hs.y; + projection += d < 0 ? -d : d; + d = (t.c*axis.x + t.g*axis.y + t.k*axis.z)*box.hs.z; + projection += d < 0 ? -d : d; + + var vectorProjection:Number = toBox.x*axis.x + toBox.y*axis.y + toBox.z*axis.z; + // Для оптимизации ось преобразуется в систему треугольника, а не его вершины в мировую систему + t = tri.transform; + var ax:Number = t.a*axis.x + t.e*axis.y + t.i*axis.z; + var ay:Number = t.b*axis.x + t.f*axis.y + t.j*axis.z; + var az:Number = t.c*axis.x + t.g*axis.y + t.k*axis.z; + var max:Number = 0; + if (vectorProjection < 0) { + vectorProjection = -vectorProjection; + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d < max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d < max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d < max) max = d; + max = -max; + } else { + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d > max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d > max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d > max) max = d; + } + + return projection + max - vectorProjection; + } + + /** + * Определяет точки контакта бокса и треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function findFaceContactPoints(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIndex:int, contact:Contact):Boolean { + if (faceAxisIndex == 0) { + // Столкновение с плоскостью треугольника + return getBoxToTriContact(box, tri, toBox, contact); + } else { + // Столкновение с гранью бокса + return getTriToBoxContact(box, tri, toBox, faceAxisIndex, contact); + } + } + + /** + * Определяет точки контакта бокса с плоскостью треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getBoxToTriContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + colNormal.x = triTransform.c; + colNormal.y = triTransform.g; + colNormal.z = triTransform.k; + + var over:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + if (!over) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var incAxisDot:Number = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colNormal.x + axis.y*colNormal.y + axis.z*colNormal.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incAxisDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Обрезка грани + var negativeFace:Boolean = incAxisDot > 0; + getFaceVertsByAxis(box.hs, incFaceAxisIdx, negativeFace, points1); + boxTransform.transformVectorsN(points1, points2, 4); + triTransform.transformVectorsInverseN(points2, points1, 4); + var pnum:int = clipByTriangle(tri); + // Среди конечного списка точек определяются лежащие под плоскостью треугольника + var cp:ContactPoint; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points2[i]; + if ((over && v.z < 0) || (!over && v.z > 0)) { + cp = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z + triTransform.d; + cpPos.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z + triTransform.h; + cpPos.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z + triTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = over ? -v.z : v.z; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Оперделяет точки контакта треугольника с гранью бокса. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getTriToBoxContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + faceAxisIdx--; + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + boxTransform.getAxis(faceAxisIdx, colNormal); + var negativeFace:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + if (!negativeFace) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + var v:Vector3 = points1[0]; + v.x = tri.v0.x; + v.y = tri.v0.y; + v.z = tri.v0.z; + + v = points1[1]; + v.x = tri.v1.x; + v.y = tri.v1.y; + v.z = tri.v1.z; + + v = points1[2]; + v.x = tri.v2.x; + v.y = tri.v2.y; + v.z = tri.v2.z; + + triTransform.transformVectorsN(points1, points2, 3); + boxTransform.transformVectorsInverseN(points2, points1, 3); + + var pnum:int = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность боксу и добавляем такие точки в список контактов + var penetration:Number; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + v = points1[i]; + penetration = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace); + if (penetration > -epsilon) { + var cp:ContactPoint = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = penetration; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Вычисляет величину проникновения точки в бокс. + * + * @param hs вектор половинных размеров бокса + * @param p точка в системе координат бокса + * @param axisIndex индекс оси + * @param negativeFace если true, проверяется нижняя грань + * @return величина проникновения точки в бокс + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, negativeFace:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (negativeFace) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (negativeFace) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (negativeFace) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, вершины которой заданы в списке points1. Результат сохраняется в этом же списке. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 3; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Обрезает грань прямоугольника сторонами треугольника. Обрезка выполняется в системе координат и в проекции на + * плоскость треугольника. Входные вершины грани в количестве четырёх штук должны находиться в списке points1. + * Конечные вершины сохраняются в списке points2. + * + * @param tri треугольник + * @return количество врешин в конечном списке + */ + private function clipByTriangle(tri:CollisionTriangle):int { + var vnum:int = 4; + vnum = clipByLine(tri.v0, tri.e0, points1, vnum, points2); + if (vnum == 0) return 0; + vnum = clipByLine(tri.v1, tri.e1, points2, vnum, points1); + if (vnum == 0) return 0; + return clipByLine(tri.v2, tri.e2, points1, vnum, points2);; + } + + /** + * Обрезает полигон указанной прямой. + * + * @param linePoint точка на прямой + * @param lineDir единичный направляющий вектор прямой + * @param verticesIn список вершин исходного полигона + * @param vnum количество вершин исходного полигона + * @param verticesOut список, куда будут записаны вершины конечного полигона + * @return количество вершин конечного полигона + */ + private function clipByLine(linePoint:Vector3, lineDir:Vector3, verticesIn:Vector., vnum:int, verticesOut:Vector.):int { + var nx:Number = -lineDir.y; + var ny:Number = lineDir.x; + var offset:Number = linePoint.x*nx + linePoint.y*ny; + var v1:Vector3 = verticesIn[int(vnum - 1)]; + var offset1:Number = v1.x*nx + v1.y*ny; + var t:Number; + var v:Vector3; + var num:int = 0; + for (var i:int = 0; i < vnum; i++) { + var v2:Vector3 = verticesIn[i]; + var offset2:Number = v2.x*nx + v2.y*ny; + if (offset1 < offset) { + // Первая точка ребра во внешней полуплоскости + if (offset2 > offset) { + // Вторая точка ребра во внутренней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } else { + // Первая точка ребра во внутренней полуплоскости. Добавляем её в конечный список. + v = verticesOut[num]; + v.x = v1.x; + v.y = v1.y; + v.z = v1.z; + num++; + if (offset2 < offset) { + // Вторая точка ребра во внешней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } + v1 = v2; + offset1 = offset2; + } + return num; + } + + /** + * + * @param box + * @param tri + * @param toBox + * @param boxAxisIdx + * @param triAxisIdx + * @param contact + */ + private function findEdgesIntersection(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, boxAxisIdx:int, triAxisIdx:int, contact:Contact):void { + // Определение точки и направляющего вектора ребра треугольника + var tmpx1:Number; + var tmpy1:Number; + var tmpz1:Number; + var tmpx2:Number; + var tmpy2:Number; + var tmpz2:Number; + switch (triAxisIdx) { + case 0: + tmpx1 = tri.e0.x; + tmpy1 = tri.e0.y; + tmpz1 = tri.e0.z; + tmpx2 = tri.v0.x; + tmpy2 = tri.v0.y; + tmpz2 = tri.v0.z; + break; + case 1: + tmpx1 = tri.e1.x; + tmpy1 = tri.e1.y; + tmpz1 = tri.e1.z; + tmpx2 = tri.v1.x; + tmpy2 = tri.v1.y; + tmpz2 = tri.v1.z; + break; + case 2: + tmpx1 = tri.e2.x; + tmpy1 = tri.e2.y; + tmpz1 = tri.e2.z; + tmpx2 = tri.v2.x; + tmpy2 = tri.v2.y; + tmpz2 = tri.v2.z; + break; + } + var triTransform:Matrix4 = tri.transform; + axis20.x = triTransform.a*tmpx1 + triTransform.b*tmpy1 + triTransform.c*tmpz1; + axis20.y = triTransform.e*tmpx1 + triTransform.f*tmpy1 + triTransform.g*tmpz1; + axis20.z = triTransform.i*tmpx1 + triTransform.j*tmpy1 + triTransform.k*tmpz1; + var x2:Number = triTransform.a*tmpx2 + triTransform.b*tmpy2 + triTransform.c*tmpz2 + triTransform.d; + var y2:Number = triTransform.e*tmpx2 + triTransform.f*tmpy2 + triTransform.g*tmpz2 + triTransform.h; + var z2:Number = triTransform.i*tmpx2 + triTransform.j*tmpy2 + triTransform.k*tmpz2 + triTransform.l; + + // Определение нормали контакта, точки и направляющего вектора ребра бокса + var boxTransform:Matrix4 = box.transform; + boxTransform.getAxis(boxAxisIdx, axis10); + + // Нормаль контакта + var v:Vector3 = contact.normal; + v.x = axis10.y*axis20.z - axis10.z*axis20.y; + v.y = axis10.z*axis20.x - axis10.x*axis20.z; + v.z = axis10.x*axis20.y - axis10.y*axis20.x; + k = 1/Math.sqrt(v.x*v.x + v.y*v.y + v.z*v.z); + v.x *= k; + v.y *= k; + v.z *= k; + // Разворот нормали в сторону бокса + if (v.x*toBox.x + v.y*toBox.y + v.z*toBox.z < 0) { + v.x = -v.x; + v.y = -v.y; + v.z = -v.z; + } + + var boxHalfLen:Number; + tmpx1 = box.hs.x; + tmpy1 = box.hs.y; + tmpz1 = box.hs.z; + // X + if (boxAxisIdx == 0) { + tmpx1 = 0; + boxHalfLen = box.hs.x; + } else { + if (boxTransform.a*v.x + boxTransform.e*v.y + boxTransform.i*v.z > 0) { + tmpx1 = -tmpx1; + } + } + // Y + if (boxAxisIdx == 1) { + tmpy1 = 0; + boxHalfLen = box.hs.y; + } else { + if (boxTransform.b*v.x + boxTransform.f*v.y + boxTransform.j*v.z > 0) { + tmpy1 = -tmpy1; + } + } + // Z + if (boxAxisIdx == 2) { + tmpz1 = 0; + boxHalfLen = box.hs.z; + } else { + if (boxTransform.c*v.x + boxTransform.g*v.y + boxTransform.k*v.z > 0) { + tmpz1 = -tmpz1; + } + } + var x1:Number = boxTransform.a*tmpx1 + boxTransform.b*tmpy1 + boxTransform.c*tmpz1 + boxTransform.d; + var y1:Number = boxTransform.e*tmpx1 + boxTransform.f*tmpy1 + boxTransform.g*tmpz1 + boxTransform.h; + var z1:Number = boxTransform.i*tmpx1 + boxTransform.j*tmpy1 + boxTransform.k*tmpz1 + boxTransform.l; + + // Находим точку пересечения рёбер, решая систему уравнений + // axis10 - направляющий вектор ребра бокса + // x1, y1, z1 - средняя точка ребра бокса + // axis20 - направляющий вектор ребра треугольника + // x2, y2, z2 - начальная точка ребра треугольника + var k:Number = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + var vx:Number = x2 - x1; + var vy:Number = y2 - y1; + var vz:Number = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + + // Запись данных о контакте + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + v = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + v.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + v.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + v.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = v.x - boxTransform.d; + r.y = v.y - boxTransform.h; + r.z = v.z - boxTransform.l; + r = cp.r2; + r.x = v.x - triTransform.d; + r.y = v.y - triTransform.h; + r.z = v.z - triTransform.l; + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/BruteForceCollisionDetector.as b/0.0.6.0/src/alternativa/physics/collision/BruteForceCollisionDetector.as new file mode 100644 index 0000000..85d84fa --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/BruteForceCollisionDetector.as @@ -0,0 +1,63 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + public class BruteForceCollisionDetector implements ICollisionDetector { + public function BruteForceCollisionDetector() { + } + + public function addPrimitive(primitive:CollisionPrimitive, isStatic:Boolean=true):Boolean { + return false; + } + + public function removePrimitive(primitive:CollisionPrimitive, isStatic:Boolean=true):Boolean { + return false; + } + + public function init():void { + } + + public function getAllCollisions(contacts:Vector.):int { + return 0; + } + + public function intersectRay(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, intersection:RayIntersection):Boolean { + return false; + } + + public function intersectRayWithStatic(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean { + return false; + } + + public function testBodyPrimitiveCollision(body:Body, primitive:CollisionPrimitive):Boolean { + return false; + } + + /** + * + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + return false; + } + + /** + * + * @param prim1 + * @param prim2 + * @return + */ + public function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/CollisionKdNode.as b/0.0.6.0/src/alternativa/physics/collision/CollisionKdNode.as new file mode 100644 index 0000000..e24922c --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/CollisionKdNode.as @@ -0,0 +1,18 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.types.BoundBox; + + public class CollisionKdNode { + public var indices:Vector.; + public var splitIndices:Vector.; + public var boundBox:BoundBox; + public var parent:CollisionKdNode; + public var splitTree:CollisionKdTree2D; + + public var axis:int = -1; // 0 - x, 1 - y, 2 - z + public var coord:Number; + public var positiveNode:CollisionKdNode; + public var negativeNode:CollisionKdNode; + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/CollisionKdTree.as b/0.0.6.0/src/alternativa/physics/collision/CollisionKdTree.as new file mode 100644 index 0000000..c046f17 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/CollisionKdTree.as @@ -0,0 +1,280 @@ +package alternativa.physics.collision { + + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + + /** + * @author mike + */ + public class CollisionKdTree { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + public var rootNode:CollisionKdNode; + public var staticChildren:Vector. = new Vector.(); + public var numStaticChildren:int; + public var staticBoundBoxes:Vector. = new Vector.(); + + private var splitAxis:int; + private var splitCoord:Number; + private var splitCost:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + + /** + * @param primitive + */ + public function addStaticPrimitive(primitive:CollisionPrimitive):void { + staticChildren[numStaticChildren++] = primitive; + } + + /** + * @param primitive + * @return + */ + public function removeStaticPrimitive(primitive:CollisionPrimitive):Boolean { + var idx:int = staticChildren.indexOf(primitive); + if (idx < 0) return false; + staticChildren.splice(idx, 1); + numStaticChildren--; + return true; + } + + /** + * @param boundBox + */ + public function createTree(boundBox:BoundBox = null):void { + if (numStaticChildren == 0) return; + // Создаём корневую ноду + rootNode = new CollisionKdNode(); + rootNode.indices = new Vector.(); + // Расчитываем баунды объектов и рутовой ноды + var rootNodeBoundBox:BoundBox = rootNode.boundBox = (boundBox != null) ? boundBox : new BoundBox(); + for (var i:int = 0; i < numStaticChildren; ++i) { + var child:CollisionPrimitive = staticChildren[i]; + var childBoundBox:BoundBox = staticBoundBoxes[i] = child.calculateAABB(); + rootNodeBoundBox.addBoundBox(childBoundBox); + rootNode.indices[i] = i; + } + staticBoundBoxes.length = numStaticChildren; + // Разделяем рутовую ноду + splitNode(rootNode); + + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + var indices:Vector. = node.indices; + var numPrimitives:int = indices.length; + if (numPrimitives <= minPrimitivesPerNode) return; + + // Подготовка баунда с погрешностями + var nodeBoundBox:BoundBox = node.boundBox; + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + // Собираем опорные координаты + var i:int; + var j:int; + var numSplitCoordsX:int = 0 + var numSplitCoordsY:int = 0; + var numSplitCoordsZ:int = 0; + for (i = 0; i < numPrimitives; ++i) { + var boundBox:BoundBox = staticBoundBoxes[indices[i]]; + + if (boundBox.maxX - boundBox.minX <= doubleThreshold) { + if (boundBox.minX <= nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.minX; + else if (boundBox.maxX >= nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.maxX; + else splitCoordsX[numSplitCoordsX++] = (boundBox.minX + boundBox.maxX)*0.5; + } else { + if (boundBox.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = boundBox.minX; + if (boundBox.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = boundBox.maxX; + } + + if (boundBox.maxY - boundBox.minY <= doubleThreshold) { + if (boundBox.minY <= nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.minY; + else if (boundBox.maxY >= nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.maxY; + else splitCoordsY[numSplitCoordsY++] = (boundBox.minY + boundBox.maxY)*0.5; + } else { + if (boundBox.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = boundBox.minY; + if (boundBox.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = boundBox.maxY; + } + + if (boundBox.maxZ - boundBox.minZ <= doubleThreshold) { + if (boundBox.minZ <= nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.minZ; + else if (boundBox.maxZ >= nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.maxZ; + else splitCoordsZ[numSplitCoordsZ++] = (boundBox.minZ + boundBox.maxZ)*0.5; + } else { + if (boundBox.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.minZ; + if (boundBox.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numPrimitives; ++i) { + boundBox = staticBoundBoxes[indices[i]]; + var min:Number = axisX ? boundBox.minX : (axisY ? boundBox.minY : boundBox.minZ); + var max:Number = axisX ? boundBox.maxX : (axisY ? boundBox.maxY : boundBox.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(indices[i]); + indices[i] = -1; + } else { + if (node.splitIndices == null) node.splitIndices = new Vector.(); + node.splitIndices.push(indices[i]); + indices[i] = -1; + } + } else { + if (min >= coordMin) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(indices[i]); + indices[i] = -1; + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numPrimitives; ++i) { + if (indices[i] >= 0) indices[j++] = indices[i]; + } + if (j > 0) indices.length = j; + else node.indices = null; + + if (node.splitIndices != null) { + node.splitTree = new CollisionKdTree2D(this, node); + node.splitTree.createTree(); + } + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + /** + * + */ + public function traceTree():void { + traceNode("", rootNode); + } + + /** + * @param str + * @param node + */ + private function traceNode(str:String, node:CollisionKdNode):void { + if (node == null) return; + trace(str, node.axis == -1 ? "end" : ((node.axis == 0) ? "X" : ((node.axis == 1) ? "Y" : "Z")), "splitCoord=" + splitCoord, "bound", node.boundBox, "objs:", node.indices); + traceNode(str + "-", node.negativeNode); + traceNode(str + "+", node.positiveNode); + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/CollisionKdTree2D.as b/0.0.6.0/src/alternativa/physics/collision/CollisionKdTree2D.as new file mode 100644 index 0000000..78aaa95 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/CollisionKdTree2D.as @@ -0,0 +1,226 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.types.BoundBox; + + /** + * + */ + public class CollisionKdTree2D { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + + public var parentTree:CollisionKdTree; + public var parentNode:CollisionKdNode; + public var rootNode:CollisionKdNode; + + private var splitAxis:int; + private var splitCost:Number; + private var splitCoord:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + /** + * + * @param parentTree + * @param parentNode + */ + public function CollisionKdTree2D(parentTree:CollisionKdTree, parentNode:CollisionKdNode) { + this.parentTree = parentTree; + this.parentNode = parentNode; + } + + /** + * + */ + public function createTree():void { + rootNode = new CollisionKdNode(); + rootNode.boundBox = parentNode.boundBox.clone(); + rootNode.indices = new Vector.(); + var numObjects:int = parentNode.splitIndices.length; + for (var i:int = 0; i < numObjects; ++i) rootNode.indices[i] = parentNode.splitIndices[i]; + + splitNode(rootNode); + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + if (node.indices.length <= minPrimitivesPerNode) return; + + var objects:Vector. = node.indices; + var i:int; + var j:int; + + var nodeBoundBox:BoundBox = node.boundBox; + + // Подготовка баунда с погрешностями + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + // Собираем опорные координаты + var numSplitCoordsX:int; + var numSplitCoordsY:int; + var numSplitCoordsZ:int; + var numObjects:int = objects.length; + for (i = 0; i < numObjects; ++i) { + var bb:BoundBox = staticBoundBoxes[objects[i]]; + + if (parentNode.axis != 0) { + if (bb.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = bb.minX; + if (bb.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = bb.maxX; + } + + if (parentNode.axis != 1) { + if (bb.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = bb.minY; + if (bb.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = bb.maxY; + } + + if (parentNode.axis != 2) { + if (bb.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = bb.minZ; + if (bb.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = bb.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + if (parentNode.axis != 0) checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + if (parentNode.axis != 1) checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + if (parentNode.axis != 2) checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numObjects; ++i) { + bb = staticBoundBoxes[objects[i]]; + var min:Number = axisX ? bb.minX : (axisY ? bb.minY : bb.minZ); + var max:Number = axisX ? bb.maxX : (axisY ? bb.maxY : bb.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(objects[i]); + objects[i] = -1; + } + } else { + if (min >= coordMin) { + if (max > coordMax) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(objects[i]); + objects[i] = -1; + } + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numObjects; ++i) { + if (objects[i] >= 0) objects[j++] = objects[i]; + } + if (j > 0) objects.length = j; + else node.indices = null; + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/IBodyCollisionPredicate.as b/0.0.6.0/src/alternativa/physics/collision/IBodyCollisionPredicate.as new file mode 100644 index 0000000..9cfdca5 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/IBodyCollisionPredicate.as @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Body; + + public interface IBodyCollisionPredicate { + function considerBodies(body1:Body, body2:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/ICollider.as b/0.0.6.0/src/alternativa/physics/collision/ICollider.as new file mode 100644 index 0000000..220e717 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/ICollider.as @@ -0,0 +1,30 @@ +package alternativa.physics.collision { + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + + /** + * Интерфейс определителя столкновений между двумя примитивами. + */ + public interface ICollider { + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/ICollisionDetector.as b/0.0.6.0/src/alternativa/physics/collision/ICollisionDetector.as new file mode 100644 index 0000000..af43dfb --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/ICollisionDetector.as @@ -0,0 +1,92 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + /** + * Интерфейс детектора столкновений. + */ + public interface ICollisionDetector { + + /** + * Добавляет физический примитив в коллайдер. + * + * @param primitive добавляемый примитив + * @param isStatic указывает тип примитива: статический или динамический + * @return true если примитив был успешно добавлен, иначе false + */ + function addPrimitive(primitive:CollisionPrimitive, isStatic:Boolean = true):Boolean; + + /** + * Удаляет физический примитив из коллайдера. + * + * @param primitive удаляемый примитив + * @param isStatic указывает тип примитива: статический или динамический + * @return true если примитив был успшено удалён, иначе false + */ + function removePrimitive(primitive:CollisionPrimitive, isStatic:Boolean = true):Boolean; + + /** + * Выполняет инициализацию детектора после обновления списка примитивов. + */ + function init():void; + + /** + * Получает все столкновения в текущей конфигурации физической геометрии. + * + * @param contacts список контактов, в кторые будет записана информация о столкновении + * @return количество найденных столкновений + */ + function getAllCollisions(contacts:Vector.):int; + + /** + * Тестирует луч на пересечение с физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param dir направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRay(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * Тестирует луч на пересечение со статической физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param dir направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRayWithStatic(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @return + */ + function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/ICollisionPredicate.as b/0.0.6.0/src/alternativa/physics/collision/ICollisionPredicate.as new file mode 100644 index 0000000..bb1b756 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/ICollisionPredicate.as @@ -0,0 +1,9 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + + public interface ICollisionPredicate { + + function considerCollision(primitive:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/IRayCollisionPredicate.as b/0.0.6.0/src/alternativa/physics/collision/IRayCollisionPredicate.as new file mode 100644 index 0000000..0f3d929 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/IRayCollisionPredicate.as @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Body; + + public interface IRayCollisionPredicate { + function considerBody(body:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/KdTreeCollisionDetector.as b/0.0.6.0/src/alternativa/physics/collision/KdTreeCollisionDetector.as new file mode 100644 index 0000000..7b0b78e --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/KdTreeCollisionDetector.as @@ -0,0 +1,484 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Детектор, хранящий статическую геометрию в kD-дереве и использующий дерево для ускорения тестов на пересечения. + */ + public class KdTreeCollisionDetector implements ICollisionDetector { + + altphysics var tree:CollisionKdTree; + altphysics var dynamicPrimitives:Vector.; + 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.(); + + 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():void { + tree.createTree(); +// tree.traceTree(); + } + + /** + * + * @param contacts + * @return + */ + public function getAllCollisions(contacts:Vector.):int { + var colNum:int = 0; + for (var i:int = 0; i < dynamicPrimitivesNum; i++) { + var primitive:CollisionPrimitive = dynamicPrimitives[i]; + primitive.calculateAABB(); + if (primitive.body != null && primitive.body.frozen) continue; + colNum += getPrimitiveNodeCollisions(tree.rootNode, primitive, contacts, colNum); + + // Столкновения динамических примитивов между собой + // TODO: Попробовать различные оптимизации (сортировка по баундам, встраивание в дерево) + for (var j:int = i + 1; j < dynamicPrimitivesNum; j++) { + if (collide(primitive, dynamicPrimitives[j], contacts[colNum])) colNum++; + } + } + + return colNum; + } + + /** + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + public function collide(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 = (contact.points[0] as ContactPoint).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:Vector., startIndex:int):int { + var colNum:int = 0; + if (node.indices != null) { + // Поиск столкновений со статическими примитивами узла + var primitives:Vector. = tree.staticChildren; + var indices:Vector. = node.indices; + for (var i:int = indices.length - 1; i >= 0; i--) + if (collide(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; + } + + 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., indices:Vector., 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; + } \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/SpherePlaneCollider.as b/0.0.6.0/src/alternativa/physics/collision/SpherePlaneCollider.as new file mode 100644 index 0000000..77eb1b0 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/SpherePlaneCollider.as @@ -0,0 +1,67 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + /** + * + */ + public class SpherePlaneCollider implements ICollider { + + private var normal:Vector3 = new Vector3(); + + /** + * + */ + public function SpherePlaneCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var sphere:RigidSphere = body1 as RigidSphere; +// var plane:RigidPlane; +// if (sphere == null) { +// sphere = body2 as RigidSphere; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// +// var dist:Number = sphere.state.pos.dot(normal) - offset; +// if (dist > sphere.r) return false; +// +// collisionInfo.body1 = sphere; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// collisionInfo.pcount = 1; +// +// var cp:ContactPoint = collisionInfo.points[0]; +// cp.penetration = sphere.r - dist; +// cp.pos.copy(normal).reverse().scale(sphere.r).add(sphere.state.pos); +// cp.r1.diff(cp.pos, sphere.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/SphereSphereCollider.as b/0.0.6.0/src/alternativa/physics/collision/SphereSphereCollider.as new file mode 100644 index 0000000..25971d5 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/SphereSphereCollider.as @@ -0,0 +1,69 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + public class SphereSphereCollider implements ICollider { + + private var p1:Vector3 = new Vector3(); + private var p2:Vector3 = new Vector3(); + + public function SphereSphereCollider() { + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var s1:CollisionSphere; + var s2:CollisionSphere; + if (prim1.body != null) { + s1 = prim1 as CollisionSphere; + s2 = prim2 as CollisionSphere; + } else { + s1 = prim2 as CollisionSphere; + s2 = prim1 as CollisionSphere; + } + + s1.transform.getAxis(3, p1); + s2.transform.getAxis(3, p2); + var dx:Number = p1.x - p2.x; + var dy:Number = p1.y - p2.y; + var dz:Number = p1.z - p2.z; + var len:Number = dx*dx + dy*dy + dz*dz; + var sum:Number = s1.r + s2.r; + if (len > sum*sum) return false; + len = Math.sqrt(len); + dx /= len; + dy /= len; + dz /= len; + + contact.body1 = s1.body; + contact.body2 = s2.body; + contact.normal.x = dx; + contact.normal.y = dy; + contact.normal.z = dz; + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = sum - len; + cp.pos.x = p1.x - dx*s1.r; + cp.pos.y = p1.y - dy*s1.r; + cp.pos.z = p1.z - dz*s1.r; + cp.r1.vDiff(cp.pos, p1); + cp.r2.vDiff(cp.pos, p2); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/primitives/.svn/all-wcprops b/0.0.6.0/src/alternativa/physics/collision/primitives/.svn/all-wcprops new file mode 100644 index 0000000..f1820ca --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/primitives/.svn/all-wcprops @@ -0,0 +1,35 @@ +K 25 +svn:wc:ra_dav:version-url +V 124 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/collision/primitives +END +CollisionTriangle.as +K 25 +svn:wc:ra_dav:version-url +V 145 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/collision/primitives/CollisionTriangle.as +END +CollisionSphere.as +K 25 +svn:wc:ra_dav:version-url +V 143 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/collision/primitives/CollisionSphere.as +END +CollisionBox.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/collision/primitives/CollisionBox.as +END +CollisionPrimitive.as +K 25 +svn:wc:ra_dav:version-url +V 146 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/collision/primitives/CollisionPrimitive.as +END +CollisionRect.as +K 25 +svn:wc:ra_dav:version-url +V 141 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/collision/primitives/CollisionRect.as +END diff --git a/0.0.6.0/src/alternativa/physics/collision/primitives/.svn/entries b/0.0.6.0/src/alternativa/physics/collision/primitives/.svn/entries new file mode 100644 index 0000000..97b1a10 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/primitives/.svn/entries @@ -0,0 +1,88 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/collision/primitives +http://svndev.alternativaplatform.com + + + +2009-09-15T05:43:40.921290Z +19861 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +CollisionTriangle.as +file + + + + +2010-10-28T04:32:41.000000Z +9567a440e5f4552480eec32af2b1541a +2009-09-15T05:43:40.921290Z +19861 +mike + +CollisionSphere.as +file + + + + +2010-10-28T04:32:41.000000Z +d0d8b961b5e74673c357cd98e0963f4a +2009-09-09T12:16:47.747177Z +19632 +mike + +CollisionBox.as +file + + + + +2010-10-28T04:32:41.000000Z +6949185b7324dde6fc0a48547d56fb92 +2009-09-15T05:43:40.921290Z +19861 +mike + +CollisionPrimitive.as +file + + + + +2010-10-28T04:32:41.000000Z +709fc587a0f9d3cb6a67dce3f3e4c92e +2009-09-11T09:22:20.813670Z +19776 +mike + +CollisionRect.as +file + + + + +2010-10-28T04:32:41.000000Z +ace6d006988f2caaf2577db09794910c +2009-09-15T05:43:40.921290Z +19861 +mike + diff --git a/0.0.6.0/src/alternativa/physics/collision/primitives/.svn/format b/0.0.6.0/src/alternativa/physics/collision/primitives/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/primitives/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.6.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base b/0.0.6.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base new file mode 100644 index 0000000..ff6bbf5 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base @@ -0,0 +1,178 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + /** + * Ориентированный бокс. + */ + public class CollisionBox extends CollisionPrimitive { + + // Половинные размеры вдоль каждой из осей + public var hs:Vector3 = new Vector3(); + + /** + * @param hs + * @param collisionGroup + */ + public function CollisionBox(hs:Vector3, collisionGroup:int) { + super(BOX, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + var t:Matrix4 = transform; + aabb.maxX = hs.x*(t.a < 0 ? -t.a : t.a) + hs.y*(t.b < 0 ? -t.b : t.b) + hs.z*(t.c < 0 ? -t.c : t.c); + aabb.minX = -aabb.maxX; + + aabb.maxY = hs.x*(t.e < 0 ? -t.e : t.e) + hs.y*(t.f < 0 ? -t.f : t.f) + hs.z*(t.g < 0 ? -t.g : t.g); + aabb.minY = -aabb.maxY; + + aabb.maxZ = hs.x*(t.i < 0 ? -t.i : t.i) + hs.y*(t.j < 0 ? -t.j : t.j) + hs.z*(t.k < 0 ? -t.k : t.k); + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var box:CollisionBox = source as CollisionBox; + if (box == null) return this; + super.copyFrom(box); + hs.vCopy(box.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionBox(hs, collisionGroup); + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + var tMin:Number = -1; + var tMax:Number = 1e308; + var t1:Number; + var t2:Number; + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + // X + if (vx < epsilon && vx > -epsilon) { + if (ox < -hs.x || ox > hs.x) return -1; + } else { + t1 = (-hs.x - ox)/vx; + t2 = (hs.x - ox)/vx; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.x = -1; + normal.y = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.x = 1; + normal.y = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Y + if (vy < epsilon && vy > -epsilon) { + if (oy < -hs.y || oy > hs.y) return -1; + } else { + t1 = (-hs.y - oy)/vy; + t2 = (hs.y - oy)/vy; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.y = -1; + normal.x = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.y = 1; + normal.x = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Z + if (vz < epsilon && vz > -epsilon) { + if (oz < -hs.z || oz > hs.z) return -1; + } else { + t1 = (-hs.z - oz)/vz; + t2 = (hs.z - oz)/vz; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.z = -1; + normal.x = normal.y = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.z = 1; + normal.x = normal.y = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Перевод нормали в мировую систему координат + // Inlined normal.vDeltaTransformBy4(transform); + vx = normal.x; + vy = normal.y; + vz = normal.z; + normal.x = transform.a*vx + transform.b*vy + transform.c*vz; + normal.y = transform.e*vx + transform.f*vy + transform.g*vz; + normal.z = transform.i*vx + transform.j*vy + transform.k*vz; + + return tMin; + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionPrimitive.as.svn-base b/0.0.6.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionPrimitive.as.svn-base new file mode 100644 index 0000000..5a8b5a9 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionPrimitive.as.svn-base @@ -0,0 +1,143 @@ +package alternativa.physics.collision.primitives { + + import alternativa.physics.collision.ICollisionPredicate; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + + /** + * Базовый класс для примитивов, использующихся детектором столкновений. + */ + public class CollisionPrimitive { + // Константы типов примитива + public static const BOX:int = 1; + public static const PLANE:int = 2; + public static const SPHERE:int = 3; + public static const RECT:int = 4; + public static const TRIANGLE:int = 5; + + // Тип примитива + public var type:int; + // Группы примитива. Каждая группа определяется установленным битом. Столкновения проверяются только для примитивов, + // имеющих хотя бы одну общую группу. + public var collisionGroup:int; + // Предикат примитива, вызывающийся детектором при нахождении столкновения. В зависимости от возвращённого + // предикатом результата столкновение либо регистрируется, либо игнорируется. + // Nullable + public var postCollisionPredicate:ICollisionPredicate; + // Тело, владеющее примитивом. Поле сделано открытым для быстрого доступа на чтение. Установка значения должна выполняться вызовом метода setBody(). + // Nullable + public var body:Body; + // Трансформация примитива в системе координат тела, если оно указано. Не допускается масштабирование матрицы. + public var localTransform:Matrix4; + // Полная трансформация примитива. Не допускается масштабирование матрицы. + public var transform:Matrix4 = new Matrix4(); + // AABB в мировой системе координат. Расчитывается системой вызовом функции calculateBoundBox(). + public var aabb:BoundBox = new BoundBox(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param type тип примитива + * @param collisionGroup группа примитива + */ + public function CollisionPrimitive(type:int, collisionGroup:int) { + this.type = type; + this.collisionGroup = collisionGroup; + } + + /** + * Устанавливает тело, владеющее примитивом. + * + * @param body тело, которое владеет примитивом. Если указано значение null, матрица локальной трансформации удаляется. + * @param localTransform трансформация примитива в системе координат тела. Указание значения null равносильно + * заданию единичной матрицы, однако в первом случае не будет дополнительного умножения матриц при вычислении полной трансформации примитива. + */ + public function setBody(body:Body, localTransform:Matrix4 = null):void { + if (this.body == body) return; + this.body = body; + if (body == null) { + this.localTransform = null; + } else { + if (localTransform != null) { + if (this.localTransform == null) { + this.localTransform = new Matrix4(); + } + this.localTransform.copy(localTransform); + } else { + this.localTransform = null; + } + } + } + + /** + * Рассчитывает AABB примитива. Наследники должны переопределять этот метод, реализуя в нём корректный рассчёт. + * + * @return ссылка на свой AABB + */ + public function calculateAABB():BoundBox { + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + return -1; + } + + /** + * Клонирует примитив. Переопределять не рекомендуется. Вместо этого переопределяются методы createPrimitive() и copyFrom(). + * + * @return клон примитива + */ + public function clone():CollisionPrimitive { + var p:CollisionPrimitive = createPrimitive(); + return p.copyFrom(this); + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + if (source == null) { + throw new ArgumentError("Parameter source cannot be null"); + } + type = source.type; + transform.copy(source.transform); + collisionGroup = source.collisionGroup; + setBody(source.body, source.localTransform); + aabb.copyFrom(source.aabb); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + public function toString():String { + return "[CollisionPrimitive type=" + type + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + protected function createPrimitive():CollisionPrimitive { + return new CollisionPrimitive(type, collisionGroup); + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base b/0.0.6.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base new file mode 100644 index 0000000..bc4b34c --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base @@ -0,0 +1,123 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + /** + * Ориентированный прямоугольник. Задаётся половинами размеров вдоль осей X (ширина) и Y (длина) локальной системы + * координат примитива. Таким образом, прямоугольник лежит в плоскости XY, его стороны параллельны осям этой + * плоскости, а нормаль направлена вдоль локальной оси Z. + * + * Прямоугольник может быть одно- или двусторонним. В случае одностороннего прямоугольника, + * столкновения с внутренней стороны не регистрируются. + */ + public class CollisionRect extends CollisionPrimitive { + // Половинные размеры прямоугольника вдоль осей X и Y. Нормаль направлена вдоль оси Z. + public var hs:Vector3 = new Vector3(); + // Флаг указывает, является примитив одно- или двусторонним + public var twoSided:Boolean = true; + + // Малое значение. Используется для указания фиктивной высоты примитива и в функции определения пересечения с сегментом. + private static const EPSILON:Number = 0.005; + + /** + * Создаёт новый экземпляр примитива. + * + * @param hs половинные размерв прямоугольника вдоль осей X и Y. Значение z игнорируется. + * @param collisionGroup группа примитива + */ + public function CollisionRect(hs:Vector3, collisionGroup:int) { + super(RECT, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * Расчитывает ограничивающий бокс прямоугольника. Для избежания проблем высота примитива принимается равной + * не нулю, а малому значению. + * + * @return + */ + override public function calculateAABB():BoundBox { + // Баунд бокс прямоугольника имеет минимальную высоту, отличную от нуля во избежание проблем с построением kd-дерева + var t:Matrix4 = transform; + aabb.maxX = hs.x*(t.a < 0 ? -t.a : t.a) + hs.y*(t.b < 0 ? -t.b : t.b) + EPSILON*(t.c < 0 ? -t.c : t.c); + aabb.minX = -aabb.maxX; + + aabb.maxY = hs.x*(t.e < 0 ? -t.e : t.e) + hs.y*(t.f < 0 ? -t.f : t.f) + EPSILON*(t.g < 0 ? -t.g : t.g); + aabb.minY = -aabb.maxY; + + aabb.maxZ = hs.x*(t.i < 0 ? -t.i : t.i) + hs.y*(t.j < 0 ? -t.j : t.j) + EPSILON*(t.k < 0 ? -t.k : t.k); + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var rect:CollisionRect = source as CollisionRect; + if (rect == null) return this; + super.copyFrom(rect); + hs.vCopy(rect.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionRect(hs, collisionGroup); + } + + /** + * @param origin + * @param vector + * @param collisionGroup + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + + // Проверка параллельности сегмента и плоскости примитива + if (vz > -threshold && vz < threshold) return -1; + var t:Number = -oz/vz; + if (t < 0) return -1; + // Проверка вхождения точки пересечения в прямоугольник + ox += vx*t; + oy += vy*t; + oz = 0; + if (ox < (-hs.x - threshold) || ox > (hs.x + threshold) || oy < (-hs.y - threshold) || oy > (hs.y + threshold)) return -1; + + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + return t; + } + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base b/0.0.6.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base new file mode 100644 index 0000000..dc2328b --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base @@ -0,0 +1,80 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + + /** + * Сфера. + */ + public class CollisionSphere extends CollisionPrimitive { + + // Радиус сферы + public var r:Number = 0; + + /** + * + * @param r + * @param collisionGroup + */ + public function CollisionSphere(r:Number, collisionGroup:int) { + super(SPHERE, collisionGroup); + this.r = r; + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + aabb.maxX = transform.d + r; + aabb.minX = transform.d - r; + + aabb.maxY = transform.h + r; + aabb.minY = transform.h - r; + + aabb.maxZ = transform.l + r; + aabb.minZ = transform.l - r; + + return aabb; + } + + /** + * @param origin + * @param vector + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var px:Number = origin.x - transform.d; + var py:Number = origin.y - transform.h; + var pz:Number = origin.z - transform.l; + var k:Number = vector.x*px + vector.y*py + vector.z*pz; + if (k > 0) return -1; + var a:Number = vector.x*vector.x + vector.y*vector.y + vector.z*vector.z; + var D:Number = k*k - a*(px*px + py*py + pz*pz - r*r); + if (D < 0) return -1; + return -(k + Math.sqrt(D))/a; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var sphere:CollisionSphere = source as CollisionSphere; + if (sphere == null) return this; + super.copyFrom(sphere); + r = sphere.r; + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionSphere(r, collisionGroup); + } + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base b/0.0.6.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base new file mode 100644 index 0000000..995d3cb --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base @@ -0,0 +1,243 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + /** + * Примитив треугольник. + */ + public class CollisionTriangle extends CollisionPrimitive { + + public var v0:Vector3 = new Vector3(); + public var v1:Vector3 = new Vector3(); + public var v2:Vector3 = new Vector3(); + + public var e0:Vector3 = new Vector3(); + public var e1:Vector3 = new Vector3(); + public var e2:Vector3 = new Vector3(); + +// public var len0:Number; +// public var len1:Number; +// public var len2:Number; +// + /** + * + * @param v0 + * @param v1 + * @param v2 + * @param collisionGroup + */ + public function CollisionTriangle(v0:Vector3, v1:Vector3, v2:Vector3, collisionGroup:int) { + super(TRIANGLE, collisionGroup); + initVertices(v0, v1, v2); + } + + /** + * Рассчитывает AABB примитива. + * + * @return ссылка на свой AABB + */ + override public function calculateAABB():BoundBox { + var epsilon:Number = 0.005; + var a:Number; + var b:Number; + var eps_c:Number = epsilon*transform.c; + var eps_g:Number = epsilon*transform.g; + var eps_k:Number = epsilon*transform.k; + + // Вершина 0 + // Ось X + a = v0.x*transform.a + v0.y*transform.b; + aabb.minX = aabb.maxX = a + eps_c; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v0.x*transform.e + v0.y*transform.f; + aabb.minY = aabb.maxY = a + eps_g; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v0.x*transform.i + v0.y*transform.j; + aabb.minZ = aabb.maxZ = a + eps_k; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 1 + // Ось X + a = v1.x*transform.a + v1.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v1.x*transform.e + v1.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v1.x*transform.i + v1.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 2 + // Ось X + a = v2.x*transform.a + v2.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v2.x*transform.e + v2.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v2.x*transform.i + v2.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + aabb.minX += transform.d; + aabb.maxX += transform.d; + + aabb.minY += transform.h; + aabb.maxY += transform.h; + + aabb.minZ += transform.l; + aabb.maxZ += transform.l; + + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + // Луч трансформируется в систему координат примитива, затем проверяется пересечение + + var vz:Number = vector.x*transform.c + vector.y*transform.g + vector.z*transform.k; + // Если луч параллелен плоскости птимитива, то пересечения нет + if (vz < epsilon && vz > -epsilon) return -1; + + var tx:Number = origin.x - transform.d; + var ty:Number = origin.y - transform.h; + var tz:Number = origin.z - transform.l; + + var oz:Number = tx*transform.c + ty*transform.g + tz*transform.k; + var t:Number = -oz/vz; + if (t < 0) return -1; + + var ox:Number = tx*transform.a + ty*transform.e + tz*transform.i; + var oy:Number = tx*transform.b + ty*transform.f + tz*transform.j; + + tx = ox + t*(vector.x*transform.a + vector.y*transform.e + vector.z*transform.i); + ty = oy + t*(vector.x*transform.b + vector.y*transform.f + vector.z*transform.j); + tz = oz + t*vz; + + // Проверка вхождения точки в треугольник + if ((e0.x*(ty - v0.y) - e0.y*(tx - v0.x) < 0) || (e1.x*(ty - v1.y) - e1.y*(tx - v1.x) < 0) || (e2.x*(ty - v2.y) - e2.y*(tx - v2.x) < 0)) return -1; + + // Запись нормали + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + + return t; + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + super.copyFrom(source); + var tri:CollisionTriangle = source as CollisionTriangle; + if (tri != null) { + v0.vCopy(tri.v0); + v1.vCopy(tri.v1); + v2.vCopy(tri.v2); + + e0.vCopy(tri.e0); + e1.vCopy(tri.e1); + e2.vCopy(tri.e2); + +// len0 = tri.len0; +// len1 = tri.len1; +// len2 = tri.len2; + } + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionTriangle v0=" + v0 + ", v1=" + v1 + ", v2=" + v2 + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionTriangle(v0, v1, v2, collisionGroup); + } + + /** + * + * @param v0 + * @param v1 + * @param v2 + */ + private function initVertices(v0:Vector3, v1:Vector3, v2:Vector3):void { + this.v0.vCopy(v0); + this.v1.vCopy(v1); + this.v2.vCopy(v2); + + e0.vDiff(v1, v0); +// len0 = e0.vLength(); + e0.vNormalize(); + + e1.vDiff(v2, v1); +// len1 = e1.vLength(); + e1.vNormalize(); + + e2.vDiff(v0, v2); +// len2 = e2.vLength(); + e2.vNormalize(); + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/primitives/CollisionBox.as b/0.0.6.0/src/alternativa/physics/collision/primitives/CollisionBox.as new file mode 100644 index 0000000..ff6bbf5 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/primitives/CollisionBox.as @@ -0,0 +1,178 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + /** + * Ориентированный бокс. + */ + public class CollisionBox extends CollisionPrimitive { + + // Половинные размеры вдоль каждой из осей + public var hs:Vector3 = new Vector3(); + + /** + * @param hs + * @param collisionGroup + */ + public function CollisionBox(hs:Vector3, collisionGroup:int) { + super(BOX, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + var t:Matrix4 = transform; + aabb.maxX = hs.x*(t.a < 0 ? -t.a : t.a) + hs.y*(t.b < 0 ? -t.b : t.b) + hs.z*(t.c < 0 ? -t.c : t.c); + aabb.minX = -aabb.maxX; + + aabb.maxY = hs.x*(t.e < 0 ? -t.e : t.e) + hs.y*(t.f < 0 ? -t.f : t.f) + hs.z*(t.g < 0 ? -t.g : t.g); + aabb.minY = -aabb.maxY; + + aabb.maxZ = hs.x*(t.i < 0 ? -t.i : t.i) + hs.y*(t.j < 0 ? -t.j : t.j) + hs.z*(t.k < 0 ? -t.k : t.k); + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var box:CollisionBox = source as CollisionBox; + if (box == null) return this; + super.copyFrom(box); + hs.vCopy(box.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionBox(hs, collisionGroup); + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + var tMin:Number = -1; + var tMax:Number = 1e308; + var t1:Number; + var t2:Number; + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + // X + if (vx < epsilon && vx > -epsilon) { + if (ox < -hs.x || ox > hs.x) return -1; + } else { + t1 = (-hs.x - ox)/vx; + t2 = (hs.x - ox)/vx; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.x = -1; + normal.y = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.x = 1; + normal.y = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Y + if (vy < epsilon && vy > -epsilon) { + if (oy < -hs.y || oy > hs.y) return -1; + } else { + t1 = (-hs.y - oy)/vy; + t2 = (hs.y - oy)/vy; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.y = -1; + normal.x = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.y = 1; + normal.x = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Z + if (vz < epsilon && vz > -epsilon) { + if (oz < -hs.z || oz > hs.z) return -1; + } else { + t1 = (-hs.z - oz)/vz; + t2 = (hs.z - oz)/vz; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.z = -1; + normal.x = normal.y = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.z = 1; + normal.x = normal.y = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Перевод нормали в мировую систему координат + // Inlined normal.vDeltaTransformBy4(transform); + vx = normal.x; + vy = normal.y; + vz = normal.z; + normal.x = transform.a*vx + transform.b*vy + transform.c*vz; + normal.y = transform.e*vx + transform.f*vy + transform.g*vz; + normal.z = transform.i*vx + transform.j*vy + transform.k*vz; + + return tMin; + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/primitives/CollisionPrimitive.as b/0.0.6.0/src/alternativa/physics/collision/primitives/CollisionPrimitive.as new file mode 100644 index 0000000..5a8b5a9 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/primitives/CollisionPrimitive.as @@ -0,0 +1,143 @@ +package alternativa.physics.collision.primitives { + + import alternativa.physics.collision.ICollisionPredicate; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + + /** + * Базовый класс для примитивов, использующихся детектором столкновений. + */ + public class CollisionPrimitive { + // Константы типов примитива + public static const BOX:int = 1; + public static const PLANE:int = 2; + public static const SPHERE:int = 3; + public static const RECT:int = 4; + public static const TRIANGLE:int = 5; + + // Тип примитива + public var type:int; + // Группы примитива. Каждая группа определяется установленным битом. Столкновения проверяются только для примитивов, + // имеющих хотя бы одну общую группу. + public var collisionGroup:int; + // Предикат примитива, вызывающийся детектором при нахождении столкновения. В зависимости от возвращённого + // предикатом результата столкновение либо регистрируется, либо игнорируется. + // Nullable + public var postCollisionPredicate:ICollisionPredicate; + // Тело, владеющее примитивом. Поле сделано открытым для быстрого доступа на чтение. Установка значения должна выполняться вызовом метода setBody(). + // Nullable + public var body:Body; + // Трансформация примитива в системе координат тела, если оно указано. Не допускается масштабирование матрицы. + public var localTransform:Matrix4; + // Полная трансформация примитива. Не допускается масштабирование матрицы. + public var transform:Matrix4 = new Matrix4(); + // AABB в мировой системе координат. Расчитывается системой вызовом функции calculateBoundBox(). + public var aabb:BoundBox = new BoundBox(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param type тип примитива + * @param collisionGroup группа примитива + */ + public function CollisionPrimitive(type:int, collisionGroup:int) { + this.type = type; + this.collisionGroup = collisionGroup; + } + + /** + * Устанавливает тело, владеющее примитивом. + * + * @param body тело, которое владеет примитивом. Если указано значение null, матрица локальной трансформации удаляется. + * @param localTransform трансформация примитива в системе координат тела. Указание значения null равносильно + * заданию единичной матрицы, однако в первом случае не будет дополнительного умножения матриц при вычислении полной трансформации примитива. + */ + public function setBody(body:Body, localTransform:Matrix4 = null):void { + if (this.body == body) return; + this.body = body; + if (body == null) { + this.localTransform = null; + } else { + if (localTransform != null) { + if (this.localTransform == null) { + this.localTransform = new Matrix4(); + } + this.localTransform.copy(localTransform); + } else { + this.localTransform = null; + } + } + } + + /** + * Рассчитывает AABB примитива. Наследники должны переопределять этот метод, реализуя в нём корректный рассчёт. + * + * @return ссылка на свой AABB + */ + public function calculateAABB():BoundBox { + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + return -1; + } + + /** + * Клонирует примитив. Переопределять не рекомендуется. Вместо этого переопределяются методы createPrimitive() и copyFrom(). + * + * @return клон примитива + */ + public function clone():CollisionPrimitive { + var p:CollisionPrimitive = createPrimitive(); + return p.copyFrom(this); + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + if (source == null) { + throw new ArgumentError("Parameter source cannot be null"); + } + type = source.type; + transform.copy(source.transform); + collisionGroup = source.collisionGroup; + setBody(source.body, source.localTransform); + aabb.copyFrom(source.aabb); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + public function toString():String { + return "[CollisionPrimitive type=" + type + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + protected function createPrimitive():CollisionPrimitive { + return new CollisionPrimitive(type, collisionGroup); + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/primitives/CollisionRect.as b/0.0.6.0/src/alternativa/physics/collision/primitives/CollisionRect.as new file mode 100644 index 0000000..bc4b34c --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/primitives/CollisionRect.as @@ -0,0 +1,123 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + /** + * Ориентированный прямоугольник. Задаётся половинами размеров вдоль осей X (ширина) и Y (длина) локальной системы + * координат примитива. Таким образом, прямоугольник лежит в плоскости XY, его стороны параллельны осям этой + * плоскости, а нормаль направлена вдоль локальной оси Z. + * + * Прямоугольник может быть одно- или двусторонним. В случае одностороннего прямоугольника, + * столкновения с внутренней стороны не регистрируются. + */ + public class CollisionRect extends CollisionPrimitive { + // Половинные размеры прямоугольника вдоль осей X и Y. Нормаль направлена вдоль оси Z. + public var hs:Vector3 = new Vector3(); + // Флаг указывает, является примитив одно- или двусторонним + public var twoSided:Boolean = true; + + // Малое значение. Используется для указания фиктивной высоты примитива и в функции определения пересечения с сегментом. + private static const EPSILON:Number = 0.005; + + /** + * Создаёт новый экземпляр примитива. + * + * @param hs половинные размерв прямоугольника вдоль осей X и Y. Значение z игнорируется. + * @param collisionGroup группа примитива + */ + public function CollisionRect(hs:Vector3, collisionGroup:int) { + super(RECT, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * Расчитывает ограничивающий бокс прямоугольника. Для избежания проблем высота примитива принимается равной + * не нулю, а малому значению. + * + * @return + */ + override public function calculateAABB():BoundBox { + // Баунд бокс прямоугольника имеет минимальную высоту, отличную от нуля во избежание проблем с построением kd-дерева + var t:Matrix4 = transform; + aabb.maxX = hs.x*(t.a < 0 ? -t.a : t.a) + hs.y*(t.b < 0 ? -t.b : t.b) + EPSILON*(t.c < 0 ? -t.c : t.c); + aabb.minX = -aabb.maxX; + + aabb.maxY = hs.x*(t.e < 0 ? -t.e : t.e) + hs.y*(t.f < 0 ? -t.f : t.f) + EPSILON*(t.g < 0 ? -t.g : t.g); + aabb.minY = -aabb.maxY; + + aabb.maxZ = hs.x*(t.i < 0 ? -t.i : t.i) + hs.y*(t.j < 0 ? -t.j : t.j) + EPSILON*(t.k < 0 ? -t.k : t.k); + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var rect:CollisionRect = source as CollisionRect; + if (rect == null) return this; + super.copyFrom(rect); + hs.vCopy(rect.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionRect(hs, collisionGroup); + } + + /** + * @param origin + * @param vector + * @param collisionGroup + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + + // Проверка параллельности сегмента и плоскости примитива + if (vz > -threshold && vz < threshold) return -1; + var t:Number = -oz/vz; + if (t < 0) return -1; + // Проверка вхождения точки пересечения в прямоугольник + ox += vx*t; + oy += vy*t; + oz = 0; + if (ox < (-hs.x - threshold) || ox > (hs.x + threshold) || oy < (-hs.y - threshold) || oy > (hs.y + threshold)) return -1; + + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + return t; + } + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/primitives/CollisionSphere.as b/0.0.6.0/src/alternativa/physics/collision/primitives/CollisionSphere.as new file mode 100644 index 0000000..dc2328b --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/primitives/CollisionSphere.as @@ -0,0 +1,80 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + + /** + * Сфера. + */ + public class CollisionSphere extends CollisionPrimitive { + + // Радиус сферы + public var r:Number = 0; + + /** + * + * @param r + * @param collisionGroup + */ + public function CollisionSphere(r:Number, collisionGroup:int) { + super(SPHERE, collisionGroup); + this.r = r; + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + aabb.maxX = transform.d + r; + aabb.minX = transform.d - r; + + aabb.maxY = transform.h + r; + aabb.minY = transform.h - r; + + aabb.maxZ = transform.l + r; + aabb.minZ = transform.l - r; + + return aabb; + } + + /** + * @param origin + * @param vector + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var px:Number = origin.x - transform.d; + var py:Number = origin.y - transform.h; + var pz:Number = origin.z - transform.l; + var k:Number = vector.x*px + vector.y*py + vector.z*pz; + if (k > 0) return -1; + var a:Number = vector.x*vector.x + vector.y*vector.y + vector.z*vector.z; + var D:Number = k*k - a*(px*px + py*py + pz*pz - r*r); + if (D < 0) return -1; + return -(k + Math.sqrt(D))/a; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var sphere:CollisionSphere = source as CollisionSphere; + if (sphere == null) return this; + super.copyFrom(sphere); + r = sphere.r; + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionSphere(r, collisionGroup); + } + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/primitives/CollisionTriangle.as b/0.0.6.0/src/alternativa/physics/collision/primitives/CollisionTriangle.as new file mode 100644 index 0000000..995d3cb --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/primitives/CollisionTriangle.as @@ -0,0 +1,243 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + /** + * Примитив треугольник. + */ + public class CollisionTriangle extends CollisionPrimitive { + + public var v0:Vector3 = new Vector3(); + public var v1:Vector3 = new Vector3(); + public var v2:Vector3 = new Vector3(); + + public var e0:Vector3 = new Vector3(); + public var e1:Vector3 = new Vector3(); + public var e2:Vector3 = new Vector3(); + +// public var len0:Number; +// public var len1:Number; +// public var len2:Number; +// + /** + * + * @param v0 + * @param v1 + * @param v2 + * @param collisionGroup + */ + public function CollisionTriangle(v0:Vector3, v1:Vector3, v2:Vector3, collisionGroup:int) { + super(TRIANGLE, collisionGroup); + initVertices(v0, v1, v2); + } + + /** + * Рассчитывает AABB примитива. + * + * @return ссылка на свой AABB + */ + override public function calculateAABB():BoundBox { + var epsilon:Number = 0.005; + var a:Number; + var b:Number; + var eps_c:Number = epsilon*transform.c; + var eps_g:Number = epsilon*transform.g; + var eps_k:Number = epsilon*transform.k; + + // Вершина 0 + // Ось X + a = v0.x*transform.a + v0.y*transform.b; + aabb.minX = aabb.maxX = a + eps_c; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v0.x*transform.e + v0.y*transform.f; + aabb.minY = aabb.maxY = a + eps_g; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v0.x*transform.i + v0.y*transform.j; + aabb.minZ = aabb.maxZ = a + eps_k; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 1 + // Ось X + a = v1.x*transform.a + v1.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v1.x*transform.e + v1.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v1.x*transform.i + v1.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 2 + // Ось X + a = v2.x*transform.a + v2.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v2.x*transform.e + v2.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v2.x*transform.i + v2.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + aabb.minX += transform.d; + aabb.maxX += transform.d; + + aabb.minY += transform.h; + aabb.maxY += transform.h; + + aabb.minZ += transform.l; + aabb.maxZ += transform.l; + + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + // Луч трансформируется в систему координат примитива, затем проверяется пересечение + + var vz:Number = vector.x*transform.c + vector.y*transform.g + vector.z*transform.k; + // Если луч параллелен плоскости птимитива, то пересечения нет + if (vz < epsilon && vz > -epsilon) return -1; + + var tx:Number = origin.x - transform.d; + var ty:Number = origin.y - transform.h; + var tz:Number = origin.z - transform.l; + + var oz:Number = tx*transform.c + ty*transform.g + tz*transform.k; + var t:Number = -oz/vz; + if (t < 0) return -1; + + var ox:Number = tx*transform.a + ty*transform.e + tz*transform.i; + var oy:Number = tx*transform.b + ty*transform.f + tz*transform.j; + + tx = ox + t*(vector.x*transform.a + vector.y*transform.e + vector.z*transform.i); + ty = oy + t*(vector.x*transform.b + vector.y*transform.f + vector.z*transform.j); + tz = oz + t*vz; + + // Проверка вхождения точки в треугольник + if ((e0.x*(ty - v0.y) - e0.y*(tx - v0.x) < 0) || (e1.x*(ty - v1.y) - e1.y*(tx - v1.x) < 0) || (e2.x*(ty - v2.y) - e2.y*(tx - v2.x) < 0)) return -1; + + // Запись нормали + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + + return t; + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + super.copyFrom(source); + var tri:CollisionTriangle = source as CollisionTriangle; + if (tri != null) { + v0.vCopy(tri.v0); + v1.vCopy(tri.v1); + v2.vCopy(tri.v2); + + e0.vCopy(tri.e0); + e1.vCopy(tri.e1); + e2.vCopy(tri.e2); + +// len0 = tri.len0; +// len1 = tri.len1; +// len2 = tri.len2; + } + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionTriangle v0=" + v0 + ", v1=" + v1 + ", v2=" + v2 + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionTriangle(v0, v1, v2, collisionGroup); + } + + /** + * + * @param v0 + * @param v1 + * @param v2 + */ + private function initVertices(v0:Vector3, v1:Vector3, v2:Vector3):void { + this.v0.vCopy(v0); + this.v1.vCopy(v1); + this.v2.vCopy(v2); + + e0.vDiff(v1, v0); +// len0 = e0.vLength(); + e0.vNormalize(); + + e1.vDiff(v2, v1); +// len1 = e1.vLength(); + e1.vNormalize(); + + e2.vDiff(v0, v2); +// len2 = e2.vLength(); + e2.vNormalize(); + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/types/.svn/all-wcprops b/0.0.6.0/src/alternativa/physics/collision/types/.svn/all-wcprops new file mode 100644 index 0000000..eec00d7 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/types/.svn/all-wcprops @@ -0,0 +1,23 @@ +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/collision/types +END +BoundBox.as +K 25 +svn:wc:ra_dav:version-url +V 131 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/collision/types/BoundBox.as +END +RayIntersection.as +K 25 +svn:wc:ra_dav:version-url +V 138 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/collision/types/RayIntersection.as +END +Ray.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/collision/types/Ray.as +END diff --git a/0.0.6.0/src/alternativa/physics/collision/types/.svn/entries b/0.0.6.0/src/alternativa/physics/collision/types/.svn/entries new file mode 100644 index 0000000..31cfa9c --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/types/.svn/entries @@ -0,0 +1,64 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/collision/types +http://svndev.alternativaplatform.com + + + +2009-09-09T14:02:48.134744Z +19633 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +BoundBox.as +file + + + + +2010-10-28T04:32:41.000000Z +d92d4ab3dde698feb674fb9488f59678 +2009-09-09T14:02:48.134744Z +19633 +mike + +RayIntersection.as +file + + + + +2010-10-28T04:32:41.000000Z +a0a4cf3dd94468fdf44cd06734927c94 +2009-06-08T06:46:18.310732Z +14099 +mike + +Ray.as +file + + + + +2010-10-28T04:32:41.000000Z +55eeb1cf79a416bb863bee95f089dbcd +2009-04-17T17:39:21.367569Z +11341 +mike + diff --git a/0.0.6.0/src/alternativa/physics/collision/types/.svn/format b/0.0.6.0/src/alternativa/physics/collision/types/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/types/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.6.0/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base b/0.0.6.0/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base new file mode 100644 index 0000000..b4d5278 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base @@ -0,0 +1,75 @@ +package alternativa.physics.collision.types { + + /** + * Ограничивающий параллелепипед. + */ + public class BoundBox { + + public var minX:Number = 1e308; + public var minY:Number = 1e308; + public var minZ:Number = 1e308; + public var maxX:Number = -1e308; + public var maxY:Number = -1e308; + public var maxZ:Number = -1e308; + + public function setSize(minX:Number, minY:Number, minZ:Number, maxX:Number, maxY:Number, maxZ:Number):void { + this.minX = minX; + this.minY = minY; + this.minZ = minZ; + this.maxX = maxX; + this.maxY = maxY; + this.maxZ = maxZ; + } + + public function addBoundBox(boundBox:BoundBox):void { + minX = (boundBox.minX < minX) ? boundBox.minX : minX; + minY = (boundBox.minY < minY) ? boundBox.minY : minY; + minZ = (boundBox.minZ < minZ) ? boundBox.minZ : minZ; + maxX = (boundBox.maxX > maxX) ? boundBox.maxX : maxX; + maxY = (boundBox.maxY > maxY) ? boundBox.maxY : maxY; + maxZ = (boundBox.maxZ > maxZ) ? boundBox.maxZ : maxZ; + } + + public function addPoint(x:Number, y:Number, z:Number):void { + if (x < minX) minX = x; + if (x > maxX) maxX = x; + if (y < minY) minY = y; + if (y > maxY) maxY = y; + if (z < minZ) minZ = z; + if (z > maxZ) maxZ = z; + } + + public function infinity():void { + minX = 1e308; + minY = 1e308; + minZ = 1e308; + maxX = -1e308; + maxY = -1e308; + maxZ = -1e308; + } + + public function intersects(bb:BoundBox, epsilon:Number):Boolean { + return !(minX > bb.maxX + epsilon || maxX < bb.minX - epsilon || minY > bb.maxY + epsilon || maxY < bb.minY - epsilon || minZ > bb.maxZ + epsilon || maxZ < bb.minZ - epsilon); + } + + public function copyFrom(boundBox:BoundBox):void { + minX = boundBox.minX; + minY = boundBox.minY; + minZ = boundBox.minZ; + maxX = boundBox.maxX; + maxY = boundBox.maxY; + maxZ = boundBox.maxZ; + } + + public function clone():BoundBox { + var clone:BoundBox = new BoundBox(); + clone.copyFrom(this); + return clone; + } + + public function toString():String { + return "BoundBox [" + minX + ", " + minY + ", " + minZ + " : " + maxX + ", " + maxY + ", " + maxZ + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base b/0.0.6.0/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base new file mode 100644 index 0000000..f80620a --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base @@ -0,0 +1,13 @@ +package alternativa.physics.collision.types { + import alternativa.physics.types.Vector3; + + /** + * Структура, описывающая луч и сегмент прямой в пространстве. + */ + public class Ray { + // Начало луча. Начальная точка сегмента на прямой луча. + public var origin:Vector3 = new Vector3(); + // Направляющий вектор луча. Длина вектора задаёт длину сегмента на луче. Конечная точка сегмента вычисляется как origin + dir. + public var dir:Vector3 = new Vector3(); + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base b/0.0.6.0/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base new file mode 100644 index 0000000..8a89f75 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base @@ -0,0 +1,25 @@ +package alternativa.physics.collision.types { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.types.Vector3; + + /** + * Структура описывает точку пересечения луча с физической геометрией. + */ + public class RayIntersection { + // Физический примитив, с которым пересекается луч + public var primitive:CollisionPrimitive; + // Положенеи точки пересечения + public var pos:Vector3 = new Vector3(); + // Нормаль поверхности физического примитива в точке пересечения + public var normal:Vector3 = new Vector3(); + // Время пересечения (|pos - ray.origin|/|ray.dir|) + public var t:Number = 0; + + public function copy(source:RayIntersection):void { + primitive = source.primitive; + pos.vCopy(source.pos); + normal.vCopy(source.normal); + t = source.t; + } + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/types/BoundBox.as b/0.0.6.0/src/alternativa/physics/collision/types/BoundBox.as new file mode 100644 index 0000000..b4d5278 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/types/BoundBox.as @@ -0,0 +1,75 @@ +package alternativa.physics.collision.types { + + /** + * Ограничивающий параллелепипед. + */ + public class BoundBox { + + public var minX:Number = 1e308; + public var minY:Number = 1e308; + public var minZ:Number = 1e308; + public var maxX:Number = -1e308; + public var maxY:Number = -1e308; + public var maxZ:Number = -1e308; + + public function setSize(minX:Number, minY:Number, minZ:Number, maxX:Number, maxY:Number, maxZ:Number):void { + this.minX = minX; + this.minY = minY; + this.minZ = minZ; + this.maxX = maxX; + this.maxY = maxY; + this.maxZ = maxZ; + } + + public function addBoundBox(boundBox:BoundBox):void { + minX = (boundBox.minX < minX) ? boundBox.minX : minX; + minY = (boundBox.minY < minY) ? boundBox.minY : minY; + minZ = (boundBox.minZ < minZ) ? boundBox.minZ : minZ; + maxX = (boundBox.maxX > maxX) ? boundBox.maxX : maxX; + maxY = (boundBox.maxY > maxY) ? boundBox.maxY : maxY; + maxZ = (boundBox.maxZ > maxZ) ? boundBox.maxZ : maxZ; + } + + public function addPoint(x:Number, y:Number, z:Number):void { + if (x < minX) minX = x; + if (x > maxX) maxX = x; + if (y < minY) minY = y; + if (y > maxY) maxY = y; + if (z < minZ) minZ = z; + if (z > maxZ) maxZ = z; + } + + public function infinity():void { + minX = 1e308; + minY = 1e308; + minZ = 1e308; + maxX = -1e308; + maxY = -1e308; + maxZ = -1e308; + } + + public function intersects(bb:BoundBox, epsilon:Number):Boolean { + return !(minX > bb.maxX + epsilon || maxX < bb.minX - epsilon || minY > bb.maxY + epsilon || maxY < bb.minY - epsilon || minZ > bb.maxZ + epsilon || maxZ < bb.minZ - epsilon); + } + + public function copyFrom(boundBox:BoundBox):void { + minX = boundBox.minX; + minY = boundBox.minY; + minZ = boundBox.minZ; + maxX = boundBox.maxX; + maxY = boundBox.maxY; + maxZ = boundBox.maxZ; + } + + public function clone():BoundBox { + var clone:BoundBox = new BoundBox(); + clone.copyFrom(this); + return clone; + } + + public function toString():String { + return "BoundBox [" + minX + ", " + minY + ", " + minZ + " : " + maxX + ", " + maxY + ", " + maxZ + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/types/Ray.as b/0.0.6.0/src/alternativa/physics/collision/types/Ray.as new file mode 100644 index 0000000..f80620a --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/types/Ray.as @@ -0,0 +1,13 @@ +package alternativa.physics.collision.types { + import alternativa.physics.types.Vector3; + + /** + * Структура, описывающая луч и сегмент прямой в пространстве. + */ + public class Ray { + // Начало луча. Начальная точка сегмента на прямой луча. + public var origin:Vector3 = new Vector3(); + // Направляющий вектор луча. Длина вектора задаёт длину сегмента на луче. Конечная точка сегмента вычисляется как origin + dir. + public var dir:Vector3 = new Vector3(); + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/collision/types/RayIntersection.as b/0.0.6.0/src/alternativa/physics/collision/types/RayIntersection.as new file mode 100644 index 0000000..8a89f75 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/collision/types/RayIntersection.as @@ -0,0 +1,25 @@ +package alternativa.physics.collision.types { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.types.Vector3; + + /** + * Структура описывает точку пересечения луча с физической геометрией. + */ + public class RayIntersection { + // Физический примитив, с которым пересекается луч + public var primitive:CollisionPrimitive; + // Положенеи точки пересечения + public var pos:Vector3 = new Vector3(); + // Нормаль поверхности физического примитива в точке пересечения + public var normal:Vector3 = new Vector3(); + // Время пересечения (|pos - ray.origin|/|ray.dir|) + public var t:Number = 0; + + public function copy(source:RayIntersection):void { + primitive = source.primitive; + pos.vCopy(source.pos); + normal.vCopy(source.normal); + t = source.t; + } + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/rigid/.svn/all-wcprops b/0.0.6.0/src/alternativa/physics/rigid/.svn/all-wcprops new file mode 100644 index 0000000..85a0b63 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/rigid/.svn/all-wcprops @@ -0,0 +1,47 @@ +K 25 +svn:wc:ra_dav:version-url +V 109 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/rigid +END +Body.as +K 25 +svn:wc:ra_dav:version-url +V 117 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/rigid/Body.as +END +BodyState.as +K 25 +svn:wc:ra_dav:version-url +V 122 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/rigid/BodyState.as +END +PhysicsUtils.as +K 25 +svn:wc:ra_dav:version-url +V 125 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/rigid/PhysicsUtils.as +END +ContactPoint.as +K 25 +svn:wc:ra_dav:version-url +V 125 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/rigid/ContactPoint.as +END +RigidWorld.as +K 25 +svn:wc:ra_dav:version-url +V 123 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/rigid/RigidWorld.as +END +Contact.as +K 25 +svn:wc:ra_dav:version-url +V 120 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/rigid/Contact.as +END +BodyMaterial.as +K 25 +svn:wc:ra_dav:version-url +V 125 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/rigid/BodyMaterial.as +END diff --git a/0.0.6.0/src/alternativa/physics/rigid/.svn/entries b/0.0.6.0/src/alternativa/physics/rigid/.svn/entries new file mode 100644 index 0000000..44f18d8 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/rigid/.svn/entries @@ -0,0 +1,118 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/rigid +http://svndev.alternativaplatform.com + + + +2009-09-15T09:18:36.465808Z +19934 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Body.as +file + + + + +2010-10-28T04:32:42.000000Z +4228368100875c0d38e8924a99fc6b5e +2009-09-15T09:18:36.465808Z +19934 +mike + +BodyState.as +file + + + + +2010-10-28T04:32:42.000000Z +8a089205beccac13a05eccfb9cfc6387 +2009-06-08T06:46:18.310732Z +14099 +mike + +PhysicsUtils.as +file + + + + +2010-10-28T04:32:42.000000Z +6d88004ddab2ae19e0d15f5b1b7c016a +2009-04-26T21:40:17.813142Z +12023 +mike + +constraints +dir + +ContactPoint.as +file + + + + +2010-10-28T04:32:42.000000Z +a94e73e3bfc85be2a79d6cc939512d91 +2009-06-08T06:46:18.310732Z +14099 +mike + +RigidWorld.as +file + + + + +2010-10-28T04:32:42.000000Z +ff8118d44abedf88cf1b2b0062c93625 +2009-09-15T09:18:36.465808Z +19934 +mike + +primitives +dir + +Contact.as +file + + + + +2010-10-28T04:32:42.000000Z +d65ad3d488d4a06addcbfa4aaf958f10 +2009-04-13T03:04:08.898183Z +11018 +mike + +BodyMaterial.as +file + + + + +2010-10-28T04:32:42.000000Z +49c7a55b5b26fdb03a60f24bac460f53 +2009-04-07T08:34:06.342994Z +10587 +mike + diff --git a/0.0.6.0/src/alternativa/physics/rigid/.svn/format b/0.0.6.0/src/alternativa/physics/rigid/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/rigid/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.6.0/src/alternativa/physics/rigid/.svn/text-base/Body.as.svn-base b/0.0.6.0/src/alternativa/physics/rigid/.svn/text-base/Body.as.svn-base new file mode 100644 index 0000000..d615df2 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/rigid/.svn/text-base/Body.as.svn-base @@ -0,0 +1,347 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.IBodyCollisionPredicate; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Quaternion; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class Body { + + public static var linDamping:Number = 0.997; + public static var rotDamping:Number = 0.997; + + public var name:String; + public var movable:Boolean = true; + public var canFreeze:Boolean = false; + + public var freezeCounter:int; + public var frozen:Boolean = false; + public var aabb:BoundBox = new BoundBox(); + public var postCollisionPredicate:IBodyCollisionPredicate; + + altphysics var id:int; + // Мир, в котором находится тело + altphysics var world:RigidWorld; + // Текущее и предыдущее состояние тела. Промежуточное состояние вычисляется линейной интерполяцией. + altphysics var state:BodyState = new BodyState(); + altphysics var prevState:BodyState = new BodyState(); + // Линейное и угловое ускорение тела на текущем шаге симуляции + altphysics var accel:Vector3 = new Vector3(); + altphysics var angleAccel:Vector3 = new Vector3(); + // Материал тела + altphysics var material:BodyMaterial = new BodyMaterial(); + + altphysics var invMass:Number = 1; + altphysics var invInertia:Matrix3 = new Matrix3(); + altphysics var invInertiaWorld:Matrix3 = new Matrix3(); + altphysics var baseMatrix:Matrix3 = new Matrix3(); + + altphysics const MAX_CONTACTS:int = 20; + altphysics var contacts:Vector. = new Vector.(MAX_CONTACTS); + altphysics var contactsNum:int; + + altphysics var collisionPrimitives:Vector.; + altphysics var collisionPrimitivesNum:int; + + // Аккумулятор сил + altphysics var forceAccum:Vector3 = new Vector3(); + // Аккумулятор моментов + altphysics var torqueAccum:Vector3 = new Vector3(); + + // Внутренние переменные для избежания создания экземпляров + private static var _r:Vector3 = new Vector3(); + private static var _f:Vector3 = new Vector3(); + + /** + * + * @param invMass + * @param invInertia + */ + public function Body(invMass:Number, invInertia:Matrix3) { + this.invMass = invMass; + this.invInertia.copy(invInertia); + } + + /** + * @param primitive + * @param localTransform + */ + public function addCollisionPrimitive(primitive:CollisionPrimitive, localTransform:Matrix4 = null):void { + if (primitive == null) { + throw new ArgumentError("Primitive cannot be null"); + } + if (collisionPrimitives == null) { + collisionPrimitives = new Vector.(); + } + collisionPrimitives[collisionPrimitivesNum++] = primitive; + primitive.setBody(this, localTransform); + } + + /** + * @param t + * @param result + */ + public function interpolate(t:Number, pos:Vector3, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.w = prevState.orientation.w*t1 + state.orientation.w*t; + orientation.x = prevState.orientation.x*t1 + state.orientation.x*t; + orientation.y = prevState.orientation.y*t1 + state.orientation.y*t; + orientation.z = prevState.orientation.z*t1 + state.orientation.z*t; + } + + /** + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + state.pos.vCopy(pos); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setPositionXYZ(x:Number, y:Number, z:Number):void { + state.pos.vReset(x, y, z); + } + + /** + * + * @param vel + */ + public function setVelocity(vel:Vector3):void { + state.velocity.vCopy(vel); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setVelocityXYZ(x:Number, y:Number, z:Number):void { + state.velocity.vReset(x, y, z); + } + + /** + * + * @param rot + */ + public function setRotation(rot:Vector3):void { + state.rotation.vCopy(rot); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setRotationXYZ(x:Number, y:Number, z:Number):void { + state.rotation.vReset(x, y, z); + } + + /** + * + * @param q + */ + public function setOrientation(q:Quaternion):void { + state.orientation.copy(q); + } + + /** + * @param r + * @param dir + * @param magnitude + */ + public function applyRelPosWorldImpulse(r:Vector3, dir:Vector3, magnitude:Number):void { + var d:Number = magnitude*invMass; + // Линейная часть + state.velocity.x += d*dir.x; + state.velocity.y += d*dir.y; + state.velocity.z += d*dir.z; + + // Вращательная часть + var x:Number = (r.y*dir.z - r.z*dir.y)*magnitude; + var y:Number = (r.z*dir.x - r.x*dir.z)*magnitude; + var z:Number = (r.x*dir.y - r.y*dir.x)*magnitude; + + state.rotation.x += invInertiaWorld.a*x + invInertiaWorld.b*y + invInertiaWorld.c*z; + state.rotation.y += invInertiaWorld.e*x + invInertiaWorld.f*y + invInertiaWorld.g*z; + state.rotation.z += invInertiaWorld.i*x + invInertiaWorld.j*y + invInertiaWorld.k*z; + } + + /** + * @param f + */ + public function addForce(f:Vector3):void { + forceAccum.vAdd(f); + } + + /** + * @param pos + * @param f + */ + public function addWorldForce(pos:Vector3, force:Vector3):void { + forceAccum.vAdd(force); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(force)); + } + + /** + * @param pos + * @param f + */ + public function addWorldForceScaled(pos:Vector3, force:Vector3, scale:Number):void { + _f.x = scale*force.x; + _f.y = scale*force.y; + _f.z = scale*force.z; + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(_f)); + } + + /** + * @param pos + * @param f + */ + public function addLocalForce(pos:Vector3, force:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(pos, _r); + // Трансформируем вектор силы в мировую систему + baseMatrix.transformVector(force, _f); + // Добавляем силу и момент + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vCross(_f)); + } + + /** + * + * @param localPos + * @param worldForce + */ + public function addWorldForceAtLocalPoint(localPos:Vector3, worldForce:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(localPos, _r); + // Добавляем силу и момент + forceAccum.vAdd(worldForce); + torqueAccum.vAdd(_r.vCross(worldForce)); + } + + /** + * @param dt + */ + public function addExternalForces(dt:Number):void { + } + + /** + * @param t + */ + public function addTorque(t:Vector3):void { + torqueAccum.vAdd(t); + } + + /** + * + */ + altphysics function clearAccumulators():void { + forceAccum.x = forceAccum.y = forceAccum.z = 0; + torqueAccum.x = torqueAccum.y = torqueAccum.z = 0; + } + + /** + * + */ + altphysics function calcAccelerations():void { + accel.x = forceAccum.x*invMass; + accel.y = forceAccum.y*invMass; + accel.z = forceAccum.z*invMass; + angleAccel.x = invInertiaWorld.a*torqueAccum.x + invInertiaWorld.b*torqueAccum.y + invInertiaWorld.c*torqueAccum.z; + angleAccel.y = invInertiaWorld.e*torqueAccum.x + invInertiaWorld.f*torqueAccum.y + invInertiaWorld.g*torqueAccum.z; + angleAccel.z = invInertiaWorld.i*torqueAccum.x + invInertiaWorld.j*torqueAccum.y + invInertiaWorld.k*torqueAccum.z; + } + + /** + * Вычисляет производные данные. + */ + altphysics function calcDerivedData():void { + // Вычисление базисной матрицы и обратного тензора инерции в мировых координатах + state.orientation.toMatrix3(baseMatrix); + invInertiaWorld.copy(invInertia).append(baseMatrix).prependTransposed(baseMatrix); + if (collisionPrimitives != null) { + aabb.infinity(); + for (var i:int = 0; i < collisionPrimitivesNum; i++) { + var primitive:CollisionPrimitive = collisionPrimitives[i]; + primitive.transform.setFromMatrix3(baseMatrix, state.pos); + if (primitive.localTransform != null) { + primitive.transform.prepend(primitive.localTransform); + } + primitive.calculateAABB(); + aabb.addBoundBox(primitive.aabb); + } + } + } + + /** + * + */ + altphysics function saveState():void { + prevState.copy(state); + } + + /** + * + */ + altphysics function restoreState():void { + state.copy(prevState); + } + + /** + * @param dt + */ + altphysics function integrateVelocity(dt:Number):void { + // v = v + a*t + state.velocity.x += accel.x*dt; + state.velocity.y += accel.y*dt; + state.velocity.z += accel.z*dt; + // rot = rot + eps*t + state.rotation.x += angleAccel.x*dt; + state.rotation.y += angleAccel.y*dt; + state.rotation.z += angleAccel.z*dt; + + state.velocity.x *= linDamping; + state.velocity.y *= linDamping; + state.velocity.z *= linDamping; + + state.rotation.x *= rotDamping; + state.rotation.y *= rotDamping; + state.rotation.z *= rotDamping; + } + + /** + * + */ + altphysics function integratePosition(dt:Number):void { + // pos = pos + v*t + state.pos.x += state.velocity.x*dt; + state.pos.y += state.velocity.y*dt; + state.pos.z += state.velocity.z*dt; + // q = q + 0.5*rot*q + state.orientation.addScaledVector(state.rotation, dt); + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/rigid/.svn/text-base/BodyMaterial.as.svn-base b/0.0.6.0/src/alternativa/physics/rigid/.svn/text-base/BodyMaterial.as.svn-base new file mode 100644 index 0000000..61d4382 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/rigid/.svn/text-base/BodyMaterial.as.svn-base @@ -0,0 +1,11 @@ +package alternativa.physics.rigid { + + public class BodyMaterial { + + public var restitution:Number = 0; + public var friction:Number = 0.3; +// public var dynamicFriction:Number = 0.2; +// public var dynamicFriction:Number = 0.2; + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/rigid/.svn/text-base/BodyState.as.svn-base b/0.0.6.0/src/alternativa/physics/rigid/.svn/text-base/BodyState.as.svn-base new file mode 100644 index 0000000..408b0ea --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/rigid/.svn/text-base/BodyState.as.svn-base @@ -0,0 +1,39 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Quaternion; + import alternativa.physics.types.Vector3; + + /** + * Класс описывает состояние твёрдого тела. + */ + public class BodyState { + /** + * Положение тела. + */ + public var pos:Vector3 = new Vector3(); + /** + * Ориентация тела. + */ + public var orientation:Quaternion = new Quaternion(); + /** + * Скорость тела. + */ + public var velocity:Vector3 = new Vector3(); + /** + * Угловая скорость тела. + */ + public var rotation:Vector3 = new Vector3(); + + /** + * Копирует значение указанного объекта. + * + * @param state + */ + public function copy(state:BodyState):void { + pos.vCopy(state.pos); + orientation.copy(state.orientation); + velocity.vCopy(state.velocity); + rotation.vCopy(state.rotation); + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/rigid/.svn/text-base/Contact.as.svn-base b/0.0.6.0/src/alternativa/physics/rigid/.svn/text-base/Contact.as.svn-base new file mode 100644 index 0000000..b73fc55 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/rigid/.svn/text-base/Contact.as.svn-base @@ -0,0 +1,53 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.types.Vector3; + + /** + * Информация о контакте между двумя телами. Содержит нормаль, множество точек контакта и прочие величины. + */ + public class Contact { + +// private static var pool:Vector. = new Vector.(); +// +// public static function create():ContactInfo { +// if (pool.length > 0) { +// return pool.pop(); +// } +// return new ContactInfo(); +// } +// +// public static function destroy(collInfo:ContactInfo):void { +// collInfo.body1 = collInfo.body2 = null; +// pool.push(collInfo); +// } + + // Максимальное количество точек контакта + private const N:int = 8; + + public var body1:Body; + public var body2:Body; + // Взаимный коэффициент отскока + public var restitution:Number; + // Взаимный коэффициент трения + public var friction:Number; + // Нормаль контакта. Направлена от второго тела к первому. + public var normal:Vector3 = new Vector3(); + // Список точек контакта + public var points:Vector. = new Vector.(N, true); + // Количество точек контакта + public var pcount:int; + // Максимальная глубина пересечения тел + public var maxPenetration:Number = 0; + + public var satisfied:Boolean; + + /** + * + */ + public function Contact() { + for (var i:int = 0; i < N; i++) points[i] = new ContactPoint(); + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/rigid/.svn/text-base/ContactPoint.as.svn-base b/0.0.6.0/src/alternativa/physics/rigid/.svn/text-base/ContactPoint.as.svn-base new file mode 100644 index 0000000..c36f958 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/rigid/.svn/text-base/ContactPoint.as.svn-base @@ -0,0 +1,52 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Vector3; + + public class ContactPoint { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + + // Величины, расчитываемые перед началом фазы решения контактов + + // Требуемая проекция конечной скорости на нормаль для упругого контакта + public var normalVel:Number; + // Минимальная скорость разделения неупругого контакта + public var minSepVel:Number; + // Изменение проекции скорости на единицу нормального импульса + public var velByUnitImpulseN:Number; + + public var angularInertia1:Number; + public var angularInertia2:Number; + + // Радиус-вектор точки контакта относительно центра первого тела + public var r1:Vector3 = new Vector3(); + // Радиус-вектор точки контакта относительно центра второго тела + public var r2:Vector3 = new Vector3(); + + // Величины, накапливаемые во время фазы решения контактов + + // Накопленный импульс, применяемый для получения требуемой относительной скорости в точке контакта + public var accumImpulseN:Number; + // Накопленный импульс, применяемый для разделения тел в точке контакта. Не создаёт момента. +// public var accumSepImpulse:Number; + + public var satisfied:Boolean; + + /** + * + * @param cp + */ + public function copyFrom(cp:ContactPoint):void { + pos.vCopy(cp.pos); + penetration = cp.penetration; + feature1 = cp.feature1; + feature2 = cp.feature2; + r1.vCopy(cp.r1); + r2.vCopy(cp.r2); + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/rigid/.svn/text-base/PhysicsUtils.as.svn-base b/0.0.6.0/src/alternativa/physics/rigid/.svn/text-base/PhysicsUtils.as.svn-base new file mode 100644 index 0000000..a9aaa99 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/rigid/.svn/text-base/PhysicsUtils.as.svn-base @@ -0,0 +1,49 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + public class PhysicsUtils { + + /** + * @param mass + * @param halfSize + * @param result + */ + public static function getBoxInvInertia(mass:Number, halfSize:Vector3, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + result.a = 3/(mass*(yy + zz)); + result.f = 3/(mass*(zz + xx)); + result.k = 3/(mass*(xx + yy)); + } + + /** + * @param mass + * @param r + * @param h + * @param result + */ + public static function getCylinderInvInertia(mass:Number, r:Number, h:Number, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + + result.a = result.f = 1/(mass*(h*h/12 + r*r/4)); + result.k = 2/(mass*r*r); + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/rigid/.svn/text-base/RigidWorld.as.svn-base b/0.0.6.0/src/alternativa/physics/rigid/.svn/text-base/RigidWorld.as.svn-base new file mode 100644 index 0000000..c52ac74 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/rigid/.svn/text-base/RigidWorld.as.svn-base @@ -0,0 +1,718 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.ICollisionDetector; + import alternativa.physics.collision.KdTreeCollisionDetector; + import alternativa.physics.rigid.constraints.Constraint; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Класс реализует физическую симуляцию поведения твёрдых тел. + */ + public class RigidWorld { + + private static var lastBodyId:int; + + // Максимальное количество контактов + altphysics const MAX_CONTACTS:int = 1000; + + ////////////////////////////////////////////////////////////////// + // Настроечные параметры симуляции. Могут быть изменены в любой + // момент времени без нарушения корректной работы симуляции. + ////////////////////////////////////////////////////////////////// + // Количество шагов, за которое пересекающиеся тела должны разделиться + public var penResolutionSteps:int = 10; + // Величина допустимой глубины пересечения + public var allowedPenetration:Number = 0.1; + // Максимальная скорость, добавляемая с целью разделения тел + public var maxPenResolutionSpeed:Number = 0.5; + // Количество итераций для обработки упругих контактов + public var collisionIterations:int = 5; + // Количество итераций для обработки неупругих контактов + public var contactIterations:int = 5; + // Флаг использования предсказания состояний + public var usePrediction:Boolean = false; + + public var freezeSteps:int = 10; + public var linSpeedFreezeLimit:Number = 1; + public var angSpeedFreezeLimit:Number = 0.01; + + // Переменные для процедуры разделения тел путём непосредственного их перемещенеия. + // !!!!!!!!!!!!!!!!!!!!!!!! + // !!! Экспериментально !!! + // !!!!!!!!!!!!!!!!!!!!!!!! + public var staticSeparationIterations:int = 10; + public var staticSeparationSteps:int = 10; + public var maxAngleMove:Number = 10; + public var useStaticSeparation:Boolean = false; + + // Вектор гравитации + altphysics var _gravity:Vector3 = new Vector3(0, 0, -9.8); + // Использующийся детектор столкновений + altphysics var collisionDetector:ICollisionDetector; + // Список тел, участвующих в симуляции + altphysics var bodies:Vector. = new Vector.(); + // Количество тел, участвующих в симуляции + altphysics var bodiesNum:int; + // Список контактов на текущем шаге симуляции + altphysics var contacts:Vector. = new Vector.(MAX_CONTACTS, true); + // Количество контактов на текущем шаге симуляции + altphysics var contactsNum:int; + // Список ограничений + altphysics var constraints:Vector. = new Vector.(); + // Количество ограничений + altphysics var constraintsNum:int; + // Временная метка. Число прошедших шагов с начала симуляции. + altphysics var timeStamp:uint; + + // Временные переменные для избежания создания экземпляров + private var _r:Vector3 = new Vector3(); + private var _t:Vector3 = new Vector3(); + private var _v:Vector3 = new Vector3(); + private var _v1:Vector3 = new Vector3(); + private var _v2:Vector3 = new Vector3(); + + /** + * + */ + public function RigidWorld() { + for (var i:int = 0; i < MAX_CONTACTS; i++) contacts[i] = new Contact(); + collisionDetector = new KdTreeCollisionDetector(); + } + + /** + * Вектор гравитации. + */ + public function get gravity():Vector3 { + return _gravity.vClone(); + } + + /** + * @private + */ + public function set gravity(value:Vector3):void { + _gravity.vCopy(value); + } + + /** + * Добавляет тело в симуляцию. + * @param body + * @return + */ + public function addBody(body:Body, isStatic:Boolean):Boolean { + var i:int = bodies.indexOf(body); + if (i > -1) return false; + bodies[bodiesNum++] = body; + body.world = this; + body.id = lastBodyId++; +// if (body.collisionPrimitives != null) { +// for (i = 0; i < body.collisionPrimitivesNum; i++) collisionDetector.addPrimitive(body.collisionPrimitives[i], isStatic); +// } + return true; + } + + /** + * Удаляет тело из симуляции. + * @param body + * @return + */ + public function removeBody(body:Body, isStatic:Boolean):Boolean { + var i:int = bodies.indexOf(body); + if (i == -1) return false; + bodies.splice(i, 1); + body.world = null; +// if (body.collisionPrimitives != null) { +// for (i = 0; i < body.collisionPrimitivesNum; i++) collisionDetector.removePrimitive(body.collisionPrimitives[i], isStatic); +// } + bodiesNum--; + return true; + } + + /** + * Добавляет ограничение. + * @param c + */ + public function addConstraint(c:Constraint):void { + constraints[constraintsNum++] = c; + c.world = this; + } + + /** + * Удаляет ограничение. + * @param c + */ + public function removeConstraint(c:Constraint):Boolean { + var idx:int = constraints.indexOf(c); + if (idx < 0) return false; + constraints.splice(idx, 1) + constraintsNum--; + c.world = null; + return true; + } + + /** + * Применяет к телам действующте на них силы. Аккумуляторы сил и моментов тел очищаются после завершения шага + * симуляции, поэтому на момент вызова метода могут уже содержать некоторые значения. + * + * @param dt промежуток времени, в течении которого действуют силы + */ + private function applyForces(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i]; + body.addExternalForces(dt); + body.calcAccelerations(); + // Ускорение свободного падения применяется только к подвижным телам во избежание некорректного изменения + // фиктивной скорости неподвижных тел. + if (body.movable && !body.frozen) { + body.accel.x += _gravity.x; + body.accel.y += _gravity.y; + body.accel.z += _gravity.z; + } + } + } + + /** + * Определяет все столкновения на текущем шаге симуляции и заполняет список получившихся контактов. + * + * @param dt длительность шага симуляции + */ + private function detectCollisions(dt:Number):void { + var i:int; + var j:int; + var body:Body; + + for (i = 0; i < bodiesNum; i++) { + body = bodies[i]; + if (body.frozen) continue; + body.contactsNum = 0; + body.saveState(); + // При включённом режиме предсказания состояние тел интегрируется на один шаг вперёд + if (usePrediction) { + body.integrateVelocity(dt); + body.integratePosition(dt); + } + body.calcDerivedData(); + } + + contactsNum = collisionDetector.getAllCollisions(contacts); + + // Расчёт относительных векторов точки контакта вынесен сюда из-за необходимости учитывать + // положение тел в предсказанном состоянии + for (i = 0; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + var b1:Body = contact.body1; + var b2:Body = contact.body2; + for (j = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.r1.x = cp.pos.x - b1.state.pos.x; + cp.r1.y = cp.pos.y - b1.state.pos.y; + cp.r1.z = cp.pos.z - b1.state.pos.z; + if (b2 != null) { + cp.r2.x = cp.pos.x - b2.state.pos.x; + cp.r2.y = cp.pos.y - b2.state.pos.y; + cp.r2.z = cp.pos.z - b2.state.pos.z; + } + } + } + + // Восстановление состояния тел + if (usePrediction) { + for (i = 0; i < bodiesNum; i++) { + body = bodies[i]; + if (body.frozen) continue; + body.restoreState(); + body.calcDerivedData(); + } + } + } + + /** + * Подготваливает полученные из детектора столкновений контакты, расчитывая значения, не меняющиеся + * в ходе шага симуляции. + */ + private function preProcessContacts(dt:Number):void { + var i:int; + for (i = 0; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + var b1:Body = contact.body1; + var b2:Body = contact.body2; + // Столкнувшиеся тела размораживаются + if (b1.frozen) { + b1.frozen = false; + b1.freezeCounter = 0; + } + if (b2 != null && b2.frozen) { + b2.frozen = false; + b2.freezeCounter = 0; + } + contact.restitution = b1.material.restitution; + if (b2 != null && b2.material.restitution < contact.restitution) contact.restitution = b2.material.restitution; + contact.friction = b1.material.friction; + if (b2 != null && b2.material.friction < contact.friction) contact.friction = b2.material.friction; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.accumImpulseN = 0; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + cp.velByUnitImpulseN = 0; + if (b1.movable) { + cp.angularInertia1 = _v.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1).vDot(contact.normal); + cp.velByUnitImpulseN += b1.invMass + cp.angularInertia1; + } + if (b2 != null && b2.movable) { + cp.angularInertia2 = _v.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2).vDot(contact.normal); + cp.velByUnitImpulseN += b2.invMass + cp.angularInertia2; + } + // Расчёт требуемой конечной скорости для упругого контакта + calcSepVelocity(b1, b2, cp, _v); + cp.normalVel = _v.vDot(contact.normal); + if (cp.normalVel < 0) cp.normalVel = - contact.restitution*cp.normalVel; + // Скорость разделения неупругого контакта + cp.minSepVel = cp.penetration > allowedPenetration ? (cp.penetration - allowedPenetration)/(penResolutionSteps*dt) : 0; + if (cp.minSepVel > maxPenResolutionSpeed) cp.minSepVel = maxPenResolutionSpeed; + } + } + for (i = 0; i < constraintsNum; i++) Constraint(constraints[i]).preProcess(dt); + } + + /** + * + * @param dt + * @param forceInelastic + */ + private function processContacts(dt:Number, forceInelastic:Boolean):void { + var iterNum:int = forceInelastic ? contactIterations : collisionIterations; + var i:int; + var forwardLoop:Boolean = false; + for (var iter:int = 0; iter < iterNum; iter++) { + forwardLoop = !forwardLoop; + for (i = 0; i < contactsNum; i++) resolveContact(contacts[i], forceInelastic, forwardLoop); + // Ограничения + for (i = 0; i < constraintsNum; i++) Constraint(constraints[i]).apply(dt); + } + + // Разделение контактов путём непосредственного изменения координат и ориентации. Экспериментально. +// if (forceInelastic && useStaticSeparation) performStaticSeparation(); + } + + /** + * + */ + private function resolveContact(contactInfo:Contact, forceInelastic:Boolean, forwardLoop:Boolean):void { + var b1:Body = contactInfo.body1; + var b2:Body = contactInfo.body2; + var normal:Vector3 = contactInfo.normal; + var i:int; + if (forwardLoop) { + for (i = 0; i < contactInfo.pcount; i++) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } else { + for (i = contactInfo.pcount - 1; i >= 0; i--) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } + } + + /** + * + * @param idx + * @param b1 + * @param b2 + * @param colInfo + * @param normal + * @param forceInelastic + */ + private function resolveContactPoint(idx:int, b1:Body, b2:Body, contact:Contact, normal:Vector3, forceInelastic:Boolean):void { + var cp:ContactPoint = contact.points[idx]; + if (!forceInelastic) { + cp.satisfied = true; + } + + var newVel:Number = 0; + calcSepVelocity(b1, b2, cp, _v); + var cnormal:Vector3 = contact.normal; + var sepVel:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + if (forceInelastic) { + var minSpeVel:Number = useStaticSeparation ? 0 : cp.minSepVel; + if (sepVel < minSpeVel) { + cp.satisfied = false; + } else if (cp.satisfied) return; + newVel = minSpeVel; + } else { + newVel = cp.normalVel; + } + var deltaVel:Number = newVel - sepVel; + var impulse:Number = deltaVel/cp.velByUnitImpulseN; + var accumImpulse:Number = cp.accumImpulseN + impulse; + if (accumImpulse < 0) { + accumImpulse = 0; + } + var deltaImpulse:Number = accumImpulse - cp.accumImpulseN; + cp.accumImpulseN = accumImpulse; + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, normal, deltaImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, normal, -deltaImpulse); + } + + // Учёт силы трения + calcSepVelocity(b1, b2, cp, _v); + // Расчитываем изменение касательной скорости на единицу касательного импульса + var tanSpeedByUnitImpulse:Number = 0; + +// _v.vAddScaled(-_v.vDot(contact.normal), contact.normal); + var dot:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + _v.x -= dot*cnormal.x; + _v.y -= dot*cnormal.y; + _v.z -= dot*cnormal.z; + + var tanSpeed:Number = _v.vLength(); + if (tanSpeed < 0.001) return; + +// _t.vCopy(_v).vNormalize().vReverse(); + _t.x = -_v.x; + _t.y = -_v.y; + _t.z = -_v.z; + _t.vNormalize(); + + var r:Vector3; + var m:Matrix3; + var xx:Number; + var yy:Number; + var zz:Number; + // dV = b.invMass + ((invI * (r % t)) % r) * t + if (b1.movable) { +// _v.vCross2(cp.r1, _t).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1); + r = cp.r1; + m = b1.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b1.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + if (b2 != null && b2.movable) { +// _v.vCross2(cp.r2, _t).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2); + + r = cp.r2; + m = b2.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b2.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + + var tanImpulse:Number = tanSpeed/tanSpeedByUnitImpulse; + var max:Number = contact.friction*cp.accumImpulseN; + if (max < 0) { + if (tanImpulse < max) tanImpulse = max; + } else { + if (tanImpulse > max) tanImpulse = max; + } + + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, _t, tanImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, _t, -tanImpulse); + } + } + + /** + * + * @param cp + * @param normal + * @return + */ + private function calcSepVelocity(body1:Body, body2:Body, cp:ContactPoint, result:Vector3):void { + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 +// result.vCopy(body1.state.velocity).vAdd(_v1.vCross2(body1.state.rotation, cp.r1)); + var rot:Vector3 = body1.state.rotation; + var v:Vector3 = cp.r1; + var x:Number = rot.y*v.z - rot.z*v.y; + var y:Number = rot.z*v.x - rot.x*v.z; + var z:Number = rot.x*v.y - rot.y*v.x; + v = body1.state.velocity; + result.x = v.x + x; + result.y = v.y + y; + result.z = v.z + z; + // V2 = V2_c + w2%r2 + if (body2 != null) { +// result.vSubtract(body2.state.velocity).vSubtract(_v2.vCross2(body2.state.rotation, cp.r2)); + rot = body2.state.rotation; + v = cp.r2; + x = rot.y*v.z - rot.z*v.y; + y = rot.z*v.x - rot.x*v.z; + z = rot.x*v.y - rot.y*v.x; + v = body2.state.velocity; + result.x -= v.x + x; + result.y -= v.y + y; + result.z -= v.z + z; + } + } + + /** + * + * @param dt + */ + private function intergateVelocities(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + Body(bodies[i]).integrateVelocity(dt); + } + } + + /** + * + * @param dt + */ + private function integratePositions(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i]; + if (body.movable && !body.frozen) { + body.integratePosition(dt); + } + } + } + + /** + * + */ + private function performStaticSeparation():void { + var iterNum:int = staticSeparationIterations; +// iterNum = 100; + + // 1. В начале каждой итерации для всех контактов сбрасывается флаг satisfied с одновременным поиском наихудшего контакта, т.е. имеющего наибольшую величину пересечения тел. + // 2. Если найденный контакт имеет величину пересечения меньше предельно допустимой процедура прерывается, т.к. разделение контактов не требуется. + // 3. Если процедура продолжается, то выполняется разделение найденного контакта и установка его флага satisfied в true, после чего запускается внутренний цикл, состоящий + // из contactsNum - 1 итераций. На каждой итерации ищется наихудший контакт среди оставшихся (satisfied == false). Если такой контакт найден, то выполняется его разделение + // и переход к следующей итерации внутреннего цикла, иначе внутренний цикл прерывается и выполняется переход к следующей итерации внешнего цикла (шаг 1.). + for (var iter:int = 0; iter < iterNum; iter++) { + // Ищем контакт с максимальной величиной пересечения тел, одновременно сбрасывая флаги satisfied в false + var worstContact:Contact = contacts[0]; + var i:int; + for (i = 1; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + contact.satisfied = false; + if (contact.maxPenetration > worstContact.maxPenetration) worstContact = contact; + } + if (worstContact.maxPenetration <= allowedPenetration) return; + + resolveInterpenetration(worstContact); + // Внутренний цикл по оставшимся контактам + for (i = 1; i < contactsNum; i++) { + worstContact = getWorstContact(); + if (worstContact == null) break; + resolveInterpenetration(worstContact); + } + } + } + + /** + * + */ + private function getWorstContact():Contact { + var maxPen:Number = 0; + var worst:Contact = null; + for (var i:int = 0; i < contactsNum; i++) { + var c:Contact = contacts[i]; + if (!c.satisfied && c.maxPenetration > maxPen) { + worst = c; + maxPen = c.maxPenetration; + } + } + return maxPen > allowedPenetration ? worst : null; + } + + /** + * Разделяет указанный контакт, выполняя staticSeparationSteps итераций по списку точек контакта. + * 1. В начале каждой итерации ищется точка с наибольшим пересечением, одновременно сбрасываются флаги satisfied у точек. + * 2. Для найденной точки выполняется процедура разделения. + * 3. После выполняется pcount - 1 итерация по списку точек, каждый раз ищется наихудшая среди имеющих satisfied == false, для которой выполняется процедура разделения. + * Если наихудшая точка не найдена, то выполнется переход к следующей итерации внешнего цикла (шаг 1). + * + * В конце процедуры разделения обновляются значения пересечений для остальных точек контакта, а также для всех прочих контактов, относящихся к телам текущего. + * + * @param contact контакт для разделения + */ + private function resolveInterpenetration(contact:Contact):void { + contact.satisfied = true; + + for (var step:int = 0; step < staticSeparationSteps; step++) { + var worstCp:ContactPoint = contact.points[0]; + var cp:ContactPoint; + var i:int; + for (i = 1; i < contact.pcount; i++) { + cp = contact.points[i]; + cp.satisfied = false; + if (cp.penetration > worstCp.penetration) worstCp = cp; + } + if (worstCp.penetration <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + // Разделяем оставшиеся точки + var maxPen:Number = 0; + for (i = 1; i < contact.pcount; i++) { + // Поиск наихудшей точки + for (var j:int = 0; j < contact.pcount; j++) { + cp = contact.points[j]; + if (cp.satisfied) continue; + if (cp.penetration > maxPen) { + maxPen = cp.penetration; + worstCp = cp; + } + } + if (maxPen <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + } + } + } + + /** + * + * @param cp + * @param contact + */ + private function separateContactPoint(cp:ContactPoint, contact:Contact):void { + cp.satisfied = true; + + var b1:Body = contact.body1; + var b2:Body = contact.body2; + var totalMove:Number = cp.penetration - allowedPenetration; + var moveCoeff:Number = totalMove/cp.velByUnitImpulseN; + var linMove1:Number; + var angleMove1:Number; + if (b1.movable) { + linMove1 = b1.invMass*moveCoeff; + angleMove1 = cp.angularInertia1*moveCoeff; + if (angleMove1 > maxAngleMove) { + linMove1 += angleMove1 - maxAngleMove; + angleMove1 -= maxAngleMove; + } + b1.state.pos.vAddScaled(linMove1, contact.normal); + _v1.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vScale(angleMove1); + b1.state.orientation.addScaledVector(_v1, 1); + } + var linMove2:Number; + var angleMove2:Number; + if (b2 != null && b2.movable) { + linMove2 = b2.invMass*moveCoeff; + angleMove2 = cp.angularInertia2*moveCoeff; + if (angleMove2 > maxAngleMove) { + linMove2 += angleMove2 - maxAngleMove; + angleMove2 -= maxAngleMove; + } + b2.state.pos.vAddScaled(-linMove2, contact.normal); + _v2.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vScale(angleMove2); + _v2.vReverse(); + b2.state.orientation.addScaledVector(_v2, 1); + } + cp.penetration = allowedPenetration; + // Обновляем пересечения в других точках + var i:int; + for (i = 0; i < contact.pcount; i++) { + var cp1:ContactPoint = contact.points[i]; + if (cp1 == cp) continue; + var angularMove:Number; + if (b1.movable) { + angularMove = _v.vCross2(_v1, cp1.r1).vDot(contact.normal); + cp1.penetration -= linMove1 + angularMove; + } + if (b2 != null && b2.movable) { + angularMove = _v.vCross2(_v2, cp1.r2).vDot(contact.normal); + cp1.penetration -= linMove2 - angularMove; + } + // Обновление максимального значения для контакта + if (cp1.penetration > contact.maxPenetration) contact.maxPenetration = cp1.penetration; + } + // Обновляем пересечения для других контактов + var c:Contact; + var j:int; + if (b1.movable) { + for (i = 0; i < b1.contactsNum; i++) { + c = b1.contacts[i]; + if (c == contact) continue; + for (j = 0; j < c.pcount; j++) { + cp1 = c.points[j]; + if (b1 == c.body1) cp1.penetration -= linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r1).vDot(c.normal); + else cp1.penetration += linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r2).vDot(c.normal); + if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; + } + } + } + if (b2 != null && b2.movable) { + for (i = 0; i < b2.contactsNum; i++) { + c = b2.contacts[i]; + if (c == contact) continue; + for (j = 0; j < c.pcount; j++) { + cp1 = c.points[j]; + if (b2 == c.body1) cp1.penetration -= linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r1).vDot(c.normal); + else cp1.penetration += linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r2).vDot(c.normal); + if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; + } + } + } + } + + /** + * + */ + private function postPhysics():void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i]; + body.clearAccumulators(); + body.calcDerivedData(); + if (body.canFreeze) { + if (body.state.velocity.vLength() < linSpeedFreezeLimit && body.state.rotation.vLength() < angSpeedFreezeLimit) { + if (!body.frozen) { + body.freezeCounter++; + if (body.freezeCounter >= freezeSteps) body.frozen = true; + } + } else { + body.freezeCounter = 0; + body.frozen = false; + } + } + } + } + + /** + * + * @param dt + */ + public function runPhysics(dt:Number):void { + timeStamp++; + applyForces(dt); + detectCollisions(dt); + preProcessContacts(dt); + processContacts(dt, false); + intergateVelocities(dt); + processContacts(dt, true); + integratePositions(dt); + postPhysics(); + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/rigid/Body.as b/0.0.6.0/src/alternativa/physics/rigid/Body.as new file mode 100644 index 0000000..d615df2 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/rigid/Body.as @@ -0,0 +1,347 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.IBodyCollisionPredicate; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Quaternion; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class Body { + + public static var linDamping:Number = 0.997; + public static var rotDamping:Number = 0.997; + + public var name:String; + public var movable:Boolean = true; + public var canFreeze:Boolean = false; + + public var freezeCounter:int; + public var frozen:Boolean = false; + public var aabb:BoundBox = new BoundBox(); + public var postCollisionPredicate:IBodyCollisionPredicate; + + altphysics var id:int; + // Мир, в котором находится тело + altphysics var world:RigidWorld; + // Текущее и предыдущее состояние тела. Промежуточное состояние вычисляется линейной интерполяцией. + altphysics var state:BodyState = new BodyState(); + altphysics var prevState:BodyState = new BodyState(); + // Линейное и угловое ускорение тела на текущем шаге симуляции + altphysics var accel:Vector3 = new Vector3(); + altphysics var angleAccel:Vector3 = new Vector3(); + // Материал тела + altphysics var material:BodyMaterial = new BodyMaterial(); + + altphysics var invMass:Number = 1; + altphysics var invInertia:Matrix3 = new Matrix3(); + altphysics var invInertiaWorld:Matrix3 = new Matrix3(); + altphysics var baseMatrix:Matrix3 = new Matrix3(); + + altphysics const MAX_CONTACTS:int = 20; + altphysics var contacts:Vector. = new Vector.(MAX_CONTACTS); + altphysics var contactsNum:int; + + altphysics var collisionPrimitives:Vector.; + altphysics var collisionPrimitivesNum:int; + + // Аккумулятор сил + altphysics var forceAccum:Vector3 = new Vector3(); + // Аккумулятор моментов + altphysics var torqueAccum:Vector3 = new Vector3(); + + // Внутренние переменные для избежания создания экземпляров + private static var _r:Vector3 = new Vector3(); + private static var _f:Vector3 = new Vector3(); + + /** + * + * @param invMass + * @param invInertia + */ + public function Body(invMass:Number, invInertia:Matrix3) { + this.invMass = invMass; + this.invInertia.copy(invInertia); + } + + /** + * @param primitive + * @param localTransform + */ + public function addCollisionPrimitive(primitive:CollisionPrimitive, localTransform:Matrix4 = null):void { + if (primitive == null) { + throw new ArgumentError("Primitive cannot be null"); + } + if (collisionPrimitives == null) { + collisionPrimitives = new Vector.(); + } + collisionPrimitives[collisionPrimitivesNum++] = primitive; + primitive.setBody(this, localTransform); + } + + /** + * @param t + * @param result + */ + public function interpolate(t:Number, pos:Vector3, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.w = prevState.orientation.w*t1 + state.orientation.w*t; + orientation.x = prevState.orientation.x*t1 + state.orientation.x*t; + orientation.y = prevState.orientation.y*t1 + state.orientation.y*t; + orientation.z = prevState.orientation.z*t1 + state.orientation.z*t; + } + + /** + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + state.pos.vCopy(pos); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setPositionXYZ(x:Number, y:Number, z:Number):void { + state.pos.vReset(x, y, z); + } + + /** + * + * @param vel + */ + public function setVelocity(vel:Vector3):void { + state.velocity.vCopy(vel); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setVelocityXYZ(x:Number, y:Number, z:Number):void { + state.velocity.vReset(x, y, z); + } + + /** + * + * @param rot + */ + public function setRotation(rot:Vector3):void { + state.rotation.vCopy(rot); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setRotationXYZ(x:Number, y:Number, z:Number):void { + state.rotation.vReset(x, y, z); + } + + /** + * + * @param q + */ + public function setOrientation(q:Quaternion):void { + state.orientation.copy(q); + } + + /** + * @param r + * @param dir + * @param magnitude + */ + public function applyRelPosWorldImpulse(r:Vector3, dir:Vector3, magnitude:Number):void { + var d:Number = magnitude*invMass; + // Линейная часть + state.velocity.x += d*dir.x; + state.velocity.y += d*dir.y; + state.velocity.z += d*dir.z; + + // Вращательная часть + var x:Number = (r.y*dir.z - r.z*dir.y)*magnitude; + var y:Number = (r.z*dir.x - r.x*dir.z)*magnitude; + var z:Number = (r.x*dir.y - r.y*dir.x)*magnitude; + + state.rotation.x += invInertiaWorld.a*x + invInertiaWorld.b*y + invInertiaWorld.c*z; + state.rotation.y += invInertiaWorld.e*x + invInertiaWorld.f*y + invInertiaWorld.g*z; + state.rotation.z += invInertiaWorld.i*x + invInertiaWorld.j*y + invInertiaWorld.k*z; + } + + /** + * @param f + */ + public function addForce(f:Vector3):void { + forceAccum.vAdd(f); + } + + /** + * @param pos + * @param f + */ + public function addWorldForce(pos:Vector3, force:Vector3):void { + forceAccum.vAdd(force); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(force)); + } + + /** + * @param pos + * @param f + */ + public function addWorldForceScaled(pos:Vector3, force:Vector3, scale:Number):void { + _f.x = scale*force.x; + _f.y = scale*force.y; + _f.z = scale*force.z; + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(_f)); + } + + /** + * @param pos + * @param f + */ + public function addLocalForce(pos:Vector3, force:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(pos, _r); + // Трансформируем вектор силы в мировую систему + baseMatrix.transformVector(force, _f); + // Добавляем силу и момент + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vCross(_f)); + } + + /** + * + * @param localPos + * @param worldForce + */ + public function addWorldForceAtLocalPoint(localPos:Vector3, worldForce:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(localPos, _r); + // Добавляем силу и момент + forceAccum.vAdd(worldForce); + torqueAccum.vAdd(_r.vCross(worldForce)); + } + + /** + * @param dt + */ + public function addExternalForces(dt:Number):void { + } + + /** + * @param t + */ + public function addTorque(t:Vector3):void { + torqueAccum.vAdd(t); + } + + /** + * + */ + altphysics function clearAccumulators():void { + forceAccum.x = forceAccum.y = forceAccum.z = 0; + torqueAccum.x = torqueAccum.y = torqueAccum.z = 0; + } + + /** + * + */ + altphysics function calcAccelerations():void { + accel.x = forceAccum.x*invMass; + accel.y = forceAccum.y*invMass; + accel.z = forceAccum.z*invMass; + angleAccel.x = invInertiaWorld.a*torqueAccum.x + invInertiaWorld.b*torqueAccum.y + invInertiaWorld.c*torqueAccum.z; + angleAccel.y = invInertiaWorld.e*torqueAccum.x + invInertiaWorld.f*torqueAccum.y + invInertiaWorld.g*torqueAccum.z; + angleAccel.z = invInertiaWorld.i*torqueAccum.x + invInertiaWorld.j*torqueAccum.y + invInertiaWorld.k*torqueAccum.z; + } + + /** + * Вычисляет производные данные. + */ + altphysics function calcDerivedData():void { + // Вычисление базисной матрицы и обратного тензора инерции в мировых координатах + state.orientation.toMatrix3(baseMatrix); + invInertiaWorld.copy(invInertia).append(baseMatrix).prependTransposed(baseMatrix); + if (collisionPrimitives != null) { + aabb.infinity(); + for (var i:int = 0; i < collisionPrimitivesNum; i++) { + var primitive:CollisionPrimitive = collisionPrimitives[i]; + primitive.transform.setFromMatrix3(baseMatrix, state.pos); + if (primitive.localTransform != null) { + primitive.transform.prepend(primitive.localTransform); + } + primitive.calculateAABB(); + aabb.addBoundBox(primitive.aabb); + } + } + } + + /** + * + */ + altphysics function saveState():void { + prevState.copy(state); + } + + /** + * + */ + altphysics function restoreState():void { + state.copy(prevState); + } + + /** + * @param dt + */ + altphysics function integrateVelocity(dt:Number):void { + // v = v + a*t + state.velocity.x += accel.x*dt; + state.velocity.y += accel.y*dt; + state.velocity.z += accel.z*dt; + // rot = rot + eps*t + state.rotation.x += angleAccel.x*dt; + state.rotation.y += angleAccel.y*dt; + state.rotation.z += angleAccel.z*dt; + + state.velocity.x *= linDamping; + state.velocity.y *= linDamping; + state.velocity.z *= linDamping; + + state.rotation.x *= rotDamping; + state.rotation.y *= rotDamping; + state.rotation.z *= rotDamping; + } + + /** + * + */ + altphysics function integratePosition(dt:Number):void { + // pos = pos + v*t + state.pos.x += state.velocity.x*dt; + state.pos.y += state.velocity.y*dt; + state.pos.z += state.velocity.z*dt; + // q = q + 0.5*rot*q + state.orientation.addScaledVector(state.rotation, dt); + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/rigid/BodyMaterial.as b/0.0.6.0/src/alternativa/physics/rigid/BodyMaterial.as new file mode 100644 index 0000000..61d4382 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/rigid/BodyMaterial.as @@ -0,0 +1,11 @@ +package alternativa.physics.rigid { + + public class BodyMaterial { + + public var restitution:Number = 0; + public var friction:Number = 0.3; +// public var dynamicFriction:Number = 0.2; +// public var dynamicFriction:Number = 0.2; + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/rigid/BodyState.as b/0.0.6.0/src/alternativa/physics/rigid/BodyState.as new file mode 100644 index 0000000..408b0ea --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/rigid/BodyState.as @@ -0,0 +1,39 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Quaternion; + import alternativa.physics.types.Vector3; + + /** + * Класс описывает состояние твёрдого тела. + */ + public class BodyState { + /** + * Положение тела. + */ + public var pos:Vector3 = new Vector3(); + /** + * Ориентация тела. + */ + public var orientation:Quaternion = new Quaternion(); + /** + * Скорость тела. + */ + public var velocity:Vector3 = new Vector3(); + /** + * Угловая скорость тела. + */ + public var rotation:Vector3 = new Vector3(); + + /** + * Копирует значение указанного объекта. + * + * @param state + */ + public function copy(state:BodyState):void { + pos.vCopy(state.pos); + orientation.copy(state.orientation); + velocity.vCopy(state.velocity); + rotation.vCopy(state.rotation); + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/rigid/Contact.as b/0.0.6.0/src/alternativa/physics/rigid/Contact.as new file mode 100644 index 0000000..b73fc55 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/rigid/Contact.as @@ -0,0 +1,53 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.types.Vector3; + + /** + * Информация о контакте между двумя телами. Содержит нормаль, множество точек контакта и прочие величины. + */ + public class Contact { + +// private static var pool:Vector. = new Vector.(); +// +// public static function create():ContactInfo { +// if (pool.length > 0) { +// return pool.pop(); +// } +// return new ContactInfo(); +// } +// +// public static function destroy(collInfo:ContactInfo):void { +// collInfo.body1 = collInfo.body2 = null; +// pool.push(collInfo); +// } + + // Максимальное количество точек контакта + private const N:int = 8; + + public var body1:Body; + public var body2:Body; + // Взаимный коэффициент отскока + public var restitution:Number; + // Взаимный коэффициент трения + public var friction:Number; + // Нормаль контакта. Направлена от второго тела к первому. + public var normal:Vector3 = new Vector3(); + // Список точек контакта + public var points:Vector. = new Vector.(N, true); + // Количество точек контакта + public var pcount:int; + // Максимальная глубина пересечения тел + public var maxPenetration:Number = 0; + + public var satisfied:Boolean; + + /** + * + */ + public function Contact() { + for (var i:int = 0; i < N; i++) points[i] = new ContactPoint(); + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/rigid/ContactPoint.as b/0.0.6.0/src/alternativa/physics/rigid/ContactPoint.as new file mode 100644 index 0000000..c36f958 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/rigid/ContactPoint.as @@ -0,0 +1,52 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Vector3; + + public class ContactPoint { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + + // Величины, расчитываемые перед началом фазы решения контактов + + // Требуемая проекция конечной скорости на нормаль для упругого контакта + public var normalVel:Number; + // Минимальная скорость разделения неупругого контакта + public var minSepVel:Number; + // Изменение проекции скорости на единицу нормального импульса + public var velByUnitImpulseN:Number; + + public var angularInertia1:Number; + public var angularInertia2:Number; + + // Радиус-вектор точки контакта относительно центра первого тела + public var r1:Vector3 = new Vector3(); + // Радиус-вектор точки контакта относительно центра второго тела + public var r2:Vector3 = new Vector3(); + + // Величины, накапливаемые во время фазы решения контактов + + // Накопленный импульс, применяемый для получения требуемой относительной скорости в точке контакта + public var accumImpulseN:Number; + // Накопленный импульс, применяемый для разделения тел в точке контакта. Не создаёт момента. +// public var accumSepImpulse:Number; + + public var satisfied:Boolean; + + /** + * + * @param cp + */ + public function copyFrom(cp:ContactPoint):void { + pos.vCopy(cp.pos); + penetration = cp.penetration; + feature1 = cp.feature1; + feature2 = cp.feature2; + r1.vCopy(cp.r1); + r2.vCopy(cp.r2); + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/rigid/PhysicsUtils.as b/0.0.6.0/src/alternativa/physics/rigid/PhysicsUtils.as new file mode 100644 index 0000000..a9aaa99 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/rigid/PhysicsUtils.as @@ -0,0 +1,49 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + public class PhysicsUtils { + + /** + * @param mass + * @param halfSize + * @param result + */ + public static function getBoxInvInertia(mass:Number, halfSize:Vector3, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + result.a = 3/(mass*(yy + zz)); + result.f = 3/(mass*(zz + xx)); + result.k = 3/(mass*(xx + yy)); + } + + /** + * @param mass + * @param r + * @param h + * @param result + */ + public static function getCylinderInvInertia(mass:Number, r:Number, h:Number, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + + result.a = result.f = 1/(mass*(h*h/12 + r*r/4)); + result.k = 2/(mass*r*r); + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/rigid/RigidWorld.as b/0.0.6.0/src/alternativa/physics/rigid/RigidWorld.as new file mode 100644 index 0000000..c52ac74 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/rigid/RigidWorld.as @@ -0,0 +1,718 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.ICollisionDetector; + import alternativa.physics.collision.KdTreeCollisionDetector; + import alternativa.physics.rigid.constraints.Constraint; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Класс реализует физическую симуляцию поведения твёрдых тел. + */ + public class RigidWorld { + + private static var lastBodyId:int; + + // Максимальное количество контактов + altphysics const MAX_CONTACTS:int = 1000; + + ////////////////////////////////////////////////////////////////// + // Настроечные параметры симуляции. Могут быть изменены в любой + // момент времени без нарушения корректной работы симуляции. + ////////////////////////////////////////////////////////////////// + // Количество шагов, за которое пересекающиеся тела должны разделиться + public var penResolutionSteps:int = 10; + // Величина допустимой глубины пересечения + public var allowedPenetration:Number = 0.1; + // Максимальная скорость, добавляемая с целью разделения тел + public var maxPenResolutionSpeed:Number = 0.5; + // Количество итераций для обработки упругих контактов + public var collisionIterations:int = 5; + // Количество итераций для обработки неупругих контактов + public var contactIterations:int = 5; + // Флаг использования предсказания состояний + public var usePrediction:Boolean = false; + + public var freezeSteps:int = 10; + public var linSpeedFreezeLimit:Number = 1; + public var angSpeedFreezeLimit:Number = 0.01; + + // Переменные для процедуры разделения тел путём непосредственного их перемещенеия. + // !!!!!!!!!!!!!!!!!!!!!!!! + // !!! Экспериментально !!! + // !!!!!!!!!!!!!!!!!!!!!!!! + public var staticSeparationIterations:int = 10; + public var staticSeparationSteps:int = 10; + public var maxAngleMove:Number = 10; + public var useStaticSeparation:Boolean = false; + + // Вектор гравитации + altphysics var _gravity:Vector3 = new Vector3(0, 0, -9.8); + // Использующийся детектор столкновений + altphysics var collisionDetector:ICollisionDetector; + // Список тел, участвующих в симуляции + altphysics var bodies:Vector. = new Vector.(); + // Количество тел, участвующих в симуляции + altphysics var bodiesNum:int; + // Список контактов на текущем шаге симуляции + altphysics var contacts:Vector. = new Vector.(MAX_CONTACTS, true); + // Количество контактов на текущем шаге симуляции + altphysics var contactsNum:int; + // Список ограничений + altphysics var constraints:Vector. = new Vector.(); + // Количество ограничений + altphysics var constraintsNum:int; + // Временная метка. Число прошедших шагов с начала симуляции. + altphysics var timeStamp:uint; + + // Временные переменные для избежания создания экземпляров + private var _r:Vector3 = new Vector3(); + private var _t:Vector3 = new Vector3(); + private var _v:Vector3 = new Vector3(); + private var _v1:Vector3 = new Vector3(); + private var _v2:Vector3 = new Vector3(); + + /** + * + */ + public function RigidWorld() { + for (var i:int = 0; i < MAX_CONTACTS; i++) contacts[i] = new Contact(); + collisionDetector = new KdTreeCollisionDetector(); + } + + /** + * Вектор гравитации. + */ + public function get gravity():Vector3 { + return _gravity.vClone(); + } + + /** + * @private + */ + public function set gravity(value:Vector3):void { + _gravity.vCopy(value); + } + + /** + * Добавляет тело в симуляцию. + * @param body + * @return + */ + public function addBody(body:Body, isStatic:Boolean):Boolean { + var i:int = bodies.indexOf(body); + if (i > -1) return false; + bodies[bodiesNum++] = body; + body.world = this; + body.id = lastBodyId++; +// if (body.collisionPrimitives != null) { +// for (i = 0; i < body.collisionPrimitivesNum; i++) collisionDetector.addPrimitive(body.collisionPrimitives[i], isStatic); +// } + return true; + } + + /** + * Удаляет тело из симуляции. + * @param body + * @return + */ + public function removeBody(body:Body, isStatic:Boolean):Boolean { + var i:int = bodies.indexOf(body); + if (i == -1) return false; + bodies.splice(i, 1); + body.world = null; +// if (body.collisionPrimitives != null) { +// for (i = 0; i < body.collisionPrimitivesNum; i++) collisionDetector.removePrimitive(body.collisionPrimitives[i], isStatic); +// } + bodiesNum--; + return true; + } + + /** + * Добавляет ограничение. + * @param c + */ + public function addConstraint(c:Constraint):void { + constraints[constraintsNum++] = c; + c.world = this; + } + + /** + * Удаляет ограничение. + * @param c + */ + public function removeConstraint(c:Constraint):Boolean { + var idx:int = constraints.indexOf(c); + if (idx < 0) return false; + constraints.splice(idx, 1) + constraintsNum--; + c.world = null; + return true; + } + + /** + * Применяет к телам действующте на них силы. Аккумуляторы сил и моментов тел очищаются после завершения шага + * симуляции, поэтому на момент вызова метода могут уже содержать некоторые значения. + * + * @param dt промежуток времени, в течении которого действуют силы + */ + private function applyForces(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i]; + body.addExternalForces(dt); + body.calcAccelerations(); + // Ускорение свободного падения применяется только к подвижным телам во избежание некорректного изменения + // фиктивной скорости неподвижных тел. + if (body.movable && !body.frozen) { + body.accel.x += _gravity.x; + body.accel.y += _gravity.y; + body.accel.z += _gravity.z; + } + } + } + + /** + * Определяет все столкновения на текущем шаге симуляции и заполняет список получившихся контактов. + * + * @param dt длительность шага симуляции + */ + private function detectCollisions(dt:Number):void { + var i:int; + var j:int; + var body:Body; + + for (i = 0; i < bodiesNum; i++) { + body = bodies[i]; + if (body.frozen) continue; + body.contactsNum = 0; + body.saveState(); + // При включённом режиме предсказания состояние тел интегрируется на один шаг вперёд + if (usePrediction) { + body.integrateVelocity(dt); + body.integratePosition(dt); + } + body.calcDerivedData(); + } + + contactsNum = collisionDetector.getAllCollisions(contacts); + + // Расчёт относительных векторов точки контакта вынесен сюда из-за необходимости учитывать + // положение тел в предсказанном состоянии + for (i = 0; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + var b1:Body = contact.body1; + var b2:Body = contact.body2; + for (j = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.r1.x = cp.pos.x - b1.state.pos.x; + cp.r1.y = cp.pos.y - b1.state.pos.y; + cp.r1.z = cp.pos.z - b1.state.pos.z; + if (b2 != null) { + cp.r2.x = cp.pos.x - b2.state.pos.x; + cp.r2.y = cp.pos.y - b2.state.pos.y; + cp.r2.z = cp.pos.z - b2.state.pos.z; + } + } + } + + // Восстановление состояния тел + if (usePrediction) { + for (i = 0; i < bodiesNum; i++) { + body = bodies[i]; + if (body.frozen) continue; + body.restoreState(); + body.calcDerivedData(); + } + } + } + + /** + * Подготваливает полученные из детектора столкновений контакты, расчитывая значения, не меняющиеся + * в ходе шага симуляции. + */ + private function preProcessContacts(dt:Number):void { + var i:int; + for (i = 0; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + var b1:Body = contact.body1; + var b2:Body = contact.body2; + // Столкнувшиеся тела размораживаются + if (b1.frozen) { + b1.frozen = false; + b1.freezeCounter = 0; + } + if (b2 != null && b2.frozen) { + b2.frozen = false; + b2.freezeCounter = 0; + } + contact.restitution = b1.material.restitution; + if (b2 != null && b2.material.restitution < contact.restitution) contact.restitution = b2.material.restitution; + contact.friction = b1.material.friction; + if (b2 != null && b2.material.friction < contact.friction) contact.friction = b2.material.friction; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.accumImpulseN = 0; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + cp.velByUnitImpulseN = 0; + if (b1.movable) { + cp.angularInertia1 = _v.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1).vDot(contact.normal); + cp.velByUnitImpulseN += b1.invMass + cp.angularInertia1; + } + if (b2 != null && b2.movable) { + cp.angularInertia2 = _v.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2).vDot(contact.normal); + cp.velByUnitImpulseN += b2.invMass + cp.angularInertia2; + } + // Расчёт требуемой конечной скорости для упругого контакта + calcSepVelocity(b1, b2, cp, _v); + cp.normalVel = _v.vDot(contact.normal); + if (cp.normalVel < 0) cp.normalVel = - contact.restitution*cp.normalVel; + // Скорость разделения неупругого контакта + cp.minSepVel = cp.penetration > allowedPenetration ? (cp.penetration - allowedPenetration)/(penResolutionSteps*dt) : 0; + if (cp.minSepVel > maxPenResolutionSpeed) cp.minSepVel = maxPenResolutionSpeed; + } + } + for (i = 0; i < constraintsNum; i++) Constraint(constraints[i]).preProcess(dt); + } + + /** + * + * @param dt + * @param forceInelastic + */ + private function processContacts(dt:Number, forceInelastic:Boolean):void { + var iterNum:int = forceInelastic ? contactIterations : collisionIterations; + var i:int; + var forwardLoop:Boolean = false; + for (var iter:int = 0; iter < iterNum; iter++) { + forwardLoop = !forwardLoop; + for (i = 0; i < contactsNum; i++) resolveContact(contacts[i], forceInelastic, forwardLoop); + // Ограничения + for (i = 0; i < constraintsNum; i++) Constraint(constraints[i]).apply(dt); + } + + // Разделение контактов путём непосредственного изменения координат и ориентации. Экспериментально. +// if (forceInelastic && useStaticSeparation) performStaticSeparation(); + } + + /** + * + */ + private function resolveContact(contactInfo:Contact, forceInelastic:Boolean, forwardLoop:Boolean):void { + var b1:Body = contactInfo.body1; + var b2:Body = contactInfo.body2; + var normal:Vector3 = contactInfo.normal; + var i:int; + if (forwardLoop) { + for (i = 0; i < contactInfo.pcount; i++) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } else { + for (i = contactInfo.pcount - 1; i >= 0; i--) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } + } + + /** + * + * @param idx + * @param b1 + * @param b2 + * @param colInfo + * @param normal + * @param forceInelastic + */ + private function resolveContactPoint(idx:int, b1:Body, b2:Body, contact:Contact, normal:Vector3, forceInelastic:Boolean):void { + var cp:ContactPoint = contact.points[idx]; + if (!forceInelastic) { + cp.satisfied = true; + } + + var newVel:Number = 0; + calcSepVelocity(b1, b2, cp, _v); + var cnormal:Vector3 = contact.normal; + var sepVel:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + if (forceInelastic) { + var minSpeVel:Number = useStaticSeparation ? 0 : cp.minSepVel; + if (sepVel < minSpeVel) { + cp.satisfied = false; + } else if (cp.satisfied) return; + newVel = minSpeVel; + } else { + newVel = cp.normalVel; + } + var deltaVel:Number = newVel - sepVel; + var impulse:Number = deltaVel/cp.velByUnitImpulseN; + var accumImpulse:Number = cp.accumImpulseN + impulse; + if (accumImpulse < 0) { + accumImpulse = 0; + } + var deltaImpulse:Number = accumImpulse - cp.accumImpulseN; + cp.accumImpulseN = accumImpulse; + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, normal, deltaImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, normal, -deltaImpulse); + } + + // Учёт силы трения + calcSepVelocity(b1, b2, cp, _v); + // Расчитываем изменение касательной скорости на единицу касательного импульса + var tanSpeedByUnitImpulse:Number = 0; + +// _v.vAddScaled(-_v.vDot(contact.normal), contact.normal); + var dot:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + _v.x -= dot*cnormal.x; + _v.y -= dot*cnormal.y; + _v.z -= dot*cnormal.z; + + var tanSpeed:Number = _v.vLength(); + if (tanSpeed < 0.001) return; + +// _t.vCopy(_v).vNormalize().vReverse(); + _t.x = -_v.x; + _t.y = -_v.y; + _t.z = -_v.z; + _t.vNormalize(); + + var r:Vector3; + var m:Matrix3; + var xx:Number; + var yy:Number; + var zz:Number; + // dV = b.invMass + ((invI * (r % t)) % r) * t + if (b1.movable) { +// _v.vCross2(cp.r1, _t).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1); + r = cp.r1; + m = b1.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b1.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + if (b2 != null && b2.movable) { +// _v.vCross2(cp.r2, _t).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2); + + r = cp.r2; + m = b2.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b2.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + + var tanImpulse:Number = tanSpeed/tanSpeedByUnitImpulse; + var max:Number = contact.friction*cp.accumImpulseN; + if (max < 0) { + if (tanImpulse < max) tanImpulse = max; + } else { + if (tanImpulse > max) tanImpulse = max; + } + + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, _t, tanImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, _t, -tanImpulse); + } + } + + /** + * + * @param cp + * @param normal + * @return + */ + private function calcSepVelocity(body1:Body, body2:Body, cp:ContactPoint, result:Vector3):void { + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 +// result.vCopy(body1.state.velocity).vAdd(_v1.vCross2(body1.state.rotation, cp.r1)); + var rot:Vector3 = body1.state.rotation; + var v:Vector3 = cp.r1; + var x:Number = rot.y*v.z - rot.z*v.y; + var y:Number = rot.z*v.x - rot.x*v.z; + var z:Number = rot.x*v.y - rot.y*v.x; + v = body1.state.velocity; + result.x = v.x + x; + result.y = v.y + y; + result.z = v.z + z; + // V2 = V2_c + w2%r2 + if (body2 != null) { +// result.vSubtract(body2.state.velocity).vSubtract(_v2.vCross2(body2.state.rotation, cp.r2)); + rot = body2.state.rotation; + v = cp.r2; + x = rot.y*v.z - rot.z*v.y; + y = rot.z*v.x - rot.x*v.z; + z = rot.x*v.y - rot.y*v.x; + v = body2.state.velocity; + result.x -= v.x + x; + result.y -= v.y + y; + result.z -= v.z + z; + } + } + + /** + * + * @param dt + */ + private function intergateVelocities(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + Body(bodies[i]).integrateVelocity(dt); + } + } + + /** + * + * @param dt + */ + private function integratePositions(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i]; + if (body.movable && !body.frozen) { + body.integratePosition(dt); + } + } + } + + /** + * + */ + private function performStaticSeparation():void { + var iterNum:int = staticSeparationIterations; +// iterNum = 100; + + // 1. В начале каждой итерации для всех контактов сбрасывается флаг satisfied с одновременным поиском наихудшего контакта, т.е. имеющего наибольшую величину пересечения тел. + // 2. Если найденный контакт имеет величину пересечения меньше предельно допустимой процедура прерывается, т.к. разделение контактов не требуется. + // 3. Если процедура продолжается, то выполняется разделение найденного контакта и установка его флага satisfied в true, после чего запускается внутренний цикл, состоящий + // из contactsNum - 1 итераций. На каждой итерации ищется наихудший контакт среди оставшихся (satisfied == false). Если такой контакт найден, то выполняется его разделение + // и переход к следующей итерации внутреннего цикла, иначе внутренний цикл прерывается и выполняется переход к следующей итерации внешнего цикла (шаг 1.). + for (var iter:int = 0; iter < iterNum; iter++) { + // Ищем контакт с максимальной величиной пересечения тел, одновременно сбрасывая флаги satisfied в false + var worstContact:Contact = contacts[0]; + var i:int; + for (i = 1; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + contact.satisfied = false; + if (contact.maxPenetration > worstContact.maxPenetration) worstContact = contact; + } + if (worstContact.maxPenetration <= allowedPenetration) return; + + resolveInterpenetration(worstContact); + // Внутренний цикл по оставшимся контактам + for (i = 1; i < contactsNum; i++) { + worstContact = getWorstContact(); + if (worstContact == null) break; + resolveInterpenetration(worstContact); + } + } + } + + /** + * + */ + private function getWorstContact():Contact { + var maxPen:Number = 0; + var worst:Contact = null; + for (var i:int = 0; i < contactsNum; i++) { + var c:Contact = contacts[i]; + if (!c.satisfied && c.maxPenetration > maxPen) { + worst = c; + maxPen = c.maxPenetration; + } + } + return maxPen > allowedPenetration ? worst : null; + } + + /** + * Разделяет указанный контакт, выполняя staticSeparationSteps итераций по списку точек контакта. + * 1. В начале каждой итерации ищется точка с наибольшим пересечением, одновременно сбрасываются флаги satisfied у точек. + * 2. Для найденной точки выполняется процедура разделения. + * 3. После выполняется pcount - 1 итерация по списку точек, каждый раз ищется наихудшая среди имеющих satisfied == false, для которой выполняется процедура разделения. + * Если наихудшая точка не найдена, то выполнется переход к следующей итерации внешнего цикла (шаг 1). + * + * В конце процедуры разделения обновляются значения пересечений для остальных точек контакта, а также для всех прочих контактов, относящихся к телам текущего. + * + * @param contact контакт для разделения + */ + private function resolveInterpenetration(contact:Contact):void { + contact.satisfied = true; + + for (var step:int = 0; step < staticSeparationSteps; step++) { + var worstCp:ContactPoint = contact.points[0]; + var cp:ContactPoint; + var i:int; + for (i = 1; i < contact.pcount; i++) { + cp = contact.points[i]; + cp.satisfied = false; + if (cp.penetration > worstCp.penetration) worstCp = cp; + } + if (worstCp.penetration <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + // Разделяем оставшиеся точки + var maxPen:Number = 0; + for (i = 1; i < contact.pcount; i++) { + // Поиск наихудшей точки + for (var j:int = 0; j < contact.pcount; j++) { + cp = contact.points[j]; + if (cp.satisfied) continue; + if (cp.penetration > maxPen) { + maxPen = cp.penetration; + worstCp = cp; + } + } + if (maxPen <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + } + } + } + + /** + * + * @param cp + * @param contact + */ + private function separateContactPoint(cp:ContactPoint, contact:Contact):void { + cp.satisfied = true; + + var b1:Body = contact.body1; + var b2:Body = contact.body2; + var totalMove:Number = cp.penetration - allowedPenetration; + var moveCoeff:Number = totalMove/cp.velByUnitImpulseN; + var linMove1:Number; + var angleMove1:Number; + if (b1.movable) { + linMove1 = b1.invMass*moveCoeff; + angleMove1 = cp.angularInertia1*moveCoeff; + if (angleMove1 > maxAngleMove) { + linMove1 += angleMove1 - maxAngleMove; + angleMove1 -= maxAngleMove; + } + b1.state.pos.vAddScaled(linMove1, contact.normal); + _v1.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vScale(angleMove1); + b1.state.orientation.addScaledVector(_v1, 1); + } + var linMove2:Number; + var angleMove2:Number; + if (b2 != null && b2.movable) { + linMove2 = b2.invMass*moveCoeff; + angleMove2 = cp.angularInertia2*moveCoeff; + if (angleMove2 > maxAngleMove) { + linMove2 += angleMove2 - maxAngleMove; + angleMove2 -= maxAngleMove; + } + b2.state.pos.vAddScaled(-linMove2, contact.normal); + _v2.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vScale(angleMove2); + _v2.vReverse(); + b2.state.orientation.addScaledVector(_v2, 1); + } + cp.penetration = allowedPenetration; + // Обновляем пересечения в других точках + var i:int; + for (i = 0; i < contact.pcount; i++) { + var cp1:ContactPoint = contact.points[i]; + if (cp1 == cp) continue; + var angularMove:Number; + if (b1.movable) { + angularMove = _v.vCross2(_v1, cp1.r1).vDot(contact.normal); + cp1.penetration -= linMove1 + angularMove; + } + if (b2 != null && b2.movable) { + angularMove = _v.vCross2(_v2, cp1.r2).vDot(contact.normal); + cp1.penetration -= linMove2 - angularMove; + } + // Обновление максимального значения для контакта + if (cp1.penetration > contact.maxPenetration) contact.maxPenetration = cp1.penetration; + } + // Обновляем пересечения для других контактов + var c:Contact; + var j:int; + if (b1.movable) { + for (i = 0; i < b1.contactsNum; i++) { + c = b1.contacts[i]; + if (c == contact) continue; + for (j = 0; j < c.pcount; j++) { + cp1 = c.points[j]; + if (b1 == c.body1) cp1.penetration -= linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r1).vDot(c.normal); + else cp1.penetration += linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r2).vDot(c.normal); + if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; + } + } + } + if (b2 != null && b2.movable) { + for (i = 0; i < b2.contactsNum; i++) { + c = b2.contacts[i]; + if (c == contact) continue; + for (j = 0; j < c.pcount; j++) { + cp1 = c.points[j]; + if (b2 == c.body1) cp1.penetration -= linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r1).vDot(c.normal); + else cp1.penetration += linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r2).vDot(c.normal); + if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; + } + } + } + } + + /** + * + */ + private function postPhysics():void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i]; + body.clearAccumulators(); + body.calcDerivedData(); + if (body.canFreeze) { + if (body.state.velocity.vLength() < linSpeedFreezeLimit && body.state.rotation.vLength() < angSpeedFreezeLimit) { + if (!body.frozen) { + body.freezeCounter++; + if (body.freezeCounter >= freezeSteps) body.frozen = true; + } + } else { + body.freezeCounter = 0; + body.frozen = false; + } + } + } + } + + /** + * + * @param dt + */ + public function runPhysics(dt:Number):void { + timeStamp++; + applyForces(dt); + detectCollisions(dt); + preProcessContacts(dt); + processContacts(dt, false); + intergateVelocities(dt); + processContacts(dt, true); + integratePositions(dt); + postPhysics(); + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/rigid/constraints/.svn/all-wcprops b/0.0.6.0/src/alternativa/physics/rigid/constraints/.svn/all-wcprops new file mode 100644 index 0000000..d728a83 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/rigid/constraints/.svn/all-wcprops @@ -0,0 +1,17 @@ +K 25 +svn:wc:ra_dav:version-url +V 121 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/rigid/constraints +END +Constraint.as +K 25 +svn:wc:ra_dav:version-url +V 135 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/rigid/constraints/Constraint.as +END +MaxDistanceConstraint.as +K 25 +svn:wc:ra_dav:version-url +V 146 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/rigid/constraints/MaxDistanceConstraint.as +END diff --git a/0.0.6.0/src/alternativa/physics/rigid/constraints/.svn/entries b/0.0.6.0/src/alternativa/physics/rigid/constraints/.svn/entries new file mode 100644 index 0000000..9245845 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/rigid/constraints/.svn/entries @@ -0,0 +1,52 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/rigid/constraints +http://svndev.alternativaplatform.com + + + +2009-09-15T09:18:36.465808Z +19934 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Constraint.as +file + + + + +2010-10-28T04:32:41.000000Z +757824f9eeb3c89243d104910e0d0529 +2009-04-23T13:43:38.903278Z +11779 +mike + +MaxDistanceConstraint.as +file + + + + +2010-10-28T04:32:41.000000Z +1723b9d06bf413afd453bbcd5cdf0ec1 +2009-09-15T09:18:36.465808Z +19934 +mike + diff --git a/0.0.6.0/src/alternativa/physics/rigid/constraints/.svn/format b/0.0.6.0/src/alternativa/physics/rigid/constraints/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/rigid/constraints/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.6.0/src/alternativa/physics/rigid/constraints/.svn/text-base/Constraint.as.svn-base b/0.0.6.0/src/alternativa/physics/rigid/constraints/.svn/text-base/Constraint.as.svn-base new file mode 100644 index 0000000..c2cd912 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/rigid/constraints/.svn/text-base/Constraint.as.svn-base @@ -0,0 +1,35 @@ +package alternativa.physics.rigid.constraints { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.RigidWorld; + + use namespace altphysics; + + /** + * + */ + public class Constraint { + + altphysics var satisfied:Boolean; + altphysics var world:RigidWorld; + + /** + * + */ + public function Constraint() { + } + + /** + * @param dt + */ + public function preProcess(dt:Number):void { + } + + /** + * @param dt + */ + public function apply(dt:Number):void { + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/rigid/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base b/0.0.6.0/src/alternativa/physics/rigid/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base new file mode 100644 index 0000000..b4cbb74 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/rigid/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base @@ -0,0 +1,183 @@ +package alternativa.physics.rigid.constraints { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class MaxDistanceConstraint extends Constraint { + // Первое тело ограничения + altphysics var body1:Body; + // Второе тело ограничения. Может быть null. + altphysics var body2:Body; + // Первая точка крепления в системе координат первого тела + altphysics var r1:Vector3; + // Вторая точка привязки в системе координат второго тела. В случае отсутствия второго тела, координаты задаются в мировой системе. + altphysics var r2:Vector3; + // Максимально допустимое расстояние между точками крепления + altphysics var maxDistance:Number; + // Радиус-вектор первой точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr1:Vector3 = new Vector3(); + // Радиус-вектор второй точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr2:Vector3 = new Vector3(); + // Минимальная скорость сближения для удовлетворения ограничения + private var minClosingVel:Number; + // Изменение относительной скорости на единицу импульса + private var velByUnitImpulseN:Number; + // Направление корректирующего импульса + private var impulseDirection:Vector3 = new Vector3(); + + // Вспомогательные переменные +// private static var _v:Vector3 = new Vector3(); +// private static var _v1:Vector3 = new Vector3(); +// private static var _v2:Vector3 = new Vector3(); + + /** + * @param body1 + * @param body2 + * @param r1 + * @param r2 + * @param maxDistance + */ + public function MaxDistanceConstraint(body1:Body, body2:Body, r1:Vector3, r2:Vector3, maxDistance:Number) { + super(); + this.body1 = body1; + this.body2 = body2; + this.r1 = r1.vClone(); + this.r2 = r2.vClone(); + this.maxDistance = maxDistance; + } + + /** + * @param dt + */ + override public function preProcess(dt:Number):void { + // Вычислим расстояние между точками + var m:Matrix3 = body1.baseMatrix; + wr1.x = m.a*r1.x + m.b*r1.y + m.c*r1.z; + wr1.y = m.e*r1.x + m.f*r1.y + m.g*r1.z; + wr1.z = m.i*r1.x + m.j*r1.y + m.k*r1.z; + if (body2 != null) { + m = body2.baseMatrix; + wr2.x = m.a*r2.x + m.b*r2.y + m.c*r2.z; + wr2.y = m.e*r2.x + m.f*r2.y + m.g*r2.z; + wr2.z = m.i*r2.x + m.j*r2.y + m.k*r2.z; + } else { + wr2.x = r2.x; + wr2.y = r2.y; + wr2.z = r2.z; + } + var p1:Vector3 = body1.state.pos; + impulseDirection.x = wr2.x - wr1.x - p1.x; + impulseDirection.y = wr2.y - wr1.y - p1.y; + impulseDirection.z = wr2.z - wr1.z - p1.z; + if (body2 != null) { + var p2:Vector3 = body2.state.pos; + impulseDirection.x += p2.x; + impulseDirection.y += p2.y; + impulseDirection.z += p2.z; + } + + var len:Number = Math.sqrt(impulseDirection.x*impulseDirection.x + impulseDirection.y*impulseDirection.y + impulseDirection.z*impulseDirection.z); + var delta:Number = len - maxDistance; + if (delta > 0) { + satisfied = false; + if (len < 0.001) { + impulseDirection.x = 1; + } else { + len = 1/len; + impulseDirection.x *= len; + impulseDirection.y *= len; + impulseDirection.z *= len; + } + minClosingVel = delta/(world.penResolutionSteps*dt); + if (minClosingVel > world.maxPenResolutionSpeed) { + minClosingVel = world.maxPenResolutionSpeed; + } + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + var x:Number; + var y:Number; + var z:Number; + var vx:Number; + var vy:Number; + var vz:Number; + velByUnitImpulseN = 0; + if (body1.movable) { +// velByUnitImpulseN += body1.invMass + _v.vCross2(wr1, impulseDirection).vTransformBy3(body1.invInertiaWorld).vCross(wr1).vDot(impulseDirection); + vx = wr1.y*impulseDirection.z - wr1.z*impulseDirection.y; + vy = wr1.z*impulseDirection.x - wr1.x*impulseDirection.z; + vz = wr1.x*impulseDirection.y - wr1.y*impulseDirection.x; + + m = body1.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr1.z - z*wr1.y; + vy = z*wr1.x - x*wr1.z; + vz = x*wr1.y - y*wr1.x; + velByUnitImpulseN += body1.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + if (body2 != null && body2.movable) { +// velByUnitImpulseN += body2.invMass + _v.vCross2(wr2, impulseDirection).vTransformBy3(body2.invInertiaWorld).vCross(wr2).vDot(impulseDirection); + vx = wr2.y*impulseDirection.z - wr2.z*impulseDirection.y; + vy = wr2.z*impulseDirection.x - wr2.x*impulseDirection.z; + vz = wr2.x*impulseDirection.y - wr2.y*impulseDirection.x; + + m = body2.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr2.z - z*wr2.y; + vy = z*wr2.x - x*wr2.z; + vz = x*wr2.y - y*wr2.x; + velByUnitImpulseN += body2.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + } else { + satisfied = true; + } + } + + /** + * @param dt + */ + override public function apply(dt:Number):void { + if (satisfied) return; + // Расчитываем проекцию скорости относительной скорости + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 + var vel:Vector3 = body1.state.velocity; + var rot:Vector3 = body1.state.rotation; +// v1Cross2(state.rotation, wr1); + var vx:Number = vel.x + rot.y*wr1.z - rot.z*wr1.y; + var vy:Number = vel.y + rot.z*wr1.x - rot.x*wr1.z; + var vz:Number = vel.z + rot.x*wr1.y - rot.y*wr1.x; + // V2 = V2_c + w2%r2 + if (body2 != null) { + vel = body2.state.velocity; + rot = body2.state.rotation; +// _v2.vCross2(state.rotation, wr2); + vx -= vel.x + rot.y*wr2.z - rot.z*wr2.y; + vy -= vel.y + rot.z*wr2.x - rot.x*wr2.z; + vz -= vel.z + rot.x*wr2.y - rot.y*wr2.x; + } + var closingVel:Number = vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + if (closingVel > minClosingVel) return; + var impulse:Number = (minClosingVel - closingVel)/velByUnitImpulseN; + if (body1.movable) { + body1.applyRelPosWorldImpulse(wr1, impulseDirection, impulse); + } + if (body2 != null && body2.movable) { + body2.applyRelPosWorldImpulse(wr2, impulseDirection, -impulse); + } + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/rigid/constraints/Constraint.as b/0.0.6.0/src/alternativa/physics/rigid/constraints/Constraint.as new file mode 100644 index 0000000..c2cd912 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/rigid/constraints/Constraint.as @@ -0,0 +1,35 @@ +package alternativa.physics.rigid.constraints { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.RigidWorld; + + use namespace altphysics; + + /** + * + */ + public class Constraint { + + altphysics var satisfied:Boolean; + altphysics var world:RigidWorld; + + /** + * + */ + public function Constraint() { + } + + /** + * @param dt + */ + public function preProcess(dt:Number):void { + } + + /** + * @param dt + */ + public function apply(dt:Number):void { + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/rigid/constraints/MaxDistanceConstraint.as b/0.0.6.0/src/alternativa/physics/rigid/constraints/MaxDistanceConstraint.as new file mode 100644 index 0000000..b4cbb74 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/rigid/constraints/MaxDistanceConstraint.as @@ -0,0 +1,183 @@ +package alternativa.physics.rigid.constraints { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class MaxDistanceConstraint extends Constraint { + // Первое тело ограничения + altphysics var body1:Body; + // Второе тело ограничения. Может быть null. + altphysics var body2:Body; + // Первая точка крепления в системе координат первого тела + altphysics var r1:Vector3; + // Вторая точка привязки в системе координат второго тела. В случае отсутствия второго тела, координаты задаются в мировой системе. + altphysics var r2:Vector3; + // Максимально допустимое расстояние между точками крепления + altphysics var maxDistance:Number; + // Радиус-вектор первой точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr1:Vector3 = new Vector3(); + // Радиус-вектор второй точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr2:Vector3 = new Vector3(); + // Минимальная скорость сближения для удовлетворения ограничения + private var minClosingVel:Number; + // Изменение относительной скорости на единицу импульса + private var velByUnitImpulseN:Number; + // Направление корректирующего импульса + private var impulseDirection:Vector3 = new Vector3(); + + // Вспомогательные переменные +// private static var _v:Vector3 = new Vector3(); +// private static var _v1:Vector3 = new Vector3(); +// private static var _v2:Vector3 = new Vector3(); + + /** + * @param body1 + * @param body2 + * @param r1 + * @param r2 + * @param maxDistance + */ + public function MaxDistanceConstraint(body1:Body, body2:Body, r1:Vector3, r2:Vector3, maxDistance:Number) { + super(); + this.body1 = body1; + this.body2 = body2; + this.r1 = r1.vClone(); + this.r2 = r2.vClone(); + this.maxDistance = maxDistance; + } + + /** + * @param dt + */ + override public function preProcess(dt:Number):void { + // Вычислим расстояние между точками + var m:Matrix3 = body1.baseMatrix; + wr1.x = m.a*r1.x + m.b*r1.y + m.c*r1.z; + wr1.y = m.e*r1.x + m.f*r1.y + m.g*r1.z; + wr1.z = m.i*r1.x + m.j*r1.y + m.k*r1.z; + if (body2 != null) { + m = body2.baseMatrix; + wr2.x = m.a*r2.x + m.b*r2.y + m.c*r2.z; + wr2.y = m.e*r2.x + m.f*r2.y + m.g*r2.z; + wr2.z = m.i*r2.x + m.j*r2.y + m.k*r2.z; + } else { + wr2.x = r2.x; + wr2.y = r2.y; + wr2.z = r2.z; + } + var p1:Vector3 = body1.state.pos; + impulseDirection.x = wr2.x - wr1.x - p1.x; + impulseDirection.y = wr2.y - wr1.y - p1.y; + impulseDirection.z = wr2.z - wr1.z - p1.z; + if (body2 != null) { + var p2:Vector3 = body2.state.pos; + impulseDirection.x += p2.x; + impulseDirection.y += p2.y; + impulseDirection.z += p2.z; + } + + var len:Number = Math.sqrt(impulseDirection.x*impulseDirection.x + impulseDirection.y*impulseDirection.y + impulseDirection.z*impulseDirection.z); + var delta:Number = len - maxDistance; + if (delta > 0) { + satisfied = false; + if (len < 0.001) { + impulseDirection.x = 1; + } else { + len = 1/len; + impulseDirection.x *= len; + impulseDirection.y *= len; + impulseDirection.z *= len; + } + minClosingVel = delta/(world.penResolutionSteps*dt); + if (minClosingVel > world.maxPenResolutionSpeed) { + minClosingVel = world.maxPenResolutionSpeed; + } + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + var x:Number; + var y:Number; + var z:Number; + var vx:Number; + var vy:Number; + var vz:Number; + velByUnitImpulseN = 0; + if (body1.movable) { +// velByUnitImpulseN += body1.invMass + _v.vCross2(wr1, impulseDirection).vTransformBy3(body1.invInertiaWorld).vCross(wr1).vDot(impulseDirection); + vx = wr1.y*impulseDirection.z - wr1.z*impulseDirection.y; + vy = wr1.z*impulseDirection.x - wr1.x*impulseDirection.z; + vz = wr1.x*impulseDirection.y - wr1.y*impulseDirection.x; + + m = body1.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr1.z - z*wr1.y; + vy = z*wr1.x - x*wr1.z; + vz = x*wr1.y - y*wr1.x; + velByUnitImpulseN += body1.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + if (body2 != null && body2.movable) { +// velByUnitImpulseN += body2.invMass + _v.vCross2(wr2, impulseDirection).vTransformBy3(body2.invInertiaWorld).vCross(wr2).vDot(impulseDirection); + vx = wr2.y*impulseDirection.z - wr2.z*impulseDirection.y; + vy = wr2.z*impulseDirection.x - wr2.x*impulseDirection.z; + vz = wr2.x*impulseDirection.y - wr2.y*impulseDirection.x; + + m = body2.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr2.z - z*wr2.y; + vy = z*wr2.x - x*wr2.z; + vz = x*wr2.y - y*wr2.x; + velByUnitImpulseN += body2.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + } else { + satisfied = true; + } + } + + /** + * @param dt + */ + override public function apply(dt:Number):void { + if (satisfied) return; + // Расчитываем проекцию скорости относительной скорости + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 + var vel:Vector3 = body1.state.velocity; + var rot:Vector3 = body1.state.rotation; +// v1Cross2(state.rotation, wr1); + var vx:Number = vel.x + rot.y*wr1.z - rot.z*wr1.y; + var vy:Number = vel.y + rot.z*wr1.x - rot.x*wr1.z; + var vz:Number = vel.z + rot.x*wr1.y - rot.y*wr1.x; + // V2 = V2_c + w2%r2 + if (body2 != null) { + vel = body2.state.velocity; + rot = body2.state.rotation; +// _v2.vCross2(state.rotation, wr2); + vx -= vel.x + rot.y*wr2.z - rot.z*wr2.y; + vy -= vel.y + rot.z*wr2.x - rot.x*wr2.z; + vz -= vel.z + rot.x*wr2.y - rot.y*wr2.x; + } + var closingVel:Number = vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + if (closingVel > minClosingVel) return; + var impulse:Number = (minClosingVel - closingVel)/velByUnitImpulseN; + if (body1.movable) { + body1.applyRelPosWorldImpulse(wr1, impulseDirection, impulse); + } + if (body2 != null && body2.movable) { + body2.applyRelPosWorldImpulse(wr2, impulseDirection, -impulse); + } + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/rigid/primitives/.svn/all-wcprops b/0.0.6.0/src/alternativa/physics/rigid/primitives/.svn/all-wcprops new file mode 100644 index 0000000..03966fc --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/rigid/primitives/.svn/all-wcprops @@ -0,0 +1,35 @@ +K 25 +svn:wc:ra_dav:version-url +V 120 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/rigid/primitives +END +RigidPlane.as +K 25 +svn:wc:ra_dav:version-url +V 134 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/rigid/primitives/RigidPlane.as +END +RigidSphere.as +K 25 +svn:wc:ra_dav:version-url +V 135 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/rigid/primitives/RigidSphere.as +END +RigidBox.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/rigid/primitives/RigidBox.as +END +RigidCylinder.as +K 25 +svn:wc:ra_dav:version-url +V 137 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/rigid/primitives/RigidCylinder.as +END +RigidRect.as +K 25 +svn:wc:ra_dav:version-url +V 133 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/rigid/primitives/RigidRect.as +END diff --git a/0.0.6.0/src/alternativa/physics/rigid/primitives/.svn/entries b/0.0.6.0/src/alternativa/physics/rigid/primitives/.svn/entries new file mode 100644 index 0000000..ccd631e --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/rigid/primitives/.svn/entries @@ -0,0 +1,88 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/rigid/primitives +http://svndev.alternativaplatform.com + + + +2009-06-08T06:46:18.310732Z +14099 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +RigidPlane.as +file + + + + +2010-10-28T04:32:42.000000Z +c4713421e357ee1967dca9ab3ffa7290 +2009-06-08T06:46:18.310732Z +14099 +mike + +RigidSphere.as +file + + + + +2010-10-28T04:32:42.000000Z +79ef38eab05657ad9627bdac4ade7271 +2009-04-23T13:43:38.903278Z +11779 +mike + +RigidBox.as +file + + + + +2010-10-28T04:32:42.000000Z +da834a5d208ff30f3a74321f75301cd7 +2009-06-08T06:46:18.310732Z +14099 +mike + +RigidCylinder.as +file + + + + +2010-10-28T04:32:42.000000Z +61512d31f9e211dcb0864164aa6f5210 +2009-04-23T13:43:38.903278Z +11779 +mike + +RigidRect.as +file + + + + +2010-10-28T04:32:42.000000Z +4cff4e313a1f41d4ccfa6480ea78173e +2009-04-23T13:43:38.903278Z +11779 +mike + diff --git a/0.0.6.0/src/alternativa/physics/rigid/primitives/.svn/format b/0.0.6.0/src/alternativa/physics/rigid/primitives/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/rigid/primitives/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.6.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidBox.as.svn-base b/0.0.6.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidBox.as.svn-base new file mode 100644 index 0000000..c01ff61 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidBox.as.svn-base @@ -0,0 +1,56 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class RigidBox extends Body { + + /** + * + * @param halfSize + * @param mass + */ + public function RigidBox(halfSize:Vector3, mass:Number) { + super(0, Matrix3.ZERO); + addCollisionPrimitive(new CollisionBox(halfSize, 1)); + setParams(halfSize, mass); + } + + /** + * @param halfSize + * @param mass + */ + public function setParams(halfSize:Vector3, mass:Number):void { + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + invInertia.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + invInertia.a = 3*invMass/(yy + zz); + invInertia.f = 3*invMass/(zz + xx); + invInertia.k = 3*invMass/(xx + yy); + } + var prim:CollisionBox = collisionPrimitives[0] as CollisionBox; + prim.hs.vCopy(halfSize); + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidCylinder.as.svn-base b/0.0.6.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidCylinder.as.svn-base new file mode 100644 index 0000000..0f9f2b3 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidCylinder.as.svn-base @@ -0,0 +1,32 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + + use namespace altphysics; + + public class RigidCylinder extends Body { + + altphysics var r:Number; + altphysics var h:Number; + + public function RigidCylinder(mass:Number, radius:Number, height:Number) { + super(0, Matrix3.ZERO); + setParams(mass, radius, height); + } + + public function setParams(mass:Number, radius:Number, height:Number):void { + r = radius; + h = height; + invInertiaWorld.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + invInertia.a = invInertia.f = mass*(h*h/12 + r*r/4); + invInertia.k = 0.5*mass*r*r; + invInertia.invert(); + } + } + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidPlane.as.svn-base b/0.0.6.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidPlane.as.svn-base new file mode 100644 index 0000000..26f74b2 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidPlane.as.svn-base @@ -0,0 +1,23 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + public class RigidPlane extends Body { + + public var normal:Vector3 = new Vector3(); + public var offset:Number; + + public function RigidPlane() { + super(0, Matrix3.ZERO); + this.normal.vCopy(Vector3.Z_AXIS); + this.offset = 0; + movable = false; + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidRect.as.svn-base b/0.0.6.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidRect.as.svn-base new file mode 100644 index 0000000..3c3913d --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidRect.as.svn-base @@ -0,0 +1,20 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + public class RigidRect extends Body { + + public function RigidRect(halfWidth:Number, halfLength:Number) { + super(0, Matrix3.ZERO); + movable = false; + addCollisionPrimitive(new CollisionRect(new Vector3(halfWidth, halfLength, 0), 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidSphere.as.svn-base b/0.0.6.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidSphere.as.svn-base new file mode 100644 index 0000000..9668716 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidSphere.as.svn-base @@ -0,0 +1,29 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + + use namespace altphysics; + + public class RigidSphere extends Body { + + public function RigidSphere(radius:Number, mass:Number) { + var m:Matrix3; + if (mass == Infinity) { + mass = 0; + m = Matrix3.ZERO; + } else { + mass = 1/mass; + var r2:Number = radius*radius; + m = new Matrix3(); + m.a = m.f = m.k = 2.5*mass/r2; + } + super(mass, m); + if (mass == 0) movable = false; + addCollisionPrimitive(new CollisionSphere(radius, 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/rigid/primitives/RigidBox.as b/0.0.6.0/src/alternativa/physics/rigid/primitives/RigidBox.as new file mode 100644 index 0000000..c01ff61 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/rigid/primitives/RigidBox.as @@ -0,0 +1,56 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class RigidBox extends Body { + + /** + * + * @param halfSize + * @param mass + */ + public function RigidBox(halfSize:Vector3, mass:Number) { + super(0, Matrix3.ZERO); + addCollisionPrimitive(new CollisionBox(halfSize, 1)); + setParams(halfSize, mass); + } + + /** + * @param halfSize + * @param mass + */ + public function setParams(halfSize:Vector3, mass:Number):void { + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + invInertia.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + invInertia.a = 3*invMass/(yy + zz); + invInertia.f = 3*invMass/(zz + xx); + invInertia.k = 3*invMass/(xx + yy); + } + var prim:CollisionBox = collisionPrimitives[0] as CollisionBox; + prim.hs.vCopy(halfSize); + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/rigid/primitives/RigidCylinder.as b/0.0.6.0/src/alternativa/physics/rigid/primitives/RigidCylinder.as new file mode 100644 index 0000000..0f9f2b3 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/rigid/primitives/RigidCylinder.as @@ -0,0 +1,32 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + + use namespace altphysics; + + public class RigidCylinder extends Body { + + altphysics var r:Number; + altphysics var h:Number; + + public function RigidCylinder(mass:Number, radius:Number, height:Number) { + super(0, Matrix3.ZERO); + setParams(mass, radius, height); + } + + public function setParams(mass:Number, radius:Number, height:Number):void { + r = radius; + h = height; + invInertiaWorld.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + invInertia.a = invInertia.f = mass*(h*h/12 + r*r/4); + invInertia.k = 0.5*mass*r*r; + invInertia.invert(); + } + } + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/rigid/primitives/RigidPlane.as b/0.0.6.0/src/alternativa/physics/rigid/primitives/RigidPlane.as new file mode 100644 index 0000000..26f74b2 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/rigid/primitives/RigidPlane.as @@ -0,0 +1,23 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + public class RigidPlane extends Body { + + public var normal:Vector3 = new Vector3(); + public var offset:Number; + + public function RigidPlane() { + super(0, Matrix3.ZERO); + this.normal.vCopy(Vector3.Z_AXIS); + this.offset = 0; + movable = false; + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/rigid/primitives/RigidRect.as b/0.0.6.0/src/alternativa/physics/rigid/primitives/RigidRect.as new file mode 100644 index 0000000..3c3913d --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/rigid/primitives/RigidRect.as @@ -0,0 +1,20 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + public class RigidRect extends Body { + + public function RigidRect(halfWidth:Number, halfLength:Number) { + super(0, Matrix3.ZERO); + movable = false; + addCollisionPrimitive(new CollisionRect(new Vector3(halfWidth, halfLength, 0), 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/rigid/primitives/RigidSphere.as b/0.0.6.0/src/alternativa/physics/rigid/primitives/RigidSphere.as new file mode 100644 index 0000000..9668716 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/rigid/primitives/RigidSphere.as @@ -0,0 +1,29 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + + use namespace altphysics; + + public class RigidSphere extends Body { + + public function RigidSphere(radius:Number, mass:Number) { + var m:Matrix3; + if (mass == Infinity) { + mass = 0; + m = Matrix3.ZERO; + } else { + mass = 1/mass; + var r2:Number = radius*radius; + m = new Matrix3(); + m.a = m.f = m.k = 2.5*mass/r2; + } + super(mass, m); + if (mass == 0) movable = false; + addCollisionPrimitive(new CollisionSphere(radius, 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/types/.svn/all-wcprops b/0.0.6.0/src/alternativa/physics/types/.svn/all-wcprops new file mode 100644 index 0000000..f001334 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/types/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 109 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/types +END +Quaternion.as +K 25 +svn:wc:ra_dav:version-url +V 123 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/types/Quaternion.as +END +Vector3.as +K 25 +svn:wc:ra_dav:version-url +V 120 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/types/Vector3.as +END +Matrix3.as +K 25 +svn:wc:ra_dav:version-url +V 120 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/types/Matrix3.as +END +Matrix4.as +K 25 +svn:wc:ra_dav:version-url +V 120 +/!svn/ver/20069/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/types/Matrix4.as +END diff --git a/0.0.6.0/src/alternativa/physics/types/.svn/entries b/0.0.6.0/src/alternativa/physics/types/.svn/entries new file mode 100644 index 0000000..ed161e3 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/types/.svn/entries @@ -0,0 +1,76 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.6.0/src/alternativa/physics/types +http://svndev.alternativaplatform.com + + + +2009-09-11T09:22:20.813670Z +19776 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Quaternion.as +file + + + + +2010-10-28T04:32:41.000000Z +595f36fdfea8e82e752bb989e97e3a9a +2009-05-04T04:55:56.466789Z +12306 +mike + +Vector3.as +file + + + + +2010-10-28T04:32:41.000000Z +843b476caf4f3879350b906d5a80e815 +2009-09-09T14:41:32.057202Z +19641 +mike + +Matrix3.as +file + + + + +2010-10-28T04:32:41.000000Z +3fde68593cd3eb5a7046c441a76c9c5c +2009-06-25T09:18:57.805913Z +15117 +mike + +Matrix4.as +file + + + + +2010-10-28T04:32:41.000000Z +2056f21265c08fe8b5724d022ee9975a +2009-09-11T09:22:20.813670Z +19776 +mike + diff --git a/0.0.6.0/src/alternativa/physics/types/.svn/format b/0.0.6.0/src/alternativa/physics/types/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/types/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.6.0/src/alternativa/physics/types/.svn/text-base/Matrix3.as.svn-base b/0.0.6.0/src/alternativa/physics/types/.svn/text-base/Matrix3.as.svn-base new file mode 100644 index 0000000..e07d0d1 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/types/.svn/text-base/Matrix3.as.svn-base @@ -0,0 +1,396 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + /** + * + */ + public class Matrix3 { + + public static const ZERO:Matrix3 = new Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0); + public static const IDENTITY:Matrix3 = new Matrix3(); + + public var a:Number; + public var b:Number; + public var c:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix3(a:Number = 1, b:Number = 0, c:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1) { + this.a = a; + this.b = b; + this.c = c; + + this.e = e; + this.f = f; + this.g = g; + + this.i = i; + this.j = j; + this.k = k; + } + + /** + * + * @return + */ + public function toIdentity():Matrix3 { + a = f = k = 1; + b = c = e = g = i = j = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + var det:Number = 1/(-cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk); + + a = (ff*kk - gg*jj)*det; + b = (cc*jj - bb*kk)*det; + c = (bb*gg - cc*ff)*det; + e = (gg*ii - ee*kk)*det; + f = (aa*kk - cc*ii)*det; + g = (cc*ee - aa*gg)*det; + i = (ee*jj - ff*ii)*det; + j = (bb*ii - aa*jj)*det; + k = (aa*ff - bb*ee)*det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prependTransposed(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.b + cc*m.c; + b = aa*m.e + bb*m.f + cc*m.g; + c = aa*m.i + bb*m.j + cc*m.k; + e = ee*m.a + ff*m.b + gg*m.c; + f = ee*m.e + ff*m.f + gg*m.g; + g = ee*m.i + ff*m.j + gg*m.k; + i = ii*m.a + jj*m.b + kk*m.c; + j = ii*m.e + jj*m.f + kk*m.g; + k = ii*m.i + jj*m.j + kk*m.k; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix3):Matrix3 { + a += m.a; + b += m.b; + c += m.c; + e += m.e; + f += m.f; + g += m.g; + i += m.i; + j += m.j; + k += m.k; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix3):Matrix3 { + a -= m.a; + b -= m.b; + c -= m.c; + e -= m.e; + f -= m.f; + g -= m.g; + i -= m.i; + j -= m.j; + k -= m.k; + + return this; + } + + /** + * + * @return + */ + public function transpose():Matrix3 { + var tmp:Number = b; + b = e; + e = tmp; + tmp = c; + c = i; + i = tmp; + tmp = g; + g = j; + j = tmp; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector3To3D(vin:Vector3, vout:Vector3D):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * + * @param matrix + */ + public function createSkewSymmetric(v:Vector3):Matrix3 { + a = f = k = 0; + b = -v.z; + c = v.y; + e = v.z; + g = -v.x; + i = -v.y; + j = v.x; + return this; + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(i:int, axis:Vector3):void { + switch (i) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + } + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix3):Matrix3 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @return + */ + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):Matrix3 { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + + return this; + } + + /** + * Метод формирует матрицу поворота на заданный угол относительно заданной оси. + * + * @param axis нормализованный вектор, задающий ось, относительно которой выполняется поворот + * @param angle угол поворота в радианах + */ + public function fromAxisAngle(axis:Vector3, angle:Number):void { + var c1:Number = Math.cos(angle); + var s:Number = Math.sin(angle); + var t:Number = 1 - c1; + var x:Number = axis.x; + var y:Number = axis.y; + var z:Number = axis.z; + + a = t*x*x + c1; + b = t*x*y - z*s; + c = t*x*z + y*s; + + e = t*x*y + z*s; + f = t*y*y + c1; + g = t*y*z - x*s; + + i = t*x*z - y*s; + j = t*y*z + x*s; + k = t*z*z + c1; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix3 { + return new Matrix3(a, b, c, e, f, g, i, j, k); + } + + public function toString():String { + return "[Matrix3 (" + a + ", " + b + ", " + c + "), (" + e + ", " + f + ", " + g + "), (" + i + ", " + j + ", " + k + ")]"; + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/types/.svn/text-base/Matrix4.as.svn-base b/0.0.6.0/src/alternativa/physics/types/.svn/text-base/Matrix4.as.svn-base new file mode 100644 index 0000000..d6af6c7 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/types/.svn/text-base/Matrix4.as.svn-base @@ -0,0 +1,458 @@ +package alternativa.physics.types { + import __AS3__.vec.Vector; + + /** + * + */ + public class Matrix4 { + + public var a:Number; + public var b:Number; + public var c:Number; + public var d:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + public var h:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + public var l:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix4(a:Number = 1, b:Number = 0, c:Number = 0, d:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, h:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1, l:Number = 0) { + this.a = a; + this.b = b; + this.c = c; + this.d = h; + + this.e = e; + this.f = f; + this.g = g; + this.h = h; + + this.i = i; + this.j = j; + this.k = k; + this.l = l; + } + + /** + * + * @return + */ + public function toIdentity():Matrix4 { + a = f = k = 1; + b = c = e = g = i = j = d = h = l = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + var det:Number = -cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk; + + a = (-gg*jj + ff*kk)/det; + b = (cc*jj - bb*kk)/det; + c = (-cc*ff + bb*gg)/det; + d = (dd*gg*jj - cc*hh*jj - dd*ff*kk + bb*hh*kk + cc*ff*ll - bb*gg*ll)/det; + e = (gg*ii - ee*kk)/det; + f = (-cc*ii + aa*kk)/det; + g = (cc*ee - aa*gg)/det; + h = (cc*hh*ii - dd*gg*ii + dd*ee*kk - aa*hh*kk - cc*ee*ll + aa*gg*ll)/det; + i = (-ff*ii + ee*jj)/det; + j = (bb*ii - aa*jj)/det; + k = (-bb*ee + aa*ff)/det; + l = (dd*ff*ii - bb*hh*ii - dd*ee*jj + aa*hh*jj + bb*ee*ll - aa*ff*ll)/det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + d = m.a*dd + m.b*hh + m.c*ll + m.d; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + h = m.e*dd + m.f*hh + m.g*ll + m.h; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + l = m.i*dd + m.j*hh + m.k*ll + m.l; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + d = aa*m.d + bb*m.h + cc*m.l + dd; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + h = ee*m.d + ff*m.h + gg*m.l + hh; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + l = ii*m.d + jj*m.h + kk*m.l + ll; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix4):Matrix4 { + a += m.a; + b += m.b; + c += m.c; + d += m.d; + e += m.e; + f += m.f; + g += m.g; + h += m.h; + i += m.i; + j += m.j; + k += m.k; + l += m.l; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix4):Matrix4 { + a -= m.a; + b -= m.b; + c -= m.c; + d -= m.d; + e -= m.e; + f -= m.f; + g -= m.g; + h -= m.h; + i -= m.i; + j -= m.j; + k -= m.k; + l -= m.l; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectors(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + * @param len + */ + public function transformVectorsN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverse(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverseN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(idx:int, axis:Vector3):void { + switch (idx) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + case 3: + axis.x = d; + axis.y = h; + axis.z = l; + return; + } + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix4):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = m.d; + e = m.e; + f = m.f; + g = m.g; + h = m.h; + i = m.i; + j = m.j; + k = m.k; + l = m.l; + + return this; + } + + public function setFromMatrix3(m:Matrix3, offset:Vector3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = offset.x; + e = m.e; + f = m.f; + g = m.g; + h = offset.y; + i = m.i; + j = m.j; + k = m.k; + l = offset.z; + + return this; + } + + public function setOrientationFromMatrix3(m:Matrix3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @param rotations + */ + public function getEulerAngles(rotations:Vector3):void { + if (-1 < i && i < 1) { + rotations.x = Math.atan2(j, k); + rotations.y = -Math.asin(i); + rotations.z = Math.atan2(e, a); + } else { + rotations.x = 0; + rotations.y = 0.5*((i <= -1) ? Math.PI : -Math.PI); + rotations.z = Math.atan2(-b, f); + } + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix4 { + return new Matrix4(a, b, c, d, e, f, g, h, i, j, k, l); + } + + public function toString():String { + return "[Matrix4 [" + a.toFixed(3) + " " + b.toFixed(3) + " " + c.toFixed(3) + " " + d.toFixed(3) + "] [" + e.toFixed(3) + " " + f.toFixed(3) + " " + g.toFixed(3) + " " + h.toFixed(3) + "] [" + i.toFixed(3) + " " + j.toFixed(3) + " " + k.toFixed(3) + " " + l.toFixed(3) + "]]"; + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/types/.svn/text-base/Quaternion.as.svn-base b/0.0.6.0/src/alternativa/physics/types/.svn/text-base/Quaternion.as.svn-base new file mode 100644 index 0000000..7ad2af0 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/types/.svn/text-base/Quaternion.as.svn-base @@ -0,0 +1,396 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + public class Quaternion { + + public var w:Number; + public var x:Number; + public var y:Number; + public var z:Number; + + public static function multiply(q1:Quaternion, q2:Quaternion, result:Quaternion):void { + result.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z; + result.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y; + result.y = q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z; + result.z = q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x; + } + + public static function createFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngle(axis, angle); + return q; + } + + public static function createFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngleComponents(x, y, z, angle); + return q; + } + + public function Quaternion(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0) { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + } + + public function reset(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0):Quaternion { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + return this; + } + + public function normalize():Quaternion { + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * Умножает на указанный кватернион слева: this * q + * + * @param q множитель + */ + public function prepend(q:Quaternion):Quaternion { + var ww:Number = w*q.w - x*q.x - y*q.y - z*q.z; + var xx:Number = w*q.x + x*q.w + y*q.z - z*q.y; + var yy:Number = w*q.y + y*q.w + z*q.x - x*q.z; + var zz:Number = w*q.z + z*q.w + x*q.y - y*q.x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Умножает на указанный кватернион справа: q * this + * + * @param q множитель + */ + public function append(q:Quaternion):Quaternion { + var ww:Number = q.w*w - q.x*x - q.y*y - q.z*z; + var xx:Number = q.w*x + q.x*w + q.y*z - q.z*y; + var yy:Number = q.w*y + q.y*w + q.z*x - q.x*z; + var zz:Number = q.w*z + q.z*w + q.x*y - q.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * + * @param vector + */ + public function rotateByVector(v:Vector3):Quaternion { + var ww:Number = -v.x*x - v.y*y - v.z*z; + var xx:Number = v.x*w + v.y*z - v.z*y; + var yy:Number = v.y*w + v.z*x - v.x*z; + var zz:Number = v.z*w + v.x*y - v.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Добавляет вращение, приданное вектором угловой скорости за указанное время. + * + * @param v + * @param scale + */ + public function addScaledVector(v:Vector3, scale:Number):Quaternion { + var vx:Number = v.x*scale; + var vy:Number = v.y*scale; + var vz:Number = v.z*scale; + var ww:Number = -x*vx - y*vy - z*vz; + var xx:Number = vx*w + vy*z - vz*y; + var yy:Number = vy*w + vz*x - vx*z; + var zz:Number = vz*w + vx*y - vy*x; + w += 0.5*ww; + x += 0.5*xx; + y += 0.5*yy; + z += 0.5*zz; + // inlined normalize + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param m + * @return + */ + public function toMatrix3(m:Matrix3):Quaternion { + var xx2:Number = 2*x*x; + var yy2:Number = 2*y*y; + var zz2:Number = 2*z*z; + var xy2:Number = 2*x*y; + var yz2:Number = 2*y*z; + var zx2:Number = 2*z*x; + var wx2:Number = 2*w*x; + var wy2:Number = 2*w*y; + var wz2:Number = 2*w*z; + + m.a = 1 - yy2 - zz2; + m.b = xy2 - wz2; + m.c = zx2 + wy2; + + m.e = xy2 + wz2; + m.f = 1 - xx2 - zz2; + m.g = yz2 - wx2; + + m.i = zx2 - wy2; + m.j = yz2 + wx2; + m.k = 1 - xx2 - yy2; + return this; + } + + /** + * + * @return + */ + public function length():Number { + return Math.sqrt(w*w + x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function lengthSqr():Number { + return w*w + x*x + y*y + z*z; + } + + /** + * + * @param axis + * @param angle + * @return + */ + public function setFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z); + x = axis.x*k; + y = axis.y*k; + z = axis.z*k; + return this; + } + + /** + * + * @param x + * @param y + * @param z + * @param angle + */ + public function setFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(x*x + y*y + z*z); + this.x = x*k; + this.y = y*k; + this.z = z*k; + return this; + } + + /** + * + * @param vector + */ + public function toAxisVector(v:Vector3 = null):Vector3 { + if (w < -1 || w > 1) { + normalize(); + } + if (v == null) { + v = new Vector3(); + } + if (w > -1 && w < 1) { + if (w == 0) { + v.x = x; + v.y = y; + v.z = z; + } else { + var angle:Number = 2*Math.acos(w); + var coeff:Number = 1/Math.sqrt(1 - w*w); + v.x = x*coeff*angle; + v.y = y*coeff*angle; + v.z = z*coeff*angle; + } + } else { + v.x = 0; + v.y = 0; + v.z = 0; + } + return v; + } + + /** + * + * @param rotations + */ + public function getEulerAngles(angles:Vector3):Vector3 { + var qi2:Number = 2*x*x; + var qj2:Number = 2*y*y; + var qk2:Number = 2*z*z; + var qij:Number = 2*x*y; + var qjk:Number = 2*y*z; + var qki:Number = 2*z*x; + var qri:Number = 2*w*x; + var qrj:Number = 2*w*y; + var qrk:Number = 2*w*z; + + var aa:Number = 1 - qj2 - qk2; + var bb:Number = qij - qrk; + var ee:Number = qij + qrk; + var ff:Number = 1 - qi2 - qk2; + var ii:Number = qki - qrj; + var jj:Number = qjk + qri; + var kk:Number = 1 - qi2 - qj2; + + if (-1 < ii && ii < 1) { + if (angles == null) { + angles = new Vector3(Math.atan2(jj, kk), -Math.asin(ii), Math.atan2(ee, aa)); + } else { + angles.x = Math.atan2(jj, kk); + angles.y = -Math.asin(ii); + angles.z = Math.atan2(ee, aa); + } + } else { + if (angles == null) { + angles = new Vector3(0, 0.5*((ii <= -1) ? Math.PI : -Math.PI), Math.atan2(-bb, ff)); + } else { + angles.x = 0; + angles.y = 0.5*((ii <= -1) ? Math.PI : -Math.PI); + angles.z = Math.atan2(-bb, ff); + } + } + return angles; + } + + /** + * + */ + public function conjugate():void { + x = -x; + y = -y; + z = -z; + } + + /** + * Выполняет линейную интерполяцию. + * + * @param q1 начало отрезка + * @param q2 конец отрезка + * @param t время, обычно задаётся в интервале [0, 1] + */ + public function nlerp(q1:Quaternion, q2:Quaternion, t:Number):Quaternion { + var d:Number = 1 - t; + w = q1.w*d + q2.w*t; + x = q1.x*d + q2.x*t; + y = q1.y*d + q2.y*t; + z = q1.z*d + q2.z*t; + // inlined normalize + d = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param q + * @return + */ + public function subtract(q:Quaternion):Quaternion { + w -= q.w; + x -= q.x; + y -= q.y; + z -= q.z; + return this; + } + + /** + * + * @param q1 + * @param q2 + * @return + */ + public function diff(q1:Quaternion, q2:Quaternion):Quaternion { + w = q2.w - q1.w; + x = q2.x - q1.x; + y = q2.y - q1.y; + z = q2.z - q1.z; + return this; + } + + /** + * + * @param q + * @return + */ + public function copy(q:Quaternion):Quaternion { + w = q.w; + x = q.x; + y = q.y; + z = q.z; + return this; + } + + /** + * @param reslult + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + result.w = w; + return result; + } + + /** + * + * @return + */ + public function clone():Quaternion { + return new Quaternion(w, x, y, z); + } + + /** + * + * @return + */ + public function toString():String { + return "[" + w + ", " + x + ", " + y + ", " + z + "]"; + } + } +} diff --git a/0.0.6.0/src/alternativa/physics/types/.svn/text-base/Vector3.as.svn-base b/0.0.6.0/src/alternativa/physics/types/.svn/text-base/Vector3.as.svn-base new file mode 100644 index 0000000..4f0e7e4 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/types/.svn/text-base/Vector3.as.svn-base @@ -0,0 +1,335 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + /** + * + */ + public class Vector3 extends Vector3D { + + public static const ZERO:Vector3 = new Vector3(0, 0, 0); + public static const X_AXIS:Vector3 = new Vector3(1, 0, 0); + public static const Y_AXIS:Vector3 = new Vector3(0, 1, 0); + public static const Z_AXIS:Vector3 = new Vector3(0, 0, 1); + + /** + * + * @param x + * @param y + * @param z + */ + public function Vector3(x:Number = 0, y:Number = 0, z:Number = 0) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * + * @return + */ + public function vLength():Number { + return Math.sqrt(x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function vLengthSqr():Number { + return x*x + y*y + z*z; + } + + /** + * + * @param length + * @return + */ + public function vSetLength(length:Number):Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = length; + } else { + var k:Number = length/Math.sqrt(x*x + y*y + z*z); + x *= k; + y *= k; + z *= k; + } + return this; + } + + /** + * + * @return + */ + public function vNormalize():Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = 1; + } else { + d = Math.sqrt(d); + x /= d; + y /= d; + z /= d; + } + return this; + } + + /** + * Сложение координат. + * + * @param v точка, координаты которой прибавляются к собственным + */ + public function vAdd(v:Vector3):Vector3 { + x += v.x; + y += v.y; + z += v.z; + return this; + } + + /** + * + * @param k + * @param v + * @return + */ + public function vAddScaled(k:Number, v:Vector3):Vector3 { + x += k*v.x; + y += k*v.y; + z += k*v.z; + return this; + } + + /** + * Вычитание координат. + * + * @param v точка, координаты которой вычитаются из собственных + */ + public function vSubtract(v:Vector3):Vector3 { + x -= v.x; + y -= v.y; + z -= v.z; + return this; + } + + /** + * Вычисляет сумму векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + * @return this + */ + public function vSum(a:Vector3, b:Vector3):Vector3 { + x = a.x + b.x; + y = a.y + b.y; + z = a.z + b.z; + return this; + } + + /** + * Вычисление разности векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + */ + public function vDiff(a:Vector3, b:Vector3):Vector3 { + x = a.x - b.x; + y = a.y - b.y; + z = a.z - b.z; + return this; + } + + /** + * Умножение на скаляр. + * + * @param k число, на которое умножаются координаты + */ + public function vScale(k:Number):Vector3 { + x *= k; + y *= k; + z *= k; + return this; + } + + /** + * Инвертирование вектора. + */ + public function vReverse():Vector3 { + x = -x; + y = -y; + z = -z; + return this; + } + + /** + * + * @param v + * @return + */ + public function vDot(v:Vector3):Number { + return x*v.x + y*v.y + z*v.z; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross(v:Vector3):Vector3 { + var xx:Number = y*v.z - z*v.y; + var yy:Number = z*v.x - x*v.z; + var zz:Number = x*v.y - y*v.x; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross2(a:Vector3, b:Vector3):Vector3 { + x = a.y*b.z - a.z*b.y; + y = a.z*b.x - a.x*b.z; + z = a.x*b.y - a.y*b.x; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy3(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * + * @param matrix + */ + public function vTransformBy3Tr(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz + m.d; + y = m.e*xx + m.f*yy + m.g*zz + m.h; + z = m.i*xx + m.j*yy + m.k*zz + m.l; + return this; + } + + /** + * + * @param m + * @return + */ + public function vTransformInverseBy4(m:Matrix4):Vector3 { + var xx:Number = x - m.d; + var yy:Number = y - m.h; + var zz:Number = z - m.l; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * + * @param m + */ + public function vDeltaTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * Установка координат. + */ + public function vReset(x:Number = 0, y:Number = 0, z:Number = 0):Vector3 { + this.x = x; + this.y = y; + this.z = z; + return this; + } + + /** + * Копирование координат точки. + * + * @param v точка, координаты которой копируются + */ + public function vCopy(v:Vector3):Vector3 { + x = v.x; + y = v.y; + z = v.z; + return this; + } + + /** + * Клонирование точки. + * + * @return клонированная точка + */ + public function vClone():Vector3 { + return new Vector3(x, y, z); + } + + /** + * Копирует координаты вектора в экземпляр класса Vector3D. + * + * @param result экземпляр, в который копируются координаты + * @return переданный параметр + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + return result; + } + + /** + * @param source + * @return + */ + public function copyFromVector3D(source:Vector3D):Vector3 { + x = source.x; + y = source.y; + z = source.z; + return this; + } + + override public function toString():String { + return "Vector3(" + x + "," + y + ", " + z + ")"; + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/types/Matrix3.as b/0.0.6.0/src/alternativa/physics/types/Matrix3.as new file mode 100644 index 0000000..e07d0d1 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/types/Matrix3.as @@ -0,0 +1,396 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + /** + * + */ + public class Matrix3 { + + public static const ZERO:Matrix3 = new Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0); + public static const IDENTITY:Matrix3 = new Matrix3(); + + public var a:Number; + public var b:Number; + public var c:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix3(a:Number = 1, b:Number = 0, c:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1) { + this.a = a; + this.b = b; + this.c = c; + + this.e = e; + this.f = f; + this.g = g; + + this.i = i; + this.j = j; + this.k = k; + } + + /** + * + * @return + */ + public function toIdentity():Matrix3 { + a = f = k = 1; + b = c = e = g = i = j = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + var det:Number = 1/(-cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk); + + a = (ff*kk - gg*jj)*det; + b = (cc*jj - bb*kk)*det; + c = (bb*gg - cc*ff)*det; + e = (gg*ii - ee*kk)*det; + f = (aa*kk - cc*ii)*det; + g = (cc*ee - aa*gg)*det; + i = (ee*jj - ff*ii)*det; + j = (bb*ii - aa*jj)*det; + k = (aa*ff - bb*ee)*det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prependTransposed(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.b + cc*m.c; + b = aa*m.e + bb*m.f + cc*m.g; + c = aa*m.i + bb*m.j + cc*m.k; + e = ee*m.a + ff*m.b + gg*m.c; + f = ee*m.e + ff*m.f + gg*m.g; + g = ee*m.i + ff*m.j + gg*m.k; + i = ii*m.a + jj*m.b + kk*m.c; + j = ii*m.e + jj*m.f + kk*m.g; + k = ii*m.i + jj*m.j + kk*m.k; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix3):Matrix3 { + a += m.a; + b += m.b; + c += m.c; + e += m.e; + f += m.f; + g += m.g; + i += m.i; + j += m.j; + k += m.k; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix3):Matrix3 { + a -= m.a; + b -= m.b; + c -= m.c; + e -= m.e; + f -= m.f; + g -= m.g; + i -= m.i; + j -= m.j; + k -= m.k; + + return this; + } + + /** + * + * @return + */ + public function transpose():Matrix3 { + var tmp:Number = b; + b = e; + e = tmp; + tmp = c; + c = i; + i = tmp; + tmp = g; + g = j; + j = tmp; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector3To3D(vin:Vector3, vout:Vector3D):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * + * @param matrix + */ + public function createSkewSymmetric(v:Vector3):Matrix3 { + a = f = k = 0; + b = -v.z; + c = v.y; + e = v.z; + g = -v.x; + i = -v.y; + j = v.x; + return this; + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(i:int, axis:Vector3):void { + switch (i) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + } + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix3):Matrix3 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @return + */ + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):Matrix3 { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + + return this; + } + + /** + * Метод формирует матрицу поворота на заданный угол относительно заданной оси. + * + * @param axis нормализованный вектор, задающий ось, относительно которой выполняется поворот + * @param angle угол поворота в радианах + */ + public function fromAxisAngle(axis:Vector3, angle:Number):void { + var c1:Number = Math.cos(angle); + var s:Number = Math.sin(angle); + var t:Number = 1 - c1; + var x:Number = axis.x; + var y:Number = axis.y; + var z:Number = axis.z; + + a = t*x*x + c1; + b = t*x*y - z*s; + c = t*x*z + y*s; + + e = t*x*y + z*s; + f = t*y*y + c1; + g = t*y*z - x*s; + + i = t*x*z - y*s; + j = t*y*z + x*s; + k = t*z*z + c1; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix3 { + return new Matrix3(a, b, c, e, f, g, i, j, k); + } + + public function toString():String { + return "[Matrix3 (" + a + ", " + b + ", " + c + "), (" + e + ", " + f + ", " + g + "), (" + i + ", " + j + ", " + k + ")]"; + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/types/Matrix4.as b/0.0.6.0/src/alternativa/physics/types/Matrix4.as new file mode 100644 index 0000000..d6af6c7 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/types/Matrix4.as @@ -0,0 +1,458 @@ +package alternativa.physics.types { + import __AS3__.vec.Vector; + + /** + * + */ + public class Matrix4 { + + public var a:Number; + public var b:Number; + public var c:Number; + public var d:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + public var h:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + public var l:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix4(a:Number = 1, b:Number = 0, c:Number = 0, d:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, h:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1, l:Number = 0) { + this.a = a; + this.b = b; + this.c = c; + this.d = h; + + this.e = e; + this.f = f; + this.g = g; + this.h = h; + + this.i = i; + this.j = j; + this.k = k; + this.l = l; + } + + /** + * + * @return + */ + public function toIdentity():Matrix4 { + a = f = k = 1; + b = c = e = g = i = j = d = h = l = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + var det:Number = -cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk; + + a = (-gg*jj + ff*kk)/det; + b = (cc*jj - bb*kk)/det; + c = (-cc*ff + bb*gg)/det; + d = (dd*gg*jj - cc*hh*jj - dd*ff*kk + bb*hh*kk + cc*ff*ll - bb*gg*ll)/det; + e = (gg*ii - ee*kk)/det; + f = (-cc*ii + aa*kk)/det; + g = (cc*ee - aa*gg)/det; + h = (cc*hh*ii - dd*gg*ii + dd*ee*kk - aa*hh*kk - cc*ee*ll + aa*gg*ll)/det; + i = (-ff*ii + ee*jj)/det; + j = (bb*ii - aa*jj)/det; + k = (-bb*ee + aa*ff)/det; + l = (dd*ff*ii - bb*hh*ii - dd*ee*jj + aa*hh*jj + bb*ee*ll - aa*ff*ll)/det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + d = m.a*dd + m.b*hh + m.c*ll + m.d; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + h = m.e*dd + m.f*hh + m.g*ll + m.h; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + l = m.i*dd + m.j*hh + m.k*ll + m.l; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + d = aa*m.d + bb*m.h + cc*m.l + dd; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + h = ee*m.d + ff*m.h + gg*m.l + hh; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + l = ii*m.d + jj*m.h + kk*m.l + ll; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix4):Matrix4 { + a += m.a; + b += m.b; + c += m.c; + d += m.d; + e += m.e; + f += m.f; + g += m.g; + h += m.h; + i += m.i; + j += m.j; + k += m.k; + l += m.l; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix4):Matrix4 { + a -= m.a; + b -= m.b; + c -= m.c; + d -= m.d; + e -= m.e; + f -= m.f; + g -= m.g; + h -= m.h; + i -= m.i; + j -= m.j; + k -= m.k; + l -= m.l; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectors(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + * @param len + */ + public function transformVectorsN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverse(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverseN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(idx:int, axis:Vector3):void { + switch (idx) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + case 3: + axis.x = d; + axis.y = h; + axis.z = l; + return; + } + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix4):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = m.d; + e = m.e; + f = m.f; + g = m.g; + h = m.h; + i = m.i; + j = m.j; + k = m.k; + l = m.l; + + return this; + } + + public function setFromMatrix3(m:Matrix3, offset:Vector3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = offset.x; + e = m.e; + f = m.f; + g = m.g; + h = offset.y; + i = m.i; + j = m.j; + k = m.k; + l = offset.z; + + return this; + } + + public function setOrientationFromMatrix3(m:Matrix3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @param rotations + */ + public function getEulerAngles(rotations:Vector3):void { + if (-1 < i && i < 1) { + rotations.x = Math.atan2(j, k); + rotations.y = -Math.asin(i); + rotations.z = Math.atan2(e, a); + } else { + rotations.x = 0; + rotations.y = 0.5*((i <= -1) ? Math.PI : -Math.PI); + rotations.z = Math.atan2(-b, f); + } + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix4 { + return new Matrix4(a, b, c, d, e, f, g, h, i, j, k, l); + } + + public function toString():String { + return "[Matrix4 [" + a.toFixed(3) + " " + b.toFixed(3) + " " + c.toFixed(3) + " " + d.toFixed(3) + "] [" + e.toFixed(3) + " " + f.toFixed(3) + " " + g.toFixed(3) + " " + h.toFixed(3) + "] [" + i.toFixed(3) + " " + j.toFixed(3) + " " + k.toFixed(3) + " " + l.toFixed(3) + "]]"; + } + + } +} \ No newline at end of file diff --git a/0.0.6.0/src/alternativa/physics/types/Quaternion.as b/0.0.6.0/src/alternativa/physics/types/Quaternion.as new file mode 100644 index 0000000..7ad2af0 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/types/Quaternion.as @@ -0,0 +1,396 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + public class Quaternion { + + public var w:Number; + public var x:Number; + public var y:Number; + public var z:Number; + + public static function multiply(q1:Quaternion, q2:Quaternion, result:Quaternion):void { + result.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z; + result.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y; + result.y = q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z; + result.z = q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x; + } + + public static function createFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngle(axis, angle); + return q; + } + + public static function createFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngleComponents(x, y, z, angle); + return q; + } + + public function Quaternion(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0) { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + } + + public function reset(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0):Quaternion { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + return this; + } + + public function normalize():Quaternion { + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * Умножает на указанный кватернион слева: this * q + * + * @param q множитель + */ + public function prepend(q:Quaternion):Quaternion { + var ww:Number = w*q.w - x*q.x - y*q.y - z*q.z; + var xx:Number = w*q.x + x*q.w + y*q.z - z*q.y; + var yy:Number = w*q.y + y*q.w + z*q.x - x*q.z; + var zz:Number = w*q.z + z*q.w + x*q.y - y*q.x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Умножает на указанный кватернион справа: q * this + * + * @param q множитель + */ + public function append(q:Quaternion):Quaternion { + var ww:Number = q.w*w - q.x*x - q.y*y - q.z*z; + var xx:Number = q.w*x + q.x*w + q.y*z - q.z*y; + var yy:Number = q.w*y + q.y*w + q.z*x - q.x*z; + var zz:Number = q.w*z + q.z*w + q.x*y - q.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * + * @param vector + */ + public function rotateByVector(v:Vector3):Quaternion { + var ww:Number = -v.x*x - v.y*y - v.z*z; + var xx:Number = v.x*w + v.y*z - v.z*y; + var yy:Number = v.y*w + v.z*x - v.x*z; + var zz:Number = v.z*w + v.x*y - v.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Добавляет вращение, приданное вектором угловой скорости за указанное время. + * + * @param v + * @param scale + */ + public function addScaledVector(v:Vector3, scale:Number):Quaternion { + var vx:Number = v.x*scale; + var vy:Number = v.y*scale; + var vz:Number = v.z*scale; + var ww:Number = -x*vx - y*vy - z*vz; + var xx:Number = vx*w + vy*z - vz*y; + var yy:Number = vy*w + vz*x - vx*z; + var zz:Number = vz*w + vx*y - vy*x; + w += 0.5*ww; + x += 0.5*xx; + y += 0.5*yy; + z += 0.5*zz; + // inlined normalize + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param m + * @return + */ + public function toMatrix3(m:Matrix3):Quaternion { + var xx2:Number = 2*x*x; + var yy2:Number = 2*y*y; + var zz2:Number = 2*z*z; + var xy2:Number = 2*x*y; + var yz2:Number = 2*y*z; + var zx2:Number = 2*z*x; + var wx2:Number = 2*w*x; + var wy2:Number = 2*w*y; + var wz2:Number = 2*w*z; + + m.a = 1 - yy2 - zz2; + m.b = xy2 - wz2; + m.c = zx2 + wy2; + + m.e = xy2 + wz2; + m.f = 1 - xx2 - zz2; + m.g = yz2 - wx2; + + m.i = zx2 - wy2; + m.j = yz2 + wx2; + m.k = 1 - xx2 - yy2; + return this; + } + + /** + * + * @return + */ + public function length():Number { + return Math.sqrt(w*w + x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function lengthSqr():Number { + return w*w + x*x + y*y + z*z; + } + + /** + * + * @param axis + * @param angle + * @return + */ + public function setFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z); + x = axis.x*k; + y = axis.y*k; + z = axis.z*k; + return this; + } + + /** + * + * @param x + * @param y + * @param z + * @param angle + */ + public function setFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(x*x + y*y + z*z); + this.x = x*k; + this.y = y*k; + this.z = z*k; + return this; + } + + /** + * + * @param vector + */ + public function toAxisVector(v:Vector3 = null):Vector3 { + if (w < -1 || w > 1) { + normalize(); + } + if (v == null) { + v = new Vector3(); + } + if (w > -1 && w < 1) { + if (w == 0) { + v.x = x; + v.y = y; + v.z = z; + } else { + var angle:Number = 2*Math.acos(w); + var coeff:Number = 1/Math.sqrt(1 - w*w); + v.x = x*coeff*angle; + v.y = y*coeff*angle; + v.z = z*coeff*angle; + } + } else { + v.x = 0; + v.y = 0; + v.z = 0; + } + return v; + } + + /** + * + * @param rotations + */ + public function getEulerAngles(angles:Vector3):Vector3 { + var qi2:Number = 2*x*x; + var qj2:Number = 2*y*y; + var qk2:Number = 2*z*z; + var qij:Number = 2*x*y; + var qjk:Number = 2*y*z; + var qki:Number = 2*z*x; + var qri:Number = 2*w*x; + var qrj:Number = 2*w*y; + var qrk:Number = 2*w*z; + + var aa:Number = 1 - qj2 - qk2; + var bb:Number = qij - qrk; + var ee:Number = qij + qrk; + var ff:Number = 1 - qi2 - qk2; + var ii:Number = qki - qrj; + var jj:Number = qjk + qri; + var kk:Number = 1 - qi2 - qj2; + + if (-1 < ii && ii < 1) { + if (angles == null) { + angles = new Vector3(Math.atan2(jj, kk), -Math.asin(ii), Math.atan2(ee, aa)); + } else { + angles.x = Math.atan2(jj, kk); + angles.y = -Math.asin(ii); + angles.z = Math.atan2(ee, aa); + } + } else { + if (angles == null) { + angles = new Vector3(0, 0.5*((ii <= -1) ? Math.PI : -Math.PI), Math.atan2(-bb, ff)); + } else { + angles.x = 0; + angles.y = 0.5*((ii <= -1) ? Math.PI : -Math.PI); + angles.z = Math.atan2(-bb, ff); + } + } + return angles; + } + + /** + * + */ + public function conjugate():void { + x = -x; + y = -y; + z = -z; + } + + /** + * Выполняет линейную интерполяцию. + * + * @param q1 начало отрезка + * @param q2 конец отрезка + * @param t время, обычно задаётся в интервале [0, 1] + */ + public function nlerp(q1:Quaternion, q2:Quaternion, t:Number):Quaternion { + var d:Number = 1 - t; + w = q1.w*d + q2.w*t; + x = q1.x*d + q2.x*t; + y = q1.y*d + q2.y*t; + z = q1.z*d + q2.z*t; + // inlined normalize + d = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param q + * @return + */ + public function subtract(q:Quaternion):Quaternion { + w -= q.w; + x -= q.x; + y -= q.y; + z -= q.z; + return this; + } + + /** + * + * @param q1 + * @param q2 + * @return + */ + public function diff(q1:Quaternion, q2:Quaternion):Quaternion { + w = q2.w - q1.w; + x = q2.x - q1.x; + y = q2.y - q1.y; + z = q2.z - q1.z; + return this; + } + + /** + * + * @param q + * @return + */ + public function copy(q:Quaternion):Quaternion { + w = q.w; + x = q.x; + y = q.y; + z = q.z; + return this; + } + + /** + * @param reslult + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + result.w = w; + return result; + } + + /** + * + * @return + */ + public function clone():Quaternion { + return new Quaternion(w, x, y, z); + } + + /** + * + * @return + */ + public function toString():String { + return "[" + w + ", " + x + ", " + y + ", " + z + "]"; + } + } +} diff --git a/0.0.6.0/src/alternativa/physics/types/Vector3.as b/0.0.6.0/src/alternativa/physics/types/Vector3.as new file mode 100644 index 0000000..4f0e7e4 --- /dev/null +++ b/0.0.6.0/src/alternativa/physics/types/Vector3.as @@ -0,0 +1,335 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + /** + * + */ + public class Vector3 extends Vector3D { + + public static const ZERO:Vector3 = new Vector3(0, 0, 0); + public static const X_AXIS:Vector3 = new Vector3(1, 0, 0); + public static const Y_AXIS:Vector3 = new Vector3(0, 1, 0); + public static const Z_AXIS:Vector3 = new Vector3(0, 0, 1); + + /** + * + * @param x + * @param y + * @param z + */ + public function Vector3(x:Number = 0, y:Number = 0, z:Number = 0) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * + * @return + */ + public function vLength():Number { + return Math.sqrt(x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function vLengthSqr():Number { + return x*x + y*y + z*z; + } + + /** + * + * @param length + * @return + */ + public function vSetLength(length:Number):Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = length; + } else { + var k:Number = length/Math.sqrt(x*x + y*y + z*z); + x *= k; + y *= k; + z *= k; + } + return this; + } + + /** + * + * @return + */ + public function vNormalize():Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = 1; + } else { + d = Math.sqrt(d); + x /= d; + y /= d; + z /= d; + } + return this; + } + + /** + * Сложение координат. + * + * @param v точка, координаты которой прибавляются к собственным + */ + public function vAdd(v:Vector3):Vector3 { + x += v.x; + y += v.y; + z += v.z; + return this; + } + + /** + * + * @param k + * @param v + * @return + */ + public function vAddScaled(k:Number, v:Vector3):Vector3 { + x += k*v.x; + y += k*v.y; + z += k*v.z; + return this; + } + + /** + * Вычитание координат. + * + * @param v точка, координаты которой вычитаются из собственных + */ + public function vSubtract(v:Vector3):Vector3 { + x -= v.x; + y -= v.y; + z -= v.z; + return this; + } + + /** + * Вычисляет сумму векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + * @return this + */ + public function vSum(a:Vector3, b:Vector3):Vector3 { + x = a.x + b.x; + y = a.y + b.y; + z = a.z + b.z; + return this; + } + + /** + * Вычисление разности векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + */ + public function vDiff(a:Vector3, b:Vector3):Vector3 { + x = a.x - b.x; + y = a.y - b.y; + z = a.z - b.z; + return this; + } + + /** + * Умножение на скаляр. + * + * @param k число, на которое умножаются координаты + */ + public function vScale(k:Number):Vector3 { + x *= k; + y *= k; + z *= k; + return this; + } + + /** + * Инвертирование вектора. + */ + public function vReverse():Vector3 { + x = -x; + y = -y; + z = -z; + return this; + } + + /** + * + * @param v + * @return + */ + public function vDot(v:Vector3):Number { + return x*v.x + y*v.y + z*v.z; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross(v:Vector3):Vector3 { + var xx:Number = y*v.z - z*v.y; + var yy:Number = z*v.x - x*v.z; + var zz:Number = x*v.y - y*v.x; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross2(a:Vector3, b:Vector3):Vector3 { + x = a.y*b.z - a.z*b.y; + y = a.z*b.x - a.x*b.z; + z = a.x*b.y - a.y*b.x; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy3(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * + * @param matrix + */ + public function vTransformBy3Tr(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz + m.d; + y = m.e*xx + m.f*yy + m.g*zz + m.h; + z = m.i*xx + m.j*yy + m.k*zz + m.l; + return this; + } + + /** + * + * @param m + * @return + */ + public function vTransformInverseBy4(m:Matrix4):Vector3 { + var xx:Number = x - m.d; + var yy:Number = y - m.h; + var zz:Number = z - m.l; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * + * @param m + */ + public function vDeltaTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * Установка координат. + */ + public function vReset(x:Number = 0, y:Number = 0, z:Number = 0):Vector3 { + this.x = x; + this.y = y; + this.z = z; + return this; + } + + /** + * Копирование координат точки. + * + * @param v точка, координаты которой копируются + */ + public function vCopy(v:Vector3):Vector3 { + x = v.x; + y = v.y; + z = v.z; + return this; + } + + /** + * Клонирование точки. + * + * @return клонированная точка + */ + public function vClone():Vector3 { + return new Vector3(x, y, z); + } + + /** + * Копирует координаты вектора в экземпляр класса Vector3D. + * + * @param result экземпляр, в который копируются координаты + * @return переданный параметр + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + return result; + } + + /** + * @param source + * @return + */ + public function copyFromVector3D(source:Vector3D):Vector3 { + x = source.x; + y = source.y; + z = source.z; + return this; + } + + override public function toString():String { + return "Vector3(" + x + "," + y + ", " + z + ")"; + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/.actionScriptProperties b/0.0.7.0/.actionScriptProperties new file mode 100644 index 0000000..f73adc8 --- /dev/null +++ b/0.0.7.0/.actionScriptProperties @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/0.0.7.0/.flexLibProperties b/0.0.7.0/.flexLibProperties new file mode 100644 index 0000000..652bb2a --- /dev/null +++ b/0.0.7.0/.flexLibProperties @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/0.0.7.0/.project b/0.0.7.0/.project new file mode 100644 index 0000000..1f1425f --- /dev/null +++ b/0.0.7.0/.project @@ -0,0 +1,19 @@ + + + AlternativaPhysics + + + + + + com.adobe.flexbuilder.project.flexbuilder + + + + + + org.maven.ide.eclipse.maven2Nature + com.adobe.flexbuilder.project.flexlibnature + com.adobe.flexbuilder.project.actionscriptnature + + diff --git a/0.0.7.0/.settings/.svn/all-wcprops b/0.0.7.0/.settings/.svn/all-wcprops new file mode 100644 index 0000000..1a8548a --- /dev/null +++ b/0.0.7.0/.settings/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 89 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/.settings +END +org.eclipse.core.resources.prefs +K 25 +svn:wc:ra_dav:version-url +V 122 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/.settings/org.eclipse.core.resources.prefs +END diff --git a/0.0.7.0/.settings/.svn/entries b/0.0.7.0/.settings/.svn/entries new file mode 100644 index 0000000..f7f3957 --- /dev/null +++ b/0.0.7.0/.settings/.svn/entries @@ -0,0 +1,40 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/.settings +http://svndev.alternativaplatform.com + + + +2009-04-01T12:25:29.638016Z +10301 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +org.eclipse.core.resources.prefs +file + + + + +2010-10-28T04:32:42.000000Z +4f70b476f3e5075e399505021df2f89b +2009-04-01T12:25:29.638016Z +10301 +mike + diff --git a/0.0.7.0/.settings/.svn/format b/0.0.7.0/.settings/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.7.0/.settings/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.7.0/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base b/0.0.7.0/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base new file mode 100644 index 0000000..b3d0d49 --- /dev/null +++ b/0.0.7.0/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base @@ -0,0 +1,3 @@ +#Fri Mar 20 00:19:33 YEKT 2009 +eclipse.preferences.version=1 +encoding/=UTF-8 diff --git a/0.0.7.0/.settings/org.eclipse.core.resources.prefs b/0.0.7.0/.settings/org.eclipse.core.resources.prefs new file mode 100644 index 0000000..b3d0d49 --- /dev/null +++ b/0.0.7.0/.settings/org.eclipse.core.resources.prefs @@ -0,0 +1,3 @@ +#Fri Mar 20 00:19:33 YEKT 2009 +eclipse.preferences.version=1 +encoding/=UTF-8 diff --git a/0.0.7.0/.svn/all-wcprops b/0.0.7.0/.svn/all-wcprops new file mode 100644 index 0000000..253612e --- /dev/null +++ b/0.0.7.0/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 79 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0 +END +.flexLibProperties +K 25 +svn:wc:ra_dav:version-url +V 98 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/.flexLibProperties +END +.project +K 25 +svn:wc:ra_dav:version-url +V 88 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/.project +END +pom.xml +K 25 +svn:wc:ra_dav:version-url +V 87 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/pom.xml +END +.actionScriptProperties +K 25 +svn:wc:ra_dav:version-url +V 103 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/.actionScriptProperties +END diff --git a/0.0.7.0/.svn/dir-prop-base b/0.0.7.0/.svn/dir-prop-base new file mode 100644 index 0000000..c31ca28 --- /dev/null +++ b/0.0.7.0/.svn/dir-prop-base @@ -0,0 +1,7 @@ +K 13 +svn:mergeinfo +V 278 +/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized:19806-19860 +/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.7.0.LinkedList:20360-22217,22226-22252 +/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D:13892-14098 +END diff --git a/0.0.7.0/.svn/entries b/0.0.7.0/.svn/entries new file mode 100644 index 0000000..f58c756 --- /dev/null +++ b/0.0.7.0/.svn/entries @@ -0,0 +1,85 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0 +http://svndev.alternativaplatform.com + + + +2009-10-23T06:16:33.420857Z +22450 +mike +has-props + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +META-INF +dir + +.flexLibProperties +file + + + + +2010-10-28T04:32:42.000000Z +c8e034e8e09f239471f9f69abce3a7bb +2009-10-19T07:17:33.113306Z +22253 +mike + +.project +file + + + + +2010-10-28T04:32:42.000000Z +1e90cc68b52b93173b2b5de88eb5c3a3 +2009-04-20T17:15:55.260110Z +11541 +mike + +src +dir + +pom.xml +file + + + + +2010-10-28T04:32:42.000000Z +f71bd6459301b472130be6c814cfe111 +2009-10-23T06:16:33.420857Z +22450 +mike + +.actionScriptProperties +file + + + + +2010-10-28T04:32:42.000000Z +b8656f882fff2baedd57a92db86fcf8c +2009-10-18T11:19:50.210229Z +22219 +mike + +.settings +dir + diff --git a/0.0.7.0/.svn/format b/0.0.7.0/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.7.0/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.7.0/.svn/text-base/.actionScriptProperties.svn-base b/0.0.7.0/.svn/text-base/.actionScriptProperties.svn-base new file mode 100644 index 0000000..f73adc8 --- /dev/null +++ b/0.0.7.0/.svn/text-base/.actionScriptProperties.svn-base @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/0.0.7.0/.svn/text-base/.flexLibProperties.svn-base b/0.0.7.0/.svn/text-base/.flexLibProperties.svn-base new file mode 100644 index 0000000..652bb2a --- /dev/null +++ b/0.0.7.0/.svn/text-base/.flexLibProperties.svn-base @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/0.0.7.0/.svn/text-base/.project.svn-base b/0.0.7.0/.svn/text-base/.project.svn-base new file mode 100644 index 0000000..1f1425f --- /dev/null +++ b/0.0.7.0/.svn/text-base/.project.svn-base @@ -0,0 +1,19 @@ + + + AlternativaPhysics + + + + + + com.adobe.flexbuilder.project.flexbuilder + + + + + + org.maven.ide.eclipse.maven2Nature + com.adobe.flexbuilder.project.flexlibnature + com.adobe.flexbuilder.project.actionscriptnature + + diff --git a/0.0.7.0/.svn/text-base/pom.xml.svn-base b/0.0.7.0/.svn/text-base/pom.xml.svn-base new file mode 100644 index 0000000..7ecacca --- /dev/null +++ b/0.0.7.0/.svn/text-base/pom.xml.svn-base @@ -0,0 +1,17 @@ + + 4.0.0 + platform.clients.fp10.libraries + AlternativaPhysics + swc + 0.0.7.0 + + platform.tools.maven + FlashBasePom + 1.0 + + + scm:svn:http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0 + + + + \ No newline at end of file diff --git a/0.0.7.0/META-INF/.svn/all-wcprops b/0.0.7.0/META-INF/.svn/all-wcprops new file mode 100644 index 0000000..c4b95d1 --- /dev/null +++ b/0.0.7.0/META-INF/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 88 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/META-INF +END +MANIFEST.MF +K 25 +svn:wc:ra_dav:version-url +V 100 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/META-INF/MANIFEST.MF +END diff --git a/0.0.7.0/META-INF/.svn/entries b/0.0.7.0/META-INF/.svn/entries new file mode 100644 index 0000000..80badc4 --- /dev/null +++ b/0.0.7.0/META-INF/.svn/entries @@ -0,0 +1,40 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/META-INF +http://svndev.alternativaplatform.com + + + +2009-04-20T17:15:55.260110Z +11541 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +MANIFEST.MF +file + + + + +2010-10-28T04:32:42.000000Z +7b64dceb50a6905850ff00a0bfbe77eb +2009-04-20T17:15:55.260110Z +11541 +mike + diff --git a/0.0.7.0/META-INF/.svn/format b/0.0.7.0/META-INF/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.7.0/META-INF/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.7.0/META-INF/.svn/text-base/MANIFEST.MF.svn-base b/0.0.7.0/META-INF/.svn/text-base/MANIFEST.MF.svn-base new file mode 100644 index 0000000..5066a55 --- /dev/null +++ b/0.0.7.0/META-INF/.svn/text-base/MANIFEST.MF.svn-base @@ -0,0 +1 @@ +Bundle-Name: platform.clients.fp10.libraries.AlternativaPhysics diff --git a/0.0.7.0/META-INF/MANIFEST.MF b/0.0.7.0/META-INF/MANIFEST.MF new file mode 100644 index 0000000..5066a55 --- /dev/null +++ b/0.0.7.0/META-INF/MANIFEST.MF @@ -0,0 +1 @@ +Bundle-Name: platform.clients.fp10.libraries.AlternativaPhysics diff --git a/0.0.7.0/pom.xml b/0.0.7.0/pom.xml new file mode 100644 index 0000000..7ecacca --- /dev/null +++ b/0.0.7.0/pom.xml @@ -0,0 +1,17 @@ + + 4.0.0 + platform.clients.fp10.libraries + AlternativaPhysics + swc + 0.0.7.0 + + platform.tools.maven + FlashBasePom + 1.0 + + + scm:svn:http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0 + + + + \ No newline at end of file diff --git a/0.0.7.0/src/.svn/all-wcprops b/0.0.7.0/src/.svn/all-wcprops new file mode 100644 index 0000000..e48fca7 --- /dev/null +++ b/0.0.7.0/src/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 83 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src +END diff --git a/0.0.7.0/src/.svn/entries b/0.0.7.0/src/.svn/entries new file mode 100644 index 0000000..bc53e22 --- /dev/null +++ b/0.0.7.0/src/.svn/entries @@ -0,0 +1,31 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src +http://svndev.alternativaplatform.com + + + +2009-10-23T05:59:29.567666Z +22444 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +alternativa +dir + diff --git a/0.0.7.0/src/.svn/format b/0.0.7.0/src/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.7.0/src/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.7.0/src/alternativa/.svn/all-wcprops b/0.0.7.0/src/alternativa/.svn/all-wcprops new file mode 100644 index 0000000..3f750b9 --- /dev/null +++ b/0.0.7.0/src/alternativa/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 95 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa +END diff --git a/0.0.7.0/src/alternativa/.svn/entries b/0.0.7.0/src/alternativa/.svn/entries new file mode 100644 index 0000000..3adf8d7 --- /dev/null +++ b/0.0.7.0/src/alternativa/.svn/entries @@ -0,0 +1,31 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa +http://svndev.alternativaplatform.com + + + +2009-10-23T05:59:29.567666Z +22444 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +physics +dir + diff --git a/0.0.7.0/src/alternativa/.svn/format b/0.0.7.0/src/alternativa/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.7.0/src/alternativa/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.7.0/src/alternativa/physics/.svn/all-wcprops b/0.0.7.0/src/alternativa/physics/.svn/all-wcprops new file mode 100644 index 0000000..fc79cd8 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/.svn/all-wcprops @@ -0,0 +1,77 @@ +K 25 +svn:wc:ra_dav:version-url +V 103 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics +END +Body.as +K 25 +svn:wc:ra_dav:version-url +V 111 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/Body.as +END +BodyState.as +K 25 +svn:wc:ra_dav:version-url +V 116 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/BodyState.as +END +CollisionPrimitiveListItem.as +K 25 +svn:wc:ra_dav:version-url +V 133 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/CollisionPrimitiveListItem.as +END +CollisionPrimitiveList.as +K 25 +svn:wc:ra_dav:version-url +V 129 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/CollisionPrimitiveList.as +END +altphysics.as +K 25 +svn:wc:ra_dav:version-url +V 117 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/altphysics.as +END +PhysicsUtils.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/PhysicsUtils.as +END +ContactPoint.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/ContactPoint.as +END +World.as +K 25 +svn:wc:ra_dav:version-url +V 112 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/World.as +END +BodyListItem.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/BodyListItem.as +END +BodyList.as +K 25 +svn:wc:ra_dav:version-url +V 115 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/BodyList.as +END +Contact.as +K 25 +svn:wc:ra_dav:version-url +V 114 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/Contact.as +END +BodyMaterial.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/BodyMaterial.as +END diff --git a/0.0.7.0/src/alternativa/physics/.svn/entries b/0.0.7.0/src/alternativa/physics/.svn/entries new file mode 100644 index 0000000..6b69938 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/.svn/entries @@ -0,0 +1,184 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics +http://svndev.alternativaplatform.com + + + +2009-10-23T05:59:29.567666Z +22444 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +CollisionPrimitiveList.as +file + + + + +2010-10-28T04:32:42.000000Z +4a95411d75825cdb04527c284f1708c6 +2009-10-23T05:59:29.567666Z +22444 +mike + +altphysics.as +file + + + + +2010-10-28T04:32:42.000000Z +04fbe40a27502dbbd0dba02a76b2df50 +2009-04-20T20:09:14.715403Z +11579 +mike + +math +dir + +World.as +file + + + + +2010-10-28T04:32:42.000000Z +751ad3f2ae01cf07dbb52dbe10219b75 +2009-10-20T10:42:06.632117Z +22312 +mike + +collision +dir + +primitives +dir + +BodyList.as +file + + + + +2010-10-28T04:32:42.000000Z +bdadf5987f8ba29c18eb1b464d41a943 +2009-10-19T07:17:33.113306Z +22253 +mike + +Contact.as +file + + + + +2010-10-28T04:32:42.000000Z +675ca7525e2b91ffcb1647978d39eca7 +2009-10-19T07:17:33.113306Z +22253 +mike + +Body.as +file + + + + +2010-10-28T04:32:42.000000Z +99cb2defe02f25078bfd8876dc4ed545 +2009-10-23T05:59:29.567666Z +22444 +mike + +BodyState.as +file + + + + +2010-10-28T04:32:42.000000Z +2ae471fd05b986dce6c19ac37a6beaa8 +2009-10-19T07:17:33.113306Z +22253 +mike + +CollisionPrimitiveListItem.as +file + + + + +2010-10-28T04:32:42.000000Z +cec5dbfefef812d87481478455f367b1 +2009-10-19T07:17:33.113306Z +22253 +mike + +PhysicsUtils.as +file + + + + +2010-10-28T04:32:42.000000Z +b400cdcb00dc64d1dcb1300c95c53fac +2009-10-19T07:17:33.113306Z +22253 +mike + +constraints +dir + +ContactPoint.as +file + + + + +2010-10-28T04:32:42.000000Z +88112c934e6a35adcc6b375d2c6f93b5 +2009-10-19T07:17:33.113306Z +22253 +mike + +BodyListItem.as +file + + + + +2010-10-28T04:32:42.000000Z +2fc2b598310b24aef3f8b90e14a8ba4f +2009-10-19T07:17:33.113306Z +22253 +mike + +BodyMaterial.as +file + + + + +2010-10-28T04:32:42.000000Z +9b700692ebb8017882f3e8cec095e1a9 +2009-10-19T07:17:33.113306Z +22253 +mike + diff --git a/0.0.7.0/src/alternativa/physics/.svn/format b/0.0.7.0/src/alternativa/physics/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.7.0/src/alternativa/physics/.svn/text-base/Body.as.svn-base b/0.0.7.0/src/alternativa/physics/.svn/text-base/Body.as.svn-base new file mode 100644 index 0000000..6b53304 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/.svn/text-base/Body.as.svn-base @@ -0,0 +1,406 @@ +package alternativa.physics { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.IBodyCollisionPredicate; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.math.Matrix3; + import alternativa.physics.math.Matrix4; + import alternativa.physics.math.Quaternion; + import alternativa.physics.math.Vector3; + + use namespace altphysics; + + /** + * + */ + public class Body { + + public static var linDamping:Number = 0.997; + public static var rotDamping:Number = 0.997; + + // Идентификатор тела, уникальный в пределах мира + public var id:int; + // Имя тела + public var name:String; + // Мир, в котором находится тело + public var world:World; + // Флаг подвижности тела + public var movable:Boolean = true; + // Флаг указывает, может ли тело быть заморожено + public var canFreeze:Boolean = false; + + public var freezeCounter:int; + public var frozen:Boolean = false; + // Ограничивающий бокс тела + public var aabb:BoundBox = new BoundBox(); + // Предикат, через который тело получает сообщения о столкновениях + public var postCollisionPredicate:IBodyCollisionPredicate; + + // Текущее состояние тела + public var state:BodyState = new BodyState(); + // Предыдущее состояние тела + public var prevState:BodyState = new BodyState(); + // Линейное ускорение тела на текущем шаге симуляции + public var accel:Vector3 = new Vector3(); + // Угловое ускорение тела на текущем шаге симуляции + public var angleAccel:Vector3 = new Vector3(); + // Физический материал тела + public var material:BodyMaterial = new BodyMaterial(); + // Обратная масса тела + public var invMass:Number = 1; + // Обратная матрица тензора инерции в локальных координатах + public var invInertia:Matrix3 = new Matrix3(); + // Обратная матрица тензора инерции в мировых координатах + public var invInertiaWorld:Matrix3 = new Matrix3(); + // Базисная матрица тела в мировых координатах + public var baseMatrix:Matrix3 = new Matrix3(); + + public const MAX_CONTACTS:int = 20; + public var contacts:Vector. = new Vector.(MAX_CONTACTS); + public var contactsNum:int; + + public var collisionPrimitives:CollisionPrimitiveList; + + // Аккумулятор сил + public var forceAccum:Vector3 = new Vector3(); + // Аккумулятор моментов + public var torqueAccum:Vector3 = new Vector3(); + + // Внутренние переменные для избежания создания экземпляров + private static var _r:Vector3 = new Vector3(); + private static var _f:Vector3 = new Vector3(); + + /** + * + * @param invMass + * @param invInertia + */ + public function Body(invMass:Number, invInertia:Matrix3) { + this.invMass = invMass; + this.invInertia.copy(invInertia); + } + + /** + * @param primitive + * @param localTransform + */ + public function addCollisionPrimitive(primitive:CollisionPrimitive, localTransform:Matrix4 = null):void { + if (primitive == null) { + throw new ArgumentError("Primitive cannot be null"); + } + if (collisionPrimitives == null) { + collisionPrimitives = new CollisionPrimitiveList(); + } + collisionPrimitives.append(primitive); + primitive.setBody(this, localTransform); + } + + /** + * + * @param primitive + */ + public function removeCollisionPrimitive(primitive:CollisionPrimitive):void { + if (collisionPrimitives == null) return; + primitive.setBody(null); + collisionPrimitives.remove(primitive); + if (collisionPrimitives.size == 0) { + collisionPrimitives = null; + } + } + + /** + * @param t + * @param result + */ + public function interpolate(t:Number, pos:Vector3, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.w = prevState.orientation.w*t1 + state.orientation.w*t; + orientation.x = prevState.orientation.x*t1 + state.orientation.x*t; + orientation.y = prevState.orientation.y*t1 + state.orientation.y*t; + orientation.z = prevState.orientation.z*t1 + state.orientation.z*t; + } + + /** + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + state.pos.vCopy(pos); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setPositionXYZ(x:Number, y:Number, z:Number):void { + state.pos.vReset(x, y, z); + } + + /** + * + * @param vel + */ + public function setVelocity(vel:Vector3):void { + state.velocity.vCopy(vel); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setVelocityXYZ(x:Number, y:Number, z:Number):void { + state.velocity.vReset(x, y, z); + } + + /** + * + * @param rot + */ + public function setRotation(rot:Vector3):void { + state.rotation.vCopy(rot); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setRotationXYZ(x:Number, y:Number, z:Number):void { + state.rotation.vReset(x, y, z); + } + + /** + * + * @param q + */ + public function setOrientation(q:Quaternion):void { + state.orientation.copy(q); + } + + /** + * @param r + * @param dir + * @param magnitude + */ + public function applyRelPosWorldImpulse(r:Vector3, dir:Vector3, magnitude:Number):void { + var d:Number = magnitude*invMass; + // Линейная часть + state.velocity.x += d*dir.x; + state.velocity.y += d*dir.y; + state.velocity.z += d*dir.z; + + // Вращательная часть + var x:Number = (r.y*dir.z - r.z*dir.y)*magnitude; + var y:Number = (r.z*dir.x - r.x*dir.z)*magnitude; + var z:Number = (r.x*dir.y - r.y*dir.x)*magnitude; + + state.rotation.x += invInertiaWorld.a*x + invInertiaWorld.b*y + invInertiaWorld.c*z; + state.rotation.y += invInertiaWorld.e*x + invInertiaWorld.f*y + invInertiaWorld.g*z; + state.rotation.z += invInertiaWorld.i*x + invInertiaWorld.j*y + invInertiaWorld.k*z; + } + + /** + * @param f + */ + public function addForce(f:Vector3):void { + forceAccum.vAdd(f); + } + + /** + * + * @param fx + * @param fy + * @param fz + */ + public function addForceXYZ(fx:Number, fy:Number, fz:Number):void { + forceAccum.x += fx; + forceAccum.y += fy; + forceAccum.z += fz; + } + + /** + * @param pos + * @param f + */ + public function addWorldForceXYZ(px:Number, py:Number, pz:Number, fx:Number, fy:Number, fz:Number):void { + forceAccum.x += fx; + forceAccum.y += fy; + forceAccum.z += fz; + + var pos:Vector3 = state.pos; + var rx:Number = px - pos.x; + var ry:Number = py - pos.y; + var rz:Number = pz - pos.z; + +// var x:Number = ry*fz - rz*fy; +// var y:Number = rz*fx - rx*fz; +// var z:Number = rx*fy - ry*fx; + + torqueAccum.x += ry*fz - rz*fy; + torqueAccum.y += rz*fx - rx*fz; + torqueAccum.z += rx*fy - ry*fx; + } + + /** + * @param pos + * @param f + */ + public function addWorldForce(pos:Vector3, force:Vector3):void { + forceAccum.vAdd(force); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(force)); + } + + /** + * @param pos + * @param f + */ + public function addWorldForceScaled(pos:Vector3, force:Vector3, scale:Number):void { + _f.x = scale*force.x; + _f.y = scale*force.y; + _f.z = scale*force.z; + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(_f)); + } + + /** + * @param pos + * @param f + */ + public function addLocalForce(pos:Vector3, force:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(pos, _r); + // Трансформируем вектор силы в мировую систему + baseMatrix.transformVector(force, _f); + // Добавляем силу и момент + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vCross(_f)); + } + + /** + * + * @param localPos + * @param worldForce + */ + public function addWorldForceAtLocalPoint(localPos:Vector3, worldForce:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(localPos, _r); + // Добавляем силу и момент + forceAccum.vAdd(worldForce); + torqueAccum.vAdd(_r.vCross(worldForce)); + } + + /** + * @param dt + */ + public function beforePhysicsStep(dt:Number):void { + } + + /** + * @param t + */ + public function addTorque(t:Vector3):void { + torqueAccum.vAdd(t); + } + + /** + * + */ + altphysics function clearAccumulators():void { + forceAccum.x = forceAccum.y = forceAccum.z = 0; + torqueAccum.x = torqueAccum.y = torqueAccum.z = 0; + } + + /** + * + */ + altphysics function calcAccelerations():void { + accel.x = forceAccum.x*invMass; + accel.y = forceAccum.y*invMass; + accel.z = forceAccum.z*invMass; + angleAccel.x = invInertiaWorld.a*torqueAccum.x + invInertiaWorld.b*torqueAccum.y + invInertiaWorld.c*torqueAccum.z; + angleAccel.y = invInertiaWorld.e*torqueAccum.x + invInertiaWorld.f*torqueAccum.y + invInertiaWorld.g*torqueAccum.z; + angleAccel.z = invInertiaWorld.i*torqueAccum.x + invInertiaWorld.j*torqueAccum.y + invInertiaWorld.k*torqueAccum.z; + } + + /** + * Вычисляет производные данные. + */ + public function calcDerivedData():void { + // Вычисление базисной матрицы и обратного тензора инерции в мировых координатах + state.orientation.toMatrix3(baseMatrix); + invInertiaWorld.copy(invInertia).append(baseMatrix).prependTransposed(baseMatrix); + if (collisionPrimitives != null) { + aabb.infinity(); + var item:CollisionPrimitiveListItem = collisionPrimitives.head; + while (item != null) { + var primitive:CollisionPrimitive = item.primitive; + primitive.transform.setFromMatrix3(baseMatrix, state.pos); + if (primitive.localTransform != null) { + primitive.transform.prepend(primitive.localTransform); + } + primitive.calculateAABB(); + aabb.addBoundBox(primitive.aabb); + item = item.next; + } + } + } + + /** + * + */ + altphysics function saveState():void { + prevState.copy(state); + } + + /** + * + */ + altphysics function restoreState():void { + state.copy(prevState); + } + + /** + * @param dt + */ + altphysics function integrateVelocity(dt:Number):void { + // v = v + a*t + state.velocity.x += accel.x*dt; + state.velocity.y += accel.y*dt; + state.velocity.z += accel.z*dt; + // rot = rot + eps*t + state.rotation.x += angleAccel.x*dt; + state.rotation.y += angleAccel.y*dt; + state.rotation.z += angleAccel.z*dt; + + state.velocity.x *= linDamping; + state.velocity.y *= linDamping; + state.velocity.z *= linDamping; + + state.rotation.x *= rotDamping; + state.rotation.y *= rotDamping; + state.rotation.z *= rotDamping; + } + + /** + * + */ + altphysics function integratePosition(dt:Number):void { + // pos = pos + v*t + state.pos.x += state.velocity.x*dt; + state.pos.y += state.velocity.y*dt; + state.pos.z += state.velocity.z*dt; + // q = q + 0.5*rot*q + state.orientation.addScaledVector(state.rotation, dt); + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/.svn/text-base/BodyList.as.svn-base b/0.0.7.0/src/alternativa/physics/.svn/text-base/BodyList.as.svn-base new file mode 100644 index 0000000..2a348f0 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/.svn/text-base/BodyList.as.svn-base @@ -0,0 +1,75 @@ +package alternativa.physics { + + /** + * + */ + public class BodyList { + + public var head:BodyListItem; + public var tail:BodyListItem; + public var size:int; + + /** + * + */ + public function BodyList() { + } + + /** + * + * @param body + */ + public function append(body:Body):void { + var item:BodyListItem = BodyListItem.create(body); + if (head == null) { + head = tail = item; + } else { + tail.next = item; + item.prev = tail; + tail = item; + } + size++; + } + + /** + * + * @param body + */ + public function remove(body:Body):Boolean { + var item:BodyListItem = findItem(body); + if (item == null) return false; + if (item == head) { + if (size == 1) { + head = tail = null; + } else { + head = item.next; + } + } else { + if (item == tail) { + tail = item.prev; + tail.next = null; + } else { + item.prev.next = item.next; + item.next.prev = item.prev; + } + } + item.dispose(); + size--; + return true; + } + + /** + * + * @param body + * @return + */ + public function findItem(body:Body):BodyListItem { + var item:BodyListItem = head; + while (item != null && item.body != body) { + item = item.next; + } + return item; + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/.svn/text-base/BodyListItem.as.svn-base b/0.0.7.0/src/alternativa/physics/.svn/text-base/BodyListItem.as.svn-base new file mode 100644 index 0000000..382e11c --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/.svn/text-base/BodyListItem.as.svn-base @@ -0,0 +1,74 @@ +package alternativa.physics { + + /** + * + */ + public class BodyListItem { + + // Верхний элемент хранилища + private static var poolTop:BodyListItem; + + /** + * Создаёт новый элемент списка. + * + * @param primitive примитив, содержащийся в элементе + * @return новый элемент списка + */ + public static function create(body:Body):BodyListItem { + var item:BodyListItem; + if (poolTop == null) { + item = new BodyListItem(body); + } else { + item = poolTop; + poolTop = item.next; + item.next = null; + item.body = body; + } + return item; + } + + /** + * Очищает хранилище. + */ + public static function clearPool():void { + var item:BodyListItem = poolTop; + while (item != null) { + poolTop = item.next; + item.next = null; + item = poolTop; + } + } + + /** + * + */ + public var body:Body; + /** + * + */ + public var next:BodyListItem; + /** + * + */ + public var prev:BodyListItem; + + /** + * + * @param body + */ + public function BodyListItem(body:Body) { + this.body = body; + } + + /** + * + */ + public function dispose():void { + body = null; + prev = null; + next = poolTop; + poolTop = this; + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/.svn/text-base/BodyMaterial.as.svn-base b/0.0.7.0/src/alternativa/physics/.svn/text-base/BodyMaterial.as.svn-base new file mode 100644 index 0000000..7a7c47d --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/.svn/text-base/BodyMaterial.as.svn-base @@ -0,0 +1,11 @@ +package alternativa.physics { + + public class BodyMaterial { + + public var restitution:Number = 0; + public var friction:Number = 0.3; +// public var dynamicFriction:Number = 0.2; +// public var dynamicFriction:Number = 0.2; + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/.svn/text-base/BodyState.as.svn-base b/0.0.7.0/src/alternativa/physics/.svn/text-base/BodyState.as.svn-base new file mode 100644 index 0000000..34a1b69 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/.svn/text-base/BodyState.as.svn-base @@ -0,0 +1,39 @@ +package alternativa.physics { + import alternativa.physics.math.Quaternion; + import alternativa.physics.math.Vector3; + + /** + * Класс описывает состояние твёрдого тела. + */ + public class BodyState { + /** + * Положение тела. + */ + public var pos:Vector3 = new Vector3(); + /** + * Ориентация тела. + */ + public var orientation:Quaternion = new Quaternion(); + /** + * Скорость тела. + */ + public var velocity:Vector3 = new Vector3(); + /** + * Угловая скорость тела. + */ + public var rotation:Vector3 = new Vector3(); + + /** + * Копирует значение указанного объекта. + * + * @param state + */ + public function copy(state:BodyState):void { + pos.vCopy(state.pos); + orientation.copy(state.orientation); + velocity.vCopy(state.velocity); + rotation.vCopy(state.rotation); + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/.svn/text-base/CollisionPrimitiveList.as.svn-base b/0.0.7.0/src/alternativa/physics/.svn/text-base/CollisionPrimitiveList.as.svn-base new file mode 100644 index 0000000..7581dee --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/.svn/text-base/CollisionPrimitiveList.as.svn-base @@ -0,0 +1,81 @@ +package alternativa.physics { + import alternativa.physics.collision.CollisionPrimitive; + + /** + * + */ + public class CollisionPrimitiveList { + + public var head:CollisionPrimitiveListItem; + public var tail:CollisionPrimitiveListItem; + public var size:int; + + /** + * + */ + public function CollisionPrimitiveList() { + } + + /** + * + * @param primitive + */ + public function append(primitive:CollisionPrimitive):void { + var item:CollisionPrimitiveListItem = CollisionPrimitiveListItem.create(primitive); + if (head == null) { + head = tail = item; + } else { + tail.next = item; + item.prev = tail; + tail = item; + } + size++; + } + + /** + * + * @param primitve + */ + public function remove(primitve:CollisionPrimitive):void { + if (head == null) return; + var item:CollisionPrimitiveListItem = head; + while (item != null) { + if (item.primitive == primitve) break; + item = item.next; + } + if (item == null) return; + if (item == head) { + if (size == 1) { + head = tail = null; + } else { + head = item.next; + head.prev = null; + } + } else { + if (item == tail) { + tail = tail.prev; + tail.next = null; + } else { + item.prev.next = item.next; + item.next.prev = item.prev; + } + } + item.dispose(); + size--; + } + + /** + * + */ + public function clear():void { + while (head != null) { + var item:CollisionPrimitiveListItem = head; + head = head.next; + item.dispose(); + } + tail = null; + size = 0; + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/.svn/text-base/CollisionPrimitiveListItem.as.svn-base b/0.0.7.0/src/alternativa/physics/.svn/text-base/CollisionPrimitiveListItem.as.svn-base new file mode 100644 index 0000000..d204532 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/.svn/text-base/CollisionPrimitiveListItem.as.svn-base @@ -0,0 +1,76 @@ +package alternativa.physics { + import alternativa.physics.collision.CollisionPrimitive; + + /** + * Элемент списка примитивов. + */ + public class CollisionPrimitiveListItem { + + // Верхний элемент хранилища + private static var poolTop:CollisionPrimitiveListItem; + + /** + * Создаёт новый элемент списка. + * + * @param primitive примитив, содержащийся в элементе + * @return новый элемент списка + */ + public static function create(primitive:CollisionPrimitive):CollisionPrimitiveListItem { + var item:CollisionPrimitiveListItem; + if (poolTop == null) { + item = new CollisionPrimitiveListItem(primitive); + } else { + item = poolTop; + item.primitive = primitive; + poolTop = item.next; + item.next = null; + } + return item; + } + + /** + * Очищает хранилище. + */ + public static function clearPool():void { + var curr:CollisionPrimitiveListItem = poolTop; + while (curr != null) { + poolTop = curr.next; + curr.next = null; + curr = poolTop; + } + } + + /** + * Примитив, хранящийся в элементе списка. + */ + public var primitive:CollisionPrimitive; + /** + * Ссылка не следующий элемент списка. + */ + public var next:CollisionPrimitiveListItem; + /** + * Ссылка не предыдущий элемент списка. + */ + public var prev:CollisionPrimitiveListItem; + + /** + * Создаёт новый экземпляр. + * + * @param primitive примитив, хранящийся в элементе списка + */ + public function CollisionPrimitiveListItem(primitive:CollisionPrimitive) { + this.primitive = primitive; + } + + /** + * Очищает внутренние ссылки и помещает элемент в хранилище для дальнейшего использования. + */ + public function dispose():void { + primitive = null; + prev = null; + next = poolTop; + poolTop = this; + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/.svn/text-base/Contact.as.svn-base b/0.0.7.0/src/alternativa/physics/.svn/text-base/Contact.as.svn-base new file mode 100644 index 0000000..1296996 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/.svn/text-base/Contact.as.svn-base @@ -0,0 +1,69 @@ +package alternativa.physics { + import __AS3__.vec.Vector; + + import alternativa.physics.math.Vector3; + + /** + * Описывает контакт одного или двух тел. + */ + public class Contact { + /** + * Первое тело контакта, не может быть равно null. + */ + public var body1:Body; + /** + * Второе тело контакта, может быть равно null. + */ + public var body2:Body; + /** + * Предрассчитанный взаимный коэффициент отскока. + */ + public var restitution:Number; + /** + * Предрассчитанный взаимный коэффициент трения. + */ + public var friction:Number; + /** + * Нормаль контакта. Направлена к первому телу. + */ + public var normal:Vector3 = new Vector3(); + /** + * Список точек контакта. + */ + public var points:Vector. = new Vector.(MAX_POINTS, true); + /** + * Количество точек контакта. + */ + public var pcount:int; + /** + * Максимальная глубина пересечения. + */ + public var maxPenetration:Number = 0; + /** + * Флаг показывает, разрешён контакт или нет. + */ + public var satisfied:Boolean; + /** + * Следующий контакт в списке. + */ + public var next:Contact; + /** + * Индекс контакта. Первый контакт в списке имеет индекс 0. + */ + public var index:int; + + // Максимальное количество точек контакта + private const MAX_POINTS:int = 8; + + /** + * + */ + public function Contact(index:int) { + this.index = index; + for (var i:int = 0; i < MAX_POINTS; i++) { + points[i] = new ContactPoint(); + } + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/.svn/text-base/ContactPoint.as.svn-base b/0.0.7.0/src/alternativa/physics/.svn/text-base/ContactPoint.as.svn-base new file mode 100644 index 0000000..2136e9a --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/.svn/text-base/ContactPoint.as.svn-base @@ -0,0 +1,55 @@ +package alternativa.physics { + import alternativa.physics.math.Vector3; + + /** + * + */ + public class ContactPoint { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + + // Величины, расчитываемые перед началом фазы решения контактов + + // Требуемая проекция конечной скорости на нормаль для упругого контакта + public var normalVel:Number; + // Минимальная скорость разделения неупругого контакта + public var minSepVel:Number; + // Изменение проекции скорости на единицу нормального импульса + public var velByUnitImpulseN:Number; + + public var angularInertia1:Number; + public var angularInertia2:Number; + + // Радиус-вектор точки контакта относительно центра первого тела + public var r1:Vector3 = new Vector3(); + // Радиус-вектор точки контакта относительно центра второго тела + public var r2:Vector3 = new Vector3(); + + // Величины, накапливаемые во время фазы решения контактов + + // Накопленный импульс, применяемый для получения требуемой относительной скорости в точке контакта + public var accumImpulseN:Number; + // Накопленный импульс, применяемый для разделения тел в точке контакта. Не создаёт момента. +// public var accumSepImpulse:Number; + + public var satisfied:Boolean; + + /** + * + * @param cp + */ + public function copyFrom(cp:ContactPoint):void { + pos.vCopy(cp.pos); + penetration = cp.penetration; + feature1 = cp.feature1; + feature2 = cp.feature2; + r1.vCopy(cp.r1); + r2.vCopy(cp.r2); + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/.svn/text-base/PhysicsUtils.as.svn-base b/0.0.7.0/src/alternativa/physics/.svn/text-base/PhysicsUtils.as.svn-base new file mode 100644 index 0000000..6770b0b --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/.svn/text-base/PhysicsUtils.as.svn-base @@ -0,0 +1,49 @@ +package alternativa.physics { + import alternativa.physics.math.Matrix3; + import alternativa.physics.math.Vector3; + + public class PhysicsUtils { + + /** + * @param mass + * @param halfSize + * @param result + */ + public static function getBoxInvInertia(mass:Number, halfSize:Vector3, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + result.a = 3/(mass*(yy + zz)); + result.f = 3/(mass*(zz + xx)); + result.k = 3/(mass*(xx + yy)); + } + + /** + * @param mass + * @param r + * @param h + * @param result + */ + public static function getCylinderInvInertia(mass:Number, r:Number, h:Number, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + + result.a = result.f = 1/(mass*(h*h/12 + r*r/4)); + result.k = 2/(mass*r*r); + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/.svn/text-base/World.as.svn-base b/0.0.7.0/src/alternativa/physics/.svn/text-base/World.as.svn-base new file mode 100644 index 0000000..9339f20 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/.svn/text-base/World.as.svn-base @@ -0,0 +1,743 @@ +package alternativa.physics { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.ICollisionDetector; + import alternativa.physics.collision.KdTreeCollisionDetector; + import alternativa.physics.constraints.Constraint; + import alternativa.physics.math.Matrix3; + import alternativa.physics.math.Vector3; + + use namespace altphysics; + + /** + * Класс реализует физическую симуляцию поведения твёрдых тел. + */ + public class World { + + private static var lastBodyId:int; + + // Максимальное количество контактов + altphysics const MAX_CONTACTS:int = 1000; + + ////////////////////////////////////////////////////////////////// + // Настроечные параметры симуляции. Могут быть изменены в любой + // момент времени без нарушения корректной работы симуляции. + ////////////////////////////////////////////////////////////////// + // Количество шагов, за которое пересекающиеся тела должны разделиться + public var penResolutionSteps:int = 10; + // Величина допустимой глубины пересечения + public var allowedPenetration:Number = 0.1; + // Максимальная скорость, добавляемая с целью разделения тел + public var maxPenResolutionSpeed:Number = 0.5; + // Количество итераций для обработки упругих контактов + public var collisionIterations:int = 5; + // Количество итераций для обработки неупругих контактов + public var contactIterations:int = 5; + // Флаг использования предсказания состояний + public var usePrediction:Boolean = false; + + public var freezeSteps:int = 10; + public var linSpeedFreezeLimit:Number = 1; + public var angSpeedFreezeLimit:Number = 0.01; + + // Переменные для процедуры разделения тел путём непосредственного их перемещенеия. + // !!!!!!!!!!!!!!!!!!!!!!!! + // !!! Экспериментально !!! + // !!!!!!!!!!!!!!!!!!!!!!!! + public var staticSeparationIterations:int = 10; + public var staticSeparationSteps:int = 10; + public var maxAngleMove:Number = 10; + public var useStaticSeparation:Boolean = false; + + // Вектор гравитации + public var _gravity:Vector3 = new Vector3(0, 0, -9.8); + // Модуль вектора гравитации + public var _gravityMagnitude:Number = 9.8; + + // Использующийся детектор столкновений + public var collisionDetector:ICollisionDetector; + + // Список тел, участвующих в симуляции + public var bodies:BodyList = new BodyList(); + + // Список контактов на текущем шаге симуляции + altphysics var contacts:Contact; + // Количество контактов на текущем шаге симуляции +// altphysics var contactsNum:int; + // Список ограничений + altphysics var constraints:Vector. = new Vector.(); + // Количество ограничений + altphysics var constraintsNum:int; + // Временная метка. Число прошедших шагов с начала симуляции. + public var timeStamp:uint; + // Время с начала симуляции, мс + public var time:uint; + // Первый неиспользованный контакт на текущем шаге симуляции + private var borderContact:Contact; + + // Временные переменные для избежания создания экземпляров + private var _r:Vector3 = new Vector3(); + private var _t:Vector3 = new Vector3(); + private var _v:Vector3 = new Vector3(); + private var _v1:Vector3 = new Vector3(); + private var _v2:Vector3 = new Vector3(); + + /** + * + */ + public function World() { + contacts = new Contact(0); + var contact:Contact = contacts; + for (var i:int = 1; i < MAX_CONTACTS; i++) { + contact.next = new Contact(i); + contact = contact.next; + } + collisionDetector = new KdTreeCollisionDetector(); + } + + /** + * Вектор гравитации. + */ + public function get gravity():Vector3 { + return _gravity.vClone(); + } + + /** + * @private + */ + public function set gravity(value:Vector3):void { + _gravity.vCopy(value); + _gravityMagnitude = _gravity.vLength(); + } + + /** + * Добавляет тело в симуляцию. + * + * @param body + */ + public function addBody(body:Body):void { + body.id = lastBodyId++; + body.world = this; + bodies.append(body); + } + + /** + * Удаляет тело из симуляции. + * @param body + * @return + */ + public function removeBody(body:Body):void { + if (bodies.remove(body)) { + body.world = null; + } + } + + /** + * Добавляет ограничение. + * @param c + */ + public function addConstraint(c:Constraint):void { + constraints[constraintsNum++] = c; + c.world = this; + } + + /** + * Удаляет ограничение. + * @param c + */ + public function removeConstraint(c:Constraint):Boolean { + var idx:int = constraints.indexOf(c); + if (idx < 0) return false; + constraints.splice(idx, 1) + constraintsNum--; + c.world = null; + return true; + } + + /** + * Применяет к телам действующте на них силы. Аккумуляторы сил и моментов тел очищаются после завершения шага + * симуляции, поэтому на момент вызова метода могут уже содержать некоторые значения. + * + * @param dt промежуток времени, в течении которого действуют силы + */ + private function applyForces(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + body.beforePhysicsStep(dt); + body.calcAccelerations(); + // Ускорение свободного падения применяется только к подвижным телам во избежание некорректного изменения + // фиктивной скорости неподвижных тел. + if (body.movable && !body.frozen) { + body.accel.x += _gravity.x; + body.accel.y += _gravity.y; + body.accel.z += _gravity.z; + } + item = item.next; + } + } + + /** + * Определяет все столкновения на текущем шаге симуляции и заполняет список получившихся контактов. + * + * @param dt длительность шага симуляции + */ + private function detectCollisions(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + if (!body.frozen) { + body.contactsNum = 0; + body.saveState(); + // При включённом режиме предсказания состояние тел интегрируется на один шаг вперёд + if (usePrediction) { + body.integrateVelocity(dt); + body.integratePosition(dt); + } + body.calcDerivedData(); + } + item = item.next; + } + + borderContact = collisionDetector.getAllContacts(contacts); + + // Расчёт относительных векторов точки контакта вынесен сюда из-за необходимости учитывать + // положение тел в предсказанном состоянии + var contact:Contact = contacts; + while (contact != borderContact) { + var b1:Body = contact.body1; + var b2:Body = contact.body2; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + var bPos:Vector3 = b1.state.pos; + cp.r1.x = cp.pos.x - bPos.x; + cp.r1.y = cp.pos.y - bPos.y; + cp.r1.z = cp.pos.z - bPos.z; + if (b2 != null) { + bPos = b2.state.pos; + cp.r2.x = cp.pos.x - bPos.x; + cp.r2.y = cp.pos.y - bPos.y; + cp.r2.z = cp.pos.z - bPos.z; + } + } + contact = contact.next; + } + + // Восстановление состояния тел + if (usePrediction) { + item = bodies.head; + while (item != null) { + body = item.body; + if (!body.frozen) { + body.restoreState(); + body.calcDerivedData(); + } + item = item.next; + } + } + } + + /** + * Подготваливает полученные из детектора столкновений контакты, расчитывая значения, не меняющиеся + * в ходе шага симуляции. + */ + private function preProcessContacts(dt:Number):void { + var contact:Contact = contacts; + while (contact != borderContact) { + var b1:Body = contact.body1; + var b2:Body = contact.body2; + // Столкнувшиеся тела размораживаются + if (b1.frozen) { + b1.frozen = false; + b1.freezeCounter = 0; + } + if (b2 != null && b2.frozen) { + b2.frozen = false; + b2.freezeCounter = 0; + } + contact.restitution = b1.material.restitution; + if (b2 != null && b2.material.restitution < contact.restitution) contact.restitution = b2.material.restitution; + contact.friction = b1.material.friction; + if (b2 != null && b2.material.friction < contact.friction) contact.friction = b2.material.friction; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.accumImpulseN = 0; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + cp.velByUnitImpulseN = 0; + if (b1.movable) { + cp.angularInertia1 = _v.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1).vDot(contact.normal); + cp.velByUnitImpulseN += b1.invMass + cp.angularInertia1; + } + if (b2 != null && b2.movable) { + cp.angularInertia2 = _v.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2).vDot(contact.normal); + cp.velByUnitImpulseN += b2.invMass + cp.angularInertia2; + } + // Расчёт требуемой конечной скорости для упругого контакта + calcSepVelocity(b1, b2, cp, _v); + cp.normalVel = _v.vDot(contact.normal); + if (cp.normalVel < 0) cp.normalVel = - contact.restitution*cp.normalVel; + // Скорость разделения неупругого контакта + cp.minSepVel = cp.penetration > allowedPenetration ? (cp.penetration - allowedPenetration)/(penResolutionSteps*dt) : 0; + if (cp.minSepVel > maxPenResolutionSpeed) cp.minSepVel = maxPenResolutionSpeed; + } + contact = contact.next; + } + for (var i:int = 0; i < constraintsNum; i++) { + var constraint:Constraint = constraints[i]; + constraint.preProcess(dt); + } + } + + /** + * + * @param dt + * @param forceInelastic + */ + private function processContacts(dt:Number, forceInelastic:Boolean):void { + var iterNum:int = forceInelastic ? contactIterations : collisionIterations; + var i:int; + var forwardLoop:Boolean = false; + for (var iter:int = 0; iter < iterNum; iter++) { + forwardLoop = !forwardLoop; + var contact:Contact = contacts; + while (contact != borderContact) { + resolveContact(contact, forceInelastic, forwardLoop); + contact = contact.next; + } + // Ограничения + for (i = 0; i < constraintsNum; i++) { + var constraint:Constraint = constraints[i]; + constraint.apply(dt); + } + } + + // Разделение контактов путём непосредственного изменения координат и ориентации. Экспериментально. +// if (forceInelastic && useStaticSeparation) performStaticSeparation(); + } + + /** + * + */ + private function resolveContact(contactInfo:Contact, forceInelastic:Boolean, forwardLoop:Boolean):void { + var b1:Body = contactInfo.body1; + var b2:Body = contactInfo.body2; + var normal:Vector3 = contactInfo.normal; + var i:int; + if (forwardLoop) { + for (i = 0; i < contactInfo.pcount; i++) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } else { + for (i = contactInfo.pcount - 1; i >= 0; i--) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } + } + + /** + * + * @param idx + * @param b1 + * @param b2 + * @param colInfo + * @param normal + * @param forceInelastic + */ + private function resolveContactPoint(idx:int, b1:Body, b2:Body, contact:Contact, normal:Vector3, forceInelastic:Boolean):void { + var cp:ContactPoint = contact.points[idx]; + if (!forceInelastic) { + cp.satisfied = true; + } + + var newVel:Number = 0; + calcSepVelocity(b1, b2, cp, _v); + var cnormal:Vector3 = contact.normal; + var sepVel:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + if (forceInelastic) { + var minSpeVel:Number = useStaticSeparation ? 0 : cp.minSepVel; + if (sepVel < minSpeVel) { + cp.satisfied = false; + } else if (cp.satisfied) return; + newVel = minSpeVel; + } else { + newVel = cp.normalVel; + } + var deltaVel:Number = newVel - sepVel; + var impulse:Number = deltaVel/cp.velByUnitImpulseN; + var accumImpulse:Number = cp.accumImpulseN + impulse; + if (accumImpulse < 0) { + accumImpulse = 0; + } + var deltaImpulse:Number = accumImpulse - cp.accumImpulseN; + cp.accumImpulseN = accumImpulse; + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, normal, deltaImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, normal, -deltaImpulse); + } + + // Учёт силы трения + calcSepVelocity(b1, b2, cp, _v); + // Расчитываем изменение касательной скорости на единицу касательного импульса + var tanSpeedByUnitImpulse:Number = 0; + +// _v.vAddScaled(-_v.vDot(contact.normal), contact.normal); + var dot:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + _v.x -= dot*cnormal.x; + _v.y -= dot*cnormal.y; + _v.z -= dot*cnormal.z; + + var tanSpeed:Number = _v.vLength(); + if (tanSpeed < 0.001) return; + +// _t.vCopy(_v).vNormalize().vReverse(); + _t.x = -_v.x; + _t.y = -_v.y; + _t.z = -_v.z; + _t.vNormalize(); + + var r:Vector3; + var m:Matrix3; + var xx:Number; + var yy:Number; + var zz:Number; + // dV = b.invMass + ((invI * (r % t)) % r) * t + if (b1.movable) { +// _v.vCross2(cp.r1, _t).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1); + r = cp.r1; + m = b1.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b1.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + if (b2 != null && b2.movable) { +// _v.vCross2(cp.r2, _t).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2); + + r = cp.r2; + m = b2.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b2.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + + var tanImpulse:Number = tanSpeed/tanSpeedByUnitImpulse; + var max:Number = contact.friction*cp.accumImpulseN; + if (max < 0) { + if (tanImpulse < max) tanImpulse = max; + } else { + if (tanImpulse > max) tanImpulse = max; + } + + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, _t, tanImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, _t, -tanImpulse); + } + } + + /** + * + * @param cp + * @param normal + * @return + */ + private function calcSepVelocity(body1:Body, body2:Body, cp:ContactPoint, result:Vector3):void { + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 +// result.vCopy(body1.state.velocity).vAdd(_v1.vCross2(body1.state.rotation, cp.r1)); + var rot:Vector3 = body1.state.rotation; + var v:Vector3 = cp.r1; + var x:Number = rot.y*v.z - rot.z*v.y; + var y:Number = rot.z*v.x - rot.x*v.z; + var z:Number = rot.x*v.y - rot.y*v.x; + v = body1.state.velocity; + result.x = v.x + x; + result.y = v.y + y; + result.z = v.z + z; + // V2 = V2_c + w2%r2 + if (body2 != null) { +// result.vSubtract(body2.state.velocity).vSubtract(_v2.vCross2(body2.state.rotation, cp.r2)); + rot = body2.state.rotation; + v = cp.r2; + x = rot.y*v.z - rot.z*v.y; + y = rot.z*v.x - rot.x*v.z; + z = rot.x*v.y - rot.y*v.x; + v = body2.state.velocity; + result.x -= v.x + x; + result.y -= v.y + y; + result.z -= v.z + z; + } + } + + /** + * + * @param dt + */ + private function intergateVelocities(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + item.body.integrateVelocity(dt); + item = item.next; + } + } + + /** + * + * @param dt + */ + private function integratePositions(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + if (body.movable && !body.frozen) { + body.integratePosition(dt); + } + item = item.next; + } + } + + /** + * + */ + private function performStaticSeparation():void { + var iterNum:int = staticSeparationIterations; +// iterNum = 100; + + // 1. В начале каждой итерации для всех контактов сбрасывается флаг satisfied с одновременным поиском наихудшего контакта, т.е. имеющего наибольшую величину пересечения тел. + // 2. Если найденный контакт имеет величину пересечения меньше предельно допустимой процедура прерывается, т.к. разделение контактов не требуется. + // 3. Если процедура продолжается, то выполняется разделение найденного контакта и установка его флага satisfied в true, после чего запускается внутренний цикл, состоящий + // из contactsNum - 1 итераций. На каждой итерации ищется наихудший контакт среди оставшихся (satisfied == false). Если такой контакт найден, то выполняется его разделение + // и переход к следующей итерации внутреннего цикла, иначе внутренний цикл прерывается и выполняется переход к следующей итерации внешнего цикла (шаг 1.). +// for (var iter:int = 0; iter < iterNum; iter++) { +// // Ищем контакт с максимальной величиной пересечения тел, одновременно сбрасывая флаги satisfied в false +// var worstContact:Contact = contacts[0]; +// var i:int; +// for (i = 1; i < contactsNum; i++) { +// var contact:Contact = contacts[i]; +// contact.satisfied = false; +// if (contact.maxPenetration > worstContact.maxPenetration) worstContact = contact; +// } +// if (worstContact.maxPenetration <= allowedPenetration) return; +// +// resolveInterpenetration(worstContact); +// // Внутренний цикл по оставшимся контактам +// for (i = 1; i < contactsNum; i++) { +// worstContact = getWorstContact(); +// if (worstContact == null) break; +// resolveInterpenetration(worstContact); +// } +// } + } + + /** + * + */ +// private function getWorstContact():Contact { +// var maxPen:Number = 0; +// var worst:Contact = null; +// for (var i:int = 0; i < contactsNum; i++) { +// var c:Contact = contacts[i]; +// if (!c.satisfied && c.maxPenetration > maxPen) { +// worst = c; +// maxPen = c.maxPenetration; +// } +// } +// return maxPen > allowedPenetration ? worst : null; +// } + + /** + * Разделяет указанный контакт, выполняя staticSeparationSteps итераций по списку точек контакта. + * 1. В начале каждой итерации ищется точка с наибольшим пересечением, одновременно сбрасываются флаги satisfied у точек. + * 2. Для найденной точки выполняется процедура разделения. + * 3. После выполняется pcount - 1 итерация по списку точек, каждый раз ищется наихудшая среди имеющих satisfied == false, для которой выполняется процедура разделения. + * Если наихудшая точка не найдена, то выполнется переход к следующей итерации внешнего цикла (шаг 1). + * + * В конце процедуры разделения обновляются значения пересечений для остальных точек контакта, а также для всех прочих контактов, относящихся к телам текущего. + * + * @param contact контакт для разделения + */ + private function resolveInterpenetration(contact:Contact):void { + contact.satisfied = true; + + for (var step:int = 0; step < staticSeparationSteps; step++) { + var worstCp:ContactPoint = contact.points[0]; + var cp:ContactPoint; + var i:int; + for (i = 1; i < contact.pcount; i++) { + cp = contact.points[i]; + cp.satisfied = false; + if (cp.penetration > worstCp.penetration) worstCp = cp; + } + if (worstCp.penetration <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + // Разделяем оставшиеся точки + var maxPen:Number = 0; + for (i = 1; i < contact.pcount; i++) { + // Поиск наихудшей точки + for (var j:int = 0; j < contact.pcount; j++) { + cp = contact.points[j]; + if (cp.satisfied) continue; + if (cp.penetration > maxPen) { + maxPen = cp.penetration; + worstCp = cp; + } + } + if (maxPen <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + } + } + } + + /** + * + * @param cp + * @param contact + */ + private function separateContactPoint(cp:ContactPoint, contact:Contact):void { +// cp.satisfied = true; +// +// var b1:Body = contact.body1; +// var b2:Body = contact.body2; +// var totalMove:Number = cp.penetration - allowedPenetration; +// var moveCoeff:Number = totalMove/cp.velByUnitImpulseN; +// var linMove1:Number; +// var angleMove1:Number; +// if (b1.movable) { +// linMove1 = b1.invMass*moveCoeff; +// angleMove1 = cp.angularInertia1*moveCoeff; +// if (angleMove1 > maxAngleMove) { +// linMove1 += angleMove1 - maxAngleMove; +// angleMove1 -= maxAngleMove; +// } +// b1.state.pos.vAddScaled(linMove1, contact.normal); +// _v1.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vScale(angleMove1); +// b1.state.orientation.addScaledVector(_v1, 1); +// } +// var linMove2:Number; +// var angleMove2:Number; +// if (b2 != null && b2.movable) { +// linMove2 = b2.invMass*moveCoeff; +// angleMove2 = cp.angularInertia2*moveCoeff; +// if (angleMove2 > maxAngleMove) { +// linMove2 += angleMove2 - maxAngleMove; +// angleMove2 -= maxAngleMove; +// } +// b2.state.pos.vAddScaled(-linMove2, contact.normal); +// _v2.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vScale(angleMove2); +// _v2.vReverse(); +// b2.state.orientation.addScaledVector(_v2, 1); +// } +// cp.penetration = allowedPenetration; +// // Обновляем пересечения в других точках +// var i:int; +// for (i = 0; i < contact.pcount; i++) { +// var cp1:ContactPoint = contact.points[i]; +// if (cp1 == cp) continue; +// var angularMove:Number; +// if (b1.movable) { +// angularMove = _v.vCross2(_v1, cp1.r1).vDot(contact.normal); +// cp1.penetration -= linMove1 + angularMove; +// } +// if (b2 != null && b2.movable) { +// angularMove = _v.vCross2(_v2, cp1.r2).vDot(contact.normal); +// cp1.penetration -= linMove2 - angularMove; +// } +// // Обновление максимального значения для контакта +// if (cp1.penetration > contact.maxPenetration) contact.maxPenetration = cp1.penetration; +// } +// // Обновляем пересечения для других контактов +// var c:Contact; +// var j:int; +// if (b1.movable) { +// for (i = 0; i < b1.contactsNum; i++) { +// c = b1.contacts[i]; +// if (c == contact) continue; +// for (j = 0; j < c.pcount; j++) { +// cp1 = c.points[j]; +// if (b1 == c.body1) cp1.penetration -= linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r1).vDot(c.normal); +// else cp1.penetration += linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r2).vDot(c.normal); +// if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; +// } +// } +// } +// if (b2 != null && b2.movable) { +// for (i = 0; i < b2.contactsNum; i++) { +// c = b2.contacts[i]; +// if (c == contact) continue; +// for (j = 0; j < c.pcount; j++) { +// cp1 = c.points[j]; +// if (b2 == c.body1) cp1.penetration -= linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r1).vDot(c.normal); +// else cp1.penetration += linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r2).vDot(c.normal); +// if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; +// } +// } +// } + } + + /** + * + */ + private function postPhysics():void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + body.clearAccumulators(); + body.calcDerivedData(); + if (body.canFreeze) { + if (body.state.velocity.vLength() < linSpeedFreezeLimit && body.state.rotation.vLength() < angSpeedFreezeLimit) { + if (!body.frozen) { + body.freezeCounter++; + if (body.freezeCounter >= freezeSteps) body.frozen = true; + } + } else { + body.freezeCounter = 0; + body.frozen = false; + } + } + item = item.next; + } + } + + /** + * + * @param delta + */ + public function runPhysics(delta:uint):void { + timeStamp++; + time += delta; + var dt:Number = 0.001*delta; + applyForces(dt); + detectCollisions(dt); + preProcessContacts(dt); + processContacts(dt, false); + intergateVelocities(dt); + processContacts(dt, true); + integratePositions(dt); + postPhysics(); + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base b/0.0.7.0/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base new file mode 100644 index 0000000..2715307 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base @@ -0,0 +1,3 @@ +package alternativa.physics { + public namespace altphysics = "http://alternativaplatform.com/en/altphysics"; +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/Body.as b/0.0.7.0/src/alternativa/physics/Body.as new file mode 100644 index 0000000..6b53304 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/Body.as @@ -0,0 +1,406 @@ +package alternativa.physics { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.IBodyCollisionPredicate; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.math.Matrix3; + import alternativa.physics.math.Matrix4; + import alternativa.physics.math.Quaternion; + import alternativa.physics.math.Vector3; + + use namespace altphysics; + + /** + * + */ + public class Body { + + public static var linDamping:Number = 0.997; + public static var rotDamping:Number = 0.997; + + // Идентификатор тела, уникальный в пределах мира + public var id:int; + // Имя тела + public var name:String; + // Мир, в котором находится тело + public var world:World; + // Флаг подвижности тела + public var movable:Boolean = true; + // Флаг указывает, может ли тело быть заморожено + public var canFreeze:Boolean = false; + + public var freezeCounter:int; + public var frozen:Boolean = false; + // Ограничивающий бокс тела + public var aabb:BoundBox = new BoundBox(); + // Предикат, через который тело получает сообщения о столкновениях + public var postCollisionPredicate:IBodyCollisionPredicate; + + // Текущее состояние тела + public var state:BodyState = new BodyState(); + // Предыдущее состояние тела + public var prevState:BodyState = new BodyState(); + // Линейное ускорение тела на текущем шаге симуляции + public var accel:Vector3 = new Vector3(); + // Угловое ускорение тела на текущем шаге симуляции + public var angleAccel:Vector3 = new Vector3(); + // Физический материал тела + public var material:BodyMaterial = new BodyMaterial(); + // Обратная масса тела + public var invMass:Number = 1; + // Обратная матрица тензора инерции в локальных координатах + public var invInertia:Matrix3 = new Matrix3(); + // Обратная матрица тензора инерции в мировых координатах + public var invInertiaWorld:Matrix3 = new Matrix3(); + // Базисная матрица тела в мировых координатах + public var baseMatrix:Matrix3 = new Matrix3(); + + public const MAX_CONTACTS:int = 20; + public var contacts:Vector. = new Vector.(MAX_CONTACTS); + public var contactsNum:int; + + public var collisionPrimitives:CollisionPrimitiveList; + + // Аккумулятор сил + public var forceAccum:Vector3 = new Vector3(); + // Аккумулятор моментов + public var torqueAccum:Vector3 = new Vector3(); + + // Внутренние переменные для избежания создания экземпляров + private static var _r:Vector3 = new Vector3(); + private static var _f:Vector3 = new Vector3(); + + /** + * + * @param invMass + * @param invInertia + */ + public function Body(invMass:Number, invInertia:Matrix3) { + this.invMass = invMass; + this.invInertia.copy(invInertia); + } + + /** + * @param primitive + * @param localTransform + */ + public function addCollisionPrimitive(primitive:CollisionPrimitive, localTransform:Matrix4 = null):void { + if (primitive == null) { + throw new ArgumentError("Primitive cannot be null"); + } + if (collisionPrimitives == null) { + collisionPrimitives = new CollisionPrimitiveList(); + } + collisionPrimitives.append(primitive); + primitive.setBody(this, localTransform); + } + + /** + * + * @param primitive + */ + public function removeCollisionPrimitive(primitive:CollisionPrimitive):void { + if (collisionPrimitives == null) return; + primitive.setBody(null); + collisionPrimitives.remove(primitive); + if (collisionPrimitives.size == 0) { + collisionPrimitives = null; + } + } + + /** + * @param t + * @param result + */ + public function interpolate(t:Number, pos:Vector3, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.w = prevState.orientation.w*t1 + state.orientation.w*t; + orientation.x = prevState.orientation.x*t1 + state.orientation.x*t; + orientation.y = prevState.orientation.y*t1 + state.orientation.y*t; + orientation.z = prevState.orientation.z*t1 + state.orientation.z*t; + } + + /** + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + state.pos.vCopy(pos); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setPositionXYZ(x:Number, y:Number, z:Number):void { + state.pos.vReset(x, y, z); + } + + /** + * + * @param vel + */ + public function setVelocity(vel:Vector3):void { + state.velocity.vCopy(vel); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setVelocityXYZ(x:Number, y:Number, z:Number):void { + state.velocity.vReset(x, y, z); + } + + /** + * + * @param rot + */ + public function setRotation(rot:Vector3):void { + state.rotation.vCopy(rot); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setRotationXYZ(x:Number, y:Number, z:Number):void { + state.rotation.vReset(x, y, z); + } + + /** + * + * @param q + */ + public function setOrientation(q:Quaternion):void { + state.orientation.copy(q); + } + + /** + * @param r + * @param dir + * @param magnitude + */ + public function applyRelPosWorldImpulse(r:Vector3, dir:Vector3, magnitude:Number):void { + var d:Number = magnitude*invMass; + // Линейная часть + state.velocity.x += d*dir.x; + state.velocity.y += d*dir.y; + state.velocity.z += d*dir.z; + + // Вращательная часть + var x:Number = (r.y*dir.z - r.z*dir.y)*magnitude; + var y:Number = (r.z*dir.x - r.x*dir.z)*magnitude; + var z:Number = (r.x*dir.y - r.y*dir.x)*magnitude; + + state.rotation.x += invInertiaWorld.a*x + invInertiaWorld.b*y + invInertiaWorld.c*z; + state.rotation.y += invInertiaWorld.e*x + invInertiaWorld.f*y + invInertiaWorld.g*z; + state.rotation.z += invInertiaWorld.i*x + invInertiaWorld.j*y + invInertiaWorld.k*z; + } + + /** + * @param f + */ + public function addForce(f:Vector3):void { + forceAccum.vAdd(f); + } + + /** + * + * @param fx + * @param fy + * @param fz + */ + public function addForceXYZ(fx:Number, fy:Number, fz:Number):void { + forceAccum.x += fx; + forceAccum.y += fy; + forceAccum.z += fz; + } + + /** + * @param pos + * @param f + */ + public function addWorldForceXYZ(px:Number, py:Number, pz:Number, fx:Number, fy:Number, fz:Number):void { + forceAccum.x += fx; + forceAccum.y += fy; + forceAccum.z += fz; + + var pos:Vector3 = state.pos; + var rx:Number = px - pos.x; + var ry:Number = py - pos.y; + var rz:Number = pz - pos.z; + +// var x:Number = ry*fz - rz*fy; +// var y:Number = rz*fx - rx*fz; +// var z:Number = rx*fy - ry*fx; + + torqueAccum.x += ry*fz - rz*fy; + torqueAccum.y += rz*fx - rx*fz; + torqueAccum.z += rx*fy - ry*fx; + } + + /** + * @param pos + * @param f + */ + public function addWorldForce(pos:Vector3, force:Vector3):void { + forceAccum.vAdd(force); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(force)); + } + + /** + * @param pos + * @param f + */ + public function addWorldForceScaled(pos:Vector3, force:Vector3, scale:Number):void { + _f.x = scale*force.x; + _f.y = scale*force.y; + _f.z = scale*force.z; + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(_f)); + } + + /** + * @param pos + * @param f + */ + public function addLocalForce(pos:Vector3, force:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(pos, _r); + // Трансформируем вектор силы в мировую систему + baseMatrix.transformVector(force, _f); + // Добавляем силу и момент + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vCross(_f)); + } + + /** + * + * @param localPos + * @param worldForce + */ + public function addWorldForceAtLocalPoint(localPos:Vector3, worldForce:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(localPos, _r); + // Добавляем силу и момент + forceAccum.vAdd(worldForce); + torqueAccum.vAdd(_r.vCross(worldForce)); + } + + /** + * @param dt + */ + public function beforePhysicsStep(dt:Number):void { + } + + /** + * @param t + */ + public function addTorque(t:Vector3):void { + torqueAccum.vAdd(t); + } + + /** + * + */ + altphysics function clearAccumulators():void { + forceAccum.x = forceAccum.y = forceAccum.z = 0; + torqueAccum.x = torqueAccum.y = torqueAccum.z = 0; + } + + /** + * + */ + altphysics function calcAccelerations():void { + accel.x = forceAccum.x*invMass; + accel.y = forceAccum.y*invMass; + accel.z = forceAccum.z*invMass; + angleAccel.x = invInertiaWorld.a*torqueAccum.x + invInertiaWorld.b*torqueAccum.y + invInertiaWorld.c*torqueAccum.z; + angleAccel.y = invInertiaWorld.e*torqueAccum.x + invInertiaWorld.f*torqueAccum.y + invInertiaWorld.g*torqueAccum.z; + angleAccel.z = invInertiaWorld.i*torqueAccum.x + invInertiaWorld.j*torqueAccum.y + invInertiaWorld.k*torqueAccum.z; + } + + /** + * Вычисляет производные данные. + */ + public function calcDerivedData():void { + // Вычисление базисной матрицы и обратного тензора инерции в мировых координатах + state.orientation.toMatrix3(baseMatrix); + invInertiaWorld.copy(invInertia).append(baseMatrix).prependTransposed(baseMatrix); + if (collisionPrimitives != null) { + aabb.infinity(); + var item:CollisionPrimitiveListItem = collisionPrimitives.head; + while (item != null) { + var primitive:CollisionPrimitive = item.primitive; + primitive.transform.setFromMatrix3(baseMatrix, state.pos); + if (primitive.localTransform != null) { + primitive.transform.prepend(primitive.localTransform); + } + primitive.calculateAABB(); + aabb.addBoundBox(primitive.aabb); + item = item.next; + } + } + } + + /** + * + */ + altphysics function saveState():void { + prevState.copy(state); + } + + /** + * + */ + altphysics function restoreState():void { + state.copy(prevState); + } + + /** + * @param dt + */ + altphysics function integrateVelocity(dt:Number):void { + // v = v + a*t + state.velocity.x += accel.x*dt; + state.velocity.y += accel.y*dt; + state.velocity.z += accel.z*dt; + // rot = rot + eps*t + state.rotation.x += angleAccel.x*dt; + state.rotation.y += angleAccel.y*dt; + state.rotation.z += angleAccel.z*dt; + + state.velocity.x *= linDamping; + state.velocity.y *= linDamping; + state.velocity.z *= linDamping; + + state.rotation.x *= rotDamping; + state.rotation.y *= rotDamping; + state.rotation.z *= rotDamping; + } + + /** + * + */ + altphysics function integratePosition(dt:Number):void { + // pos = pos + v*t + state.pos.x += state.velocity.x*dt; + state.pos.y += state.velocity.y*dt; + state.pos.z += state.velocity.z*dt; + // q = q + 0.5*rot*q + state.orientation.addScaledVector(state.rotation, dt); + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/BodyList.as b/0.0.7.0/src/alternativa/physics/BodyList.as new file mode 100644 index 0000000..2a348f0 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/BodyList.as @@ -0,0 +1,75 @@ +package alternativa.physics { + + /** + * + */ + public class BodyList { + + public var head:BodyListItem; + public var tail:BodyListItem; + public var size:int; + + /** + * + */ + public function BodyList() { + } + + /** + * + * @param body + */ + public function append(body:Body):void { + var item:BodyListItem = BodyListItem.create(body); + if (head == null) { + head = tail = item; + } else { + tail.next = item; + item.prev = tail; + tail = item; + } + size++; + } + + /** + * + * @param body + */ + public function remove(body:Body):Boolean { + var item:BodyListItem = findItem(body); + if (item == null) return false; + if (item == head) { + if (size == 1) { + head = tail = null; + } else { + head = item.next; + } + } else { + if (item == tail) { + tail = item.prev; + tail.next = null; + } else { + item.prev.next = item.next; + item.next.prev = item.prev; + } + } + item.dispose(); + size--; + return true; + } + + /** + * + * @param body + * @return + */ + public function findItem(body:Body):BodyListItem { + var item:BodyListItem = head; + while (item != null && item.body != body) { + item = item.next; + } + return item; + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/BodyListItem.as b/0.0.7.0/src/alternativa/physics/BodyListItem.as new file mode 100644 index 0000000..382e11c --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/BodyListItem.as @@ -0,0 +1,74 @@ +package alternativa.physics { + + /** + * + */ + public class BodyListItem { + + // Верхний элемент хранилища + private static var poolTop:BodyListItem; + + /** + * Создаёт новый элемент списка. + * + * @param primitive примитив, содержащийся в элементе + * @return новый элемент списка + */ + public static function create(body:Body):BodyListItem { + var item:BodyListItem; + if (poolTop == null) { + item = new BodyListItem(body); + } else { + item = poolTop; + poolTop = item.next; + item.next = null; + item.body = body; + } + return item; + } + + /** + * Очищает хранилище. + */ + public static function clearPool():void { + var item:BodyListItem = poolTop; + while (item != null) { + poolTop = item.next; + item.next = null; + item = poolTop; + } + } + + /** + * + */ + public var body:Body; + /** + * + */ + public var next:BodyListItem; + /** + * + */ + public var prev:BodyListItem; + + /** + * + * @param body + */ + public function BodyListItem(body:Body) { + this.body = body; + } + + /** + * + */ + public function dispose():void { + body = null; + prev = null; + next = poolTop; + poolTop = this; + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/BodyMaterial.as b/0.0.7.0/src/alternativa/physics/BodyMaterial.as new file mode 100644 index 0000000..7a7c47d --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/BodyMaterial.as @@ -0,0 +1,11 @@ +package alternativa.physics { + + public class BodyMaterial { + + public var restitution:Number = 0; + public var friction:Number = 0.3; +// public var dynamicFriction:Number = 0.2; +// public var dynamicFriction:Number = 0.2; + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/BodyState.as b/0.0.7.0/src/alternativa/physics/BodyState.as new file mode 100644 index 0000000..34a1b69 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/BodyState.as @@ -0,0 +1,39 @@ +package alternativa.physics { + import alternativa.physics.math.Quaternion; + import alternativa.physics.math.Vector3; + + /** + * Класс описывает состояние твёрдого тела. + */ + public class BodyState { + /** + * Положение тела. + */ + public var pos:Vector3 = new Vector3(); + /** + * Ориентация тела. + */ + public var orientation:Quaternion = new Quaternion(); + /** + * Скорость тела. + */ + public var velocity:Vector3 = new Vector3(); + /** + * Угловая скорость тела. + */ + public var rotation:Vector3 = new Vector3(); + + /** + * Копирует значение указанного объекта. + * + * @param state + */ + public function copy(state:BodyState):void { + pos.vCopy(state.pos); + orientation.copy(state.orientation); + velocity.vCopy(state.velocity); + rotation.vCopy(state.rotation); + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/CollisionPrimitiveList.as b/0.0.7.0/src/alternativa/physics/CollisionPrimitiveList.as new file mode 100644 index 0000000..7581dee --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/CollisionPrimitiveList.as @@ -0,0 +1,81 @@ +package alternativa.physics { + import alternativa.physics.collision.CollisionPrimitive; + + /** + * + */ + public class CollisionPrimitiveList { + + public var head:CollisionPrimitiveListItem; + public var tail:CollisionPrimitiveListItem; + public var size:int; + + /** + * + */ + public function CollisionPrimitiveList() { + } + + /** + * + * @param primitive + */ + public function append(primitive:CollisionPrimitive):void { + var item:CollisionPrimitiveListItem = CollisionPrimitiveListItem.create(primitive); + if (head == null) { + head = tail = item; + } else { + tail.next = item; + item.prev = tail; + tail = item; + } + size++; + } + + /** + * + * @param primitve + */ + public function remove(primitve:CollisionPrimitive):void { + if (head == null) return; + var item:CollisionPrimitiveListItem = head; + while (item != null) { + if (item.primitive == primitve) break; + item = item.next; + } + if (item == null) return; + if (item == head) { + if (size == 1) { + head = tail = null; + } else { + head = item.next; + head.prev = null; + } + } else { + if (item == tail) { + tail = tail.prev; + tail.next = null; + } else { + item.prev.next = item.next; + item.next.prev = item.prev; + } + } + item.dispose(); + size--; + } + + /** + * + */ + public function clear():void { + while (head != null) { + var item:CollisionPrimitiveListItem = head; + head = head.next; + item.dispose(); + } + tail = null; + size = 0; + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/CollisionPrimitiveListItem.as b/0.0.7.0/src/alternativa/physics/CollisionPrimitiveListItem.as new file mode 100644 index 0000000..d204532 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/CollisionPrimitiveListItem.as @@ -0,0 +1,76 @@ +package alternativa.physics { + import alternativa.physics.collision.CollisionPrimitive; + + /** + * Элемент списка примитивов. + */ + public class CollisionPrimitiveListItem { + + // Верхний элемент хранилища + private static var poolTop:CollisionPrimitiveListItem; + + /** + * Создаёт новый элемент списка. + * + * @param primitive примитив, содержащийся в элементе + * @return новый элемент списка + */ + public static function create(primitive:CollisionPrimitive):CollisionPrimitiveListItem { + var item:CollisionPrimitiveListItem; + if (poolTop == null) { + item = new CollisionPrimitiveListItem(primitive); + } else { + item = poolTop; + item.primitive = primitive; + poolTop = item.next; + item.next = null; + } + return item; + } + + /** + * Очищает хранилище. + */ + public static function clearPool():void { + var curr:CollisionPrimitiveListItem = poolTop; + while (curr != null) { + poolTop = curr.next; + curr.next = null; + curr = poolTop; + } + } + + /** + * Примитив, хранящийся в элементе списка. + */ + public var primitive:CollisionPrimitive; + /** + * Ссылка не следующий элемент списка. + */ + public var next:CollisionPrimitiveListItem; + /** + * Ссылка не предыдущий элемент списка. + */ + public var prev:CollisionPrimitiveListItem; + + /** + * Создаёт новый экземпляр. + * + * @param primitive примитив, хранящийся в элементе списка + */ + public function CollisionPrimitiveListItem(primitive:CollisionPrimitive) { + this.primitive = primitive; + } + + /** + * Очищает внутренние ссылки и помещает элемент в хранилище для дальнейшего использования. + */ + public function dispose():void { + primitive = null; + prev = null; + next = poolTop; + poolTop = this; + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/Contact.as b/0.0.7.0/src/alternativa/physics/Contact.as new file mode 100644 index 0000000..1296996 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/Contact.as @@ -0,0 +1,69 @@ +package alternativa.physics { + import __AS3__.vec.Vector; + + import alternativa.physics.math.Vector3; + + /** + * Описывает контакт одного или двух тел. + */ + public class Contact { + /** + * Первое тело контакта, не может быть равно null. + */ + public var body1:Body; + /** + * Второе тело контакта, может быть равно null. + */ + public var body2:Body; + /** + * Предрассчитанный взаимный коэффициент отскока. + */ + public var restitution:Number; + /** + * Предрассчитанный взаимный коэффициент трения. + */ + public var friction:Number; + /** + * Нормаль контакта. Направлена к первому телу. + */ + public var normal:Vector3 = new Vector3(); + /** + * Список точек контакта. + */ + public var points:Vector. = new Vector.(MAX_POINTS, true); + /** + * Количество точек контакта. + */ + public var pcount:int; + /** + * Максимальная глубина пересечения. + */ + public var maxPenetration:Number = 0; + /** + * Флаг показывает, разрешён контакт или нет. + */ + public var satisfied:Boolean; + /** + * Следующий контакт в списке. + */ + public var next:Contact; + /** + * Индекс контакта. Первый контакт в списке имеет индекс 0. + */ + public var index:int; + + // Максимальное количество точек контакта + private const MAX_POINTS:int = 8; + + /** + * + */ + public function Contact(index:int) { + this.index = index; + for (var i:int = 0; i < MAX_POINTS; i++) { + points[i] = new ContactPoint(); + } + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/ContactPoint.as b/0.0.7.0/src/alternativa/physics/ContactPoint.as new file mode 100644 index 0000000..2136e9a --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/ContactPoint.as @@ -0,0 +1,55 @@ +package alternativa.physics { + import alternativa.physics.math.Vector3; + + /** + * + */ + public class ContactPoint { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + + // Величины, расчитываемые перед началом фазы решения контактов + + // Требуемая проекция конечной скорости на нормаль для упругого контакта + public var normalVel:Number; + // Минимальная скорость разделения неупругого контакта + public var minSepVel:Number; + // Изменение проекции скорости на единицу нормального импульса + public var velByUnitImpulseN:Number; + + public var angularInertia1:Number; + public var angularInertia2:Number; + + // Радиус-вектор точки контакта относительно центра первого тела + public var r1:Vector3 = new Vector3(); + // Радиус-вектор точки контакта относительно центра второго тела + public var r2:Vector3 = new Vector3(); + + // Величины, накапливаемые во время фазы решения контактов + + // Накопленный импульс, применяемый для получения требуемой относительной скорости в точке контакта + public var accumImpulseN:Number; + // Накопленный импульс, применяемый для разделения тел в точке контакта. Не создаёт момента. +// public var accumSepImpulse:Number; + + public var satisfied:Boolean; + + /** + * + * @param cp + */ + public function copyFrom(cp:ContactPoint):void { + pos.vCopy(cp.pos); + penetration = cp.penetration; + feature1 = cp.feature1; + feature2 = cp.feature2; + r1.vCopy(cp.r1); + r2.vCopy(cp.r2); + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/PhysicsUtils.as b/0.0.7.0/src/alternativa/physics/PhysicsUtils.as new file mode 100644 index 0000000..6770b0b --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/PhysicsUtils.as @@ -0,0 +1,49 @@ +package alternativa.physics { + import alternativa.physics.math.Matrix3; + import alternativa.physics.math.Vector3; + + public class PhysicsUtils { + + /** + * @param mass + * @param halfSize + * @param result + */ + public static function getBoxInvInertia(mass:Number, halfSize:Vector3, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + result.a = 3/(mass*(yy + zz)); + result.f = 3/(mass*(zz + xx)); + result.k = 3/(mass*(xx + yy)); + } + + /** + * @param mass + * @param r + * @param h + * @param result + */ + public static function getCylinderInvInertia(mass:Number, r:Number, h:Number, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + + result.a = result.f = 1/(mass*(h*h/12 + r*r/4)); + result.k = 2/(mass*r*r); + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/World.as b/0.0.7.0/src/alternativa/physics/World.as new file mode 100644 index 0000000..9339f20 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/World.as @@ -0,0 +1,743 @@ +package alternativa.physics { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.ICollisionDetector; + import alternativa.physics.collision.KdTreeCollisionDetector; + import alternativa.physics.constraints.Constraint; + import alternativa.physics.math.Matrix3; + import alternativa.physics.math.Vector3; + + use namespace altphysics; + + /** + * Класс реализует физическую симуляцию поведения твёрдых тел. + */ + public class World { + + private static var lastBodyId:int; + + // Максимальное количество контактов + altphysics const MAX_CONTACTS:int = 1000; + + ////////////////////////////////////////////////////////////////// + // Настроечные параметры симуляции. Могут быть изменены в любой + // момент времени без нарушения корректной работы симуляции. + ////////////////////////////////////////////////////////////////// + // Количество шагов, за которое пересекающиеся тела должны разделиться + public var penResolutionSteps:int = 10; + // Величина допустимой глубины пересечения + public var allowedPenetration:Number = 0.1; + // Максимальная скорость, добавляемая с целью разделения тел + public var maxPenResolutionSpeed:Number = 0.5; + // Количество итераций для обработки упругих контактов + public var collisionIterations:int = 5; + // Количество итераций для обработки неупругих контактов + public var contactIterations:int = 5; + // Флаг использования предсказания состояний + public var usePrediction:Boolean = false; + + public var freezeSteps:int = 10; + public var linSpeedFreezeLimit:Number = 1; + public var angSpeedFreezeLimit:Number = 0.01; + + // Переменные для процедуры разделения тел путём непосредственного их перемещенеия. + // !!!!!!!!!!!!!!!!!!!!!!!! + // !!! Экспериментально !!! + // !!!!!!!!!!!!!!!!!!!!!!!! + public var staticSeparationIterations:int = 10; + public var staticSeparationSteps:int = 10; + public var maxAngleMove:Number = 10; + public var useStaticSeparation:Boolean = false; + + // Вектор гравитации + public var _gravity:Vector3 = new Vector3(0, 0, -9.8); + // Модуль вектора гравитации + public var _gravityMagnitude:Number = 9.8; + + // Использующийся детектор столкновений + public var collisionDetector:ICollisionDetector; + + // Список тел, участвующих в симуляции + public var bodies:BodyList = new BodyList(); + + // Список контактов на текущем шаге симуляции + altphysics var contacts:Contact; + // Количество контактов на текущем шаге симуляции +// altphysics var contactsNum:int; + // Список ограничений + altphysics var constraints:Vector. = new Vector.(); + // Количество ограничений + altphysics var constraintsNum:int; + // Временная метка. Число прошедших шагов с начала симуляции. + public var timeStamp:uint; + // Время с начала симуляции, мс + public var time:uint; + // Первый неиспользованный контакт на текущем шаге симуляции + private var borderContact:Contact; + + // Временные переменные для избежания создания экземпляров + private var _r:Vector3 = new Vector3(); + private var _t:Vector3 = new Vector3(); + private var _v:Vector3 = new Vector3(); + private var _v1:Vector3 = new Vector3(); + private var _v2:Vector3 = new Vector3(); + + /** + * + */ + public function World() { + contacts = new Contact(0); + var contact:Contact = contacts; + for (var i:int = 1; i < MAX_CONTACTS; i++) { + contact.next = new Contact(i); + contact = contact.next; + } + collisionDetector = new KdTreeCollisionDetector(); + } + + /** + * Вектор гравитации. + */ + public function get gravity():Vector3 { + return _gravity.vClone(); + } + + /** + * @private + */ + public function set gravity(value:Vector3):void { + _gravity.vCopy(value); + _gravityMagnitude = _gravity.vLength(); + } + + /** + * Добавляет тело в симуляцию. + * + * @param body + */ + public function addBody(body:Body):void { + body.id = lastBodyId++; + body.world = this; + bodies.append(body); + } + + /** + * Удаляет тело из симуляции. + * @param body + * @return + */ + public function removeBody(body:Body):void { + if (bodies.remove(body)) { + body.world = null; + } + } + + /** + * Добавляет ограничение. + * @param c + */ + public function addConstraint(c:Constraint):void { + constraints[constraintsNum++] = c; + c.world = this; + } + + /** + * Удаляет ограничение. + * @param c + */ + public function removeConstraint(c:Constraint):Boolean { + var idx:int = constraints.indexOf(c); + if (idx < 0) return false; + constraints.splice(idx, 1) + constraintsNum--; + c.world = null; + return true; + } + + /** + * Применяет к телам действующте на них силы. Аккумуляторы сил и моментов тел очищаются после завершения шага + * симуляции, поэтому на момент вызова метода могут уже содержать некоторые значения. + * + * @param dt промежуток времени, в течении которого действуют силы + */ + private function applyForces(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + body.beforePhysicsStep(dt); + body.calcAccelerations(); + // Ускорение свободного падения применяется только к подвижным телам во избежание некорректного изменения + // фиктивной скорости неподвижных тел. + if (body.movable && !body.frozen) { + body.accel.x += _gravity.x; + body.accel.y += _gravity.y; + body.accel.z += _gravity.z; + } + item = item.next; + } + } + + /** + * Определяет все столкновения на текущем шаге симуляции и заполняет список получившихся контактов. + * + * @param dt длительность шага симуляции + */ + private function detectCollisions(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + if (!body.frozen) { + body.contactsNum = 0; + body.saveState(); + // При включённом режиме предсказания состояние тел интегрируется на один шаг вперёд + if (usePrediction) { + body.integrateVelocity(dt); + body.integratePosition(dt); + } + body.calcDerivedData(); + } + item = item.next; + } + + borderContact = collisionDetector.getAllContacts(contacts); + + // Расчёт относительных векторов точки контакта вынесен сюда из-за необходимости учитывать + // положение тел в предсказанном состоянии + var contact:Contact = contacts; + while (contact != borderContact) { + var b1:Body = contact.body1; + var b2:Body = contact.body2; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + var bPos:Vector3 = b1.state.pos; + cp.r1.x = cp.pos.x - bPos.x; + cp.r1.y = cp.pos.y - bPos.y; + cp.r1.z = cp.pos.z - bPos.z; + if (b2 != null) { + bPos = b2.state.pos; + cp.r2.x = cp.pos.x - bPos.x; + cp.r2.y = cp.pos.y - bPos.y; + cp.r2.z = cp.pos.z - bPos.z; + } + } + contact = contact.next; + } + + // Восстановление состояния тел + if (usePrediction) { + item = bodies.head; + while (item != null) { + body = item.body; + if (!body.frozen) { + body.restoreState(); + body.calcDerivedData(); + } + item = item.next; + } + } + } + + /** + * Подготваливает полученные из детектора столкновений контакты, расчитывая значения, не меняющиеся + * в ходе шага симуляции. + */ + private function preProcessContacts(dt:Number):void { + var contact:Contact = contacts; + while (contact != borderContact) { + var b1:Body = contact.body1; + var b2:Body = contact.body2; + // Столкнувшиеся тела размораживаются + if (b1.frozen) { + b1.frozen = false; + b1.freezeCounter = 0; + } + if (b2 != null && b2.frozen) { + b2.frozen = false; + b2.freezeCounter = 0; + } + contact.restitution = b1.material.restitution; + if (b2 != null && b2.material.restitution < contact.restitution) contact.restitution = b2.material.restitution; + contact.friction = b1.material.friction; + if (b2 != null && b2.material.friction < contact.friction) contact.friction = b2.material.friction; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.accumImpulseN = 0; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + cp.velByUnitImpulseN = 0; + if (b1.movable) { + cp.angularInertia1 = _v.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1).vDot(contact.normal); + cp.velByUnitImpulseN += b1.invMass + cp.angularInertia1; + } + if (b2 != null && b2.movable) { + cp.angularInertia2 = _v.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2).vDot(contact.normal); + cp.velByUnitImpulseN += b2.invMass + cp.angularInertia2; + } + // Расчёт требуемой конечной скорости для упругого контакта + calcSepVelocity(b1, b2, cp, _v); + cp.normalVel = _v.vDot(contact.normal); + if (cp.normalVel < 0) cp.normalVel = - contact.restitution*cp.normalVel; + // Скорость разделения неупругого контакта + cp.minSepVel = cp.penetration > allowedPenetration ? (cp.penetration - allowedPenetration)/(penResolutionSteps*dt) : 0; + if (cp.minSepVel > maxPenResolutionSpeed) cp.minSepVel = maxPenResolutionSpeed; + } + contact = contact.next; + } + for (var i:int = 0; i < constraintsNum; i++) { + var constraint:Constraint = constraints[i]; + constraint.preProcess(dt); + } + } + + /** + * + * @param dt + * @param forceInelastic + */ + private function processContacts(dt:Number, forceInelastic:Boolean):void { + var iterNum:int = forceInelastic ? contactIterations : collisionIterations; + var i:int; + var forwardLoop:Boolean = false; + for (var iter:int = 0; iter < iterNum; iter++) { + forwardLoop = !forwardLoop; + var contact:Contact = contacts; + while (contact != borderContact) { + resolveContact(contact, forceInelastic, forwardLoop); + contact = contact.next; + } + // Ограничения + for (i = 0; i < constraintsNum; i++) { + var constraint:Constraint = constraints[i]; + constraint.apply(dt); + } + } + + // Разделение контактов путём непосредственного изменения координат и ориентации. Экспериментально. +// if (forceInelastic && useStaticSeparation) performStaticSeparation(); + } + + /** + * + */ + private function resolveContact(contactInfo:Contact, forceInelastic:Boolean, forwardLoop:Boolean):void { + var b1:Body = contactInfo.body1; + var b2:Body = contactInfo.body2; + var normal:Vector3 = contactInfo.normal; + var i:int; + if (forwardLoop) { + for (i = 0; i < contactInfo.pcount; i++) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } else { + for (i = contactInfo.pcount - 1; i >= 0; i--) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } + } + + /** + * + * @param idx + * @param b1 + * @param b2 + * @param colInfo + * @param normal + * @param forceInelastic + */ + private function resolveContactPoint(idx:int, b1:Body, b2:Body, contact:Contact, normal:Vector3, forceInelastic:Boolean):void { + var cp:ContactPoint = contact.points[idx]; + if (!forceInelastic) { + cp.satisfied = true; + } + + var newVel:Number = 0; + calcSepVelocity(b1, b2, cp, _v); + var cnormal:Vector3 = contact.normal; + var sepVel:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + if (forceInelastic) { + var minSpeVel:Number = useStaticSeparation ? 0 : cp.minSepVel; + if (sepVel < minSpeVel) { + cp.satisfied = false; + } else if (cp.satisfied) return; + newVel = minSpeVel; + } else { + newVel = cp.normalVel; + } + var deltaVel:Number = newVel - sepVel; + var impulse:Number = deltaVel/cp.velByUnitImpulseN; + var accumImpulse:Number = cp.accumImpulseN + impulse; + if (accumImpulse < 0) { + accumImpulse = 0; + } + var deltaImpulse:Number = accumImpulse - cp.accumImpulseN; + cp.accumImpulseN = accumImpulse; + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, normal, deltaImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, normal, -deltaImpulse); + } + + // Учёт силы трения + calcSepVelocity(b1, b2, cp, _v); + // Расчитываем изменение касательной скорости на единицу касательного импульса + var tanSpeedByUnitImpulse:Number = 0; + +// _v.vAddScaled(-_v.vDot(contact.normal), contact.normal); + var dot:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + _v.x -= dot*cnormal.x; + _v.y -= dot*cnormal.y; + _v.z -= dot*cnormal.z; + + var tanSpeed:Number = _v.vLength(); + if (tanSpeed < 0.001) return; + +// _t.vCopy(_v).vNormalize().vReverse(); + _t.x = -_v.x; + _t.y = -_v.y; + _t.z = -_v.z; + _t.vNormalize(); + + var r:Vector3; + var m:Matrix3; + var xx:Number; + var yy:Number; + var zz:Number; + // dV = b.invMass + ((invI * (r % t)) % r) * t + if (b1.movable) { +// _v.vCross2(cp.r1, _t).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1); + r = cp.r1; + m = b1.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b1.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + if (b2 != null && b2.movable) { +// _v.vCross2(cp.r2, _t).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2); + + r = cp.r2; + m = b2.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b2.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + + var tanImpulse:Number = tanSpeed/tanSpeedByUnitImpulse; + var max:Number = contact.friction*cp.accumImpulseN; + if (max < 0) { + if (tanImpulse < max) tanImpulse = max; + } else { + if (tanImpulse > max) tanImpulse = max; + } + + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, _t, tanImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, _t, -tanImpulse); + } + } + + /** + * + * @param cp + * @param normal + * @return + */ + private function calcSepVelocity(body1:Body, body2:Body, cp:ContactPoint, result:Vector3):void { + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 +// result.vCopy(body1.state.velocity).vAdd(_v1.vCross2(body1.state.rotation, cp.r1)); + var rot:Vector3 = body1.state.rotation; + var v:Vector3 = cp.r1; + var x:Number = rot.y*v.z - rot.z*v.y; + var y:Number = rot.z*v.x - rot.x*v.z; + var z:Number = rot.x*v.y - rot.y*v.x; + v = body1.state.velocity; + result.x = v.x + x; + result.y = v.y + y; + result.z = v.z + z; + // V2 = V2_c + w2%r2 + if (body2 != null) { +// result.vSubtract(body2.state.velocity).vSubtract(_v2.vCross2(body2.state.rotation, cp.r2)); + rot = body2.state.rotation; + v = cp.r2; + x = rot.y*v.z - rot.z*v.y; + y = rot.z*v.x - rot.x*v.z; + z = rot.x*v.y - rot.y*v.x; + v = body2.state.velocity; + result.x -= v.x + x; + result.y -= v.y + y; + result.z -= v.z + z; + } + } + + /** + * + * @param dt + */ + private function intergateVelocities(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + item.body.integrateVelocity(dt); + item = item.next; + } + } + + /** + * + * @param dt + */ + private function integratePositions(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + if (body.movable && !body.frozen) { + body.integratePosition(dt); + } + item = item.next; + } + } + + /** + * + */ + private function performStaticSeparation():void { + var iterNum:int = staticSeparationIterations; +// iterNum = 100; + + // 1. В начале каждой итерации для всех контактов сбрасывается флаг satisfied с одновременным поиском наихудшего контакта, т.е. имеющего наибольшую величину пересечения тел. + // 2. Если найденный контакт имеет величину пересечения меньше предельно допустимой процедура прерывается, т.к. разделение контактов не требуется. + // 3. Если процедура продолжается, то выполняется разделение найденного контакта и установка его флага satisfied в true, после чего запускается внутренний цикл, состоящий + // из contactsNum - 1 итераций. На каждой итерации ищется наихудший контакт среди оставшихся (satisfied == false). Если такой контакт найден, то выполняется его разделение + // и переход к следующей итерации внутреннего цикла, иначе внутренний цикл прерывается и выполняется переход к следующей итерации внешнего цикла (шаг 1.). +// for (var iter:int = 0; iter < iterNum; iter++) { +// // Ищем контакт с максимальной величиной пересечения тел, одновременно сбрасывая флаги satisfied в false +// var worstContact:Contact = contacts[0]; +// var i:int; +// for (i = 1; i < contactsNum; i++) { +// var contact:Contact = contacts[i]; +// contact.satisfied = false; +// if (contact.maxPenetration > worstContact.maxPenetration) worstContact = contact; +// } +// if (worstContact.maxPenetration <= allowedPenetration) return; +// +// resolveInterpenetration(worstContact); +// // Внутренний цикл по оставшимся контактам +// for (i = 1; i < contactsNum; i++) { +// worstContact = getWorstContact(); +// if (worstContact == null) break; +// resolveInterpenetration(worstContact); +// } +// } + } + + /** + * + */ +// private function getWorstContact():Contact { +// var maxPen:Number = 0; +// var worst:Contact = null; +// for (var i:int = 0; i < contactsNum; i++) { +// var c:Contact = contacts[i]; +// if (!c.satisfied && c.maxPenetration > maxPen) { +// worst = c; +// maxPen = c.maxPenetration; +// } +// } +// return maxPen > allowedPenetration ? worst : null; +// } + + /** + * Разделяет указанный контакт, выполняя staticSeparationSteps итераций по списку точек контакта. + * 1. В начале каждой итерации ищется точка с наибольшим пересечением, одновременно сбрасываются флаги satisfied у точек. + * 2. Для найденной точки выполняется процедура разделения. + * 3. После выполняется pcount - 1 итерация по списку точек, каждый раз ищется наихудшая среди имеющих satisfied == false, для которой выполняется процедура разделения. + * Если наихудшая точка не найдена, то выполнется переход к следующей итерации внешнего цикла (шаг 1). + * + * В конце процедуры разделения обновляются значения пересечений для остальных точек контакта, а также для всех прочих контактов, относящихся к телам текущего. + * + * @param contact контакт для разделения + */ + private function resolveInterpenetration(contact:Contact):void { + contact.satisfied = true; + + for (var step:int = 0; step < staticSeparationSteps; step++) { + var worstCp:ContactPoint = contact.points[0]; + var cp:ContactPoint; + var i:int; + for (i = 1; i < contact.pcount; i++) { + cp = contact.points[i]; + cp.satisfied = false; + if (cp.penetration > worstCp.penetration) worstCp = cp; + } + if (worstCp.penetration <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + // Разделяем оставшиеся точки + var maxPen:Number = 0; + for (i = 1; i < contact.pcount; i++) { + // Поиск наихудшей точки + for (var j:int = 0; j < contact.pcount; j++) { + cp = contact.points[j]; + if (cp.satisfied) continue; + if (cp.penetration > maxPen) { + maxPen = cp.penetration; + worstCp = cp; + } + } + if (maxPen <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + } + } + } + + /** + * + * @param cp + * @param contact + */ + private function separateContactPoint(cp:ContactPoint, contact:Contact):void { +// cp.satisfied = true; +// +// var b1:Body = contact.body1; +// var b2:Body = contact.body2; +// var totalMove:Number = cp.penetration - allowedPenetration; +// var moveCoeff:Number = totalMove/cp.velByUnitImpulseN; +// var linMove1:Number; +// var angleMove1:Number; +// if (b1.movable) { +// linMove1 = b1.invMass*moveCoeff; +// angleMove1 = cp.angularInertia1*moveCoeff; +// if (angleMove1 > maxAngleMove) { +// linMove1 += angleMove1 - maxAngleMove; +// angleMove1 -= maxAngleMove; +// } +// b1.state.pos.vAddScaled(linMove1, contact.normal); +// _v1.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vScale(angleMove1); +// b1.state.orientation.addScaledVector(_v1, 1); +// } +// var linMove2:Number; +// var angleMove2:Number; +// if (b2 != null && b2.movable) { +// linMove2 = b2.invMass*moveCoeff; +// angleMove2 = cp.angularInertia2*moveCoeff; +// if (angleMove2 > maxAngleMove) { +// linMove2 += angleMove2 - maxAngleMove; +// angleMove2 -= maxAngleMove; +// } +// b2.state.pos.vAddScaled(-linMove2, contact.normal); +// _v2.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vScale(angleMove2); +// _v2.vReverse(); +// b2.state.orientation.addScaledVector(_v2, 1); +// } +// cp.penetration = allowedPenetration; +// // Обновляем пересечения в других точках +// var i:int; +// for (i = 0; i < contact.pcount; i++) { +// var cp1:ContactPoint = contact.points[i]; +// if (cp1 == cp) continue; +// var angularMove:Number; +// if (b1.movable) { +// angularMove = _v.vCross2(_v1, cp1.r1).vDot(contact.normal); +// cp1.penetration -= linMove1 + angularMove; +// } +// if (b2 != null && b2.movable) { +// angularMove = _v.vCross2(_v2, cp1.r2).vDot(contact.normal); +// cp1.penetration -= linMove2 - angularMove; +// } +// // Обновление максимального значения для контакта +// if (cp1.penetration > contact.maxPenetration) contact.maxPenetration = cp1.penetration; +// } +// // Обновляем пересечения для других контактов +// var c:Contact; +// var j:int; +// if (b1.movable) { +// for (i = 0; i < b1.contactsNum; i++) { +// c = b1.contacts[i]; +// if (c == contact) continue; +// for (j = 0; j < c.pcount; j++) { +// cp1 = c.points[j]; +// if (b1 == c.body1) cp1.penetration -= linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r1).vDot(c.normal); +// else cp1.penetration += linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r2).vDot(c.normal); +// if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; +// } +// } +// } +// if (b2 != null && b2.movable) { +// for (i = 0; i < b2.contactsNum; i++) { +// c = b2.contacts[i]; +// if (c == contact) continue; +// for (j = 0; j < c.pcount; j++) { +// cp1 = c.points[j]; +// if (b2 == c.body1) cp1.penetration -= linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r1).vDot(c.normal); +// else cp1.penetration += linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r2).vDot(c.normal); +// if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; +// } +// } +// } + } + + /** + * + */ + private function postPhysics():void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + body.clearAccumulators(); + body.calcDerivedData(); + if (body.canFreeze) { + if (body.state.velocity.vLength() < linSpeedFreezeLimit && body.state.rotation.vLength() < angSpeedFreezeLimit) { + if (!body.frozen) { + body.freezeCounter++; + if (body.freezeCounter >= freezeSteps) body.frozen = true; + } + } else { + body.freezeCounter = 0; + body.frozen = false; + } + } + item = item.next; + } + } + + /** + * + * @param delta + */ + public function runPhysics(delta:uint):void { + timeStamp++; + time += delta; + var dt:Number = 0.001*delta; + applyForces(dt); + detectCollisions(dt); + preProcessContacts(dt); + processContacts(dt, false); + intergateVelocities(dt); + processContacts(dt, true); + integratePositions(dt); + postPhysics(); + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/altphysics.as b/0.0.7.0/src/alternativa/physics/altphysics.as new file mode 100644 index 0000000..2715307 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/altphysics.as @@ -0,0 +1,3 @@ +package alternativa.physics { + public namespace altphysics = "http://alternativaplatform.com/en/altphysics"; +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/.svn/all-wcprops b/0.0.7.0/src/alternativa/physics/collision/.svn/all-wcprops new file mode 100644 index 0000000..1dd60e2 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/.svn/all-wcprops @@ -0,0 +1,65 @@ +K 25 +svn:wc:ra_dav:version-url +V 113 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/collision +END +ICollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 135 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/collision/ICollisionDetector.as +END +CollisionKdNode.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/collision/CollisionKdNode.as +END +CollisionKdTree2D.as +K 25 +svn:wc:ra_dav:version-url +V 134 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/collision/CollisionKdTree2D.as +END +IRayCollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 139 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/collision/IRayCollisionPredicate.as +END +IBodyCollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/collision/IBodyCollisionPredicate.as +END +CollisionPrimitive.as +K 25 +svn:wc:ra_dav:version-url +V 135 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/collision/CollisionPrimitive.as +END +KdTreeCollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/collision/KdTreeCollisionDetector.as +END +ICollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 136 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/collision/ICollisionPredicate.as +END +ICollider.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/collision/ICollider.as +END +CollisionKdTree.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/collision/CollisionKdTree.as +END diff --git a/0.0.7.0/src/alternativa/physics/collision/.svn/entries b/0.0.7.0/src/alternativa/physics/collision/.svn/entries new file mode 100644 index 0000000..957d518 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/.svn/entries @@ -0,0 +1,157 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/collision +http://svndev.alternativaplatform.com + + + +2009-10-23T05:59:29.567666Z +22444 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +colliders +dir + +ICollisionDetector.as +file + + + + +2010-10-28T04:32:42.000000Z +04aac68b82def9fa814b66f90a32dbcb +2009-10-19T07:17:33.113306Z +22253 +mike + +CollisionKdNode.as +file + + + + +2010-10-28T04:32:42.000000Z +1d52aba28ce259ca1f9916d70619263d +2009-07-20T03:48:26.517738Z +16522 +mike + +CollisionKdTree2D.as +file + + + + +2010-10-28T04:32:42.000000Z +d8211949ef77817543bc2882480da12f +2009-07-20T03:48:26.517738Z +16522 +mike + +IRayCollisionPredicate.as +file + + + + +2010-10-28T04:32:42.000000Z +32f9b1eaeacfcb40e7ae1927f52b8ab8 +2009-10-19T07:17:33.113306Z +22253 +mike + +types +dir + +IBodyCollisionPredicate.as +file + + + + +2010-10-28T04:32:42.000000Z +618f907a588979a8d06d4f4bcf6618a0 +2009-10-19T07:17:33.113306Z +22253 +mike + +CollisionPrimitive.as +file + + + + +2010-10-28T04:32:42.000000Z +c969fea7a18b12ac1c98faaf9900bf84 +2009-10-20T10:42:06.632117Z +22312 +mike + +KdTreeCollisionDetector.as +file + + + + +2010-10-28T04:32:42.000000Z +fa4965812a9e0e0de2bce66b0de53d44 +2009-10-20T10:42:06.632117Z +22312 +mike + +ICollisionPredicate.as +file + + + + +2010-10-28T04:32:42.000000Z +65f3250d36556fffc457c740fb7d38b3 +2009-10-19T07:17:33.113306Z +22253 +mike + +ICollider.as +file + + + + +2010-10-28T04:32:42.000000Z +128a7d7479e13caeccecb45161b27b22 +2009-10-19T07:17:33.113306Z +22253 +mike + +primitives +dir + +CollisionKdTree.as +file + + + + +2010-10-28T04:32:42.000000Z +1f0db7a1be451e638b7ec88a10219517 +2009-10-19T07:17:33.113306Z +22253 +mike + diff --git a/0.0.7.0/src/alternativa/physics/collision/.svn/format b/0.0.7.0/src/alternativa/physics/collision/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.7.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base b/0.0.7.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base new file mode 100644 index 0000000..e24922c --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base @@ -0,0 +1,18 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.types.BoundBox; + + public class CollisionKdNode { + public var indices:Vector.; + public var splitIndices:Vector.; + public var boundBox:BoundBox; + public var parent:CollisionKdNode; + public var splitTree:CollisionKdTree2D; + + public var axis:int = -1; // 0 - x, 1 - y, 2 - z + public var coord:Number; + public var positiveNode:CollisionKdNode; + public var negativeNode:CollisionKdNode; + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base b/0.0.7.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base new file mode 100644 index 0000000..50ba94a --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base @@ -0,0 +1,261 @@ +package alternativa.physics.collision { + + import __AS3__.vec.Vector; + + import alternativa.physics.collision.types.BoundBox; + + /** + * @author mike + */ + public class CollisionKdTree { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + public var rootNode:CollisionKdNode; + public var staticChildren:Vector.; + public var numStaticChildren:int; + public var staticBoundBoxes:Vector. = new Vector.(); + + private var splitAxis:int; + private var splitCoord:Number; + private var splitCost:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + + /** + * @param boundBox + */ + public function createTree(collisionPrimitives:Vector., boundBox:BoundBox = null):void { + staticChildren = collisionPrimitives.concat(); + numStaticChildren = staticChildren.length; + // Создаём корневую ноду + rootNode = new CollisionKdNode(); + rootNode.indices = new Vector.(); + // Расчитываем баунды объектов и рутовой ноды + var rootNodeBoundBox:BoundBox = rootNode.boundBox = (boundBox != null) ? boundBox : new BoundBox(); + for (var i:int = 0; i < numStaticChildren; ++i) { + var child:CollisionPrimitive = staticChildren[i]; + var childBoundBox:BoundBox = staticBoundBoxes[i] = child.calculateAABB(); + rootNodeBoundBox.addBoundBox(childBoundBox); + rootNode.indices[i] = i; + } + staticBoundBoxes.length = numStaticChildren; + // Разделяем рутовую ноду + splitNode(rootNode); + + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + var indices:Vector. = node.indices; + var numPrimitives:int = indices.length; + if (numPrimitives <= minPrimitivesPerNode) return; + + // Подготовка баунда с погрешностями + var nodeBoundBox:BoundBox = node.boundBox; + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + // Собираем опорные координаты + var i:int; + var j:int; + var numSplitCoordsX:int = 0 + var numSplitCoordsY:int = 0; + var numSplitCoordsZ:int = 0; + for (i = 0; i < numPrimitives; ++i) { + var boundBox:BoundBox = staticBoundBoxes[indices[i]]; + + if (boundBox.maxX - boundBox.minX <= doubleThreshold) { + if (boundBox.minX <= nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.minX; + else if (boundBox.maxX >= nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.maxX; + else splitCoordsX[numSplitCoordsX++] = (boundBox.minX + boundBox.maxX)*0.5; + } else { + if (boundBox.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = boundBox.minX; + if (boundBox.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = boundBox.maxX; + } + + if (boundBox.maxY - boundBox.minY <= doubleThreshold) { + if (boundBox.minY <= nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.minY; + else if (boundBox.maxY >= nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.maxY; + else splitCoordsY[numSplitCoordsY++] = (boundBox.minY + boundBox.maxY)*0.5; + } else { + if (boundBox.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = boundBox.minY; + if (boundBox.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = boundBox.maxY; + } + + if (boundBox.maxZ - boundBox.minZ <= doubleThreshold) { + if (boundBox.minZ <= nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.minZ; + else if (boundBox.maxZ >= nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.maxZ; + else splitCoordsZ[numSplitCoordsZ++] = (boundBox.minZ + boundBox.maxZ)*0.5; + } else { + if (boundBox.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.minZ; + if (boundBox.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numPrimitives; ++i) { + boundBox = staticBoundBoxes[indices[i]]; + var min:Number = axisX ? boundBox.minX : (axisY ? boundBox.minY : boundBox.minZ); + var max:Number = axisX ? boundBox.maxX : (axisY ? boundBox.maxY : boundBox.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(indices[i]); + indices[i] = -1; + } else { + if (node.splitIndices == null) node.splitIndices = new Vector.(); + node.splitIndices.push(indices[i]); + indices[i] = -1; + } + } else { + if (min >= coordMin) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(indices[i]); + indices[i] = -1; + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numPrimitives; ++i) { + if (indices[i] >= 0) indices[j++] = indices[i]; + } + if (j > 0) indices.length = j; + else node.indices = null; + + if (node.splitIndices != null) { + node.splitTree = new CollisionKdTree2D(this, node); + node.splitTree.createTree(); + } + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + /** + * + */ + public function traceTree():void { + traceNode("", rootNode); + } + + /** + * @param str + * @param node + */ + private function traceNode(str:String, node:CollisionKdNode):void { + if (node == null) return; + trace(str, node.axis == -1 ? "end" : ((node.axis == 0) ? "X" : ((node.axis == 1) ? "Y" : "Z")), "splitCoord=" + splitCoord, "bound", node.boundBox, "objs:", node.indices); + traceNode(str + "-", node.negativeNode); + traceNode(str + "+", node.positiveNode); + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base b/0.0.7.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base new file mode 100644 index 0000000..78aaa95 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base @@ -0,0 +1,226 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.types.BoundBox; + + /** + * + */ + public class CollisionKdTree2D { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + + public var parentTree:CollisionKdTree; + public var parentNode:CollisionKdNode; + public var rootNode:CollisionKdNode; + + private var splitAxis:int; + private var splitCost:Number; + private var splitCoord:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + /** + * + * @param parentTree + * @param parentNode + */ + public function CollisionKdTree2D(parentTree:CollisionKdTree, parentNode:CollisionKdNode) { + this.parentTree = parentTree; + this.parentNode = parentNode; + } + + /** + * + */ + public function createTree():void { + rootNode = new CollisionKdNode(); + rootNode.boundBox = parentNode.boundBox.clone(); + rootNode.indices = new Vector.(); + var numObjects:int = parentNode.splitIndices.length; + for (var i:int = 0; i < numObjects; ++i) rootNode.indices[i] = parentNode.splitIndices[i]; + + splitNode(rootNode); + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + if (node.indices.length <= minPrimitivesPerNode) return; + + var objects:Vector. = node.indices; + var i:int; + var j:int; + + var nodeBoundBox:BoundBox = node.boundBox; + + // Подготовка баунда с погрешностями + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + // Собираем опорные координаты + var numSplitCoordsX:int; + var numSplitCoordsY:int; + var numSplitCoordsZ:int; + var numObjects:int = objects.length; + for (i = 0; i < numObjects; ++i) { + var bb:BoundBox = staticBoundBoxes[objects[i]]; + + if (parentNode.axis != 0) { + if (bb.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = bb.minX; + if (bb.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = bb.maxX; + } + + if (parentNode.axis != 1) { + if (bb.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = bb.minY; + if (bb.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = bb.maxY; + } + + if (parentNode.axis != 2) { + if (bb.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = bb.minZ; + if (bb.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = bb.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + if (parentNode.axis != 0) checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + if (parentNode.axis != 1) checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + if (parentNode.axis != 2) checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numObjects; ++i) { + bb = staticBoundBoxes[objects[i]]; + var min:Number = axisX ? bb.minX : (axisY ? bb.minY : bb.minZ); + var max:Number = axisX ? bb.maxX : (axisY ? bb.maxY : bb.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(objects[i]); + objects[i] = -1; + } + } else { + if (min >= coordMin) { + if (max > coordMax) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(objects[i]); + objects[i] = -1; + } + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numObjects; ++i) { + if (objects[i] >= 0) objects[j++] = objects[i]; + } + if (j > 0) objects.length = j; + else node.indices = null; + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/.svn/text-base/CollisionPrimitive.as.svn-base b/0.0.7.0/src/alternativa/physics/collision/.svn/text-base/CollisionPrimitive.as.svn-base new file mode 100644 index 0000000..e42dfb3 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/.svn/text-base/CollisionPrimitive.as.svn-base @@ -0,0 +1,140 @@ +package alternativa.physics.collision { + + import alternativa.physics.Body; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.math.Matrix4; + import alternativa.physics.math.Vector3; + + /** + * Базовый класс для примитивов, использующихся детектором столкновений. + */ + public class CollisionPrimitive { + // Константы типов примитива + public static const BOX:int = 1; + public static const PLANE:int = 2; + public static const SPHERE:int = 3; + public static const RECT:int = 4; + public static const TRIANGLE:int = 5; + + // Тип примитива + public var type:int; + // Группы примитива. Каждая группа определяется установленным битом. Столкновения проверяются только для примитивов, + // имеющих хотя бы одну общую группу. + public var collisionGroup:int; + // Предикат примитива, вызывающийся детектором при нахождении столкновения. В зависимости от возвращённого + // предикатом результата столкновение либо регистрируется, либо игнорируется. + // Nullable + public var postCollisionPredicate:ICollisionPredicate; + // Тело, владеющее примитивом. Поле сделано открытым для быстрого доступа на чтение. Установка значения должна выполняться вызовом метода setBody(). + // Nullable + public var body:Body; + // Трансформация примитива в системе координат тела, если оно указано. Не допускается масштабирование матрицы. + public var localTransform:Matrix4; + // Полная трансформация примитива. Не допускается масштабирование матрицы. + public var transform:Matrix4 = new Matrix4(); + // AABB в мировой системе координат. Расчитывается системой вызовом функции calculateBoundBox(). + public var aabb:BoundBox = new BoundBox(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param type тип примитива + * @param collisionGroup группа примитива + */ + public function CollisionPrimitive(type:int, collisionGroup:int) { + this.type = type; + this.collisionGroup = collisionGroup; + } + + /** + * Устанавливает тело, владеющее примитивом. + * + * @param body тело, которое владеет примитивом + * @param localTransform трансформация примитива в системе координат тела. Указание значения null равносильно + * заданию единичной матрицы, однако в первом случае не будет дополнительного умножения матриц при вычислении полной трансформации примитива. + */ + public function setBody(body:Body, localTransform:Matrix4 = null):void { + if (this.body == body) return; + this.body = body; + if (body != null) { + if (localTransform != null) { + if (this.localTransform == null) { + this.localTransform = new Matrix4(); + } + this.localTransform.copy(localTransform); + } else { + this.localTransform = null; + } + } + } + + /** + * Рассчитывает AABB примитива. Наследники должны переопределять этот метод, реализуя в нём корректный рассчёт. + * + * @return ссылка на свой AABB + */ + public function calculateAABB():BoundBox { + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + return -1; + } + + /** + * Клонирует примитив. Переопределять не рекомендуется. Вместо этого переопределяются методы createPrimitive() и copyFrom(). + * + * @return клон примитива + */ + public function clone():CollisionPrimitive { + var p:CollisionPrimitive = createPrimitive(); + return p.copyFrom(this); + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + if (source == null) { + throw new ArgumentError("Parameter source cannot be null"); + } + type = source.type; + transform.copy(source.transform); + collisionGroup = source.collisionGroup; + setBody(source.body, source.localTransform); + aabb.copyFrom(source.aabb); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + public function toString():String { + return "[CollisionPrimitive type=" + type + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + protected function createPrimitive():CollisionPrimitive { + return new CollisionPrimitive(type, collisionGroup); + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base b/0.0.7.0/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base new file mode 100644 index 0000000..4ace681 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.Body; + + public interface IBodyCollisionPredicate { + function considerBodies(body1:Body, body2:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base b/0.0.7.0/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base new file mode 100644 index 0000000..e6aac2a --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base @@ -0,0 +1,29 @@ +package alternativa.physics.collision { + + import alternativa.physics.Contact; + + /** + * Интерфейс определителя столкновений между двумя примитивами. + */ + public interface ICollider { + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base b/0.0.7.0/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base new file mode 100644 index 0000000..25a71d9 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base @@ -0,0 +1,52 @@ +package alternativa.physics.collision { + import alternativa.physics.Contact; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.math.Vector3; + + /** + * Интерфейс детектора столкновений. + */ + public interface ICollisionDetector { + + /** + * Получает все столкновения в текущей конфигурации физической геометрии. + * + * @param contacts список контактов, в кторые будет записана информация о столкновении + * @return количество найденных столкновений + */ + function getAllContacts(contacts:Contact):Contact; + + /** + * Тестирует луч на пересечение с физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param direction направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRay(origin:Vector3, direction:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @return + */ + function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base b/0.0.7.0/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base new file mode 100644 index 0000000..33d30ca --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base @@ -0,0 +1,8 @@ +package alternativa.physics.collision { + + public interface ICollisionPredicate { + + function considerCollision(primitive:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base b/0.0.7.0/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base new file mode 100644 index 0000000..b319f62 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.Body; + + public interface IRayCollisionPredicate { + function considerBody(body:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base b/0.0.7.0/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base new file mode 100644 index 0000000..14202ec --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base @@ -0,0 +1,486 @@ +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.; + 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.(); + + 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.):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. = tree.staticChildren; +// var indices:Vector. = 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., indices:Vector., 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; + } \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/CollisionKdNode.as b/0.0.7.0/src/alternativa/physics/collision/CollisionKdNode.as new file mode 100644 index 0000000..e24922c --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/CollisionKdNode.as @@ -0,0 +1,18 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.types.BoundBox; + + public class CollisionKdNode { + public var indices:Vector.; + public var splitIndices:Vector.; + public var boundBox:BoundBox; + public var parent:CollisionKdNode; + public var splitTree:CollisionKdTree2D; + + public var axis:int = -1; // 0 - x, 1 - y, 2 - z + public var coord:Number; + public var positiveNode:CollisionKdNode; + public var negativeNode:CollisionKdNode; + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/CollisionKdTree.as b/0.0.7.0/src/alternativa/physics/collision/CollisionKdTree.as new file mode 100644 index 0000000..50ba94a --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/CollisionKdTree.as @@ -0,0 +1,261 @@ +package alternativa.physics.collision { + + import __AS3__.vec.Vector; + + import alternativa.physics.collision.types.BoundBox; + + /** + * @author mike + */ + public class CollisionKdTree { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + public var rootNode:CollisionKdNode; + public var staticChildren:Vector.; + public var numStaticChildren:int; + public var staticBoundBoxes:Vector. = new Vector.(); + + private var splitAxis:int; + private var splitCoord:Number; + private var splitCost:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + + /** + * @param boundBox + */ + public function createTree(collisionPrimitives:Vector., boundBox:BoundBox = null):void { + staticChildren = collisionPrimitives.concat(); + numStaticChildren = staticChildren.length; + // Создаём корневую ноду + rootNode = new CollisionKdNode(); + rootNode.indices = new Vector.(); + // Расчитываем баунды объектов и рутовой ноды + var rootNodeBoundBox:BoundBox = rootNode.boundBox = (boundBox != null) ? boundBox : new BoundBox(); + for (var i:int = 0; i < numStaticChildren; ++i) { + var child:CollisionPrimitive = staticChildren[i]; + var childBoundBox:BoundBox = staticBoundBoxes[i] = child.calculateAABB(); + rootNodeBoundBox.addBoundBox(childBoundBox); + rootNode.indices[i] = i; + } + staticBoundBoxes.length = numStaticChildren; + // Разделяем рутовую ноду + splitNode(rootNode); + + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + var indices:Vector. = node.indices; + var numPrimitives:int = indices.length; + if (numPrimitives <= minPrimitivesPerNode) return; + + // Подготовка баунда с погрешностями + var nodeBoundBox:BoundBox = node.boundBox; + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + // Собираем опорные координаты + var i:int; + var j:int; + var numSplitCoordsX:int = 0 + var numSplitCoordsY:int = 0; + var numSplitCoordsZ:int = 0; + for (i = 0; i < numPrimitives; ++i) { + var boundBox:BoundBox = staticBoundBoxes[indices[i]]; + + if (boundBox.maxX - boundBox.minX <= doubleThreshold) { + if (boundBox.minX <= nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.minX; + else if (boundBox.maxX >= nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.maxX; + else splitCoordsX[numSplitCoordsX++] = (boundBox.minX + boundBox.maxX)*0.5; + } else { + if (boundBox.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = boundBox.minX; + if (boundBox.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = boundBox.maxX; + } + + if (boundBox.maxY - boundBox.minY <= doubleThreshold) { + if (boundBox.minY <= nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.minY; + else if (boundBox.maxY >= nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.maxY; + else splitCoordsY[numSplitCoordsY++] = (boundBox.minY + boundBox.maxY)*0.5; + } else { + if (boundBox.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = boundBox.minY; + if (boundBox.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = boundBox.maxY; + } + + if (boundBox.maxZ - boundBox.minZ <= doubleThreshold) { + if (boundBox.minZ <= nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.minZ; + else if (boundBox.maxZ >= nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.maxZ; + else splitCoordsZ[numSplitCoordsZ++] = (boundBox.minZ + boundBox.maxZ)*0.5; + } else { + if (boundBox.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.minZ; + if (boundBox.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numPrimitives; ++i) { + boundBox = staticBoundBoxes[indices[i]]; + var min:Number = axisX ? boundBox.minX : (axisY ? boundBox.minY : boundBox.minZ); + var max:Number = axisX ? boundBox.maxX : (axisY ? boundBox.maxY : boundBox.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(indices[i]); + indices[i] = -1; + } else { + if (node.splitIndices == null) node.splitIndices = new Vector.(); + node.splitIndices.push(indices[i]); + indices[i] = -1; + } + } else { + if (min >= coordMin) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(indices[i]); + indices[i] = -1; + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numPrimitives; ++i) { + if (indices[i] >= 0) indices[j++] = indices[i]; + } + if (j > 0) indices.length = j; + else node.indices = null; + + if (node.splitIndices != null) { + node.splitTree = new CollisionKdTree2D(this, node); + node.splitTree.createTree(); + } + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + /** + * + */ + public function traceTree():void { + traceNode("", rootNode); + } + + /** + * @param str + * @param node + */ + private function traceNode(str:String, node:CollisionKdNode):void { + if (node == null) return; + trace(str, node.axis == -1 ? "end" : ((node.axis == 0) ? "X" : ((node.axis == 1) ? "Y" : "Z")), "splitCoord=" + splitCoord, "bound", node.boundBox, "objs:", node.indices); + traceNode(str + "-", node.negativeNode); + traceNode(str + "+", node.positiveNode); + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/CollisionKdTree2D.as b/0.0.7.0/src/alternativa/physics/collision/CollisionKdTree2D.as new file mode 100644 index 0000000..78aaa95 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/CollisionKdTree2D.as @@ -0,0 +1,226 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.types.BoundBox; + + /** + * + */ + public class CollisionKdTree2D { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + + public var parentTree:CollisionKdTree; + public var parentNode:CollisionKdNode; + public var rootNode:CollisionKdNode; + + private var splitAxis:int; + private var splitCost:Number; + private var splitCoord:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + /** + * + * @param parentTree + * @param parentNode + */ + public function CollisionKdTree2D(parentTree:CollisionKdTree, parentNode:CollisionKdNode) { + this.parentTree = parentTree; + this.parentNode = parentNode; + } + + /** + * + */ + public function createTree():void { + rootNode = new CollisionKdNode(); + rootNode.boundBox = parentNode.boundBox.clone(); + rootNode.indices = new Vector.(); + var numObjects:int = parentNode.splitIndices.length; + for (var i:int = 0; i < numObjects; ++i) rootNode.indices[i] = parentNode.splitIndices[i]; + + splitNode(rootNode); + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + if (node.indices.length <= minPrimitivesPerNode) return; + + var objects:Vector. = node.indices; + var i:int; + var j:int; + + var nodeBoundBox:BoundBox = node.boundBox; + + // Подготовка баунда с погрешностями + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + // Собираем опорные координаты + var numSplitCoordsX:int; + var numSplitCoordsY:int; + var numSplitCoordsZ:int; + var numObjects:int = objects.length; + for (i = 0; i < numObjects; ++i) { + var bb:BoundBox = staticBoundBoxes[objects[i]]; + + if (parentNode.axis != 0) { + if (bb.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = bb.minX; + if (bb.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = bb.maxX; + } + + if (parentNode.axis != 1) { + if (bb.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = bb.minY; + if (bb.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = bb.maxY; + } + + if (parentNode.axis != 2) { + if (bb.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = bb.minZ; + if (bb.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = bb.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + if (parentNode.axis != 0) checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + if (parentNode.axis != 1) checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + if (parentNode.axis != 2) checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numObjects; ++i) { + bb = staticBoundBoxes[objects[i]]; + var min:Number = axisX ? bb.minX : (axisY ? bb.minY : bb.minZ); + var max:Number = axisX ? bb.maxX : (axisY ? bb.maxY : bb.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(objects[i]); + objects[i] = -1; + } + } else { + if (min >= coordMin) { + if (max > coordMax) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(objects[i]); + objects[i] = -1; + } + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numObjects; ++i) { + if (objects[i] >= 0) objects[j++] = objects[i]; + } + if (j > 0) objects.length = j; + else node.indices = null; + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/CollisionPrimitive.as b/0.0.7.0/src/alternativa/physics/collision/CollisionPrimitive.as new file mode 100644 index 0000000..e42dfb3 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/CollisionPrimitive.as @@ -0,0 +1,140 @@ +package alternativa.physics.collision { + + import alternativa.physics.Body; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.math.Matrix4; + import alternativa.physics.math.Vector3; + + /** + * Базовый класс для примитивов, использующихся детектором столкновений. + */ + public class CollisionPrimitive { + // Константы типов примитива + public static const BOX:int = 1; + public static const PLANE:int = 2; + public static const SPHERE:int = 3; + public static const RECT:int = 4; + public static const TRIANGLE:int = 5; + + // Тип примитива + public var type:int; + // Группы примитива. Каждая группа определяется установленным битом. Столкновения проверяются только для примитивов, + // имеющих хотя бы одну общую группу. + public var collisionGroup:int; + // Предикат примитива, вызывающийся детектором при нахождении столкновения. В зависимости от возвращённого + // предикатом результата столкновение либо регистрируется, либо игнорируется. + // Nullable + public var postCollisionPredicate:ICollisionPredicate; + // Тело, владеющее примитивом. Поле сделано открытым для быстрого доступа на чтение. Установка значения должна выполняться вызовом метода setBody(). + // Nullable + public var body:Body; + // Трансформация примитива в системе координат тела, если оно указано. Не допускается масштабирование матрицы. + public var localTransform:Matrix4; + // Полная трансформация примитива. Не допускается масштабирование матрицы. + public var transform:Matrix4 = new Matrix4(); + // AABB в мировой системе координат. Расчитывается системой вызовом функции calculateBoundBox(). + public var aabb:BoundBox = new BoundBox(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param type тип примитива + * @param collisionGroup группа примитива + */ + public function CollisionPrimitive(type:int, collisionGroup:int) { + this.type = type; + this.collisionGroup = collisionGroup; + } + + /** + * Устанавливает тело, владеющее примитивом. + * + * @param body тело, которое владеет примитивом + * @param localTransform трансформация примитива в системе координат тела. Указание значения null равносильно + * заданию единичной матрицы, однако в первом случае не будет дополнительного умножения матриц при вычислении полной трансформации примитива. + */ + public function setBody(body:Body, localTransform:Matrix4 = null):void { + if (this.body == body) return; + this.body = body; + if (body != null) { + if (localTransform != null) { + if (this.localTransform == null) { + this.localTransform = new Matrix4(); + } + this.localTransform.copy(localTransform); + } else { + this.localTransform = null; + } + } + } + + /** + * Рассчитывает AABB примитива. Наследники должны переопределять этот метод, реализуя в нём корректный рассчёт. + * + * @return ссылка на свой AABB + */ + public function calculateAABB():BoundBox { + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + return -1; + } + + /** + * Клонирует примитив. Переопределять не рекомендуется. Вместо этого переопределяются методы createPrimitive() и copyFrom(). + * + * @return клон примитива + */ + public function clone():CollisionPrimitive { + var p:CollisionPrimitive = createPrimitive(); + return p.copyFrom(this); + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + if (source == null) { + throw new ArgumentError("Parameter source cannot be null"); + } + type = source.type; + transform.copy(source.transform); + collisionGroup = source.collisionGroup; + setBody(source.body, source.localTransform); + aabb.copyFrom(source.aabb); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + public function toString():String { + return "[CollisionPrimitive type=" + type + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + protected function createPrimitive():CollisionPrimitive { + return new CollisionPrimitive(type, collisionGroup); + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/IBodyCollisionPredicate.as b/0.0.7.0/src/alternativa/physics/collision/IBodyCollisionPredicate.as new file mode 100644 index 0000000..4ace681 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/IBodyCollisionPredicate.as @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.Body; + + public interface IBodyCollisionPredicate { + function considerBodies(body1:Body, body2:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/ICollider.as b/0.0.7.0/src/alternativa/physics/collision/ICollider.as new file mode 100644 index 0000000..e6aac2a --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/ICollider.as @@ -0,0 +1,29 @@ +package alternativa.physics.collision { + + import alternativa.physics.Contact; + + /** + * Интерфейс определителя столкновений между двумя примитивами. + */ + public interface ICollider { + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/ICollisionDetector.as b/0.0.7.0/src/alternativa/physics/collision/ICollisionDetector.as new file mode 100644 index 0000000..25a71d9 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/ICollisionDetector.as @@ -0,0 +1,52 @@ +package alternativa.physics.collision { + import alternativa.physics.Contact; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.math.Vector3; + + /** + * Интерфейс детектора столкновений. + */ + public interface ICollisionDetector { + + /** + * Получает все столкновения в текущей конфигурации физической геометрии. + * + * @param contacts список контактов, в кторые будет записана информация о столкновении + * @return количество найденных столкновений + */ + function getAllContacts(contacts:Contact):Contact; + + /** + * Тестирует луч на пересечение с физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param direction направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRay(origin:Vector3, direction:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @return + */ + function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/ICollisionPredicate.as b/0.0.7.0/src/alternativa/physics/collision/ICollisionPredicate.as new file mode 100644 index 0000000..33d30ca --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/ICollisionPredicate.as @@ -0,0 +1,8 @@ +package alternativa.physics.collision { + + public interface ICollisionPredicate { + + function considerCollision(primitive:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/IRayCollisionPredicate.as b/0.0.7.0/src/alternativa/physics/collision/IRayCollisionPredicate.as new file mode 100644 index 0000000..b319f62 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/IRayCollisionPredicate.as @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.Body; + + public interface IRayCollisionPredicate { + function considerBody(body:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/KdTreeCollisionDetector.as b/0.0.7.0/src/alternativa/physics/collision/KdTreeCollisionDetector.as new file mode 100644 index 0000000..14202ec --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/KdTreeCollisionDetector.as @@ -0,0 +1,486 @@ +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.; + 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.(); + + 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.):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. = tree.staticChildren; +// var indices:Vector. = 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., indices:Vector., 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; + } \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/colliders/.svn/all-wcprops b/0.0.7.0/src/alternativa/physics/collision/colliders/.svn/all-wcprops new file mode 100644 index 0000000..4d7441f --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/colliders/.svn/all-wcprops @@ -0,0 +1,53 @@ +K 25 +svn:wc:ra_dav:version-url +V 123 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/collision/colliders +END +BoxSphereCollider.as +K 25 +svn:wc:ra_dav:version-url +V 144 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/collision/colliders/BoxSphereCollider.as +END +BoxBoxCollider.as +K 25 +svn:wc:ra_dav:version-url +V 141 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/collision/colliders/BoxBoxCollider.as +END +BoxRectCollider.as +K 25 +svn:wc:ra_dav:version-url +V 142 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/collision/colliders/BoxRectCollider.as +END +SpherePlaneCollider.as +K 25 +svn:wc:ra_dav:version-url +V 146 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/collision/colliders/SpherePlaneCollider.as +END +BoxCollider.as +K 25 +svn:wc:ra_dav:version-url +V 138 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/collision/colliders/BoxCollider.as +END +BoxPlaneCollider.as +K 25 +svn:wc:ra_dav:version-url +V 143 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/collision/colliders/BoxPlaneCollider.as +END +SphereSphereCollider.as +K 25 +svn:wc:ra_dav:version-url +V 147 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/collision/colliders/SphereSphereCollider.as +END +BoxTriangleCollider.as +K 25 +svn:wc:ra_dav:version-url +V 146 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/collision/colliders/BoxTriangleCollider.as +END diff --git a/0.0.7.0/src/alternativa/physics/collision/colliders/.svn/entries b/0.0.7.0/src/alternativa/physics/collision/colliders/.svn/entries new file mode 100644 index 0000000..c118427 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/colliders/.svn/entries @@ -0,0 +1,124 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/collision/colliders +http://svndev.alternativaplatform.com + + + +2009-10-23T05:59:29.567666Z +22444 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +BoxSphereCollider.as +file + + + + +2010-10-28T04:32:42.000000Z +b2478004208a1d598d9f57747d83ccbb +2009-10-19T07:17:33.113306Z +22253 +mike + +BoxBoxCollider.as +file + + + + +2010-10-28T04:32:42.000000Z +543790ceff5b23a0b72bde58c111183d +2009-10-23T05:59:29.567666Z +22444 +mike + +BoxRectCollider.as +file + + + + +2010-10-28T04:32:42.000000Z +7ebc58be651f18d7e0374e6a936f36e0 +2009-10-23T05:59:29.567666Z +22444 +mike + +SpherePlaneCollider.as +file + + + + +2010-10-28T04:32:42.000000Z +e7f53348558f035e2f34a9e1fe1e1e4b +2009-10-19T07:17:33.113306Z +22253 +mike + +BoxCollider.as +file + + + + +2010-10-28T04:32:42.000000Z +5ddcd853975ee9d6a0c776a14f674e0e +2009-10-19T07:17:33.113306Z +22253 +mike + +BoxPlaneCollider.as +file + + + + +2010-10-28T04:32:42.000000Z +71648e823c289cb67b79e7c8af3045d2 +2009-10-19T07:17:33.113306Z +22253 +mike + +SphereSphereCollider.as +file + + + + +2010-10-28T04:32:42.000000Z +b336de4614439b93424863b1ace66c9b +2009-10-19T07:17:33.113306Z +22253 +mike + +BoxTriangleCollider.as +file + + + + +2010-10-28T04:32:42.000000Z +72a48b0dde7454a9c2f2b76345d77de2 +2009-10-23T05:59:29.567666Z +22444 +mike + diff --git a/0.0.7.0/src/alternativa/physics/collision/colliders/.svn/format b/0.0.7.0/src/alternativa/physics/collision/colliders/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/colliders/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.7.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxBoxCollider.as.svn-base b/0.0.7.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxBoxCollider.as.svn-base new file mode 100644 index 0000000..b003e98 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxBoxCollider.as.svn-base @@ -0,0 +1,589 @@ +package alternativa.physics.collision.colliders { + import __AS3__.vec.Vector; + + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.math.Matrix4; + import alternativa.physics.math.Vector3; + + use namespace altphysics; + + /** + * Расчитывает точки контакта двух боксов. Нормаль контакта направляется в сторону бокса с меньшим ID. + */ + public class BoxBoxCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + private var vectorToBox1:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var bestAxisIndex:int; + private var minOverlap:Number; + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + private var tmpPoints:Vector. = new Vector.(8, true); + private var pcount:int; + + /** + * + */ + public function BoxBoxCollider() { + for (var i:int = 0; i < 8; i++) { + tmpPoints[i] = new ContactPoint(); + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + + if (bestAxisIndex < 6) { + // Контакт грань-(грань|ребро|вершина) + if (!findFaceContactPoints(box1, box2, vectorToBox1, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 6; + findEdgesIntersection(box1, box2, vectorToBox1, int(bestAxisIndex/3), bestAxisIndex%3, contact); + } + contact.body1 = box1.body; + contact.body2 = box2.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + // Вектор из центра второго бокса в центр первого + vectorToBox1.x = transform1.d - transform2.d; + vectorToBox1.y = transform1.h - transform2.h; + vectorToBox1.z = transform1.l - transform2.l; + + // Проверка пересечения по основным осям + axis10.x = transform1.a; + axis10.y = transform1.e; + axis10.z = transform1.i; + if (!testMainAxis(box1, box2, axis10, 0, vectorToBox1)) return false; + axis11.x = transform1.b; + axis11.y = transform1.f; + axis11.z = transform1.j; + if (!testMainAxis(box1, box2, axis11, 1, vectorToBox1)) return false; + axis12.x = transform1.c; + axis12.y = transform1.g; + axis12.z = transform1.k; + if (!testMainAxis(box1, box2, axis12, 2, vectorToBox1)) return false; + + axis20.x = transform2.a; + axis20.y = transform2.e; + axis20.z = transform2.i; + if (!testMainAxis(box1, box2, axis20, 3, vectorToBox1)) return false; + axis21.x = transform2.b; + axis21.y = transform2.f; + axis21.z = transform2.j; + if (!testMainAxis(box1, box2, axis21, 4, vectorToBox1)) return false; + axis22.x = transform2.c; + axis22.y = transform2.g; + axis22.z = transform2.k; + if (!testMainAxis(box1, box2, axis22, 5, vectorToBox1)) return false; + + // Проверка производных осей + if (!testDerivedAxis(box1, box2, axis10, axis20, 6, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis21, 7, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis22, 8, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis11, axis20, 9, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis21, 10, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis22, 11, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis12, axis20, 12, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis21, 13, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis22, 14, vectorToBox1)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта грани одного бокса с гранью/ребром/вершиной другого. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param faceAxisIdx индекс оси первого бокса, перпендикулярной грани, с которой произошло столкновение + * @param contactInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var swapNormal:Boolean = false; + if (faceAxisIdx > 2) { + // Столкновение с гранью второго бокса. Для дальнейших расчётов боксы меняются местами, + // но нормаль контакта всё равно должна быть направлена в сторону первоначального box1 + var tmpBox:CollisionBox = box1; + box1 = box2; + box2 = tmpBox; + vectorToBox1.x = -vectorToBox1.x; + vectorToBox1.y = -vectorToBox1.y; + vectorToBox1.z = -vectorToBox1.z; + faceAxisIdx -= 3; + swapNormal = true; + } + + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + var colAxis:Vector3 = contact.normal; + + transform1.getAxis(faceAxisIdx, colAxis); + var negativeFace:Boolean = colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z > 0; + + var code:int = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box1.excludedFaces) != 0) return false; + + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + // Ищем ось второго бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + transform2.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisDot = dot; + incidentAxisIdx = axisIdx; + } + } + // Получаем список вершин грани второго бокса, переводим их в систему координат первого бокса и выполняем обрезку + // по грани первого бокса. Таким образом получается список потенциальных точек контакта. + transform2.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box2.hs, incidentAxisIdx, incidentAxisDot < 0, points1); + + // TODO: Вычислить результирующую матрицу, затем преобразовать векторы за один заход + transform2.transformVectorsN(points1, points2, 4); + transform1.transformVectorsInverseN(points2, points1, 4); + + var pnum:int = clip(box1.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points1[i]; + if ((pen = getPointBoxPenetration(box1.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + var cp:ContactPoint = tmpPoints[pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = transform1.a*v.x + transform1.b*v.y + transform1.c*v.z + transform1.d; + cpPos.y = transform1.e*v.x + transform1.f*v.y + transform1.g*v.z + transform1.h; + cpPos.z = transform1.i*v.x + transform1.j*v.y + transform1.k*v.z + transform1.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + + cp.penetration = pen; + } + } + if (swapNormal) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (pcount > 4) { + reducePoints(); + } + for (i = 0; i < pcount; i++) { + cp = contact.points[i]; + cp.copyFrom(tmpPoints[i]); + } + contact.pcount = pcount; + return true; + } + + /** + * + * @param contactInfo + */ + private function reducePoints():void { + var i:int; + var minIdx:int; + var cp1:ContactPoint; + var cp2:ContactPoint; + while (pcount > 4) { + var minLen:Number = 1e10; + var p1:Vector3 = (tmpPoints[int(pcount - 1)] as ContactPoint).pos; + var p2:Vector3; + for (i = 0; i < pcount; i++) { + p2 = (tmpPoints[i] as ContactPoint).pos; + var dx:Number = p2.x - p1.x; + var dy:Number = p2.y - p1.y; + var dz:Number = p2.z - p1.z; + var len:Number = dx*dx + dy*dy + dz*dz; + if (len < minLen) { + minLen = len; + minIdx = i; + } + p1 = p2; + } + var ii:int = minIdx == 0 ? (pcount - 1) : (minIdx - 1); + cp1 = tmpPoints[ii]; + cp2 = tmpPoints[minIdx]; + p1 = cp1.pos; + p2 = cp2.pos; + p2.x = 0.5*(p1.x + p2.x); + p2.y = 0.5*(p1.y + p2.y); + p2.z = 0.5*(p1.z + p2.z); + cp2.penetration = 0.5*(cp1.penetration + cp2.penetration); + if (minIdx > 0) { + for (i = minIdx; i < pcount; i++) { + tmpPoints[int(i - 1)] = tmpPoints[i]; + } + tmpPoints[int(pcount - 1)] = cp1; + } + pcount--; + } + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clip(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param contactInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):void { + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + transform1.getAxis(axisIdx1, axis10); + transform2.getAxis(axisIdx2, axis20); + + var colAxis:Vector3 = contact.normal; + + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону первого бокса + if (colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + + var tx:Number = box1.hs.x; + var ty:Number = box1.hs.y; + var tz:Number = box1.hs.z; + + var x2:Number = box2.hs.x; + var y2:Number = box2.hs.y; + var z2:Number = box2.hs.z; + // x + if (axisIdx1 == 0) { + tx = 0; + halfLen1 = box1.hs.x; + } else { + if (colAxis.x*transform1.a + colAxis.y*transform1.e + colAxis.z*transform1.i > 0) { + tx = -tx; + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = box2.hs.x; + } else { + if (colAxis.x*transform2.a + colAxis.y*transform2.e + colAxis.z*transform2.i < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + ty = 0; + halfLen1 = box1.hs.y; + } else { + if (colAxis.x*transform1.b + colAxis.y*transform1.f + colAxis.z*transform1.j > 0) { + ty = -ty; + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = box2.hs.y; + } else { + if (colAxis.x*transform2.b + colAxis.y*transform2.f + colAxis.z*transform2.j < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + tz = 0; + halfLen1 = box1.hs.z; + } else { + if (colAxis.x*transform1.c + colAxis.y*transform1.g + colAxis.z*transform1.k > 0) { + tz = -tz; + } + } + if (axisIdx2 == 2) { + z2 = 0; + halfLen2 = box2.hs.z; + } else { + if (colAxis.x*transform2.c + colAxis.y*transform2.g + colAxis.z*transform2.k < 0) { + z2 = -z2; + } + } + // Получаем глобальные координаты средних точек рёбер + var x1:Number = transform1.a*tx + transform1.b*ty + transform1.c*tz + transform1.d; + var y1:Number = transform1.e*tx + transform1.f*ty + transform1.g*tz + transform1.h; + var z1:Number = transform1.i*tx + transform1.j*ty + transform1.k*tz + transform1.l; + tx = x2; + ty = y2; + tz = z2; + x2 = transform2.a*tx + transform2.b*ty + transform2.c*tz + transform2.d; + y2 = transform2.e*tx + transform2.f*ty + transform2.g*tz + transform2.h; + z2 = transform2.i*tx + transform2.j*ty + transform2.k*tz + transform2.l; + + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + tx = x2 - x1; + ty = y2 - y1; + tz = z2 - z1; + var c1:Number = axis10.x*tx + axis10.y*ty + axis10.z*tz; + var c2:Number = axis20.x*tx + axis20.y*ty + axis20.z*tz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cp.pos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cp.pos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение боксов вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box1 + * @param box2 + * @param axis + * @param axisIndex + * @param toBox1 + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box1 + * @param box2 + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox1 + * @return + */ + private function testDerivedAxis(box1:CollisionBox, box2:CollisionBox, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия двух боксов вдоль заданной оси. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param axis ось + * @param vectorToBox1 вектор, соединяющий центр второго бокса с центром первого + * @return величина перекрытия боксов вдоль оси + */ + public function overlapOnAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, vectorToBox1:Vector3):Number { + var m:Matrix4 = box1.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box1.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box1.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box1.hs.z; + if (d < 0) d = -d; + projection += d; + + m = box2.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box2.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box2.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box2.hs.z; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox1.x*axis.x + vectorToBox1.y*axis.y + vectorToBox1.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} + import alternativa.physics.math.Vector3; + + class CollisionPointTmp { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + } diff --git a/0.0.7.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxCollider.as.svn-base b/0.0.7.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxCollider.as.svn-base new file mode 100644 index 0000000..0021854 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxCollider.as.svn-base @@ -0,0 +1,427 @@ +package alternativa.physics.collision.colliders { + import __AS3__.vec.Vector; + + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.math.Vector3; + + /** + * + */ + public class BoxCollider implements ICollider { + + /** + * + */ + public function BoxCollider() { + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + return false; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + protected function getFaceVertsByAxis(hs:Vector3, axisIdx:int, negativeFace:Boolean, result:Vector.):void { + var v:Vector3; + switch (axisIdx) { + case 0: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + } + break; + case 1: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + } + break; + case 2: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + } else { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } + break; + } + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxPlaneCollider.as.svn-base b/0.0.7.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxPlaneCollider.as.svn-base new file mode 100644 index 0000000..bb1ed0f --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxPlaneCollider.as.svn-base @@ -0,0 +1,88 @@ +package alternativa.physics.collision.colliders { + import __AS3__.vec.Vector; + + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.math.Vector3; + + public class BoxPlaneCollider implements ICollider { + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var normal:Vector3 = new Vector3(); + + public function BoxPlaneCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var box:RigidBox = body1 as RigidBox; +// var plane:RigidPlane; +// if (box == null) { +// box = body2 as RigidBox; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные координаты вершин бокса +// var sx:Number = box.halfSize.x; +// var sy:Number = box.halfSize.y; +// var sz:Number = box.halfSize.z; +// (verts1[0] as Vector3).reset(-sx, -sy, -sz); +// (verts1[1] as Vector3).reset(sx, -sy, -sz); +// (verts1[2] as Vector3).reset(sx, sy, -sz); +// (verts1[3] as Vector3).reset(-sx, sy, -sz); +// (verts1[4] as Vector3).reset(-sx, -sy, sz); +// (verts1[5] as Vector3).reset(sx, -sy, sz); +// (verts1[6] as Vector3).reset(sx, sy, sz); +// (verts1[7] as Vector3).reset(-sx, sy, sz); +// +// box.transform.transformVectors(verts1, verts2); +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// // Проверяем наличие столкновений с каждой вершиной +// collisionInfo.pcount = 0; +// for (var i:int = 0; i < 8; i++) { +// // Вершина добавляется в список точек столкновения, если лежит под плоскостью +// var dist:Number = (verts2[i] as Vector3).dot(normal); +// if (dist < offset) { +// var cp:ContactPoint; +// if (collisionInfo.pcount == collisionInfo.points.length) { +// cp = new ContactPoint(); +// collisionInfo.points[collisionInfo.pcount] = cp; +// } else { +// cp = collisionInfo.points[collisionInfo.pcount]; +// } +// cp.pos.copy(verts2[i]); +// cp.r1.diff(cp.pos, box.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); +// cp.penetration = offset - dist; +// collisionInfo.pcount++; +// } +// } +// if (collisionInfo.pcount > 0) { +// collisionInfo.body1 = box; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// return true; +// } + return false; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxRectCollider.as.svn-base b/0.0.7.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxRectCollider.as.svn-base new file mode 100644 index 0000000..35f904e --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxRectCollider.as.svn-base @@ -0,0 +1,568 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.physics.math.Matrix4; + import alternativa.physics.math.Vector3; + use namespace altphysics; + + /** + * + */ + public class BoxRectCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + + private var vectorToBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxRectCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, rect, vectorToBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + if (!findEdgesIntersection(box, rect, vectorToBox, int(bestAxisIndex/2), bestAxisIndex%2, contact)) { + return false; + } + } + contact.body1 = box.body; + contact.body2 = rect.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + // Вектор из центра прямоугольника в центр бокса + vectorToBox.x = boxTransform.d - rectTransform.d; + vectorToBox.y = boxTransform.h - rectTransform.h; + vectorToBox.z = boxTransform.l - rectTransform.l; + + // Проверка пересечения по нормали прямоугольника + rectTransform.getAxis(2, axis22); + if (!testMainAxis(box, rect, axis22, 0, vectorToBox)) return false; + + // Проверка пересечения по основным осям бокса + boxTransform.getAxis(0, axis10); + if (!testMainAxis(box, rect, axis10, 1, vectorToBox)) return false; + boxTransform.getAxis(1, axis11); + if (!testMainAxis(box, rect, axis11, 2, vectorToBox)) return false; + boxTransform.getAxis(2, axis12); + if (!testMainAxis(box, rect, axis12, 3, vectorToBox)) return false; + + // Получаем направляющие рёбер прямоугольника + rectTransform.getAxis(0, axis20); + rectTransform.getAxis(1, axis21); + + // Проверка производных осей + if (!testDerivedAxis(box, rect, axis10, axis20, 4, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis10, axis21, 5, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis11, axis20, 6, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis11, axis21, 7, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis12, axis20, 8, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis12, axis21, 9, vectorToBox)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта бокса с прямоугольником. + * + * @param box бокс + * @param rect прямоугольник + * @param vectorToBox вектор, направленный из центра прямоугольника в центр бокса + * @param faceAxisIdx индекс оси, идентифицирующей полскость столкновения (грань бокса или полскость прямоугольника) + * @param colInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var pnum:int; + var i:int; + var v:Vector3; + var cp:ContactPoint; + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + var colAxis:Vector3 = contact.normal; + + var negativeFace:Boolean; + var code:int; + + if (faceAxisIdx == 0) { + // Столкновение с плоскостью прямоугольника + + // Проверим положение бокса относительно плоскости прямоугольника + colAxis.x = rectTransform.c; + colAxis.y = rectTransform.g; + colAxis.z = rectTransform.k; + +// var offset:Number = colAxis.x*rectTransform.d + colAxis.y*rectTransform.h + colAxis.z*rectTransform.l; +// if (bbPos.vDot(colAxis) < offset) return false; + + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisIdx = axisIdx; + incidentAxisDot = dot; + } + } + negativeFace = incidentAxisDot > 0; + + code = 1 << (incidentAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + + // Получаем список вершин грани бокса, переводим их в систему координат прямоугольника и выполняем обрезку + // по прямоугольнику. Таким образом получается список потенциальных точек контакта. + boxTransform.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box.hs, incidentAxisIdx, negativeFace, points1); + boxTransform.transformVectorsN(points1, points2, 4); + rectTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByRect(rect.hs); + // Проверяем каждую потенциальную точку на принадлежность нижней полуплоскости прямоугольника и добавляем такие точки в список контактов + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if (v.z < epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = -v.z; + var cpPos:Vector3 = cp.pos; + cpPos.x = rectTransform.a*v.x + rectTransform.b*v.y + rectTransform.c*v.z + rectTransform.d; + cpPos.y = rectTransform.e*v.x + rectTransform.f*v.y + rectTransform.g*v.z + rectTransform.h; + cpPos.z = rectTransform.i*v.x + rectTransform.j*v.y + rectTransform.k*v.z + rectTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } else { + // Столкновение с гранью бокса + faceAxisIdx--; + boxTransform.getAxis(faceAxisIdx, colAxis); + negativeFace = colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z > 0; + + code = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) { + return false; + } + + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (rectTransform.c*colAxis.x + rectTransform.g*colAxis.y + rectTransform.k*colAxis.z < 0) return false; + + getFaceVertsByAxis(rect.hs, 2, false, points1); + rectTransform.transformVectorsN(points1, points2, 4); + boxTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if ((pen = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = pen; + cpPos = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } + return true; + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * + * @param hs + * @return + */ + private function clipByRect(hs:Vector3):int { + var pnum:int = 4; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box первый бокс + * @param rect второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param colInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + boxTransform.getAxis(axisIdx1, axis10); + rectTransform.getAxis(axisIdx2, axis20); + var colAxis:Vector3 = contact.normal; + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону бокса + if (colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + var vx:Number = box.hs.x; + var vy:Number = box.hs.y; + var vz:Number = box.hs.z; + var x2:Number = rect.hs.x; + var y2:Number = rect.hs.y; + var z2:Number = rect.hs.z; + // x + if (axisIdx1 == 0) { + vx = 0; + halfLen1 = box.hs.x; + } else { + if (boxTransform.a*colAxis.x + boxTransform.e*colAxis.y + boxTransform.i*colAxis.z > 0) { + vx = -vx; + if ((box.excludedFaces & 2) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 1) != 0) { + return false; + } + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = rect.hs.x; + } else { + if (rectTransform.a*colAxis.x + rectTransform.e*colAxis.y + rectTransform.i*colAxis.z < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + vy = 0; + halfLen1 = box.hs.y; + } else { + if (boxTransform.b*colAxis.x + boxTransform.f*colAxis.y + boxTransform.j*colAxis.z > 0) { + vy = -vy; + if ((box.excludedFaces & 8) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 4) != 0) { + return false; + } + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = rect.hs.y; + } else { + if (rectTransform.b*colAxis.x + rectTransform.f*colAxis.y + rectTransform.j*colAxis.z < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + vz = 0; + halfLen1 = box.hs.z; + } else { + if (boxTransform.c*colAxis.x + boxTransform.g*colAxis.y + boxTransform.k*colAxis.z > 0) { + vz = -vz; + if ((box.excludedFaces & 32) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 16) != 0) { + return false; + } + } + } + // Получаем глобальные координаты средних точек рёбер + + var x1:Number = boxTransform.a*vx + boxTransform.b*vy + boxTransform.c*vz + boxTransform.d; + var y1:Number = boxTransform.e*vx + boxTransform.f*vy + boxTransform.g*vz + boxTransform.h; + var z1:Number = boxTransform.i*vx + boxTransform.j*vy + boxTransform.k*vz + boxTransform.l; + vx = x2; + vy = y2; + vz = z2; + x2 = rectTransform.a*vx + rectTransform.b*vy + rectTransform.c*vz + rectTransform.d; + y2 = rectTransform.e*vx + rectTransform.f*vy + rectTransform.g*vz + rectTransform.h; + z2 = rectTransform.i*vx + rectTransform.j*vy + rectTransform.k*vz + rectTransform.l; + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + vx = x2 - x1; + vy = y2 - y1; + vz = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cpPos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cpPos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cpPos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var v:Vector3 = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + return true; + } + + /** + * Проверяет пересечение вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box + * @param rect + * @param axis + * @param axisIndex + * @param vectorToBox + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param rect + * @param axis1 + * @param axis2 + * @param axisIndex + * @param vectorToBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, rect:CollisionRect, axis1:Vector3, axis2:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия вдоль заданной оси. + * + * @param box бокс + * @param rect прямоугольник + * @param axis ось + * @param vectorToBox вектор, соединяющий центр прямоугольника с центром бокса + * @return величина перекрытия вдоль оси + */ + public function overlapOnAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, vectorToBox:Vector3):Number { + var m:Matrix4 = box.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box.hs.z; + if (d < 0) d = -d; + projection += d; + + m = rect.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*rect.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*rect.hs.y; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox.x*axis.x + vectorToBox.y*axis.y + vectorToBox.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxSphereCollider.as.svn-base b/0.0.7.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxSphereCollider.as.svn-base new file mode 100644 index 0000000..2ba3ce2 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxSphereCollider.as.svn-base @@ -0,0 +1,114 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.math.Vector3; + use namespace altphysics; + /** + * + */ + public class BoxSphereCollider implements ICollider { + + private var center:Vector3 = new Vector3(); + private var closestPt:Vector3 = new Vector3(); + + private var bPos:Vector3 = new Vector3(); + private var sPos:Vector3 = new Vector3(); + + /** + * + */ + public function BoxSphereCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + // Зафиксированно столкновение + contact.body1 = sphere.body; + contact.body2 = box.body; + contact.normal.vCopy(closestPt).vTransformBy4(box.transform).vSubtract(sPos).vNormalize().vReverse(); + contact.pcount = 1; + + var cp:ContactPoint = contact.points[0]; + cp.penetration = sphere.r - Math.sqrt(distSqr); + cp.pos.vCopy(contact.normal).vScale(-sphere.r).vAdd(sPos); + cp.r1.vDiff(cp.pos, sPos); + cp.r2.vDiff(cp.pos, bPos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxTriangleCollider.as.svn-base b/0.0.7.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxTriangleCollider.as.svn-base new file mode 100644 index 0000000..b22db8e --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxTriangleCollider.as.svn-base @@ -0,0 +1,718 @@ +package alternativa.physics.collision.colliders { + import __AS3__.vec.Vector; + + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionTriangle; + import alternativa.physics.math.Matrix4; + import alternativa.physics.math.Vector3; + + /** + * + */ + public class BoxTriangleCollider extends BoxCollider { + + public var epsilon:Number = 0.001; + + private var bestAxisIndex:int; + private var minOverlap:Number; + private var toBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var colNormal:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxTriangleCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, tri, toBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + if (!findEdgesIntersection(box, tri, toBox, bestAxisIndex%3, int(bestAxisIndex/3), contact)) return false; + } + + contact.body1 = box.body; + contact.body2 = tri.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + toBox.x = boxTransform.d - triTransform.d; + toBox.y = boxTransform.h - triTransform.h; + toBox.z = boxTransform.l - triTransform.l; + + minOverlap = 1e308; + + // Сначала проверяется нормаль треугольника + axis.x = triTransform.c; + axis.y = triTransform.g; + axis.z = triTransform.k; + if (!testMainAxis(box, tri, axis, 0, toBox)) return false; + + // Проверка основных осей бокса + axis10.x = boxTransform.a; + axis10.y = boxTransform.e; + axis10.z = boxTransform.i; + if (!testMainAxis(box, tri, axis10, 1, toBox)) return false; + axis11.x = boxTransform.b; + axis11.y = boxTransform.f; + axis11.z = boxTransform.j; + if (!testMainAxis(box, tri, axis11, 2, toBox)) return false; + axis12.x = boxTransform.c; + axis12.y = boxTransform.g; + axis12.z = boxTransform.k; + if (!testMainAxis(box, tri, axis12, 3, toBox)) return false; + + // Проверка производных осей + // TODO: заменить вычисления векторных произведений инлайнами + var v:Vector3 = tri.e0; + axis20.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis20.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis20.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis20, 4, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis20, 5, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis20, 6, toBox)) return false; + + v = tri.e1; + axis21.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis21.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis21.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis21, 7, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis21, 8, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis21, 9, toBox)) return false; + + v = tri.e2; + axis22.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis22.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis22.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis22, 10, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis22, 11, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis22, 12, toBox)) return false; + + return true; + } + + /** + * Тестирует пересечение примитивов вдоль заданной оси. + * + * @param box бокс + * @param tri треугольник + * @param axis ось + * @param axisIndex индекс оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return true, если примитивы имеют перекрытие проекций вдоль указанной оси, иначе false + */ + private function testMainAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, axisIndex:int, toBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param tri + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, tri:CollisionTriangle, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Рассчитывает величину перекрытия проекций бокса и треугольника на заданную ось. + * + * @param box бокс + * @param tri треугольник + * @param axis единичный направляющий вектор оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return величина перекрытия. Положительное значение указывает на наличие перекрытия, нулевое или отрицательное значение указывает на отсутствие перекрытия. + */ + private function overlapOnAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, toBox:Vector3):Number { + var t:Matrix4 = box.transform; + var projection:Number = (t.a*axis.x + t.e*axis.y + t.i*axis.z)*box.hs.x; + if (projection < 0) projection = -projection; + var d:Number = (t.b*axis.x + t.f*axis.y + t.j*axis.z)*box.hs.y; + projection += d < 0 ? -d : d; + d = (t.c*axis.x + t.g*axis.y + t.k*axis.z)*box.hs.z; + projection += d < 0 ? -d : d; + + var vectorProjection:Number = toBox.x*axis.x + toBox.y*axis.y + toBox.z*axis.z; + // Для оптимизации ось преобразуется в систему треугольника, а не его вершины в мировую систему + t = tri.transform; + var ax:Number = t.a*axis.x + t.e*axis.y + t.i*axis.z; + var ay:Number = t.b*axis.x + t.f*axis.y + t.j*axis.z; + var az:Number = t.c*axis.x + t.g*axis.y + t.k*axis.z; + var max:Number = 0; + if (vectorProjection < 0) { + vectorProjection = -vectorProjection; + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d < max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d < max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d < max) max = d; + max = -max; + } else { + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d > max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d > max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d > max) max = d; + } + + return projection + max - vectorProjection; + } + + /** + * Определяет точки контакта бокса и треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function findFaceContactPoints(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIndex:int, contact:Contact):Boolean { + if (faceAxisIndex == 0) { + // Столкновение с плоскостью треугольника + return getBoxToTriContact(box, tri, toBox, contact); + } else { + // Столкновение с гранью бокса + return getTriToBoxContact(box, tri, toBox, faceAxisIndex, contact); + } + } + + /** + * Определяет точки контакта бокса с плоскостью треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getBoxToTriContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + colNormal.x = triTransform.c; + colNormal.y = triTransform.g; + colNormal.z = triTransform.k; + + var over:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + if (!over) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var incAxisDot:Number = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colNormal.x + axis.y*colNormal.y + axis.z*colNormal.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incAxisDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Обрезка грани + var negativeFace:Boolean = incAxisDot > 0; + + var code:int = 1 << (incFaceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + getFaceVertsByAxis(box.hs, incFaceAxisIdx, negativeFace, points1); + boxTransform.transformVectorsN(points1, points2, 4); + triTransform.transformVectorsInverseN(points2, points1, 4); + var pnum:int = clipByTriangle(tri); + // Среди конечного списка точек определяются лежащие под плоскостью треугольника + var cp:ContactPoint; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points2[i]; + if ((over && v.z < 0) || (!over && v.z > 0)) { + cp = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z + triTransform.d; + cpPos.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z + triTransform.h; + cpPos.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z + triTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = over ? -v.z : v.z; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Оперделяет точки контакта треугольника с гранью бокса. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getTriToBoxContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + faceAxisIdx--; + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + boxTransform.getAxis(faceAxisIdx, colNormal); + var negativeFace:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + + var code:int = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + if (!negativeFace) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + var v:Vector3 = points1[0]; + v.x = tri.v0.x; + v.y = tri.v0.y; + v.z = tri.v0.z; + + v = points1[1]; + v.x = tri.v1.x; + v.y = tri.v1.y; + v.z = tri.v1.z; + + v = points1[2]; + v.x = tri.v2.x; + v.y = tri.v2.y; + v.z = tri.v2.z; + + triTransform.transformVectorsN(points1, points2, 3); + boxTransform.transformVectorsInverseN(points2, points1, 3); + + var pnum:int = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность боксу и добавляем такие точки в список контактов + var penetration:Number; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + v = points1[i]; + penetration = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace); + if (penetration > -epsilon) { + var cp:ContactPoint = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = penetration; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Вычисляет величину проникновения точки в бокс. + * + * @param hs вектор половинных размеров бокса + * @param p точка в системе координат бокса + * @param axisIndex индекс оси + * @param negativeFace если true, проверяется нижняя грань + * @return величина проникновения точки в бокс + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, negativeFace:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (negativeFace) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (negativeFace) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (negativeFace) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, вершины которой заданы в списке points1. Результат сохраняется в этом же списке. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 3; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Обрезает грань прямоугольника сторонами треугольника. Обрезка выполняется в системе координат и в проекции на + * плоскость треугольника. Входные вершины грани в количестве четырёх штук должны находиться в списке points1. + * Конечные вершины сохраняются в списке points2. + * + * @param tri треугольник + * @return количество врешин в конечном списке + */ + private function clipByTriangle(tri:CollisionTriangle):int { + var vnum:int = 4; + vnum = clipByLine(tri.v0, tri.e0, points1, vnum, points2); + if (vnum == 0) return 0; + vnum = clipByLine(tri.v1, tri.e1, points2, vnum, points1); + if (vnum == 0) return 0; + return clipByLine(tri.v2, tri.e2, points1, vnum, points2);; + } + + /** + * Обрезает полигон указанной прямой. + * + * @param linePoint точка на прямой + * @param lineDir единичный направляющий вектор прямой + * @param verticesIn список вершин исходного полигона + * @param vnum количество вершин исходного полигона + * @param verticesOut список, куда будут записаны вершины конечного полигона + * @return количество вершин конечного полигона + */ + private function clipByLine(linePoint:Vector3, lineDir:Vector3, verticesIn:Vector., vnum:int, verticesOut:Vector.):int { + var nx:Number = -lineDir.y; + var ny:Number = lineDir.x; + var offset:Number = linePoint.x*nx + linePoint.y*ny; + var v1:Vector3 = verticesIn[int(vnum - 1)]; + var offset1:Number = v1.x*nx + v1.y*ny; + var t:Number; + var v:Vector3; + var num:int = 0; + for (var i:int = 0; i < vnum; i++) { + var v2:Vector3 = verticesIn[i]; + var offset2:Number = v2.x*nx + v2.y*ny; + if (offset1 < offset) { + // Первая точка ребра во внешней полуплоскости + if (offset2 > offset) { + // Вторая точка ребра во внутренней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } else { + // Первая точка ребра во внутренней полуплоскости. Добавляем её в конечный список. + v = verticesOut[num]; + v.x = v1.x; + v.y = v1.y; + v.z = v1.z; + num++; + if (offset2 < offset) { + // Вторая точка ребра во внешней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } + v1 = v2; + offset1 = offset2; + } + return num; + } + + /** + * + * @param box + * @param tri + * @param toBox + * @param boxAxisIdx + * @param triAxisIdx + * @param contact + */ + private function findEdgesIntersection(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, boxAxisIdx:int, triAxisIdx:int, contact:Contact):Boolean { + // Определение точки и направляющего вектора ребра треугольника + var tmpx1:Number; + var tmpy1:Number; + var tmpz1:Number; + var tmpx2:Number; + var tmpy2:Number; + var tmpz2:Number; + switch (triAxisIdx) { + case 0: + tmpx1 = tri.e0.x; + tmpy1 = tri.e0.y; + tmpz1 = tri.e0.z; + tmpx2 = tri.v0.x; + tmpy2 = tri.v0.y; + tmpz2 = tri.v0.z; + break; + case 1: + tmpx1 = tri.e1.x; + tmpy1 = tri.e1.y; + tmpz1 = tri.e1.z; + tmpx2 = tri.v1.x; + tmpy2 = tri.v1.y; + tmpz2 = tri.v1.z; + break; + case 2: + tmpx1 = tri.e2.x; + tmpy1 = tri.e2.y; + tmpz1 = tri.e2.z; + tmpx2 = tri.v2.x; + tmpy2 = tri.v2.y; + tmpz2 = tri.v2.z; + break; + } + var triTransform:Matrix4 = tri.transform; + axis20.x = triTransform.a*tmpx1 + triTransform.b*tmpy1 + triTransform.c*tmpz1; + axis20.y = triTransform.e*tmpx1 + triTransform.f*tmpy1 + triTransform.g*tmpz1; + axis20.z = triTransform.i*tmpx1 + triTransform.j*tmpy1 + triTransform.k*tmpz1; + var x2:Number = triTransform.a*tmpx2 + triTransform.b*tmpy2 + triTransform.c*tmpz2 + triTransform.d; + var y2:Number = triTransform.e*tmpx2 + triTransform.f*tmpy2 + triTransform.g*tmpz2 + triTransform.h; + var z2:Number = triTransform.i*tmpx2 + triTransform.j*tmpy2 + triTransform.k*tmpz2 + triTransform.l; + + // Определение нормали контакта, точки и направляющего вектора ребра бокса + var boxTransform:Matrix4 = box.transform; + boxTransform.getAxis(boxAxisIdx, axis10); + + // Нормаль контакта + var v:Vector3 = contact.normal; + v.x = axis10.y*axis20.z - axis10.z*axis20.y; + v.y = axis10.z*axis20.x - axis10.x*axis20.z; + v.z = axis10.x*axis20.y - axis10.y*axis20.x; + k = 1/Math.sqrt(v.x*v.x + v.y*v.y + v.z*v.z); + v.x *= k; + v.y *= k; + v.z *= k; + // Разворот нормали в сторону бокса + if (v.x*toBox.x + v.y*toBox.y + v.z*toBox.z < 0) { + v.x = -v.x; + v.y = -v.y; + v.z = -v.z; + } + + var boxHalfLen:Number; + tmpx1 = box.hs.x; + tmpy1 = box.hs.y; + tmpz1 = box.hs.z; + // X + if (boxAxisIdx == 0) { + tmpx1 = 0; + boxHalfLen = box.hs.x; + } else { + if (boxTransform.a*v.x + boxTransform.e*v.y + boxTransform.i*v.z > 0) { + tmpx1 = -tmpx1; + if ((box.excludedFaces & 2) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 1) != 0) { + return false; + } + } + } + // Y + if (boxAxisIdx == 1) { + tmpy1 = 0; + boxHalfLen = box.hs.y; + } else { + if (boxTransform.b*v.x + boxTransform.f*v.y + boxTransform.j*v.z > 0) { + tmpy1 = -tmpy1; + if ((box.excludedFaces & 8) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 4) != 0) { + return false; + } + } + } + // Z + if (boxAxisIdx == 2) { + tmpz1 = 0; + boxHalfLen = box.hs.z; + } else { + if (boxTransform.c*v.x + boxTransform.g*v.y + boxTransform.k*v.z > 0) { + tmpz1 = -tmpz1; + if ((box.excludedFaces & 32) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 16) != 0) { + return false; + } + } + } + var x1:Number = boxTransform.a*tmpx1 + boxTransform.b*tmpy1 + boxTransform.c*tmpz1 + boxTransform.d; + var y1:Number = boxTransform.e*tmpx1 + boxTransform.f*tmpy1 + boxTransform.g*tmpz1 + boxTransform.h; + var z1:Number = boxTransform.i*tmpx1 + boxTransform.j*tmpy1 + boxTransform.k*tmpz1 + boxTransform.l; + + // Находим точку пересечения рёбер, решая систему уравнений + // axis10 - направляющий вектор ребра бокса + // x1, y1, z1 - средняя точка ребра бокса + // axis20 - направляющий вектор ребра треугольника + // x2, y2, z2 - начальная точка ребра треугольника + var k:Number = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + var vx:Number = x2 - x1; + var vy:Number = y2 - y1; + var vz:Number = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + + // Запись данных о контакте + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + v = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + v.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + v.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + v.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = v.x - boxTransform.d; + r.y = v.y - boxTransform.h; + r.z = v.z - boxTransform.l; + r = cp.r2; + r.x = v.x - triTransform.d; + r.y = v.y - triTransform.h; + r.z = v.z - triTransform.l; + return true; + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/colliders/.svn/text-base/SpherePlaneCollider.as.svn-base b/0.0.7.0/src/alternativa/physics/collision/colliders/.svn/text-base/SpherePlaneCollider.as.svn-base new file mode 100644 index 0000000..1ac5d35 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/colliders/.svn/text-base/SpherePlaneCollider.as.svn-base @@ -0,0 +1,68 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.math.Vector3; + + /** + * + */ + public class SpherePlaneCollider implements ICollider { + + private var normal:Vector3 = new Vector3(); + + /** + * + */ + public function SpherePlaneCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var sphere:RigidSphere = body1 as RigidSphere; +// var plane:RigidPlane; +// if (sphere == null) { +// sphere = body2 as RigidSphere; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// +// var dist:Number = sphere.state.pos.dot(normal) - offset; +// if (dist > sphere.r) return false; +// +// collisionInfo.body1 = sphere; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// collisionInfo.pcount = 1; +// +// var cp:ContactPoint = collisionInfo.points[0]; +// cp.penetration = sphere.r - dist; +// cp.pos.copy(normal).reverse().scale(sphere.r).add(sphere.state.pos); +// cp.r1.diff(cp.pos, sphere.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/colliders/.svn/text-base/SphereSphereCollider.as.svn-base b/0.0.7.0/src/alternativa/physics/collision/colliders/.svn/text-base/SphereSphereCollider.as.svn-base new file mode 100644 index 0000000..a90fabb --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/colliders/.svn/text-base/SphereSphereCollider.as.svn-base @@ -0,0 +1,70 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.math.Vector3; + use namespace altphysics; + + public class SphereSphereCollider implements ICollider { + + private var p1:Vector3 = new Vector3(); + private var p2:Vector3 = new Vector3(); + + public function SphereSphereCollider() { + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var s1:CollisionSphere; + var s2:CollisionSphere; + if (prim1.body != null) { + s1 = prim1 as CollisionSphere; + s2 = prim2 as CollisionSphere; + } else { + s1 = prim2 as CollisionSphere; + s2 = prim1 as CollisionSphere; + } + + s1.transform.getAxis(3, p1); + s2.transform.getAxis(3, p2); + var dx:Number = p1.x - p2.x; + var dy:Number = p1.y - p2.y; + var dz:Number = p1.z - p2.z; + var len:Number = dx*dx + dy*dy + dz*dz; + var sum:Number = s1.r + s2.r; + if (len > sum*sum) return false; + len = Math.sqrt(len); + dx /= len; + dy /= len; + dz /= len; + + contact.body1 = s1.body; + contact.body2 = s2.body; + contact.normal.x = dx; + contact.normal.y = dy; + contact.normal.z = dz; + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = sum - len; + cp.pos.x = p1.x - dx*s1.r; + cp.pos.y = p1.y - dy*s1.r; + cp.pos.z = p1.z - dz*s1.r; + cp.r1.vDiff(cp.pos, p1); + cp.r2.vDiff(cp.pos, p2); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/colliders/BoxBoxCollider.as b/0.0.7.0/src/alternativa/physics/collision/colliders/BoxBoxCollider.as new file mode 100644 index 0000000..b003e98 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/colliders/BoxBoxCollider.as @@ -0,0 +1,589 @@ +package alternativa.physics.collision.colliders { + import __AS3__.vec.Vector; + + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.math.Matrix4; + import alternativa.physics.math.Vector3; + + use namespace altphysics; + + /** + * Расчитывает точки контакта двух боксов. Нормаль контакта направляется в сторону бокса с меньшим ID. + */ + public class BoxBoxCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + private var vectorToBox1:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var bestAxisIndex:int; + private var minOverlap:Number; + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + private var tmpPoints:Vector. = new Vector.(8, true); + private var pcount:int; + + /** + * + */ + public function BoxBoxCollider() { + for (var i:int = 0; i < 8; i++) { + tmpPoints[i] = new ContactPoint(); + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + + if (bestAxisIndex < 6) { + // Контакт грань-(грань|ребро|вершина) + if (!findFaceContactPoints(box1, box2, vectorToBox1, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 6; + findEdgesIntersection(box1, box2, vectorToBox1, int(bestAxisIndex/3), bestAxisIndex%3, contact); + } + contact.body1 = box1.body; + contact.body2 = box2.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + // Вектор из центра второго бокса в центр первого + vectorToBox1.x = transform1.d - transform2.d; + vectorToBox1.y = transform1.h - transform2.h; + vectorToBox1.z = transform1.l - transform2.l; + + // Проверка пересечения по основным осям + axis10.x = transform1.a; + axis10.y = transform1.e; + axis10.z = transform1.i; + if (!testMainAxis(box1, box2, axis10, 0, vectorToBox1)) return false; + axis11.x = transform1.b; + axis11.y = transform1.f; + axis11.z = transform1.j; + if (!testMainAxis(box1, box2, axis11, 1, vectorToBox1)) return false; + axis12.x = transform1.c; + axis12.y = transform1.g; + axis12.z = transform1.k; + if (!testMainAxis(box1, box2, axis12, 2, vectorToBox1)) return false; + + axis20.x = transform2.a; + axis20.y = transform2.e; + axis20.z = transform2.i; + if (!testMainAxis(box1, box2, axis20, 3, vectorToBox1)) return false; + axis21.x = transform2.b; + axis21.y = transform2.f; + axis21.z = transform2.j; + if (!testMainAxis(box1, box2, axis21, 4, vectorToBox1)) return false; + axis22.x = transform2.c; + axis22.y = transform2.g; + axis22.z = transform2.k; + if (!testMainAxis(box1, box2, axis22, 5, vectorToBox1)) return false; + + // Проверка производных осей + if (!testDerivedAxis(box1, box2, axis10, axis20, 6, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis21, 7, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis22, 8, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis11, axis20, 9, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis21, 10, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis22, 11, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis12, axis20, 12, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis21, 13, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis22, 14, vectorToBox1)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта грани одного бокса с гранью/ребром/вершиной другого. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param faceAxisIdx индекс оси первого бокса, перпендикулярной грани, с которой произошло столкновение + * @param contactInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var swapNormal:Boolean = false; + if (faceAxisIdx > 2) { + // Столкновение с гранью второго бокса. Для дальнейших расчётов боксы меняются местами, + // но нормаль контакта всё равно должна быть направлена в сторону первоначального box1 + var tmpBox:CollisionBox = box1; + box1 = box2; + box2 = tmpBox; + vectorToBox1.x = -vectorToBox1.x; + vectorToBox1.y = -vectorToBox1.y; + vectorToBox1.z = -vectorToBox1.z; + faceAxisIdx -= 3; + swapNormal = true; + } + + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + var colAxis:Vector3 = contact.normal; + + transform1.getAxis(faceAxisIdx, colAxis); + var negativeFace:Boolean = colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z > 0; + + var code:int = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box1.excludedFaces) != 0) return false; + + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + // Ищем ось второго бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + transform2.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisDot = dot; + incidentAxisIdx = axisIdx; + } + } + // Получаем список вершин грани второго бокса, переводим их в систему координат первого бокса и выполняем обрезку + // по грани первого бокса. Таким образом получается список потенциальных точек контакта. + transform2.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box2.hs, incidentAxisIdx, incidentAxisDot < 0, points1); + + // TODO: Вычислить результирующую матрицу, затем преобразовать векторы за один заход + transform2.transformVectorsN(points1, points2, 4); + transform1.transformVectorsInverseN(points2, points1, 4); + + var pnum:int = clip(box1.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points1[i]; + if ((pen = getPointBoxPenetration(box1.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + var cp:ContactPoint = tmpPoints[pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = transform1.a*v.x + transform1.b*v.y + transform1.c*v.z + transform1.d; + cpPos.y = transform1.e*v.x + transform1.f*v.y + transform1.g*v.z + transform1.h; + cpPos.z = transform1.i*v.x + transform1.j*v.y + transform1.k*v.z + transform1.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + + cp.penetration = pen; + } + } + if (swapNormal) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (pcount > 4) { + reducePoints(); + } + for (i = 0; i < pcount; i++) { + cp = contact.points[i]; + cp.copyFrom(tmpPoints[i]); + } + contact.pcount = pcount; + return true; + } + + /** + * + * @param contactInfo + */ + private function reducePoints():void { + var i:int; + var minIdx:int; + var cp1:ContactPoint; + var cp2:ContactPoint; + while (pcount > 4) { + var minLen:Number = 1e10; + var p1:Vector3 = (tmpPoints[int(pcount - 1)] as ContactPoint).pos; + var p2:Vector3; + for (i = 0; i < pcount; i++) { + p2 = (tmpPoints[i] as ContactPoint).pos; + var dx:Number = p2.x - p1.x; + var dy:Number = p2.y - p1.y; + var dz:Number = p2.z - p1.z; + var len:Number = dx*dx + dy*dy + dz*dz; + if (len < minLen) { + minLen = len; + minIdx = i; + } + p1 = p2; + } + var ii:int = minIdx == 0 ? (pcount - 1) : (minIdx - 1); + cp1 = tmpPoints[ii]; + cp2 = tmpPoints[minIdx]; + p1 = cp1.pos; + p2 = cp2.pos; + p2.x = 0.5*(p1.x + p2.x); + p2.y = 0.5*(p1.y + p2.y); + p2.z = 0.5*(p1.z + p2.z); + cp2.penetration = 0.5*(cp1.penetration + cp2.penetration); + if (minIdx > 0) { + for (i = minIdx; i < pcount; i++) { + tmpPoints[int(i - 1)] = tmpPoints[i]; + } + tmpPoints[int(pcount - 1)] = cp1; + } + pcount--; + } + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clip(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param contactInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):void { + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + transform1.getAxis(axisIdx1, axis10); + transform2.getAxis(axisIdx2, axis20); + + var colAxis:Vector3 = contact.normal; + + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону первого бокса + if (colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + + var tx:Number = box1.hs.x; + var ty:Number = box1.hs.y; + var tz:Number = box1.hs.z; + + var x2:Number = box2.hs.x; + var y2:Number = box2.hs.y; + var z2:Number = box2.hs.z; + // x + if (axisIdx1 == 0) { + tx = 0; + halfLen1 = box1.hs.x; + } else { + if (colAxis.x*transform1.a + colAxis.y*transform1.e + colAxis.z*transform1.i > 0) { + tx = -tx; + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = box2.hs.x; + } else { + if (colAxis.x*transform2.a + colAxis.y*transform2.e + colAxis.z*transform2.i < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + ty = 0; + halfLen1 = box1.hs.y; + } else { + if (colAxis.x*transform1.b + colAxis.y*transform1.f + colAxis.z*transform1.j > 0) { + ty = -ty; + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = box2.hs.y; + } else { + if (colAxis.x*transform2.b + colAxis.y*transform2.f + colAxis.z*transform2.j < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + tz = 0; + halfLen1 = box1.hs.z; + } else { + if (colAxis.x*transform1.c + colAxis.y*transform1.g + colAxis.z*transform1.k > 0) { + tz = -tz; + } + } + if (axisIdx2 == 2) { + z2 = 0; + halfLen2 = box2.hs.z; + } else { + if (colAxis.x*transform2.c + colAxis.y*transform2.g + colAxis.z*transform2.k < 0) { + z2 = -z2; + } + } + // Получаем глобальные координаты средних точек рёбер + var x1:Number = transform1.a*tx + transform1.b*ty + transform1.c*tz + transform1.d; + var y1:Number = transform1.e*tx + transform1.f*ty + transform1.g*tz + transform1.h; + var z1:Number = transform1.i*tx + transform1.j*ty + transform1.k*tz + transform1.l; + tx = x2; + ty = y2; + tz = z2; + x2 = transform2.a*tx + transform2.b*ty + transform2.c*tz + transform2.d; + y2 = transform2.e*tx + transform2.f*ty + transform2.g*tz + transform2.h; + z2 = transform2.i*tx + transform2.j*ty + transform2.k*tz + transform2.l; + + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + tx = x2 - x1; + ty = y2 - y1; + tz = z2 - z1; + var c1:Number = axis10.x*tx + axis10.y*ty + axis10.z*tz; + var c2:Number = axis20.x*tx + axis20.y*ty + axis20.z*tz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cp.pos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cp.pos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение боксов вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box1 + * @param box2 + * @param axis + * @param axisIndex + * @param toBox1 + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box1 + * @param box2 + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox1 + * @return + */ + private function testDerivedAxis(box1:CollisionBox, box2:CollisionBox, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия двух боксов вдоль заданной оси. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param axis ось + * @param vectorToBox1 вектор, соединяющий центр второго бокса с центром первого + * @return величина перекрытия боксов вдоль оси + */ + public function overlapOnAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, vectorToBox1:Vector3):Number { + var m:Matrix4 = box1.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box1.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box1.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box1.hs.z; + if (d < 0) d = -d; + projection += d; + + m = box2.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box2.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box2.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box2.hs.z; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox1.x*axis.x + vectorToBox1.y*axis.y + vectorToBox1.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} + import alternativa.physics.math.Vector3; + + class CollisionPointTmp { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + } diff --git a/0.0.7.0/src/alternativa/physics/collision/colliders/BoxCollider.as b/0.0.7.0/src/alternativa/physics/collision/colliders/BoxCollider.as new file mode 100644 index 0000000..0021854 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/colliders/BoxCollider.as @@ -0,0 +1,427 @@ +package alternativa.physics.collision.colliders { + import __AS3__.vec.Vector; + + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.math.Vector3; + + /** + * + */ + public class BoxCollider implements ICollider { + + /** + * + */ + public function BoxCollider() { + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + return false; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + protected function getFaceVertsByAxis(hs:Vector3, axisIdx:int, negativeFace:Boolean, result:Vector.):void { + var v:Vector3; + switch (axisIdx) { + case 0: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + } + break; + case 1: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + } + break; + case 2: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + } else { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } + break; + } + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/colliders/BoxPlaneCollider.as b/0.0.7.0/src/alternativa/physics/collision/colliders/BoxPlaneCollider.as new file mode 100644 index 0000000..bb1ed0f --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/colliders/BoxPlaneCollider.as @@ -0,0 +1,88 @@ +package alternativa.physics.collision.colliders { + import __AS3__.vec.Vector; + + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.math.Vector3; + + public class BoxPlaneCollider implements ICollider { + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var normal:Vector3 = new Vector3(); + + public function BoxPlaneCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var box:RigidBox = body1 as RigidBox; +// var plane:RigidPlane; +// if (box == null) { +// box = body2 as RigidBox; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные координаты вершин бокса +// var sx:Number = box.halfSize.x; +// var sy:Number = box.halfSize.y; +// var sz:Number = box.halfSize.z; +// (verts1[0] as Vector3).reset(-sx, -sy, -sz); +// (verts1[1] as Vector3).reset(sx, -sy, -sz); +// (verts1[2] as Vector3).reset(sx, sy, -sz); +// (verts1[3] as Vector3).reset(-sx, sy, -sz); +// (verts1[4] as Vector3).reset(-sx, -sy, sz); +// (verts1[5] as Vector3).reset(sx, -sy, sz); +// (verts1[6] as Vector3).reset(sx, sy, sz); +// (verts1[7] as Vector3).reset(-sx, sy, sz); +// +// box.transform.transformVectors(verts1, verts2); +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// // Проверяем наличие столкновений с каждой вершиной +// collisionInfo.pcount = 0; +// for (var i:int = 0; i < 8; i++) { +// // Вершина добавляется в список точек столкновения, если лежит под плоскостью +// var dist:Number = (verts2[i] as Vector3).dot(normal); +// if (dist < offset) { +// var cp:ContactPoint; +// if (collisionInfo.pcount == collisionInfo.points.length) { +// cp = new ContactPoint(); +// collisionInfo.points[collisionInfo.pcount] = cp; +// } else { +// cp = collisionInfo.points[collisionInfo.pcount]; +// } +// cp.pos.copy(verts2[i]); +// cp.r1.diff(cp.pos, box.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); +// cp.penetration = offset - dist; +// collisionInfo.pcount++; +// } +// } +// if (collisionInfo.pcount > 0) { +// collisionInfo.body1 = box; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// return true; +// } + return false; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/colliders/BoxRectCollider.as b/0.0.7.0/src/alternativa/physics/collision/colliders/BoxRectCollider.as new file mode 100644 index 0000000..35f904e --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/colliders/BoxRectCollider.as @@ -0,0 +1,568 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.physics.math.Matrix4; + import alternativa.physics.math.Vector3; + use namespace altphysics; + + /** + * + */ + public class BoxRectCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + + private var vectorToBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxRectCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, rect, vectorToBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + if (!findEdgesIntersection(box, rect, vectorToBox, int(bestAxisIndex/2), bestAxisIndex%2, contact)) { + return false; + } + } + contact.body1 = box.body; + contact.body2 = rect.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + // Вектор из центра прямоугольника в центр бокса + vectorToBox.x = boxTransform.d - rectTransform.d; + vectorToBox.y = boxTransform.h - rectTransform.h; + vectorToBox.z = boxTransform.l - rectTransform.l; + + // Проверка пересечения по нормали прямоугольника + rectTransform.getAxis(2, axis22); + if (!testMainAxis(box, rect, axis22, 0, vectorToBox)) return false; + + // Проверка пересечения по основным осям бокса + boxTransform.getAxis(0, axis10); + if (!testMainAxis(box, rect, axis10, 1, vectorToBox)) return false; + boxTransform.getAxis(1, axis11); + if (!testMainAxis(box, rect, axis11, 2, vectorToBox)) return false; + boxTransform.getAxis(2, axis12); + if (!testMainAxis(box, rect, axis12, 3, vectorToBox)) return false; + + // Получаем направляющие рёбер прямоугольника + rectTransform.getAxis(0, axis20); + rectTransform.getAxis(1, axis21); + + // Проверка производных осей + if (!testDerivedAxis(box, rect, axis10, axis20, 4, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis10, axis21, 5, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis11, axis20, 6, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis11, axis21, 7, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis12, axis20, 8, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis12, axis21, 9, vectorToBox)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта бокса с прямоугольником. + * + * @param box бокс + * @param rect прямоугольник + * @param vectorToBox вектор, направленный из центра прямоугольника в центр бокса + * @param faceAxisIdx индекс оси, идентифицирующей полскость столкновения (грань бокса или полскость прямоугольника) + * @param colInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var pnum:int; + var i:int; + var v:Vector3; + var cp:ContactPoint; + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + var colAxis:Vector3 = contact.normal; + + var negativeFace:Boolean; + var code:int; + + if (faceAxisIdx == 0) { + // Столкновение с плоскостью прямоугольника + + // Проверим положение бокса относительно плоскости прямоугольника + colAxis.x = rectTransform.c; + colAxis.y = rectTransform.g; + colAxis.z = rectTransform.k; + +// var offset:Number = colAxis.x*rectTransform.d + colAxis.y*rectTransform.h + colAxis.z*rectTransform.l; +// if (bbPos.vDot(colAxis) < offset) return false; + + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisIdx = axisIdx; + incidentAxisDot = dot; + } + } + negativeFace = incidentAxisDot > 0; + + code = 1 << (incidentAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + + // Получаем список вершин грани бокса, переводим их в систему координат прямоугольника и выполняем обрезку + // по прямоугольнику. Таким образом получается список потенциальных точек контакта. + boxTransform.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box.hs, incidentAxisIdx, negativeFace, points1); + boxTransform.transformVectorsN(points1, points2, 4); + rectTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByRect(rect.hs); + // Проверяем каждую потенциальную точку на принадлежность нижней полуплоскости прямоугольника и добавляем такие точки в список контактов + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if (v.z < epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = -v.z; + var cpPos:Vector3 = cp.pos; + cpPos.x = rectTransform.a*v.x + rectTransform.b*v.y + rectTransform.c*v.z + rectTransform.d; + cpPos.y = rectTransform.e*v.x + rectTransform.f*v.y + rectTransform.g*v.z + rectTransform.h; + cpPos.z = rectTransform.i*v.x + rectTransform.j*v.y + rectTransform.k*v.z + rectTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } else { + // Столкновение с гранью бокса + faceAxisIdx--; + boxTransform.getAxis(faceAxisIdx, colAxis); + negativeFace = colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z > 0; + + code = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) { + return false; + } + + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (rectTransform.c*colAxis.x + rectTransform.g*colAxis.y + rectTransform.k*colAxis.z < 0) return false; + + getFaceVertsByAxis(rect.hs, 2, false, points1); + rectTransform.transformVectorsN(points1, points2, 4); + boxTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if ((pen = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = pen; + cpPos = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } + return true; + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * + * @param hs + * @return + */ + private function clipByRect(hs:Vector3):int { + var pnum:int = 4; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box первый бокс + * @param rect второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param colInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + boxTransform.getAxis(axisIdx1, axis10); + rectTransform.getAxis(axisIdx2, axis20); + var colAxis:Vector3 = contact.normal; + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону бокса + if (colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + var vx:Number = box.hs.x; + var vy:Number = box.hs.y; + var vz:Number = box.hs.z; + var x2:Number = rect.hs.x; + var y2:Number = rect.hs.y; + var z2:Number = rect.hs.z; + // x + if (axisIdx1 == 0) { + vx = 0; + halfLen1 = box.hs.x; + } else { + if (boxTransform.a*colAxis.x + boxTransform.e*colAxis.y + boxTransform.i*colAxis.z > 0) { + vx = -vx; + if ((box.excludedFaces & 2) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 1) != 0) { + return false; + } + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = rect.hs.x; + } else { + if (rectTransform.a*colAxis.x + rectTransform.e*colAxis.y + rectTransform.i*colAxis.z < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + vy = 0; + halfLen1 = box.hs.y; + } else { + if (boxTransform.b*colAxis.x + boxTransform.f*colAxis.y + boxTransform.j*colAxis.z > 0) { + vy = -vy; + if ((box.excludedFaces & 8) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 4) != 0) { + return false; + } + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = rect.hs.y; + } else { + if (rectTransform.b*colAxis.x + rectTransform.f*colAxis.y + rectTransform.j*colAxis.z < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + vz = 0; + halfLen1 = box.hs.z; + } else { + if (boxTransform.c*colAxis.x + boxTransform.g*colAxis.y + boxTransform.k*colAxis.z > 0) { + vz = -vz; + if ((box.excludedFaces & 32) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 16) != 0) { + return false; + } + } + } + // Получаем глобальные координаты средних точек рёбер + + var x1:Number = boxTransform.a*vx + boxTransform.b*vy + boxTransform.c*vz + boxTransform.d; + var y1:Number = boxTransform.e*vx + boxTransform.f*vy + boxTransform.g*vz + boxTransform.h; + var z1:Number = boxTransform.i*vx + boxTransform.j*vy + boxTransform.k*vz + boxTransform.l; + vx = x2; + vy = y2; + vz = z2; + x2 = rectTransform.a*vx + rectTransform.b*vy + rectTransform.c*vz + rectTransform.d; + y2 = rectTransform.e*vx + rectTransform.f*vy + rectTransform.g*vz + rectTransform.h; + z2 = rectTransform.i*vx + rectTransform.j*vy + rectTransform.k*vz + rectTransform.l; + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + vx = x2 - x1; + vy = y2 - y1; + vz = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cpPos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cpPos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cpPos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var v:Vector3 = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + return true; + } + + /** + * Проверяет пересечение вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box + * @param rect + * @param axis + * @param axisIndex + * @param vectorToBox + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param rect + * @param axis1 + * @param axis2 + * @param axisIndex + * @param vectorToBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, rect:CollisionRect, axis1:Vector3, axis2:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия вдоль заданной оси. + * + * @param box бокс + * @param rect прямоугольник + * @param axis ось + * @param vectorToBox вектор, соединяющий центр прямоугольника с центром бокса + * @return величина перекрытия вдоль оси + */ + public function overlapOnAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, vectorToBox:Vector3):Number { + var m:Matrix4 = box.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box.hs.z; + if (d < 0) d = -d; + projection += d; + + m = rect.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*rect.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*rect.hs.y; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox.x*axis.x + vectorToBox.y*axis.y + vectorToBox.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/colliders/BoxSphereCollider.as b/0.0.7.0/src/alternativa/physics/collision/colliders/BoxSphereCollider.as new file mode 100644 index 0000000..2ba3ce2 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/colliders/BoxSphereCollider.as @@ -0,0 +1,114 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.math.Vector3; + use namespace altphysics; + /** + * + */ + public class BoxSphereCollider implements ICollider { + + private var center:Vector3 = new Vector3(); + private var closestPt:Vector3 = new Vector3(); + + private var bPos:Vector3 = new Vector3(); + private var sPos:Vector3 = new Vector3(); + + /** + * + */ + public function BoxSphereCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + // Зафиксированно столкновение + contact.body1 = sphere.body; + contact.body2 = box.body; + contact.normal.vCopy(closestPt).vTransformBy4(box.transform).vSubtract(sPos).vNormalize().vReverse(); + contact.pcount = 1; + + var cp:ContactPoint = contact.points[0]; + cp.penetration = sphere.r - Math.sqrt(distSqr); + cp.pos.vCopy(contact.normal).vScale(-sphere.r).vAdd(sPos); + cp.r1.vDiff(cp.pos, sPos); + cp.r2.vDiff(cp.pos, bPos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/colliders/BoxTriangleCollider.as b/0.0.7.0/src/alternativa/physics/collision/colliders/BoxTriangleCollider.as new file mode 100644 index 0000000..b22db8e --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/colliders/BoxTriangleCollider.as @@ -0,0 +1,718 @@ +package alternativa.physics.collision.colliders { + import __AS3__.vec.Vector; + + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionTriangle; + import alternativa.physics.math.Matrix4; + import alternativa.physics.math.Vector3; + + /** + * + */ + public class BoxTriangleCollider extends BoxCollider { + + public var epsilon:Number = 0.001; + + private var bestAxisIndex:int; + private var minOverlap:Number; + private var toBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var colNormal:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxTriangleCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, tri, toBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + if (!findEdgesIntersection(box, tri, toBox, bestAxisIndex%3, int(bestAxisIndex/3), contact)) return false; + } + + contact.body1 = box.body; + contact.body2 = tri.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + toBox.x = boxTransform.d - triTransform.d; + toBox.y = boxTransform.h - triTransform.h; + toBox.z = boxTransform.l - triTransform.l; + + minOverlap = 1e308; + + // Сначала проверяется нормаль треугольника + axis.x = triTransform.c; + axis.y = triTransform.g; + axis.z = triTransform.k; + if (!testMainAxis(box, tri, axis, 0, toBox)) return false; + + // Проверка основных осей бокса + axis10.x = boxTransform.a; + axis10.y = boxTransform.e; + axis10.z = boxTransform.i; + if (!testMainAxis(box, tri, axis10, 1, toBox)) return false; + axis11.x = boxTransform.b; + axis11.y = boxTransform.f; + axis11.z = boxTransform.j; + if (!testMainAxis(box, tri, axis11, 2, toBox)) return false; + axis12.x = boxTransform.c; + axis12.y = boxTransform.g; + axis12.z = boxTransform.k; + if (!testMainAxis(box, tri, axis12, 3, toBox)) return false; + + // Проверка производных осей + // TODO: заменить вычисления векторных произведений инлайнами + var v:Vector3 = tri.e0; + axis20.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis20.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis20.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis20, 4, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis20, 5, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis20, 6, toBox)) return false; + + v = tri.e1; + axis21.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis21.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis21.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis21, 7, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis21, 8, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis21, 9, toBox)) return false; + + v = tri.e2; + axis22.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis22.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis22.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis22, 10, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis22, 11, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis22, 12, toBox)) return false; + + return true; + } + + /** + * Тестирует пересечение примитивов вдоль заданной оси. + * + * @param box бокс + * @param tri треугольник + * @param axis ось + * @param axisIndex индекс оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return true, если примитивы имеют перекрытие проекций вдоль указанной оси, иначе false + */ + private function testMainAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, axisIndex:int, toBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param tri + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, tri:CollisionTriangle, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Рассчитывает величину перекрытия проекций бокса и треугольника на заданную ось. + * + * @param box бокс + * @param tri треугольник + * @param axis единичный направляющий вектор оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return величина перекрытия. Положительное значение указывает на наличие перекрытия, нулевое или отрицательное значение указывает на отсутствие перекрытия. + */ + private function overlapOnAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, toBox:Vector3):Number { + var t:Matrix4 = box.transform; + var projection:Number = (t.a*axis.x + t.e*axis.y + t.i*axis.z)*box.hs.x; + if (projection < 0) projection = -projection; + var d:Number = (t.b*axis.x + t.f*axis.y + t.j*axis.z)*box.hs.y; + projection += d < 0 ? -d : d; + d = (t.c*axis.x + t.g*axis.y + t.k*axis.z)*box.hs.z; + projection += d < 0 ? -d : d; + + var vectorProjection:Number = toBox.x*axis.x + toBox.y*axis.y + toBox.z*axis.z; + // Для оптимизации ось преобразуется в систему треугольника, а не его вершины в мировую систему + t = tri.transform; + var ax:Number = t.a*axis.x + t.e*axis.y + t.i*axis.z; + var ay:Number = t.b*axis.x + t.f*axis.y + t.j*axis.z; + var az:Number = t.c*axis.x + t.g*axis.y + t.k*axis.z; + var max:Number = 0; + if (vectorProjection < 0) { + vectorProjection = -vectorProjection; + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d < max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d < max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d < max) max = d; + max = -max; + } else { + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d > max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d > max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d > max) max = d; + } + + return projection + max - vectorProjection; + } + + /** + * Определяет точки контакта бокса и треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function findFaceContactPoints(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIndex:int, contact:Contact):Boolean { + if (faceAxisIndex == 0) { + // Столкновение с плоскостью треугольника + return getBoxToTriContact(box, tri, toBox, contact); + } else { + // Столкновение с гранью бокса + return getTriToBoxContact(box, tri, toBox, faceAxisIndex, contact); + } + } + + /** + * Определяет точки контакта бокса с плоскостью треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getBoxToTriContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + colNormal.x = triTransform.c; + colNormal.y = triTransform.g; + colNormal.z = triTransform.k; + + var over:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + if (!over) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var incAxisDot:Number = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colNormal.x + axis.y*colNormal.y + axis.z*colNormal.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incAxisDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Обрезка грани + var negativeFace:Boolean = incAxisDot > 0; + + var code:int = 1 << (incFaceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + getFaceVertsByAxis(box.hs, incFaceAxisIdx, negativeFace, points1); + boxTransform.transformVectorsN(points1, points2, 4); + triTransform.transformVectorsInverseN(points2, points1, 4); + var pnum:int = clipByTriangle(tri); + // Среди конечного списка точек определяются лежащие под плоскостью треугольника + var cp:ContactPoint; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points2[i]; + if ((over && v.z < 0) || (!over && v.z > 0)) { + cp = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z + triTransform.d; + cpPos.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z + triTransform.h; + cpPos.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z + triTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = over ? -v.z : v.z; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Оперделяет точки контакта треугольника с гранью бокса. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getTriToBoxContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + faceAxisIdx--; + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + boxTransform.getAxis(faceAxisIdx, colNormal); + var negativeFace:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + + var code:int = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + if (!negativeFace) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + var v:Vector3 = points1[0]; + v.x = tri.v0.x; + v.y = tri.v0.y; + v.z = tri.v0.z; + + v = points1[1]; + v.x = tri.v1.x; + v.y = tri.v1.y; + v.z = tri.v1.z; + + v = points1[2]; + v.x = tri.v2.x; + v.y = tri.v2.y; + v.z = tri.v2.z; + + triTransform.transformVectorsN(points1, points2, 3); + boxTransform.transformVectorsInverseN(points2, points1, 3); + + var pnum:int = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность боксу и добавляем такие точки в список контактов + var penetration:Number; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + v = points1[i]; + penetration = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace); + if (penetration > -epsilon) { + var cp:ContactPoint = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = penetration; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Вычисляет величину проникновения точки в бокс. + * + * @param hs вектор половинных размеров бокса + * @param p точка в системе координат бокса + * @param axisIndex индекс оси + * @param negativeFace если true, проверяется нижняя грань + * @return величина проникновения точки в бокс + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, negativeFace:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (negativeFace) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (negativeFace) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (negativeFace) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, вершины которой заданы в списке points1. Результат сохраняется в этом же списке. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 3; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Обрезает грань прямоугольника сторонами треугольника. Обрезка выполняется в системе координат и в проекции на + * плоскость треугольника. Входные вершины грани в количестве четырёх штук должны находиться в списке points1. + * Конечные вершины сохраняются в списке points2. + * + * @param tri треугольник + * @return количество врешин в конечном списке + */ + private function clipByTriangle(tri:CollisionTriangle):int { + var vnum:int = 4; + vnum = clipByLine(tri.v0, tri.e0, points1, vnum, points2); + if (vnum == 0) return 0; + vnum = clipByLine(tri.v1, tri.e1, points2, vnum, points1); + if (vnum == 0) return 0; + return clipByLine(tri.v2, tri.e2, points1, vnum, points2);; + } + + /** + * Обрезает полигон указанной прямой. + * + * @param linePoint точка на прямой + * @param lineDir единичный направляющий вектор прямой + * @param verticesIn список вершин исходного полигона + * @param vnum количество вершин исходного полигона + * @param verticesOut список, куда будут записаны вершины конечного полигона + * @return количество вершин конечного полигона + */ + private function clipByLine(linePoint:Vector3, lineDir:Vector3, verticesIn:Vector., vnum:int, verticesOut:Vector.):int { + var nx:Number = -lineDir.y; + var ny:Number = lineDir.x; + var offset:Number = linePoint.x*nx + linePoint.y*ny; + var v1:Vector3 = verticesIn[int(vnum - 1)]; + var offset1:Number = v1.x*nx + v1.y*ny; + var t:Number; + var v:Vector3; + var num:int = 0; + for (var i:int = 0; i < vnum; i++) { + var v2:Vector3 = verticesIn[i]; + var offset2:Number = v2.x*nx + v2.y*ny; + if (offset1 < offset) { + // Первая точка ребра во внешней полуплоскости + if (offset2 > offset) { + // Вторая точка ребра во внутренней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } else { + // Первая точка ребра во внутренней полуплоскости. Добавляем её в конечный список. + v = verticesOut[num]; + v.x = v1.x; + v.y = v1.y; + v.z = v1.z; + num++; + if (offset2 < offset) { + // Вторая точка ребра во внешней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } + v1 = v2; + offset1 = offset2; + } + return num; + } + + /** + * + * @param box + * @param tri + * @param toBox + * @param boxAxisIdx + * @param triAxisIdx + * @param contact + */ + private function findEdgesIntersection(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, boxAxisIdx:int, triAxisIdx:int, contact:Contact):Boolean { + // Определение точки и направляющего вектора ребра треугольника + var tmpx1:Number; + var tmpy1:Number; + var tmpz1:Number; + var tmpx2:Number; + var tmpy2:Number; + var tmpz2:Number; + switch (triAxisIdx) { + case 0: + tmpx1 = tri.e0.x; + tmpy1 = tri.e0.y; + tmpz1 = tri.e0.z; + tmpx2 = tri.v0.x; + tmpy2 = tri.v0.y; + tmpz2 = tri.v0.z; + break; + case 1: + tmpx1 = tri.e1.x; + tmpy1 = tri.e1.y; + tmpz1 = tri.e1.z; + tmpx2 = tri.v1.x; + tmpy2 = tri.v1.y; + tmpz2 = tri.v1.z; + break; + case 2: + tmpx1 = tri.e2.x; + tmpy1 = tri.e2.y; + tmpz1 = tri.e2.z; + tmpx2 = tri.v2.x; + tmpy2 = tri.v2.y; + tmpz2 = tri.v2.z; + break; + } + var triTransform:Matrix4 = tri.transform; + axis20.x = triTransform.a*tmpx1 + triTransform.b*tmpy1 + triTransform.c*tmpz1; + axis20.y = triTransform.e*tmpx1 + triTransform.f*tmpy1 + triTransform.g*tmpz1; + axis20.z = triTransform.i*tmpx1 + triTransform.j*tmpy1 + triTransform.k*tmpz1; + var x2:Number = triTransform.a*tmpx2 + triTransform.b*tmpy2 + triTransform.c*tmpz2 + triTransform.d; + var y2:Number = triTransform.e*tmpx2 + triTransform.f*tmpy2 + triTransform.g*tmpz2 + triTransform.h; + var z2:Number = triTransform.i*tmpx2 + triTransform.j*tmpy2 + triTransform.k*tmpz2 + triTransform.l; + + // Определение нормали контакта, точки и направляющего вектора ребра бокса + var boxTransform:Matrix4 = box.transform; + boxTransform.getAxis(boxAxisIdx, axis10); + + // Нормаль контакта + var v:Vector3 = contact.normal; + v.x = axis10.y*axis20.z - axis10.z*axis20.y; + v.y = axis10.z*axis20.x - axis10.x*axis20.z; + v.z = axis10.x*axis20.y - axis10.y*axis20.x; + k = 1/Math.sqrt(v.x*v.x + v.y*v.y + v.z*v.z); + v.x *= k; + v.y *= k; + v.z *= k; + // Разворот нормали в сторону бокса + if (v.x*toBox.x + v.y*toBox.y + v.z*toBox.z < 0) { + v.x = -v.x; + v.y = -v.y; + v.z = -v.z; + } + + var boxHalfLen:Number; + tmpx1 = box.hs.x; + tmpy1 = box.hs.y; + tmpz1 = box.hs.z; + // X + if (boxAxisIdx == 0) { + tmpx1 = 0; + boxHalfLen = box.hs.x; + } else { + if (boxTransform.a*v.x + boxTransform.e*v.y + boxTransform.i*v.z > 0) { + tmpx1 = -tmpx1; + if ((box.excludedFaces & 2) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 1) != 0) { + return false; + } + } + } + // Y + if (boxAxisIdx == 1) { + tmpy1 = 0; + boxHalfLen = box.hs.y; + } else { + if (boxTransform.b*v.x + boxTransform.f*v.y + boxTransform.j*v.z > 0) { + tmpy1 = -tmpy1; + if ((box.excludedFaces & 8) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 4) != 0) { + return false; + } + } + } + // Z + if (boxAxisIdx == 2) { + tmpz1 = 0; + boxHalfLen = box.hs.z; + } else { + if (boxTransform.c*v.x + boxTransform.g*v.y + boxTransform.k*v.z > 0) { + tmpz1 = -tmpz1; + if ((box.excludedFaces & 32) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 16) != 0) { + return false; + } + } + } + var x1:Number = boxTransform.a*tmpx1 + boxTransform.b*tmpy1 + boxTransform.c*tmpz1 + boxTransform.d; + var y1:Number = boxTransform.e*tmpx1 + boxTransform.f*tmpy1 + boxTransform.g*tmpz1 + boxTransform.h; + var z1:Number = boxTransform.i*tmpx1 + boxTransform.j*tmpy1 + boxTransform.k*tmpz1 + boxTransform.l; + + // Находим точку пересечения рёбер, решая систему уравнений + // axis10 - направляющий вектор ребра бокса + // x1, y1, z1 - средняя точка ребра бокса + // axis20 - направляющий вектор ребра треугольника + // x2, y2, z2 - начальная точка ребра треугольника + var k:Number = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + var vx:Number = x2 - x1; + var vy:Number = y2 - y1; + var vz:Number = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + + // Запись данных о контакте + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + v = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + v.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + v.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + v.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = v.x - boxTransform.d; + r.y = v.y - boxTransform.h; + r.z = v.z - boxTransform.l; + r = cp.r2; + r.x = v.x - triTransform.d; + r.y = v.y - triTransform.h; + r.z = v.z - triTransform.l; + return true; + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/colliders/SpherePlaneCollider.as b/0.0.7.0/src/alternativa/physics/collision/colliders/SpherePlaneCollider.as new file mode 100644 index 0000000..1ac5d35 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/colliders/SpherePlaneCollider.as @@ -0,0 +1,68 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.math.Vector3; + + /** + * + */ + public class SpherePlaneCollider implements ICollider { + + private var normal:Vector3 = new Vector3(); + + /** + * + */ + public function SpherePlaneCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var sphere:RigidSphere = body1 as RigidSphere; +// var plane:RigidPlane; +// if (sphere == null) { +// sphere = body2 as RigidSphere; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// +// var dist:Number = sphere.state.pos.dot(normal) - offset; +// if (dist > sphere.r) return false; +// +// collisionInfo.body1 = sphere; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// collisionInfo.pcount = 1; +// +// var cp:ContactPoint = collisionInfo.points[0]; +// cp.penetration = sphere.r - dist; +// cp.pos.copy(normal).reverse().scale(sphere.r).add(sphere.state.pos); +// cp.r1.diff(cp.pos, sphere.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/colliders/SphereSphereCollider.as b/0.0.7.0/src/alternativa/physics/collision/colliders/SphereSphereCollider.as new file mode 100644 index 0000000..a90fabb --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/colliders/SphereSphereCollider.as @@ -0,0 +1,70 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.math.Vector3; + use namespace altphysics; + + public class SphereSphereCollider implements ICollider { + + private var p1:Vector3 = new Vector3(); + private var p2:Vector3 = new Vector3(); + + public function SphereSphereCollider() { + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var s1:CollisionSphere; + var s2:CollisionSphere; + if (prim1.body != null) { + s1 = prim1 as CollisionSphere; + s2 = prim2 as CollisionSphere; + } else { + s1 = prim2 as CollisionSphere; + s2 = prim1 as CollisionSphere; + } + + s1.transform.getAxis(3, p1); + s2.transform.getAxis(3, p2); + var dx:Number = p1.x - p2.x; + var dy:Number = p1.y - p2.y; + var dz:Number = p1.z - p2.z; + var len:Number = dx*dx + dy*dy + dz*dz; + var sum:Number = s1.r + s2.r; + if (len > sum*sum) return false; + len = Math.sqrt(len); + dx /= len; + dy /= len; + dz /= len; + + contact.body1 = s1.body; + contact.body2 = s2.body; + contact.normal.x = dx; + contact.normal.y = dy; + contact.normal.z = dz; + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = sum - len; + cp.pos.x = p1.x - dx*s1.r; + cp.pos.y = p1.y - dy*s1.r; + cp.pos.z = p1.z - dz*s1.r; + cp.r1.vDiff(cp.pos, p1); + cp.r2.vDiff(cp.pos, p2); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/primitives/.svn/all-wcprops b/0.0.7.0/src/alternativa/physics/collision/primitives/.svn/all-wcprops new file mode 100644 index 0000000..a81c9c9 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/primitives/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 124 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/collision/primitives +END +CollisionTriangle.as +K 25 +svn:wc:ra_dav:version-url +V 145 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/collision/primitives/CollisionTriangle.as +END +CollisionSphere.as +K 25 +svn:wc:ra_dav:version-url +V 143 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/collision/primitives/CollisionSphere.as +END +CollisionBox.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/collision/primitives/CollisionBox.as +END +CollisionRect.as +K 25 +svn:wc:ra_dav:version-url +V 141 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/collision/primitives/CollisionRect.as +END diff --git a/0.0.7.0/src/alternativa/physics/collision/primitives/.svn/entries b/0.0.7.0/src/alternativa/physics/collision/primitives/.svn/entries new file mode 100644 index 0000000..31bec06 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/primitives/.svn/entries @@ -0,0 +1,76 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/collision/primitives +http://svndev.alternativaplatform.com + + + +2009-10-23T05:59:29.567666Z +22444 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +CollisionTriangle.as +file + + + + +2010-10-28T04:32:42.000000Z +f974b5617d3a5068378d72f540fd4290 +2009-10-19T07:17:33.113306Z +22253 +mike + +CollisionSphere.as +file + + + + +2010-10-28T04:32:42.000000Z +759c8a4e451e224d04fff0bc21d559f2 +2009-10-19T07:17:33.113306Z +22253 +mike + +CollisionBox.as +file + + + + +2010-10-28T04:32:42.000000Z +eaf5410bc4fb8c70444ca57e2a9f4c34 +2009-10-23T05:59:29.567666Z +22444 +mike + +CollisionRect.as +file + + + + +2010-10-28T04:32:42.000000Z +2e5629bb7cf350dd18485fe68675d0f0 +2009-10-19T07:17:33.113306Z +22253 +mike + diff --git a/0.0.7.0/src/alternativa/physics/collision/primitives/.svn/format b/0.0.7.0/src/alternativa/physics/collision/primitives/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/primitives/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.7.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base b/0.0.7.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base new file mode 100644 index 0000000..3d2f391 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base @@ -0,0 +1,204 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.math.Matrix4; + import alternativa.physics.math.Vector3; + use namespace altphysics; + + /** + * Ориентированный бокс. + */ + public class CollisionBox extends CollisionPrimitive { + + // Половинные размеры вдоль каждой из осей + public var hs:Vector3 = new Vector3(); + + public var excludedFaces:int; + + /** + * @param hs + * @param collisionGroup + */ + public function CollisionBox(hs:Vector3, collisionGroup:int) { + super(BOX, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + var t:Matrix4 = transform; + + var xx:Number; + var yy:Number; + var zz:Number; + + xx = t.a < 0 ? -t.a : t.a; + yy = t.b < 0 ? -t.b : t.b; + zz = t.c < 0 ? -t.c : t.c; + aabb.maxX = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minX = -aabb.maxX; + + xx = t.e < 0 ? -t.e : t.e; + yy = t.f < 0 ? -t.f : t.f; + zz = t.g < 0 ? -t.g : t.g; + aabb.maxY = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minY = -aabb.maxY; + + xx = t.i < 0 ? -t.i : t.i; + yy = t.j < 0 ? -t.j : t.j; + zz = t.k < 0 ? -t.k : t.k; + aabb.maxZ = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var box:CollisionBox = source as CollisionBox; + if (box == null) return this; + super.copyFrom(box); + hs.vCopy(box.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionBox(hs, collisionGroup); + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + var tMin:Number = -1; + var tMax:Number = 1e308; + var t1:Number; + var t2:Number; + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + // X + if (vx < epsilon && vx > -epsilon) { + if (ox < -hs.x || ox > hs.x) return -1; + } else { + t1 = (-hs.x - ox)/vx; + t2 = (hs.x - ox)/vx; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.x = -1; + normal.y = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.x = 1; + normal.y = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Y + if (vy < epsilon && vy > -epsilon) { + if (oy < -hs.y || oy > hs.y) return -1; + } else { + t1 = (-hs.y - oy)/vy; + t2 = (hs.y - oy)/vy; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.y = -1; + normal.x = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.y = 1; + normal.x = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Z + if (vz < epsilon && vz > -epsilon) { + if (oz < -hs.z || oz > hs.z) return -1; + } else { + t1 = (-hs.z - oz)/vz; + t2 = (hs.z - oz)/vz; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.z = -1; + normal.x = normal.y = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.z = 1; + normal.x = normal.y = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Перевод нормали в мировую систему координат + // Inlined normal.vDeltaTransformBy4(transform); + vx = normal.x; + vy = normal.y; + vz = normal.z; + normal.x = transform.a*vx + transform.b*vy + transform.c*vz; + normal.y = transform.e*vx + transform.f*vy + transform.g*vz; + normal.z = transform.i*vx + transform.j*vy + transform.k*vz; + + return tMin; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionBox hs=" + hs + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base b/0.0.7.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base new file mode 100644 index 0000000..08001ad --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base @@ -0,0 +1,143 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.math.Matrix4; + import alternativa.physics.math.Vector3; + use namespace altphysics; + + /** + * Ориентированный прямоугольник. Задаётся половинами размеров вдоль осей X (ширина) и Y (длина) локальной системы + * координат примитива. Таким образом, прямоугольник лежит в плоскости XY, его стороны параллельны осям этой + * плоскости, а нормаль направлена вдоль локальной оси Z. + * + * Прямоугольник может быть одно- или двусторонним. В случае одностороннего прямоугольника, + * столкновения с внутренней стороны не регистрируются. + */ + public class CollisionRect extends CollisionPrimitive { + // Половинные размеры прямоугольника вдоль осей X и Y. Нормаль направлена вдоль оси Z. + public var hs:Vector3 = new Vector3(); + // Флаг указывает, является примитив одно- или двусторонним + public var twoSided:Boolean = true; + + // Малое значение. Используется для указания фиктивной высоты примитива и в функции определения пересечения с сегментом. + private static const EPSILON:Number = 0.005; + + /** + * Создаёт новый экземпляр примитива. + * + * @param hs половинные размерв прямоугольника вдоль осей X и Y. Значение z игнорируется. + * @param collisionGroup группа примитива + */ + public function CollisionRect(hs:Vector3, collisionGroup:int) { + super(RECT, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * Расчитывает ограничивающий бокс прямоугольника. Для избежания проблем высота примитива принимается равной + * не нулю, а малому значению. + * + * @return + */ + override public function calculateAABB():BoundBox { + // Баунд бокс прямоугольника имеет минимальную высоту, отличную от нуля во избежание проблем с построением kd-дерева + var t:Matrix4 = transform; + var xx:Number = t.a < 0 ? -t.a : t.a; + var yy:Number = t.b < 0 ? -t.b : t.b; + var zz:Number = t.c < 0 ? -t.c : t.c; + aabb.maxX = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minX = -aabb.maxX; + + xx = t.e < 0 ? -t.e : t.e; + yy = t.f < 0 ? -t.f : t.f; + zz = t.g < 0 ? -t.g : t.g; + aabb.maxY = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minY = -aabb.maxY; + + xx = t.i < 0 ? -t.i : t.i; + yy = t.j < 0 ? -t.j : t.j; + zz = t.k < 0 ? -t.k : t.k; + aabb.maxZ = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var rect:CollisionRect = source as CollisionRect; + if (rect == null) return this; + super.copyFrom(rect); + hs.vCopy(rect.hs); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionRect hs=" + hs + "]"; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionRect(hs, collisionGroup); + } + + /** + * @param origin + * @param vector + * @param collisionGroup + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + + // Проверка параллельности сегмента и плоскости примитива + if (vz > -threshold && vz < threshold) return -1; + var t:Number = -oz/vz; + if (t < 0) return -1; + // Проверка вхождения точки пересечения в прямоугольник + ox += vx*t; + oy += vy*t; + oz = 0; + if (ox < (-hs.x - threshold) || ox > (hs.x + threshold) || oy < (-hs.y - threshold) || oy > (hs.y + threshold)) return -1; + + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + return t; + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base b/0.0.7.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base new file mode 100644 index 0000000..887a19a --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base @@ -0,0 +1,81 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.math.Vector3; + use namespace altphysics; + + + /** + * Сфера. + */ + public class CollisionSphere extends CollisionPrimitive { + + // Радиус сферы + public var r:Number = 0; + + /** + * + * @param r + * @param collisionGroup + */ + public function CollisionSphere(r:Number, collisionGroup:int) { + super(SPHERE, collisionGroup); + this.r = r; + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + aabb.maxX = transform.d + r; + aabb.minX = transform.d - r; + + aabb.maxY = transform.h + r; + aabb.minY = transform.h - r; + + aabb.maxZ = transform.l + r; + aabb.minZ = transform.l - r; + + return aabb; + } + + /** + * @param origin + * @param vector + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var px:Number = origin.x - transform.d; + var py:Number = origin.y - transform.h; + var pz:Number = origin.z - transform.l; + var k:Number = vector.x*px + vector.y*py + vector.z*pz; + if (k > 0) return -1; + var a:Number = vector.x*vector.x + vector.y*vector.y + vector.z*vector.z; + var D:Number = k*k - a*(px*px + py*py + pz*pz - r*r); + if (D < 0) return -1; + return -(k + Math.sqrt(D))/a; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var sphere:CollisionSphere = source as CollisionSphere; + if (sphere == null) return this; + super.copyFrom(sphere); + r = sphere.r; + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionSphere(r, collisionGroup); + } + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base b/0.0.7.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base new file mode 100644 index 0000000..9cd6d00 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base @@ -0,0 +1,244 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.math.Vector3; + use namespace altphysics; + + /** + * Примитив треугольник. + */ + public class CollisionTriangle extends CollisionPrimitive { + + public var v0:Vector3 = new Vector3(); + public var v1:Vector3 = new Vector3(); + public var v2:Vector3 = new Vector3(); + + public var e0:Vector3 = new Vector3(); + public var e1:Vector3 = new Vector3(); + public var e2:Vector3 = new Vector3(); + +// public var len0:Number; +// public var len1:Number; +// public var len2:Number; +// + /** + * + * @param v0 + * @param v1 + * @param v2 + * @param collisionGroup + */ + public function CollisionTriangle(v0:Vector3, v1:Vector3, v2:Vector3, collisionGroup:int) { + super(TRIANGLE, collisionGroup); + initVertices(v0, v1, v2); + } + + /** + * Рассчитывает AABB примитива. + * + * @return ссылка на свой AABB + */ + override public function calculateAABB():BoundBox { + var epsilon:Number = 0.005; + var a:Number; + var b:Number; + var eps_c:Number = epsilon*transform.c; + var eps_g:Number = epsilon*transform.g; + var eps_k:Number = epsilon*transform.k; + + // Вершина 0 + // Ось X + a = v0.x*transform.a + v0.y*transform.b; + aabb.minX = aabb.maxX = a + eps_c; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v0.x*transform.e + v0.y*transform.f; + aabb.minY = aabb.maxY = a + eps_g; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v0.x*transform.i + v0.y*transform.j; + aabb.minZ = aabb.maxZ = a + eps_k; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 1 + // Ось X + a = v1.x*transform.a + v1.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v1.x*transform.e + v1.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v1.x*transform.i + v1.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 2 + // Ось X + a = v2.x*transform.a + v2.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v2.x*transform.e + v2.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v2.x*transform.i + v2.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + aabb.minX += transform.d; + aabb.maxX += transform.d; + + aabb.minY += transform.h; + aabb.maxY += transform.h; + + aabb.minZ += transform.l; + aabb.maxZ += transform.l; + + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + // Луч трансформируется в систему координат примитива, затем проверяется пересечение + + var vz:Number = vector.x*transform.c + vector.y*transform.g + vector.z*transform.k; + // Если луч параллелен плоскости птимитива, то пересечения нет + if (vz < epsilon && vz > -epsilon) return -1; + + var tx:Number = origin.x - transform.d; + var ty:Number = origin.y - transform.h; + var tz:Number = origin.z - transform.l; + + var oz:Number = tx*transform.c + ty*transform.g + tz*transform.k; + var t:Number = -oz/vz; + if (t < 0) return -1; + + var ox:Number = tx*transform.a + ty*transform.e + tz*transform.i; + var oy:Number = tx*transform.b + ty*transform.f + tz*transform.j; + + tx = ox + t*(vector.x*transform.a + vector.y*transform.e + vector.z*transform.i); + ty = oy + t*(vector.x*transform.b + vector.y*transform.f + vector.z*transform.j); + tz = oz + t*vz; + + // Проверка вхождения точки в треугольник + if ((e0.x*(ty - v0.y) - e0.y*(tx - v0.x) < 0) || (e1.x*(ty - v1.y) - e1.y*(tx - v1.x) < 0) || (e2.x*(ty - v2.y) - e2.y*(tx - v2.x) < 0)) return -1; + + // Запись нормали + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + + return t; + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + super.copyFrom(source); + var tri:CollisionTriangle = source as CollisionTriangle; + if (tri != null) { + v0.vCopy(tri.v0); + v1.vCopy(tri.v1); + v2.vCopy(tri.v2); + + e0.vCopy(tri.e0); + e1.vCopy(tri.e1); + e2.vCopy(tri.e2); + +// len0 = tri.len0; +// len1 = tri.len1; +// len2 = tri.len2; + } + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionTriangle v0=" + v0 + ", v1=" + v1 + ", v2=" + v2 + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionTriangle(v0, v1, v2, collisionGroup); + } + + /** + * + * @param v0 + * @param v1 + * @param v2 + */ + private function initVertices(v0:Vector3, v1:Vector3, v2:Vector3):void { + this.v0.vCopy(v0); + this.v1.vCopy(v1); + this.v2.vCopy(v2); + + e0.vDiff(v1, v0); +// len0 = e0.vLength(); + e0.vNormalize(); + + e1.vDiff(v2, v1); +// len1 = e1.vLength(); + e1.vNormalize(); + + e2.vDiff(v0, v2); +// len2 = e2.vLength(); + e2.vNormalize(); + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/primitives/CollisionBox.as b/0.0.7.0/src/alternativa/physics/collision/primitives/CollisionBox.as new file mode 100644 index 0000000..3d2f391 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/primitives/CollisionBox.as @@ -0,0 +1,204 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.math.Matrix4; + import alternativa.physics.math.Vector3; + use namespace altphysics; + + /** + * Ориентированный бокс. + */ + public class CollisionBox extends CollisionPrimitive { + + // Половинные размеры вдоль каждой из осей + public var hs:Vector3 = new Vector3(); + + public var excludedFaces:int; + + /** + * @param hs + * @param collisionGroup + */ + public function CollisionBox(hs:Vector3, collisionGroup:int) { + super(BOX, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + var t:Matrix4 = transform; + + var xx:Number; + var yy:Number; + var zz:Number; + + xx = t.a < 0 ? -t.a : t.a; + yy = t.b < 0 ? -t.b : t.b; + zz = t.c < 0 ? -t.c : t.c; + aabb.maxX = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minX = -aabb.maxX; + + xx = t.e < 0 ? -t.e : t.e; + yy = t.f < 0 ? -t.f : t.f; + zz = t.g < 0 ? -t.g : t.g; + aabb.maxY = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minY = -aabb.maxY; + + xx = t.i < 0 ? -t.i : t.i; + yy = t.j < 0 ? -t.j : t.j; + zz = t.k < 0 ? -t.k : t.k; + aabb.maxZ = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var box:CollisionBox = source as CollisionBox; + if (box == null) return this; + super.copyFrom(box); + hs.vCopy(box.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionBox(hs, collisionGroup); + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + var tMin:Number = -1; + var tMax:Number = 1e308; + var t1:Number; + var t2:Number; + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + // X + if (vx < epsilon && vx > -epsilon) { + if (ox < -hs.x || ox > hs.x) return -1; + } else { + t1 = (-hs.x - ox)/vx; + t2 = (hs.x - ox)/vx; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.x = -1; + normal.y = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.x = 1; + normal.y = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Y + if (vy < epsilon && vy > -epsilon) { + if (oy < -hs.y || oy > hs.y) return -1; + } else { + t1 = (-hs.y - oy)/vy; + t2 = (hs.y - oy)/vy; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.y = -1; + normal.x = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.y = 1; + normal.x = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Z + if (vz < epsilon && vz > -epsilon) { + if (oz < -hs.z || oz > hs.z) return -1; + } else { + t1 = (-hs.z - oz)/vz; + t2 = (hs.z - oz)/vz; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.z = -1; + normal.x = normal.y = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.z = 1; + normal.x = normal.y = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Перевод нормали в мировую систему координат + // Inlined normal.vDeltaTransformBy4(transform); + vx = normal.x; + vy = normal.y; + vz = normal.z; + normal.x = transform.a*vx + transform.b*vy + transform.c*vz; + normal.y = transform.e*vx + transform.f*vy + transform.g*vz; + normal.z = transform.i*vx + transform.j*vy + transform.k*vz; + + return tMin; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionBox hs=" + hs + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/primitives/CollisionRect.as b/0.0.7.0/src/alternativa/physics/collision/primitives/CollisionRect.as new file mode 100644 index 0000000..08001ad --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/primitives/CollisionRect.as @@ -0,0 +1,143 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.math.Matrix4; + import alternativa.physics.math.Vector3; + use namespace altphysics; + + /** + * Ориентированный прямоугольник. Задаётся половинами размеров вдоль осей X (ширина) и Y (длина) локальной системы + * координат примитива. Таким образом, прямоугольник лежит в плоскости XY, его стороны параллельны осям этой + * плоскости, а нормаль направлена вдоль локальной оси Z. + * + * Прямоугольник может быть одно- или двусторонним. В случае одностороннего прямоугольника, + * столкновения с внутренней стороны не регистрируются. + */ + public class CollisionRect extends CollisionPrimitive { + // Половинные размеры прямоугольника вдоль осей X и Y. Нормаль направлена вдоль оси Z. + public var hs:Vector3 = new Vector3(); + // Флаг указывает, является примитив одно- или двусторонним + public var twoSided:Boolean = true; + + // Малое значение. Используется для указания фиктивной высоты примитива и в функции определения пересечения с сегментом. + private static const EPSILON:Number = 0.005; + + /** + * Создаёт новый экземпляр примитива. + * + * @param hs половинные размерв прямоугольника вдоль осей X и Y. Значение z игнорируется. + * @param collisionGroup группа примитива + */ + public function CollisionRect(hs:Vector3, collisionGroup:int) { + super(RECT, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * Расчитывает ограничивающий бокс прямоугольника. Для избежания проблем высота примитива принимается равной + * не нулю, а малому значению. + * + * @return + */ + override public function calculateAABB():BoundBox { + // Баунд бокс прямоугольника имеет минимальную высоту, отличную от нуля во избежание проблем с построением kd-дерева + var t:Matrix4 = transform; + var xx:Number = t.a < 0 ? -t.a : t.a; + var yy:Number = t.b < 0 ? -t.b : t.b; + var zz:Number = t.c < 0 ? -t.c : t.c; + aabb.maxX = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minX = -aabb.maxX; + + xx = t.e < 0 ? -t.e : t.e; + yy = t.f < 0 ? -t.f : t.f; + zz = t.g < 0 ? -t.g : t.g; + aabb.maxY = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minY = -aabb.maxY; + + xx = t.i < 0 ? -t.i : t.i; + yy = t.j < 0 ? -t.j : t.j; + zz = t.k < 0 ? -t.k : t.k; + aabb.maxZ = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var rect:CollisionRect = source as CollisionRect; + if (rect == null) return this; + super.copyFrom(rect); + hs.vCopy(rect.hs); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionRect hs=" + hs + "]"; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionRect(hs, collisionGroup); + } + + /** + * @param origin + * @param vector + * @param collisionGroup + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + + // Проверка параллельности сегмента и плоскости примитива + if (vz > -threshold && vz < threshold) return -1; + var t:Number = -oz/vz; + if (t < 0) return -1; + // Проверка вхождения точки пересечения в прямоугольник + ox += vx*t; + oy += vy*t; + oz = 0; + if (ox < (-hs.x - threshold) || ox > (hs.x + threshold) || oy < (-hs.y - threshold) || oy > (hs.y + threshold)) return -1; + + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + return t; + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/primitives/CollisionSphere.as b/0.0.7.0/src/alternativa/physics/collision/primitives/CollisionSphere.as new file mode 100644 index 0000000..887a19a --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/primitives/CollisionSphere.as @@ -0,0 +1,81 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.math.Vector3; + use namespace altphysics; + + + /** + * Сфера. + */ + public class CollisionSphere extends CollisionPrimitive { + + // Радиус сферы + public var r:Number = 0; + + /** + * + * @param r + * @param collisionGroup + */ + public function CollisionSphere(r:Number, collisionGroup:int) { + super(SPHERE, collisionGroup); + this.r = r; + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + aabb.maxX = transform.d + r; + aabb.minX = transform.d - r; + + aabb.maxY = transform.h + r; + aabb.minY = transform.h - r; + + aabb.maxZ = transform.l + r; + aabb.minZ = transform.l - r; + + return aabb; + } + + /** + * @param origin + * @param vector + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var px:Number = origin.x - transform.d; + var py:Number = origin.y - transform.h; + var pz:Number = origin.z - transform.l; + var k:Number = vector.x*px + vector.y*py + vector.z*pz; + if (k > 0) return -1; + var a:Number = vector.x*vector.x + vector.y*vector.y + vector.z*vector.z; + var D:Number = k*k - a*(px*px + py*py + pz*pz - r*r); + if (D < 0) return -1; + return -(k + Math.sqrt(D))/a; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var sphere:CollisionSphere = source as CollisionSphere; + if (sphere == null) return this; + super.copyFrom(sphere); + r = sphere.r; + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionSphere(r, collisionGroup); + } + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/primitives/CollisionTriangle.as b/0.0.7.0/src/alternativa/physics/collision/primitives/CollisionTriangle.as new file mode 100644 index 0000000..9cd6d00 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/primitives/CollisionTriangle.as @@ -0,0 +1,244 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.math.Vector3; + use namespace altphysics; + + /** + * Примитив треугольник. + */ + public class CollisionTriangle extends CollisionPrimitive { + + public var v0:Vector3 = new Vector3(); + public var v1:Vector3 = new Vector3(); + public var v2:Vector3 = new Vector3(); + + public var e0:Vector3 = new Vector3(); + public var e1:Vector3 = new Vector3(); + public var e2:Vector3 = new Vector3(); + +// public var len0:Number; +// public var len1:Number; +// public var len2:Number; +// + /** + * + * @param v0 + * @param v1 + * @param v2 + * @param collisionGroup + */ + public function CollisionTriangle(v0:Vector3, v1:Vector3, v2:Vector3, collisionGroup:int) { + super(TRIANGLE, collisionGroup); + initVertices(v0, v1, v2); + } + + /** + * Рассчитывает AABB примитива. + * + * @return ссылка на свой AABB + */ + override public function calculateAABB():BoundBox { + var epsilon:Number = 0.005; + var a:Number; + var b:Number; + var eps_c:Number = epsilon*transform.c; + var eps_g:Number = epsilon*transform.g; + var eps_k:Number = epsilon*transform.k; + + // Вершина 0 + // Ось X + a = v0.x*transform.a + v0.y*transform.b; + aabb.minX = aabb.maxX = a + eps_c; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v0.x*transform.e + v0.y*transform.f; + aabb.minY = aabb.maxY = a + eps_g; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v0.x*transform.i + v0.y*transform.j; + aabb.minZ = aabb.maxZ = a + eps_k; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 1 + // Ось X + a = v1.x*transform.a + v1.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v1.x*transform.e + v1.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v1.x*transform.i + v1.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 2 + // Ось X + a = v2.x*transform.a + v2.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v2.x*transform.e + v2.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v2.x*transform.i + v2.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + aabb.minX += transform.d; + aabb.maxX += transform.d; + + aabb.minY += transform.h; + aabb.maxY += transform.h; + + aabb.minZ += transform.l; + aabb.maxZ += transform.l; + + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + // Луч трансформируется в систему координат примитива, затем проверяется пересечение + + var vz:Number = vector.x*transform.c + vector.y*transform.g + vector.z*transform.k; + // Если луч параллелен плоскости птимитива, то пересечения нет + if (vz < epsilon && vz > -epsilon) return -1; + + var tx:Number = origin.x - transform.d; + var ty:Number = origin.y - transform.h; + var tz:Number = origin.z - transform.l; + + var oz:Number = tx*transform.c + ty*transform.g + tz*transform.k; + var t:Number = -oz/vz; + if (t < 0) return -1; + + var ox:Number = tx*transform.a + ty*transform.e + tz*transform.i; + var oy:Number = tx*transform.b + ty*transform.f + tz*transform.j; + + tx = ox + t*(vector.x*transform.a + vector.y*transform.e + vector.z*transform.i); + ty = oy + t*(vector.x*transform.b + vector.y*transform.f + vector.z*transform.j); + tz = oz + t*vz; + + // Проверка вхождения точки в треугольник + if ((e0.x*(ty - v0.y) - e0.y*(tx - v0.x) < 0) || (e1.x*(ty - v1.y) - e1.y*(tx - v1.x) < 0) || (e2.x*(ty - v2.y) - e2.y*(tx - v2.x) < 0)) return -1; + + // Запись нормали + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + + return t; + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + super.copyFrom(source); + var tri:CollisionTriangle = source as CollisionTriangle; + if (tri != null) { + v0.vCopy(tri.v0); + v1.vCopy(tri.v1); + v2.vCopy(tri.v2); + + e0.vCopy(tri.e0); + e1.vCopy(tri.e1); + e2.vCopy(tri.e2); + +// len0 = tri.len0; +// len1 = tri.len1; +// len2 = tri.len2; + } + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionTriangle v0=" + v0 + ", v1=" + v1 + ", v2=" + v2 + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionTriangle(v0, v1, v2, collisionGroup); + } + + /** + * + * @param v0 + * @param v1 + * @param v2 + */ + private function initVertices(v0:Vector3, v1:Vector3, v2:Vector3):void { + this.v0.vCopy(v0); + this.v1.vCopy(v1); + this.v2.vCopy(v2); + + e0.vDiff(v1, v0); +// len0 = e0.vLength(); + e0.vNormalize(); + + e1.vDiff(v2, v1); +// len1 = e1.vLength(); + e1.vNormalize(); + + e2.vDiff(v0, v2); +// len2 = e2.vLength(); + e2.vNormalize(); + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/types/.svn/all-wcprops b/0.0.7.0/src/alternativa/physics/collision/types/.svn/all-wcprops new file mode 100644 index 0000000..3ea7134 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/types/.svn/all-wcprops @@ -0,0 +1,23 @@ +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/collision/types +END +BoundBox.as +K 25 +svn:wc:ra_dav:version-url +V 131 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/collision/types/BoundBox.as +END +RayIntersection.as +K 25 +svn:wc:ra_dav:version-url +V 138 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/collision/types/RayIntersection.as +END +Ray.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/collision/types/Ray.as +END diff --git a/0.0.7.0/src/alternativa/physics/collision/types/.svn/entries b/0.0.7.0/src/alternativa/physics/collision/types/.svn/entries new file mode 100644 index 0000000..0e62e32 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/types/.svn/entries @@ -0,0 +1,64 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/collision/types +http://svndev.alternativaplatform.com + + + +2009-10-19T07:17:33.113306Z +22253 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +BoundBox.as +file + + + + +2010-10-28T04:32:42.000000Z +d92d4ab3dde698feb674fb9488f59678 +2009-09-09T14:02:48.134744Z +19633 +mike + +RayIntersection.as +file + + + + +2010-10-28T04:32:42.000000Z +4002cb4f3effa05aedf5c38dfb3ade7c +2009-10-19T07:17:33.113306Z +22253 +mike + +Ray.as +file + + + + +2010-10-28T04:32:42.000000Z +77598eacffac4c232aeeaaf83f4b27e5 +2009-10-19T07:17:33.113306Z +22253 +mike + diff --git a/0.0.7.0/src/alternativa/physics/collision/types/.svn/format b/0.0.7.0/src/alternativa/physics/collision/types/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/types/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.7.0/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base b/0.0.7.0/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base new file mode 100644 index 0000000..b4d5278 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base @@ -0,0 +1,75 @@ +package alternativa.physics.collision.types { + + /** + * Ограничивающий параллелепипед. + */ + public class BoundBox { + + public var minX:Number = 1e308; + public var minY:Number = 1e308; + public var minZ:Number = 1e308; + public var maxX:Number = -1e308; + public var maxY:Number = -1e308; + public var maxZ:Number = -1e308; + + public function setSize(minX:Number, minY:Number, minZ:Number, maxX:Number, maxY:Number, maxZ:Number):void { + this.minX = minX; + this.minY = minY; + this.minZ = minZ; + this.maxX = maxX; + this.maxY = maxY; + this.maxZ = maxZ; + } + + public function addBoundBox(boundBox:BoundBox):void { + minX = (boundBox.minX < minX) ? boundBox.minX : minX; + minY = (boundBox.minY < minY) ? boundBox.minY : minY; + minZ = (boundBox.minZ < minZ) ? boundBox.minZ : minZ; + maxX = (boundBox.maxX > maxX) ? boundBox.maxX : maxX; + maxY = (boundBox.maxY > maxY) ? boundBox.maxY : maxY; + maxZ = (boundBox.maxZ > maxZ) ? boundBox.maxZ : maxZ; + } + + public function addPoint(x:Number, y:Number, z:Number):void { + if (x < minX) minX = x; + if (x > maxX) maxX = x; + if (y < minY) minY = y; + if (y > maxY) maxY = y; + if (z < minZ) minZ = z; + if (z > maxZ) maxZ = z; + } + + public function infinity():void { + minX = 1e308; + minY = 1e308; + minZ = 1e308; + maxX = -1e308; + maxY = -1e308; + maxZ = -1e308; + } + + public function intersects(bb:BoundBox, epsilon:Number):Boolean { + return !(minX > bb.maxX + epsilon || maxX < bb.minX - epsilon || minY > bb.maxY + epsilon || maxY < bb.minY - epsilon || minZ > bb.maxZ + epsilon || maxZ < bb.minZ - epsilon); + } + + public function copyFrom(boundBox:BoundBox):void { + minX = boundBox.minX; + minY = boundBox.minY; + minZ = boundBox.minZ; + maxX = boundBox.maxX; + maxY = boundBox.maxY; + maxZ = boundBox.maxZ; + } + + public function clone():BoundBox { + var clone:BoundBox = new BoundBox(); + clone.copyFrom(this); + return clone; + } + + public function toString():String { + return "BoundBox [" + minX + ", " + minY + ", " + minZ + " : " + maxX + ", " + maxY + ", " + maxZ + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base b/0.0.7.0/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base new file mode 100644 index 0000000..01c48e3 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base @@ -0,0 +1,13 @@ +package alternativa.physics.collision.types { + import alternativa.physics.math.Vector3; + + /** + * Структура, описывающая луч и сегмент прямой в пространстве. + */ + public class Ray { + // Начало луча. Начальная точка сегмента на прямой луча. + public var origin:Vector3 = new Vector3(); + // Направляющий вектор луча. Длина вектора задаёт длину сегмента на луче. Конечная точка сегмента вычисляется как origin + dir. + public var dir:Vector3 = new Vector3(); + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base b/0.0.7.0/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base new file mode 100644 index 0000000..cbf3607 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base @@ -0,0 +1,25 @@ +package alternativa.physics.collision.types { + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.math.Vector3; + + /** + * Структура описывает точку пересечения луча с физической геометрией. + */ + public class RayIntersection { + // Физический примитив, с которым пересекается луч + public var primitive:CollisionPrimitive; + // Положенеи точки пересечения + public var pos:Vector3 = new Vector3(); + // Нормаль поверхности физического примитива в точке пересечения + public var normal:Vector3 = new Vector3(); + // Время пересечения (|pos - ray.origin|/|ray.dir|) + public var t:Number = 0; + + public function copy(source:RayIntersection):void { + primitive = source.primitive; + pos.vCopy(source.pos); + normal.vCopy(source.normal); + t = source.t; + } + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/types/BoundBox.as b/0.0.7.0/src/alternativa/physics/collision/types/BoundBox.as new file mode 100644 index 0000000..b4d5278 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/types/BoundBox.as @@ -0,0 +1,75 @@ +package alternativa.physics.collision.types { + + /** + * Ограничивающий параллелепипед. + */ + public class BoundBox { + + public var minX:Number = 1e308; + public var minY:Number = 1e308; + public var minZ:Number = 1e308; + public var maxX:Number = -1e308; + public var maxY:Number = -1e308; + public var maxZ:Number = -1e308; + + public function setSize(minX:Number, minY:Number, minZ:Number, maxX:Number, maxY:Number, maxZ:Number):void { + this.minX = minX; + this.minY = minY; + this.minZ = minZ; + this.maxX = maxX; + this.maxY = maxY; + this.maxZ = maxZ; + } + + public function addBoundBox(boundBox:BoundBox):void { + minX = (boundBox.minX < minX) ? boundBox.minX : minX; + minY = (boundBox.minY < minY) ? boundBox.minY : minY; + minZ = (boundBox.minZ < minZ) ? boundBox.minZ : minZ; + maxX = (boundBox.maxX > maxX) ? boundBox.maxX : maxX; + maxY = (boundBox.maxY > maxY) ? boundBox.maxY : maxY; + maxZ = (boundBox.maxZ > maxZ) ? boundBox.maxZ : maxZ; + } + + public function addPoint(x:Number, y:Number, z:Number):void { + if (x < minX) minX = x; + if (x > maxX) maxX = x; + if (y < minY) minY = y; + if (y > maxY) maxY = y; + if (z < minZ) minZ = z; + if (z > maxZ) maxZ = z; + } + + public function infinity():void { + minX = 1e308; + minY = 1e308; + minZ = 1e308; + maxX = -1e308; + maxY = -1e308; + maxZ = -1e308; + } + + public function intersects(bb:BoundBox, epsilon:Number):Boolean { + return !(minX > bb.maxX + epsilon || maxX < bb.minX - epsilon || minY > bb.maxY + epsilon || maxY < bb.minY - epsilon || minZ > bb.maxZ + epsilon || maxZ < bb.minZ - epsilon); + } + + public function copyFrom(boundBox:BoundBox):void { + minX = boundBox.minX; + minY = boundBox.minY; + minZ = boundBox.minZ; + maxX = boundBox.maxX; + maxY = boundBox.maxY; + maxZ = boundBox.maxZ; + } + + public function clone():BoundBox { + var clone:BoundBox = new BoundBox(); + clone.copyFrom(this); + return clone; + } + + public function toString():String { + return "BoundBox [" + minX + ", " + minY + ", " + minZ + " : " + maxX + ", " + maxY + ", " + maxZ + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/types/Ray.as b/0.0.7.0/src/alternativa/physics/collision/types/Ray.as new file mode 100644 index 0000000..01c48e3 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/types/Ray.as @@ -0,0 +1,13 @@ +package alternativa.physics.collision.types { + import alternativa.physics.math.Vector3; + + /** + * Структура, описывающая луч и сегмент прямой в пространстве. + */ + public class Ray { + // Начало луча. Начальная точка сегмента на прямой луча. + public var origin:Vector3 = new Vector3(); + // Направляющий вектор луча. Длина вектора задаёт длину сегмента на луче. Конечная точка сегмента вычисляется как origin + dir. + public var dir:Vector3 = new Vector3(); + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/collision/types/RayIntersection.as b/0.0.7.0/src/alternativa/physics/collision/types/RayIntersection.as new file mode 100644 index 0000000..cbf3607 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/collision/types/RayIntersection.as @@ -0,0 +1,25 @@ +package alternativa.physics.collision.types { + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.math.Vector3; + + /** + * Структура описывает точку пересечения луча с физической геометрией. + */ + public class RayIntersection { + // Физический примитив, с которым пересекается луч + public var primitive:CollisionPrimitive; + // Положенеи точки пересечения + public var pos:Vector3 = new Vector3(); + // Нормаль поверхности физического примитива в точке пересечения + public var normal:Vector3 = new Vector3(); + // Время пересечения (|pos - ray.origin|/|ray.dir|) + public var t:Number = 0; + + public function copy(source:RayIntersection):void { + primitive = source.primitive; + pos.vCopy(source.pos); + normal.vCopy(source.normal); + t = source.t; + } + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/constraints/.svn/all-wcprops b/0.0.7.0/src/alternativa/physics/constraints/.svn/all-wcprops new file mode 100644 index 0000000..e42f295 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/constraints/.svn/all-wcprops @@ -0,0 +1,17 @@ +K 25 +svn:wc:ra_dav:version-url +V 115 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/constraints +END +Constraint.as +K 25 +svn:wc:ra_dav:version-url +V 129 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/constraints/Constraint.as +END +MaxDistanceConstraint.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/constraints/MaxDistanceConstraint.as +END diff --git a/0.0.7.0/src/alternativa/physics/constraints/.svn/entries b/0.0.7.0/src/alternativa/physics/constraints/.svn/entries new file mode 100644 index 0000000..17c7048 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/constraints/.svn/entries @@ -0,0 +1,52 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/constraints +http://svndev.alternativaplatform.com + + + +2009-10-19T07:17:33.113306Z +22253 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Constraint.as +file + + + + +2010-10-28T04:32:42.000000Z +2a02ebce2cfc45515549bd41adc8215b +2009-10-19T07:17:33.113306Z +22253 +mike + +MaxDistanceConstraint.as +file + + + + +2010-10-28T04:32:42.000000Z +d6159b2e8bdaa7da4f1b419002c71a09 +2009-10-19T07:17:33.113306Z +22253 +mike + diff --git a/0.0.7.0/src/alternativa/physics/constraints/.svn/format b/0.0.7.0/src/alternativa/physics/constraints/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/constraints/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.7.0/src/alternativa/physics/constraints/.svn/text-base/Constraint.as.svn-base b/0.0.7.0/src/alternativa/physics/constraints/.svn/text-base/Constraint.as.svn-base new file mode 100644 index 0000000..4676d0d --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/constraints/.svn/text-base/Constraint.as.svn-base @@ -0,0 +1,35 @@ +package alternativa.physics.constraints { + + import alternativa.physics.World; + import alternativa.physics.altphysics; + + use namespace altphysics; + + /** + * + */ + public class Constraint { + + altphysics var satisfied:Boolean; + altphysics var world:World; + + /** + * + */ + public function Constraint() { + } + + /** + * @param dt + */ + public function preProcess(dt:Number):void { + } + + /** + * @param dt + */ + public function apply(dt:Number):void { + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base b/0.0.7.0/src/alternativa/physics/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base new file mode 100644 index 0000000..c87e0a3 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base @@ -0,0 +1,183 @@ +package alternativa.physics.constraints { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.math.Matrix3; + import alternativa.physics.math.Vector3; + + use namespace altphysics; + + /** + * + */ + public class MaxDistanceConstraint extends Constraint { + // Первое тело ограничения + altphysics var body1:Body; + // Второе тело ограничения. Может быть null. + altphysics var body2:Body; + // Первая точка крепления в системе координат первого тела + altphysics var r1:Vector3; + // Вторая точка привязки в системе координат второго тела. В случае отсутствия второго тела, координаты задаются в мировой системе. + altphysics var r2:Vector3; + // Максимально допустимое расстояние между точками крепления + altphysics var maxDistance:Number; + // Радиус-вектор первой точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr1:Vector3 = new Vector3(); + // Радиус-вектор второй точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr2:Vector3 = new Vector3(); + // Минимальная скорость сближения для удовлетворения ограничения + private var minClosingVel:Number; + // Изменение относительной скорости на единицу импульса + private var velByUnitImpulseN:Number; + // Направление корректирующего импульса + private var impulseDirection:Vector3 = new Vector3(); + + // Вспомогательные переменные +// private static var _v:Vector3 = new Vector3(); +// private static var _v1:Vector3 = new Vector3(); +// private static var _v2:Vector3 = new Vector3(); + + /** + * @param body1 + * @param body2 + * @param r1 + * @param r2 + * @param maxDistance + */ + public function MaxDistanceConstraint(body1:Body, body2:Body, r1:Vector3, r2:Vector3, maxDistance:Number) { + super(); + this.body1 = body1; + this.body2 = body2; + this.r1 = r1.vClone(); + this.r2 = r2.vClone(); + this.maxDistance = maxDistance; + } + + /** + * @param dt + */ + override public function preProcess(dt:Number):void { + // Вычислим расстояние между точками + var m:Matrix3 = body1.baseMatrix; + wr1.x = m.a*r1.x + m.b*r1.y + m.c*r1.z; + wr1.y = m.e*r1.x + m.f*r1.y + m.g*r1.z; + wr1.z = m.i*r1.x + m.j*r1.y + m.k*r1.z; + if (body2 != null) { + m = body2.baseMatrix; + wr2.x = m.a*r2.x + m.b*r2.y + m.c*r2.z; + wr2.y = m.e*r2.x + m.f*r2.y + m.g*r2.z; + wr2.z = m.i*r2.x + m.j*r2.y + m.k*r2.z; + } else { + wr2.x = r2.x; + wr2.y = r2.y; + wr2.z = r2.z; + } + var p1:Vector3 = body1.state.pos; + impulseDirection.x = wr2.x - wr1.x - p1.x; + impulseDirection.y = wr2.y - wr1.y - p1.y; + impulseDirection.z = wr2.z - wr1.z - p1.z; + if (body2 != null) { + var p2:Vector3 = body2.state.pos; + impulseDirection.x += p2.x; + impulseDirection.y += p2.y; + impulseDirection.z += p2.z; + } + + var len:Number = Math.sqrt(impulseDirection.x*impulseDirection.x + impulseDirection.y*impulseDirection.y + impulseDirection.z*impulseDirection.z); + var delta:Number = len - maxDistance; + if (delta > 0) { + satisfied = false; + if (len < 0.001) { + impulseDirection.x = 1; + } else { + len = 1/len; + impulseDirection.x *= len; + impulseDirection.y *= len; + impulseDirection.z *= len; + } + minClosingVel = delta/(world.penResolutionSteps*dt); + if (minClosingVel > world.maxPenResolutionSpeed) { + minClosingVel = world.maxPenResolutionSpeed; + } + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + var x:Number; + var y:Number; + var z:Number; + var vx:Number; + var vy:Number; + var vz:Number; + velByUnitImpulseN = 0; + if (body1.movable) { +// velByUnitImpulseN += body1.invMass + _v.vCross2(wr1, impulseDirection).vTransformBy3(body1.invInertiaWorld).vCross(wr1).vDot(impulseDirection); + vx = wr1.y*impulseDirection.z - wr1.z*impulseDirection.y; + vy = wr1.z*impulseDirection.x - wr1.x*impulseDirection.z; + vz = wr1.x*impulseDirection.y - wr1.y*impulseDirection.x; + + m = body1.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr1.z - z*wr1.y; + vy = z*wr1.x - x*wr1.z; + vz = x*wr1.y - y*wr1.x; + velByUnitImpulseN += body1.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + if (body2 != null && body2.movable) { +// velByUnitImpulseN += body2.invMass + _v.vCross2(wr2, impulseDirection).vTransformBy3(body2.invInertiaWorld).vCross(wr2).vDot(impulseDirection); + vx = wr2.y*impulseDirection.z - wr2.z*impulseDirection.y; + vy = wr2.z*impulseDirection.x - wr2.x*impulseDirection.z; + vz = wr2.x*impulseDirection.y - wr2.y*impulseDirection.x; + + m = body2.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr2.z - z*wr2.y; + vy = z*wr2.x - x*wr2.z; + vz = x*wr2.y - y*wr2.x; + velByUnitImpulseN += body2.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + } else { + satisfied = true; + } + } + + /** + * @param dt + */ + override public function apply(dt:Number):void { + if (satisfied) return; + // Расчитываем проекцию скорости относительной скорости + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 + var vel:Vector3 = body1.state.velocity; + var rot:Vector3 = body1.state.rotation; +// v1Cross2(state.rotation, wr1); + var vx:Number = vel.x + rot.y*wr1.z - rot.z*wr1.y; + var vy:Number = vel.y + rot.z*wr1.x - rot.x*wr1.z; + var vz:Number = vel.z + rot.x*wr1.y - rot.y*wr1.x; + // V2 = V2_c + w2%r2 + if (body2 != null) { + vel = body2.state.velocity; + rot = body2.state.rotation; +// _v2.vCross2(state.rotation, wr2); + vx -= vel.x + rot.y*wr2.z - rot.z*wr2.y; + vy -= vel.y + rot.z*wr2.x - rot.x*wr2.z; + vz -= vel.z + rot.x*wr2.y - rot.y*wr2.x; + } + var closingVel:Number = vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + if (closingVel > minClosingVel) return; + var impulse:Number = (minClosingVel - closingVel)/velByUnitImpulseN; + if (body1.movable) { + body1.applyRelPosWorldImpulse(wr1, impulseDirection, impulse); + } + if (body2 != null && body2.movable) { + body2.applyRelPosWorldImpulse(wr2, impulseDirection, -impulse); + } + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/constraints/Constraint.as b/0.0.7.0/src/alternativa/physics/constraints/Constraint.as new file mode 100644 index 0000000..4676d0d --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/constraints/Constraint.as @@ -0,0 +1,35 @@ +package alternativa.physics.constraints { + + import alternativa.physics.World; + import alternativa.physics.altphysics; + + use namespace altphysics; + + /** + * + */ + public class Constraint { + + altphysics var satisfied:Boolean; + altphysics var world:World; + + /** + * + */ + public function Constraint() { + } + + /** + * @param dt + */ + public function preProcess(dt:Number):void { + } + + /** + * @param dt + */ + public function apply(dt:Number):void { + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/constraints/MaxDistanceConstraint.as b/0.0.7.0/src/alternativa/physics/constraints/MaxDistanceConstraint.as new file mode 100644 index 0000000..c87e0a3 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/constraints/MaxDistanceConstraint.as @@ -0,0 +1,183 @@ +package alternativa.physics.constraints { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.math.Matrix3; + import alternativa.physics.math.Vector3; + + use namespace altphysics; + + /** + * + */ + public class MaxDistanceConstraint extends Constraint { + // Первое тело ограничения + altphysics var body1:Body; + // Второе тело ограничения. Может быть null. + altphysics var body2:Body; + // Первая точка крепления в системе координат первого тела + altphysics var r1:Vector3; + // Вторая точка привязки в системе координат второго тела. В случае отсутствия второго тела, координаты задаются в мировой системе. + altphysics var r2:Vector3; + // Максимально допустимое расстояние между точками крепления + altphysics var maxDistance:Number; + // Радиус-вектор первой точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr1:Vector3 = new Vector3(); + // Радиус-вектор второй точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr2:Vector3 = new Vector3(); + // Минимальная скорость сближения для удовлетворения ограничения + private var minClosingVel:Number; + // Изменение относительной скорости на единицу импульса + private var velByUnitImpulseN:Number; + // Направление корректирующего импульса + private var impulseDirection:Vector3 = new Vector3(); + + // Вспомогательные переменные +// private static var _v:Vector3 = new Vector3(); +// private static var _v1:Vector3 = new Vector3(); +// private static var _v2:Vector3 = new Vector3(); + + /** + * @param body1 + * @param body2 + * @param r1 + * @param r2 + * @param maxDistance + */ + public function MaxDistanceConstraint(body1:Body, body2:Body, r1:Vector3, r2:Vector3, maxDistance:Number) { + super(); + this.body1 = body1; + this.body2 = body2; + this.r1 = r1.vClone(); + this.r2 = r2.vClone(); + this.maxDistance = maxDistance; + } + + /** + * @param dt + */ + override public function preProcess(dt:Number):void { + // Вычислим расстояние между точками + var m:Matrix3 = body1.baseMatrix; + wr1.x = m.a*r1.x + m.b*r1.y + m.c*r1.z; + wr1.y = m.e*r1.x + m.f*r1.y + m.g*r1.z; + wr1.z = m.i*r1.x + m.j*r1.y + m.k*r1.z; + if (body2 != null) { + m = body2.baseMatrix; + wr2.x = m.a*r2.x + m.b*r2.y + m.c*r2.z; + wr2.y = m.e*r2.x + m.f*r2.y + m.g*r2.z; + wr2.z = m.i*r2.x + m.j*r2.y + m.k*r2.z; + } else { + wr2.x = r2.x; + wr2.y = r2.y; + wr2.z = r2.z; + } + var p1:Vector3 = body1.state.pos; + impulseDirection.x = wr2.x - wr1.x - p1.x; + impulseDirection.y = wr2.y - wr1.y - p1.y; + impulseDirection.z = wr2.z - wr1.z - p1.z; + if (body2 != null) { + var p2:Vector3 = body2.state.pos; + impulseDirection.x += p2.x; + impulseDirection.y += p2.y; + impulseDirection.z += p2.z; + } + + var len:Number = Math.sqrt(impulseDirection.x*impulseDirection.x + impulseDirection.y*impulseDirection.y + impulseDirection.z*impulseDirection.z); + var delta:Number = len - maxDistance; + if (delta > 0) { + satisfied = false; + if (len < 0.001) { + impulseDirection.x = 1; + } else { + len = 1/len; + impulseDirection.x *= len; + impulseDirection.y *= len; + impulseDirection.z *= len; + } + minClosingVel = delta/(world.penResolutionSteps*dt); + if (minClosingVel > world.maxPenResolutionSpeed) { + minClosingVel = world.maxPenResolutionSpeed; + } + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + var x:Number; + var y:Number; + var z:Number; + var vx:Number; + var vy:Number; + var vz:Number; + velByUnitImpulseN = 0; + if (body1.movable) { +// velByUnitImpulseN += body1.invMass + _v.vCross2(wr1, impulseDirection).vTransformBy3(body1.invInertiaWorld).vCross(wr1).vDot(impulseDirection); + vx = wr1.y*impulseDirection.z - wr1.z*impulseDirection.y; + vy = wr1.z*impulseDirection.x - wr1.x*impulseDirection.z; + vz = wr1.x*impulseDirection.y - wr1.y*impulseDirection.x; + + m = body1.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr1.z - z*wr1.y; + vy = z*wr1.x - x*wr1.z; + vz = x*wr1.y - y*wr1.x; + velByUnitImpulseN += body1.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + if (body2 != null && body2.movable) { +// velByUnitImpulseN += body2.invMass + _v.vCross2(wr2, impulseDirection).vTransformBy3(body2.invInertiaWorld).vCross(wr2).vDot(impulseDirection); + vx = wr2.y*impulseDirection.z - wr2.z*impulseDirection.y; + vy = wr2.z*impulseDirection.x - wr2.x*impulseDirection.z; + vz = wr2.x*impulseDirection.y - wr2.y*impulseDirection.x; + + m = body2.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr2.z - z*wr2.y; + vy = z*wr2.x - x*wr2.z; + vz = x*wr2.y - y*wr2.x; + velByUnitImpulseN += body2.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + } else { + satisfied = true; + } + } + + /** + * @param dt + */ + override public function apply(dt:Number):void { + if (satisfied) return; + // Расчитываем проекцию скорости относительной скорости + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 + var vel:Vector3 = body1.state.velocity; + var rot:Vector3 = body1.state.rotation; +// v1Cross2(state.rotation, wr1); + var vx:Number = vel.x + rot.y*wr1.z - rot.z*wr1.y; + var vy:Number = vel.y + rot.z*wr1.x - rot.x*wr1.z; + var vz:Number = vel.z + rot.x*wr1.y - rot.y*wr1.x; + // V2 = V2_c + w2%r2 + if (body2 != null) { + vel = body2.state.velocity; + rot = body2.state.rotation; +// _v2.vCross2(state.rotation, wr2); + vx -= vel.x + rot.y*wr2.z - rot.z*wr2.y; + vy -= vel.y + rot.z*wr2.x - rot.x*wr2.z; + vz -= vel.z + rot.x*wr2.y - rot.y*wr2.x; + } + var closingVel:Number = vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + if (closingVel > minClosingVel) return; + var impulse:Number = (minClosingVel - closingVel)/velByUnitImpulseN; + if (body1.movable) { + body1.applyRelPosWorldImpulse(wr1, impulseDirection, impulse); + } + if (body2 != null && body2.movable) { + body2.applyRelPosWorldImpulse(wr2, impulseDirection, -impulse); + } + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/math/.svn/all-wcprops b/0.0.7.0/src/alternativa/physics/math/.svn/all-wcprops new file mode 100644 index 0000000..b1ac5db --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/math/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 108 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/math +END +Quaternion.as +K 25 +svn:wc:ra_dav:version-url +V 122 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/math/Quaternion.as +END +Vector3.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/math/Vector3.as +END +Matrix3.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/math/Matrix3.as +END +Matrix4.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/math/Matrix4.as +END diff --git a/0.0.7.0/src/alternativa/physics/math/.svn/entries b/0.0.7.0/src/alternativa/physics/math/.svn/entries new file mode 100644 index 0000000..e5a2c67 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/math/.svn/entries @@ -0,0 +1,76 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/math +http://svndev.alternativaplatform.com + + + +2009-10-19T07:17:33.113306Z +22253 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Quaternion.as +file + + + + +2010-10-28T04:32:42.000000Z +813c4e874ea2dae729983537e27d5648 +2009-10-19T07:17:33.113306Z +22253 +mike + +Vector3.as +file + + + + +2010-10-28T04:32:42.000000Z +72a5b87dac5c32096c716639488b92e2 +2009-10-19T07:17:33.113306Z +22253 +mike + +Matrix3.as +file + + + + +2010-10-28T04:32:42.000000Z +c99517d0c388915e157464d4b31a0b86 +2009-10-19T07:17:33.113306Z +22253 +mike + +Matrix4.as +file + + + + +2010-10-28T04:32:42.000000Z +3f37b8a6a7e9f111a35e6ee1fe63dfbc +2009-10-19T07:17:33.113306Z +22253 +mike + diff --git a/0.0.7.0/src/alternativa/physics/math/.svn/format b/0.0.7.0/src/alternativa/physics/math/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/math/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.7.0/src/alternativa/physics/math/.svn/text-base/Matrix3.as.svn-base b/0.0.7.0/src/alternativa/physics/math/.svn/text-base/Matrix3.as.svn-base new file mode 100644 index 0000000..0b4dcd0 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/math/.svn/text-base/Matrix3.as.svn-base @@ -0,0 +1,391 @@ +package alternativa.physics.math { + import flash.geom.Vector3D; + + + /** + * + */ + public class Matrix3 { + + public static const ZERO:Matrix3 = new Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0); + public static const IDENTITY:Matrix3 = new Matrix3(); + + public var a:Number; + public var b:Number; + public var c:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix3(a:Number = 1, b:Number = 0, c:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1) { + this.a = a; + this.b = b; + this.c = c; + + this.e = e; + this.f = f; + this.g = g; + + this.i = i; + this.j = j; + this.k = k; + } + + /** + * + * @return + */ + public function toIdentity():Matrix3 { + a = f = k = 1; + b = c = e = g = i = j = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + var det:Number = 1/(-cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk); + + a = (ff*kk - gg*jj)*det; + b = (cc*jj - bb*kk)*det; + c = (bb*gg - cc*ff)*det; + e = (gg*ii - ee*kk)*det; + f = (aa*kk - cc*ii)*det; + g = (cc*ee - aa*gg)*det; + i = (ee*jj - ff*ii)*det; + j = (bb*ii - aa*jj)*det; + k = (aa*ff - bb*ee)*det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prependTransposed(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.b + cc*m.c; + b = aa*m.e + bb*m.f + cc*m.g; + c = aa*m.i + bb*m.j + cc*m.k; + e = ee*m.a + ff*m.b + gg*m.c; + f = ee*m.e + ff*m.f + gg*m.g; + g = ee*m.i + ff*m.j + gg*m.k; + i = ii*m.a + jj*m.b + kk*m.c; + j = ii*m.e + jj*m.f + kk*m.g; + k = ii*m.i + jj*m.j + kk*m.k; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix3):Matrix3 { + a += m.a; + b += m.b; + c += m.c; + e += m.e; + f += m.f; + g += m.g; + i += m.i; + j += m.j; + k += m.k; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix3):Matrix3 { + a -= m.a; + b -= m.b; + c -= m.c; + e -= m.e; + f -= m.f; + g -= m.g; + i -= m.i; + j -= m.j; + k -= m.k; + + return this; + } + + /** + * + * @return + */ + public function transpose():Matrix3 { + var tmp:Number = b; + b = e; + e = tmp; + tmp = c; + c = i; + i = tmp; + tmp = g; + g = j; + j = tmp; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector3To3D(vin:Vector3, vout:Vector3D):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * + * @param matrix + */ + public function createSkewSymmetric(v:Vector3):Matrix3 { + a = f = k = 0; + b = -v.z; + c = v.y; + e = v.z; + g = -v.x; + i = -v.y; + j = v.x; + return this; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix3):Matrix3 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @return + */ + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):Matrix3 { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + + return this; + } + + /** + * Метод формирует матрицу поворота на заданный угол относительно заданной оси. + * + * @param axis нормализованный вектор, задающий ось, относительно которой выполняется поворот + * @param angle угол поворота в радианах + */ + public function fromAxisAngle(axis:Vector3, angle:Number):void { + var c1:Number = Math.cos(angle); + var s:Number = Math.sin(angle); + var t:Number = 1 - c1; + var x:Number = axis.x; + var y:Number = axis.y; + var z:Number = axis.z; + + a = t*x*x + c1; + b = t*x*y - z*s; + c = t*x*z + y*s; + + e = t*x*y + z*s; + f = t*y*y + c1; + g = t*y*z - x*s; + + i = t*x*z - y*s; + j = t*y*z + x*s; + k = t*z*z + c1; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix3 { + return new Matrix3(a, b, c, e, f, g, i, j, k); + } + + /** + * + * @return + */ + public function toString():String { + return "[Matrix3 (" + a + ", " + b + ", " + c + "), (" + e + ", " + f + ", " + g + "), (" + i + ", " + j + ", " + k + ")]"; + } + + /** + * @param angles + */ + public function getEulerAngles(angles:Vector3):void { + if (-1 < i && i < 1) { + angles.x = Math.atan2(j, k); + angles.y = -Math.asin(i); + angles.z = Math.atan2(e, a); + } else { + angles.x = 0; + angles.y = (i <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-b, f); + } + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/math/.svn/text-base/Matrix4.as.svn-base b/0.0.7.0/src/alternativa/physics/math/.svn/text-base/Matrix4.as.svn-base new file mode 100644 index 0000000..939d46a --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/math/.svn/text-base/Matrix4.as.svn-base @@ -0,0 +1,461 @@ +package alternativa.physics.math { + import __AS3__.vec.Vector; + + /** + * + */ + public class Matrix4 { + + public static const IDENTITY:Matrix4 = new Matrix4(); + + public var a:Number; + public var b:Number; + public var c:Number; + public var d:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + public var h:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + public var l:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix4(a:Number = 1, b:Number = 0, c:Number = 0, d:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, h:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1, l:Number = 0) { + this.a = a; + this.b = b; + this.c = c; + this.d = d; + + this.e = e; + this.f = f; + this.g = g; + this.h = h; + + this.i = i; + this.j = j; + this.k = k; + this.l = l; + } + + /** + * + * @return + */ + public function toIdentity():Matrix4 { + a = f = k = 1; + b = c = e = g = i = j = d = h = l = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + var det:Number = -cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk; + + a = (-gg*jj + ff*kk)/det; + b = (cc*jj - bb*kk)/det; + c = (-cc*ff + bb*gg)/det; + d = (dd*gg*jj - cc*hh*jj - dd*ff*kk + bb*hh*kk + cc*ff*ll - bb*gg*ll)/det; + e = (gg*ii - ee*kk)/det; + f = (-cc*ii + aa*kk)/det; + g = (cc*ee - aa*gg)/det; + h = (cc*hh*ii - dd*gg*ii + dd*ee*kk - aa*hh*kk - cc*ee*ll + aa*gg*ll)/det; + i = (-ff*ii + ee*jj)/det; + j = (bb*ii - aa*jj)/det; + k = (-bb*ee + aa*ff)/det; + l = (dd*ff*ii - bb*hh*ii - dd*ee*jj + aa*hh*jj + bb*ee*ll - aa*ff*ll)/det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + d = m.a*dd + m.b*hh + m.c*ll + m.d; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + h = m.e*dd + m.f*hh + m.g*ll + m.h; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + l = m.i*dd + m.j*hh + m.k*ll + m.l; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + d = aa*m.d + bb*m.h + cc*m.l + dd; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + h = ee*m.d + ff*m.h + gg*m.l + hh; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + l = ii*m.d + jj*m.h + kk*m.l + ll; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix4):Matrix4 { + a += m.a; + b += m.b; + c += m.c; + d += m.d; + e += m.e; + f += m.f; + g += m.g; + h += m.h; + i += m.i; + j += m.j; + k += m.k; + l += m.l; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix4):Matrix4 { + a -= m.a; + b -= m.b; + c -= m.c; + d -= m.d; + e -= m.e; + f -= m.f; + g -= m.g; + h -= m.h; + i -= m.i; + j -= m.j; + k -= m.k; + l -= m.l; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectors(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + * @param len + */ + public function transformVectorsN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverse(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverseN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(idx:int, axis:Vector3):void { + switch (idx) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + case 3: + axis.x = d; + axis.y = h; + axis.z = l; + return; + } + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix4):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = m.d; + e = m.e; + f = m.f; + g = m.g; + h = m.h; + i = m.i; + j = m.j; + k = m.k; + l = m.l; + + return this; + } + + public function setFromMatrix3(m:Matrix3, offset:Vector3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = offset.x; + e = m.e; + f = m.f; + g = m.g; + h = offset.y; + i = m.i; + j = m.j; + k = m.k; + l = offset.z; + + return this; + } + + public function setOrientationFromMatrix3(m:Matrix3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @param angles + */ + public function getEulerAngles(angles:Vector3):void { + if (-1 < i && i < 1) { + angles.x = Math.atan2(j, k); + angles.y = -Math.asin(i); + angles.z = Math.atan2(e, a); + } else { + angles.x = 0; + angles.y = (i <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-b, f); + } + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix4 { + return new Matrix4(a, b, c, d, e, f, g, h, i, j, k, l); + } + + public function toString():String { + return "[Matrix4 [" + a.toFixed(3) + " " + b.toFixed(3) + " " + c.toFixed(3) + " " + d.toFixed(3) + "] [" + e.toFixed(3) + " " + f.toFixed(3) + " " + g.toFixed(3) + " " + h.toFixed(3) + "] [" + i.toFixed(3) + " " + j.toFixed(3) + " " + k.toFixed(3) + " " + l.toFixed(3) + "]]"; + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/math/.svn/text-base/Quaternion.as.svn-base b/0.0.7.0/src/alternativa/physics/math/.svn/text-base/Quaternion.as.svn-base new file mode 100644 index 0000000..cf39d7b --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/math/.svn/text-base/Quaternion.as.svn-base @@ -0,0 +1,397 @@ +package alternativa.physics.math { + import flash.geom.Vector3D; + + + public class Quaternion { + + public var w:Number; + public var x:Number; + public var y:Number; + public var z:Number; + + public static function multiply(q1:Quaternion, q2:Quaternion, result:Quaternion):void { + result.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z; + result.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y; + result.y = q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z; + result.z = q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x; + } + + public static function createFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngle(axis, angle); + return q; + } + + public static function createFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngleComponents(x, y, z, angle); + return q; + } + + public function Quaternion(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0) { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + } + + public function reset(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0):Quaternion { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + return this; + } + + public function normalize():Quaternion { + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * Умножает на указанный кватернион слева: this * q + * + * @param q множитель + */ + public function prepend(q:Quaternion):Quaternion { + var ww:Number = w*q.w - x*q.x - y*q.y - z*q.z; + var xx:Number = w*q.x + x*q.w + y*q.z - z*q.y; + var yy:Number = w*q.y + y*q.w + z*q.x - x*q.z; + var zz:Number = w*q.z + z*q.w + x*q.y - y*q.x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Умножает на указанный кватернион справа: q * this + * + * @param q множитель + */ + public function append(q:Quaternion):Quaternion { + var ww:Number = q.w*w - q.x*x - q.y*y - q.z*z; + var xx:Number = q.w*x + q.x*w + q.y*z - q.z*y; + var yy:Number = q.w*y + q.y*w + q.z*x - q.x*z; + var zz:Number = q.w*z + q.z*w + q.x*y - q.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * + * @param vector + */ + public function rotateByVector(v:Vector3):Quaternion { + var ww:Number = -v.x*x - v.y*y - v.z*z; + var xx:Number = v.x*w + v.y*z - v.z*y; + var yy:Number = v.y*w + v.z*x - v.x*z; + var zz:Number = v.z*w + v.x*y - v.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Добавляет вращение, приданное вектором угловой скорости за указанное время. + * + * @param v + * @param scale + */ + public function addScaledVector(v:Vector3, scale:Number):Quaternion { + var vx:Number = v.x*scale; + var vy:Number = v.y*scale; + var vz:Number = v.z*scale; + var ww:Number = -x*vx - y*vy - z*vz; + var xx:Number = vx*w + vy*z - vz*y; + var yy:Number = vy*w + vz*x - vx*z; + var zz:Number = vz*w + vx*y - vy*x; + w += 0.5*ww; + x += 0.5*xx; + y += 0.5*yy; + z += 0.5*zz; + // inlined normalize + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param m + * @return + */ + public function toMatrix3(m:Matrix3):Quaternion { + var xx2:Number = 2.0*x*x; + var yy2:Number = 2.0*y*y; + var zz2:Number = 2.0*z*z; + var xy2:Number = 2.0*x*y; + var yz2:Number = 2.0*y*z; + var zx2:Number = 2.0*z*x; + var wx2:Number = 2.0*w*x; + var wy2:Number = 2.0*w*y; + var wz2:Number = 2.0*w*z; + + m.a = 1.0 - yy2 - zz2; + m.b = xy2 - wz2; + m.c = zx2 + wy2; + + m.e = xy2 + wz2; + m.f = 1.0 - xx2 - zz2; + m.g = yz2 - wx2; + + m.i = zx2 - wy2; + m.j = yz2 + wx2; + m.k = 1.0 - xx2 - yy2; + return this; + } + + /** + * + * @return + */ + public function length():Number { + return Math.sqrt(w*w + x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function lengthSqr():Number { + return w*w + x*x + y*y + z*z; + } + + /** + * + * @param axis + * @param angle + * @return + */ + public function setFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z); + x = axis.x*k; + y = axis.y*k; + z = axis.z*k; + return this; + } + + /** + * + * @param x + * @param y + * @param z + * @param angle + */ + public function setFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(x*x + y*y + z*z); + this.x = x*k; + this.y = y*k; + this.z = z*k; + return this; + } + + /** + * + * @param vector + */ + public function toAxisVector(v:Vector3 = null):Vector3 { + if (w < -1 || w > 1) { + normalize(); + } + if (v == null) { + v = new Vector3(); + } + if (w > -1 && w < 1) { + if (w == 0) { + v.x = x; + v.y = y; + v.z = z; + } else { + var angle:Number = 2*Math.acos(w); + var coeff:Number = 1/Math.sqrt(1 - w*w); + v.x = x*coeff*angle; + v.y = y*coeff*angle; + v.z = z*coeff*angle; + } + } else { + v.x = 0; + v.y = 0; + v.z = 0; + } + return v; + } + + /** + * + * @param rotations + */ + public function getEulerAngles(angles:Vector3):Vector3 { + var qi2:Number = 2*x*x; + var qj2:Number = 2*y*y; + var qk2:Number = 2*z*z; + var qij:Number = 2*x*y; + var qjk:Number = 2*y*z; + var qki:Number = 2*z*x; + var qri:Number = 2*w*x; + var qrj:Number = 2*w*y; + var qrk:Number = 2*w*z; + + var aa:Number = 1 - qj2 - qk2; + var bb:Number = qij - qrk; + var ee:Number = qij + qrk; + var ff:Number = 1 - qi2 - qk2; + var ii:Number = qki - qrj; + var jj:Number = qjk + qri; + var kk:Number = 1 - qi2 - qj2; + + if (-1 < ii && ii < 1) { + if (angles == null) { + angles = new Vector3(Math.atan2(jj, kk), -Math.asin(ii), Math.atan2(ee, aa)); + } else { + angles.x = Math.atan2(jj, kk); + angles.y = -Math.asin(ii); + angles.z = Math.atan2(ee, aa); + } + } else { + if (angles == null) { + angles = new Vector3(0, 0.5*((ii <= -1) ? Math.PI : -Math.PI), Math.atan2(-bb, ff)); + } else { + angles.x = 0; + angles.y = (ii <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-bb, ff); + } + } + return angles; + } + + /** + * + */ + public function conjugate():void { + x = -x; + y = -y; + z = -z; + } + + /** + * Выполняет линейную интерполяцию. + * + * @param q1 начало отрезка + * @param q2 конец отрезка + * @param t время, обычно задаётся в интервале [0, 1] + */ + public function nlerp(q1:Quaternion, q2:Quaternion, t:Number):Quaternion { + var d:Number = 1 - t; + w = q1.w*d + q2.w*t; + x = q1.x*d + q2.x*t; + y = q1.y*d + q2.y*t; + z = q1.z*d + q2.z*t; + // inlined normalize + d = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param q + * @return + */ + public function subtract(q:Quaternion):Quaternion { + w -= q.w; + x -= q.x; + y -= q.y; + z -= q.z; + return this; + } + + /** + * + * @param q1 + * @param q2 + * @return + */ + public function diff(q1:Quaternion, q2:Quaternion):Quaternion { + w = q2.w - q1.w; + x = q2.x - q1.x; + y = q2.y - q1.y; + z = q2.z - q1.z; + return this; + } + + /** + * + * @param q + * @return + */ + public function copy(q:Quaternion):Quaternion { + w = q.w; + x = q.x; + y = q.y; + z = q.z; + return this; + } + + /** + * @param reslult + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + result.w = w; + return result; + } + + /** + * + * @return + */ + public function clone():Quaternion { + return new Quaternion(w, x, y, z); + } + + /** + * + * @return + */ + public function toString():String { + return "[" + w + ", " + x + ", " + y + ", " + z + "]"; + } + } +} diff --git a/0.0.7.0/src/alternativa/physics/math/.svn/text-base/Vector3.as.svn-base b/0.0.7.0/src/alternativa/physics/math/.svn/text-base/Vector3.as.svn-base new file mode 100644 index 0000000..232e499 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/math/.svn/text-base/Vector3.as.svn-base @@ -0,0 +1,335 @@ +package alternativa.physics.math { + import flash.geom.Vector3D; + + + /** + * + */ + public class Vector3 extends Vector3D { + + public static const ZERO:Vector3 = new Vector3(0, 0, 0); + public static const X_AXIS:Vector3 = new Vector3(1, 0, 0); + public static const Y_AXIS:Vector3 = new Vector3(0, 1, 0); + public static const Z_AXIS:Vector3 = new Vector3(0, 0, 1); + + /** + * + * @param x + * @param y + * @param z + */ + public function Vector3(x:Number = 0, y:Number = 0, z:Number = 0) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * + * @return + */ + public function vLength():Number { + return Math.sqrt(x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function vLengthSqr():Number { + return x*x + y*y + z*z; + } + + /** + * + * @param length + * @return + */ + public function vSetLength(length:Number):Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = length; + } else { + var k:Number = length/Math.sqrt(x*x + y*y + z*z); + x *= k; + y *= k; + z *= k; + } + return this; + } + + /** + * + * @return + */ + public function vNormalize():Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = 1; + } else { + d = Math.sqrt(d); + x /= d; + y /= d; + z /= d; + } + return this; + } + + /** + * Сложение координат. + * + * @param v точка, координаты которой прибавляются к собственным + */ + public function vAdd(v:Vector3):Vector3 { + x += v.x; + y += v.y; + z += v.z; + return this; + } + + /** + * + * @param k + * @param v + * @return + */ + public function vAddScaled(k:Number, v:Vector3):Vector3 { + x += k*v.x; + y += k*v.y; + z += k*v.z; + return this; + } + + /** + * Вычитание координат. + * + * @param v точка, координаты которой вычитаются из собственных + */ + public function vSubtract(v:Vector3):Vector3 { + x -= v.x; + y -= v.y; + z -= v.z; + return this; + } + + /** + * Вычисляет сумму векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + * @return this + */ + public function vSum(a:Vector3, b:Vector3):Vector3 { + x = a.x + b.x; + y = a.y + b.y; + z = a.z + b.z; + return this; + } + + /** + * Вычисление разности векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + */ + public function vDiff(a:Vector3, b:Vector3):Vector3 { + x = a.x - b.x; + y = a.y - b.y; + z = a.z - b.z; + return this; + } + + /** + * Умножение на скаляр. + * + * @param k число, на которое умножаются координаты + */ + public function vScale(k:Number):Vector3 { + x *= k; + y *= k; + z *= k; + return this; + } + + /** + * Инвертирование вектора. + */ + public function vReverse():Vector3 { + x = -x; + y = -y; + z = -z; + return this; + } + + /** + * + * @param v + * @return + */ + public function vDot(v:Vector3):Number { + return x*v.x + y*v.y + z*v.z; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross(v:Vector3):Vector3 { + var xx:Number = y*v.z - z*v.y; + var yy:Number = z*v.x - x*v.z; + var zz:Number = x*v.y - y*v.x; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross2(a:Vector3, b:Vector3):Vector3 { + x = a.y*b.z - a.z*b.y; + y = a.z*b.x - a.x*b.z; + z = a.x*b.y - a.y*b.x; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy3(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * + * @param matrix + */ + public function vTransformBy3Tr(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz + m.d; + y = m.e*xx + m.f*yy + m.g*zz + m.h; + z = m.i*xx + m.j*yy + m.k*zz + m.l; + return this; + } + + /** + * + * @param m + * @return + */ + public function vTransformInverseBy4(m:Matrix4):Vector3 { + var xx:Number = x - m.d; + var yy:Number = y - m.h; + var zz:Number = z - m.l; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * + * @param m + */ + public function vDeltaTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * Установка координат. + */ + public function vReset(x:Number = 0, y:Number = 0, z:Number = 0):Vector3 { + this.x = x; + this.y = y; + this.z = z; + return this; + } + + /** + * Копирование координат точки. + * + * @param v точка, координаты которой копируются + */ + public function vCopy(v:Vector3):Vector3 { + x = v.x; + y = v.y; + z = v.z; + return this; + } + + /** + * Клонирование точки. + * + * @return клонированная точка + */ + public function vClone():Vector3 { + return new Vector3(x, y, z); + } + + /** + * Копирует координаты вектора в экземпляр класса Vector3D. + * + * @param result экземпляр, в который копируются координаты + * @return переданный параметр + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + return result; + } + + /** + * @param source + * @return + */ + public function copyFromVector3D(source:Vector3D):Vector3 { + x = source.x; + y = source.y; + z = source.z; + return this; + } + + override public function toString():String { + return "Vector3(" + x + "," + y + ", " + z + ")"; + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/math/Matrix3.as b/0.0.7.0/src/alternativa/physics/math/Matrix3.as new file mode 100644 index 0000000..0b4dcd0 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/math/Matrix3.as @@ -0,0 +1,391 @@ +package alternativa.physics.math { + import flash.geom.Vector3D; + + + /** + * + */ + public class Matrix3 { + + public static const ZERO:Matrix3 = new Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0); + public static const IDENTITY:Matrix3 = new Matrix3(); + + public var a:Number; + public var b:Number; + public var c:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix3(a:Number = 1, b:Number = 0, c:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1) { + this.a = a; + this.b = b; + this.c = c; + + this.e = e; + this.f = f; + this.g = g; + + this.i = i; + this.j = j; + this.k = k; + } + + /** + * + * @return + */ + public function toIdentity():Matrix3 { + a = f = k = 1; + b = c = e = g = i = j = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + var det:Number = 1/(-cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk); + + a = (ff*kk - gg*jj)*det; + b = (cc*jj - bb*kk)*det; + c = (bb*gg - cc*ff)*det; + e = (gg*ii - ee*kk)*det; + f = (aa*kk - cc*ii)*det; + g = (cc*ee - aa*gg)*det; + i = (ee*jj - ff*ii)*det; + j = (bb*ii - aa*jj)*det; + k = (aa*ff - bb*ee)*det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prependTransposed(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.b + cc*m.c; + b = aa*m.e + bb*m.f + cc*m.g; + c = aa*m.i + bb*m.j + cc*m.k; + e = ee*m.a + ff*m.b + gg*m.c; + f = ee*m.e + ff*m.f + gg*m.g; + g = ee*m.i + ff*m.j + gg*m.k; + i = ii*m.a + jj*m.b + kk*m.c; + j = ii*m.e + jj*m.f + kk*m.g; + k = ii*m.i + jj*m.j + kk*m.k; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix3):Matrix3 { + a += m.a; + b += m.b; + c += m.c; + e += m.e; + f += m.f; + g += m.g; + i += m.i; + j += m.j; + k += m.k; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix3):Matrix3 { + a -= m.a; + b -= m.b; + c -= m.c; + e -= m.e; + f -= m.f; + g -= m.g; + i -= m.i; + j -= m.j; + k -= m.k; + + return this; + } + + /** + * + * @return + */ + public function transpose():Matrix3 { + var tmp:Number = b; + b = e; + e = tmp; + tmp = c; + c = i; + i = tmp; + tmp = g; + g = j; + j = tmp; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector3To3D(vin:Vector3, vout:Vector3D):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * + * @param matrix + */ + public function createSkewSymmetric(v:Vector3):Matrix3 { + a = f = k = 0; + b = -v.z; + c = v.y; + e = v.z; + g = -v.x; + i = -v.y; + j = v.x; + return this; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix3):Matrix3 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @return + */ + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):Matrix3 { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + + return this; + } + + /** + * Метод формирует матрицу поворота на заданный угол относительно заданной оси. + * + * @param axis нормализованный вектор, задающий ось, относительно которой выполняется поворот + * @param angle угол поворота в радианах + */ + public function fromAxisAngle(axis:Vector3, angle:Number):void { + var c1:Number = Math.cos(angle); + var s:Number = Math.sin(angle); + var t:Number = 1 - c1; + var x:Number = axis.x; + var y:Number = axis.y; + var z:Number = axis.z; + + a = t*x*x + c1; + b = t*x*y - z*s; + c = t*x*z + y*s; + + e = t*x*y + z*s; + f = t*y*y + c1; + g = t*y*z - x*s; + + i = t*x*z - y*s; + j = t*y*z + x*s; + k = t*z*z + c1; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix3 { + return new Matrix3(a, b, c, e, f, g, i, j, k); + } + + /** + * + * @return + */ + public function toString():String { + return "[Matrix3 (" + a + ", " + b + ", " + c + "), (" + e + ", " + f + ", " + g + "), (" + i + ", " + j + ", " + k + ")]"; + } + + /** + * @param angles + */ + public function getEulerAngles(angles:Vector3):void { + if (-1 < i && i < 1) { + angles.x = Math.atan2(j, k); + angles.y = -Math.asin(i); + angles.z = Math.atan2(e, a); + } else { + angles.x = 0; + angles.y = (i <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-b, f); + } + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/math/Matrix4.as b/0.0.7.0/src/alternativa/physics/math/Matrix4.as new file mode 100644 index 0000000..939d46a --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/math/Matrix4.as @@ -0,0 +1,461 @@ +package alternativa.physics.math { + import __AS3__.vec.Vector; + + /** + * + */ + public class Matrix4 { + + public static const IDENTITY:Matrix4 = new Matrix4(); + + public var a:Number; + public var b:Number; + public var c:Number; + public var d:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + public var h:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + public var l:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix4(a:Number = 1, b:Number = 0, c:Number = 0, d:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, h:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1, l:Number = 0) { + this.a = a; + this.b = b; + this.c = c; + this.d = d; + + this.e = e; + this.f = f; + this.g = g; + this.h = h; + + this.i = i; + this.j = j; + this.k = k; + this.l = l; + } + + /** + * + * @return + */ + public function toIdentity():Matrix4 { + a = f = k = 1; + b = c = e = g = i = j = d = h = l = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + var det:Number = -cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk; + + a = (-gg*jj + ff*kk)/det; + b = (cc*jj - bb*kk)/det; + c = (-cc*ff + bb*gg)/det; + d = (dd*gg*jj - cc*hh*jj - dd*ff*kk + bb*hh*kk + cc*ff*ll - bb*gg*ll)/det; + e = (gg*ii - ee*kk)/det; + f = (-cc*ii + aa*kk)/det; + g = (cc*ee - aa*gg)/det; + h = (cc*hh*ii - dd*gg*ii + dd*ee*kk - aa*hh*kk - cc*ee*ll + aa*gg*ll)/det; + i = (-ff*ii + ee*jj)/det; + j = (bb*ii - aa*jj)/det; + k = (-bb*ee + aa*ff)/det; + l = (dd*ff*ii - bb*hh*ii - dd*ee*jj + aa*hh*jj + bb*ee*ll - aa*ff*ll)/det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + d = m.a*dd + m.b*hh + m.c*ll + m.d; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + h = m.e*dd + m.f*hh + m.g*ll + m.h; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + l = m.i*dd + m.j*hh + m.k*ll + m.l; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + d = aa*m.d + bb*m.h + cc*m.l + dd; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + h = ee*m.d + ff*m.h + gg*m.l + hh; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + l = ii*m.d + jj*m.h + kk*m.l + ll; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix4):Matrix4 { + a += m.a; + b += m.b; + c += m.c; + d += m.d; + e += m.e; + f += m.f; + g += m.g; + h += m.h; + i += m.i; + j += m.j; + k += m.k; + l += m.l; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix4):Matrix4 { + a -= m.a; + b -= m.b; + c -= m.c; + d -= m.d; + e -= m.e; + f -= m.f; + g -= m.g; + h -= m.h; + i -= m.i; + j -= m.j; + k -= m.k; + l -= m.l; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectors(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + * @param len + */ + public function transformVectorsN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverse(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverseN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(idx:int, axis:Vector3):void { + switch (idx) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + case 3: + axis.x = d; + axis.y = h; + axis.z = l; + return; + } + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix4):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = m.d; + e = m.e; + f = m.f; + g = m.g; + h = m.h; + i = m.i; + j = m.j; + k = m.k; + l = m.l; + + return this; + } + + public function setFromMatrix3(m:Matrix3, offset:Vector3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = offset.x; + e = m.e; + f = m.f; + g = m.g; + h = offset.y; + i = m.i; + j = m.j; + k = m.k; + l = offset.z; + + return this; + } + + public function setOrientationFromMatrix3(m:Matrix3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @param angles + */ + public function getEulerAngles(angles:Vector3):void { + if (-1 < i && i < 1) { + angles.x = Math.atan2(j, k); + angles.y = -Math.asin(i); + angles.z = Math.atan2(e, a); + } else { + angles.x = 0; + angles.y = (i <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-b, f); + } + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix4 { + return new Matrix4(a, b, c, d, e, f, g, h, i, j, k, l); + } + + public function toString():String { + return "[Matrix4 [" + a.toFixed(3) + " " + b.toFixed(3) + " " + c.toFixed(3) + " " + d.toFixed(3) + "] [" + e.toFixed(3) + " " + f.toFixed(3) + " " + g.toFixed(3) + " " + h.toFixed(3) + "] [" + i.toFixed(3) + " " + j.toFixed(3) + " " + k.toFixed(3) + " " + l.toFixed(3) + "]]"; + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/math/Quaternion.as b/0.0.7.0/src/alternativa/physics/math/Quaternion.as new file mode 100644 index 0000000..cf39d7b --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/math/Quaternion.as @@ -0,0 +1,397 @@ +package alternativa.physics.math { + import flash.geom.Vector3D; + + + public class Quaternion { + + public var w:Number; + public var x:Number; + public var y:Number; + public var z:Number; + + public static function multiply(q1:Quaternion, q2:Quaternion, result:Quaternion):void { + result.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z; + result.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y; + result.y = q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z; + result.z = q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x; + } + + public static function createFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngle(axis, angle); + return q; + } + + public static function createFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngleComponents(x, y, z, angle); + return q; + } + + public function Quaternion(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0) { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + } + + public function reset(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0):Quaternion { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + return this; + } + + public function normalize():Quaternion { + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * Умножает на указанный кватернион слева: this * q + * + * @param q множитель + */ + public function prepend(q:Quaternion):Quaternion { + var ww:Number = w*q.w - x*q.x - y*q.y - z*q.z; + var xx:Number = w*q.x + x*q.w + y*q.z - z*q.y; + var yy:Number = w*q.y + y*q.w + z*q.x - x*q.z; + var zz:Number = w*q.z + z*q.w + x*q.y - y*q.x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Умножает на указанный кватернион справа: q * this + * + * @param q множитель + */ + public function append(q:Quaternion):Quaternion { + var ww:Number = q.w*w - q.x*x - q.y*y - q.z*z; + var xx:Number = q.w*x + q.x*w + q.y*z - q.z*y; + var yy:Number = q.w*y + q.y*w + q.z*x - q.x*z; + var zz:Number = q.w*z + q.z*w + q.x*y - q.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * + * @param vector + */ + public function rotateByVector(v:Vector3):Quaternion { + var ww:Number = -v.x*x - v.y*y - v.z*z; + var xx:Number = v.x*w + v.y*z - v.z*y; + var yy:Number = v.y*w + v.z*x - v.x*z; + var zz:Number = v.z*w + v.x*y - v.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Добавляет вращение, приданное вектором угловой скорости за указанное время. + * + * @param v + * @param scale + */ + public function addScaledVector(v:Vector3, scale:Number):Quaternion { + var vx:Number = v.x*scale; + var vy:Number = v.y*scale; + var vz:Number = v.z*scale; + var ww:Number = -x*vx - y*vy - z*vz; + var xx:Number = vx*w + vy*z - vz*y; + var yy:Number = vy*w + vz*x - vx*z; + var zz:Number = vz*w + vx*y - vy*x; + w += 0.5*ww; + x += 0.5*xx; + y += 0.5*yy; + z += 0.5*zz; + // inlined normalize + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param m + * @return + */ + public function toMatrix3(m:Matrix3):Quaternion { + var xx2:Number = 2.0*x*x; + var yy2:Number = 2.0*y*y; + var zz2:Number = 2.0*z*z; + var xy2:Number = 2.0*x*y; + var yz2:Number = 2.0*y*z; + var zx2:Number = 2.0*z*x; + var wx2:Number = 2.0*w*x; + var wy2:Number = 2.0*w*y; + var wz2:Number = 2.0*w*z; + + m.a = 1.0 - yy2 - zz2; + m.b = xy2 - wz2; + m.c = zx2 + wy2; + + m.e = xy2 + wz2; + m.f = 1.0 - xx2 - zz2; + m.g = yz2 - wx2; + + m.i = zx2 - wy2; + m.j = yz2 + wx2; + m.k = 1.0 - xx2 - yy2; + return this; + } + + /** + * + * @return + */ + public function length():Number { + return Math.sqrt(w*w + x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function lengthSqr():Number { + return w*w + x*x + y*y + z*z; + } + + /** + * + * @param axis + * @param angle + * @return + */ + public function setFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z); + x = axis.x*k; + y = axis.y*k; + z = axis.z*k; + return this; + } + + /** + * + * @param x + * @param y + * @param z + * @param angle + */ + public function setFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(x*x + y*y + z*z); + this.x = x*k; + this.y = y*k; + this.z = z*k; + return this; + } + + /** + * + * @param vector + */ + public function toAxisVector(v:Vector3 = null):Vector3 { + if (w < -1 || w > 1) { + normalize(); + } + if (v == null) { + v = new Vector3(); + } + if (w > -1 && w < 1) { + if (w == 0) { + v.x = x; + v.y = y; + v.z = z; + } else { + var angle:Number = 2*Math.acos(w); + var coeff:Number = 1/Math.sqrt(1 - w*w); + v.x = x*coeff*angle; + v.y = y*coeff*angle; + v.z = z*coeff*angle; + } + } else { + v.x = 0; + v.y = 0; + v.z = 0; + } + return v; + } + + /** + * + * @param rotations + */ + public function getEulerAngles(angles:Vector3):Vector3 { + var qi2:Number = 2*x*x; + var qj2:Number = 2*y*y; + var qk2:Number = 2*z*z; + var qij:Number = 2*x*y; + var qjk:Number = 2*y*z; + var qki:Number = 2*z*x; + var qri:Number = 2*w*x; + var qrj:Number = 2*w*y; + var qrk:Number = 2*w*z; + + var aa:Number = 1 - qj2 - qk2; + var bb:Number = qij - qrk; + var ee:Number = qij + qrk; + var ff:Number = 1 - qi2 - qk2; + var ii:Number = qki - qrj; + var jj:Number = qjk + qri; + var kk:Number = 1 - qi2 - qj2; + + if (-1 < ii && ii < 1) { + if (angles == null) { + angles = new Vector3(Math.atan2(jj, kk), -Math.asin(ii), Math.atan2(ee, aa)); + } else { + angles.x = Math.atan2(jj, kk); + angles.y = -Math.asin(ii); + angles.z = Math.atan2(ee, aa); + } + } else { + if (angles == null) { + angles = new Vector3(0, 0.5*((ii <= -1) ? Math.PI : -Math.PI), Math.atan2(-bb, ff)); + } else { + angles.x = 0; + angles.y = (ii <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-bb, ff); + } + } + return angles; + } + + /** + * + */ + public function conjugate():void { + x = -x; + y = -y; + z = -z; + } + + /** + * Выполняет линейную интерполяцию. + * + * @param q1 начало отрезка + * @param q2 конец отрезка + * @param t время, обычно задаётся в интервале [0, 1] + */ + public function nlerp(q1:Quaternion, q2:Quaternion, t:Number):Quaternion { + var d:Number = 1 - t; + w = q1.w*d + q2.w*t; + x = q1.x*d + q2.x*t; + y = q1.y*d + q2.y*t; + z = q1.z*d + q2.z*t; + // inlined normalize + d = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param q + * @return + */ + public function subtract(q:Quaternion):Quaternion { + w -= q.w; + x -= q.x; + y -= q.y; + z -= q.z; + return this; + } + + /** + * + * @param q1 + * @param q2 + * @return + */ + public function diff(q1:Quaternion, q2:Quaternion):Quaternion { + w = q2.w - q1.w; + x = q2.x - q1.x; + y = q2.y - q1.y; + z = q2.z - q1.z; + return this; + } + + /** + * + * @param q + * @return + */ + public function copy(q:Quaternion):Quaternion { + w = q.w; + x = q.x; + y = q.y; + z = q.z; + return this; + } + + /** + * @param reslult + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + result.w = w; + return result; + } + + /** + * + * @return + */ + public function clone():Quaternion { + return new Quaternion(w, x, y, z); + } + + /** + * + * @return + */ + public function toString():String { + return "[" + w + ", " + x + ", " + y + ", " + z + "]"; + } + } +} diff --git a/0.0.7.0/src/alternativa/physics/math/Vector3.as b/0.0.7.0/src/alternativa/physics/math/Vector3.as new file mode 100644 index 0000000..232e499 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/math/Vector3.as @@ -0,0 +1,335 @@ +package alternativa.physics.math { + import flash.geom.Vector3D; + + + /** + * + */ + public class Vector3 extends Vector3D { + + public static const ZERO:Vector3 = new Vector3(0, 0, 0); + public static const X_AXIS:Vector3 = new Vector3(1, 0, 0); + public static const Y_AXIS:Vector3 = new Vector3(0, 1, 0); + public static const Z_AXIS:Vector3 = new Vector3(0, 0, 1); + + /** + * + * @param x + * @param y + * @param z + */ + public function Vector3(x:Number = 0, y:Number = 0, z:Number = 0) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * + * @return + */ + public function vLength():Number { + return Math.sqrt(x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function vLengthSqr():Number { + return x*x + y*y + z*z; + } + + /** + * + * @param length + * @return + */ + public function vSetLength(length:Number):Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = length; + } else { + var k:Number = length/Math.sqrt(x*x + y*y + z*z); + x *= k; + y *= k; + z *= k; + } + return this; + } + + /** + * + * @return + */ + public function vNormalize():Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = 1; + } else { + d = Math.sqrt(d); + x /= d; + y /= d; + z /= d; + } + return this; + } + + /** + * Сложение координат. + * + * @param v точка, координаты которой прибавляются к собственным + */ + public function vAdd(v:Vector3):Vector3 { + x += v.x; + y += v.y; + z += v.z; + return this; + } + + /** + * + * @param k + * @param v + * @return + */ + public function vAddScaled(k:Number, v:Vector3):Vector3 { + x += k*v.x; + y += k*v.y; + z += k*v.z; + return this; + } + + /** + * Вычитание координат. + * + * @param v точка, координаты которой вычитаются из собственных + */ + public function vSubtract(v:Vector3):Vector3 { + x -= v.x; + y -= v.y; + z -= v.z; + return this; + } + + /** + * Вычисляет сумму векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + * @return this + */ + public function vSum(a:Vector3, b:Vector3):Vector3 { + x = a.x + b.x; + y = a.y + b.y; + z = a.z + b.z; + return this; + } + + /** + * Вычисление разности векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + */ + public function vDiff(a:Vector3, b:Vector3):Vector3 { + x = a.x - b.x; + y = a.y - b.y; + z = a.z - b.z; + return this; + } + + /** + * Умножение на скаляр. + * + * @param k число, на которое умножаются координаты + */ + public function vScale(k:Number):Vector3 { + x *= k; + y *= k; + z *= k; + return this; + } + + /** + * Инвертирование вектора. + */ + public function vReverse():Vector3 { + x = -x; + y = -y; + z = -z; + return this; + } + + /** + * + * @param v + * @return + */ + public function vDot(v:Vector3):Number { + return x*v.x + y*v.y + z*v.z; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross(v:Vector3):Vector3 { + var xx:Number = y*v.z - z*v.y; + var yy:Number = z*v.x - x*v.z; + var zz:Number = x*v.y - y*v.x; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross2(a:Vector3, b:Vector3):Vector3 { + x = a.y*b.z - a.z*b.y; + y = a.z*b.x - a.x*b.z; + z = a.x*b.y - a.y*b.x; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy3(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * + * @param matrix + */ + public function vTransformBy3Tr(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz + m.d; + y = m.e*xx + m.f*yy + m.g*zz + m.h; + z = m.i*xx + m.j*yy + m.k*zz + m.l; + return this; + } + + /** + * + * @param m + * @return + */ + public function vTransformInverseBy4(m:Matrix4):Vector3 { + var xx:Number = x - m.d; + var yy:Number = y - m.h; + var zz:Number = z - m.l; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * + * @param m + */ + public function vDeltaTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * Установка координат. + */ + public function vReset(x:Number = 0, y:Number = 0, z:Number = 0):Vector3 { + this.x = x; + this.y = y; + this.z = z; + return this; + } + + /** + * Копирование координат точки. + * + * @param v точка, координаты которой копируются + */ + public function vCopy(v:Vector3):Vector3 { + x = v.x; + y = v.y; + z = v.z; + return this; + } + + /** + * Клонирование точки. + * + * @return клонированная точка + */ + public function vClone():Vector3 { + return new Vector3(x, y, z); + } + + /** + * Копирует координаты вектора в экземпляр класса Vector3D. + * + * @param result экземпляр, в который копируются координаты + * @return переданный параметр + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + return result; + } + + /** + * @param source + * @return + */ + public function copyFromVector3D(source:Vector3D):Vector3 { + x = source.x; + y = source.y; + z = source.z; + return this; + } + + override public function toString():String { + return "Vector3(" + x + "," + y + ", " + z + ")"; + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/primitives/.svn/all-wcprops b/0.0.7.0/src/alternativa/physics/primitives/.svn/all-wcprops new file mode 100644 index 0000000..99198ef --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/primitives/.svn/all-wcprops @@ -0,0 +1,35 @@ +K 25 +svn:wc:ra_dav:version-url +V 114 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/primitives +END +RigidPlane.as +K 25 +svn:wc:ra_dav:version-url +V 128 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/primitives/RigidPlane.as +END +RigidSphere.as +K 25 +svn:wc:ra_dav:version-url +V 129 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/primitives/RigidSphere.as +END +RigidBox.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/primitives/RigidBox.as +END +RigidCylinder.as +K 25 +svn:wc:ra_dav:version-url +V 131 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/primitives/RigidCylinder.as +END +RigidRect.as +K 25 +svn:wc:ra_dav:version-url +V 127 +/!svn/ver/22450/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/primitives/RigidRect.as +END diff --git a/0.0.7.0/src/alternativa/physics/primitives/.svn/entries b/0.0.7.0/src/alternativa/physics/primitives/.svn/entries new file mode 100644 index 0000000..afff9c8 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/primitives/.svn/entries @@ -0,0 +1,88 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.0/src/alternativa/physics/primitives +http://svndev.alternativaplatform.com + + + +2009-10-19T07:17:33.113306Z +22253 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +RigidPlane.as +file + + + + +2010-10-28T04:32:42.000000Z +3a8402814cf99a68d018a90077f62746 +2009-10-19T07:17:33.113306Z +22253 +mike + +RigidSphere.as +file + + + + +2010-10-28T04:32:42.000000Z +1a898ad68fc203a75a157cbf21f7dc92 +2009-10-19T07:17:33.113306Z +22253 +mike + +RigidBox.as +file + + + + +2010-10-28T04:32:42.000000Z +ae534a73f34403a99de94e649d45e238 +2009-10-19T07:17:33.113306Z +22253 +mike + +RigidCylinder.as +file + + + + +2010-10-28T04:32:42.000000Z +86388b2dc5c35fd45cc1b0bcbd09c517 +2009-10-19T07:17:33.113306Z +22253 +mike + +RigidRect.as +file + + + + +2010-10-28T04:32:42.000000Z +bdc2ecbdfebbd0417a19519c338ef54c +2009-10-19T07:17:33.113306Z +22253 +mike + diff --git a/0.0.7.0/src/alternativa/physics/primitives/.svn/format b/0.0.7.0/src/alternativa/physics/primitives/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/primitives/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.7.0/src/alternativa/physics/primitives/.svn/text-base/RigidBox.as.svn-base b/0.0.7.0/src/alternativa/physics/primitives/.svn/text-base/RigidBox.as.svn-base new file mode 100644 index 0000000..bd59d52 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/primitives/.svn/text-base/RigidBox.as.svn-base @@ -0,0 +1,56 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.math.Matrix3; + import alternativa.physics.math.Vector3; + + use namespace altphysics; + + /** + * + */ + public class RigidBox extends Body { + + /** + * + * @param halfSize + * @param mass + */ + public function RigidBox(halfSize:Vector3, mass:Number) { + super(0, Matrix3.ZERO); + addCollisionPrimitive(new CollisionBox(halfSize, 1)); + setParams(halfSize, mass); + } + + /** + * @param halfSize + * @param mass + */ + public function setParams(halfSize:Vector3, mass:Number):void { + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + invInertia.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + invInertia.a = 3*invMass/(yy + zz); + invInertia.f = 3*invMass/(zz + xx); + invInertia.k = 3*invMass/(xx + yy); + } + var prim:CollisionBox = CollisionBox(collisionPrimitives.head.primitive); + prim.hs.vCopy(halfSize); + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/primitives/.svn/text-base/RigidCylinder.as.svn-base b/0.0.7.0/src/alternativa/physics/primitives/.svn/text-base/RigidCylinder.as.svn-base new file mode 100644 index 0000000..a3ef3cd --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/primitives/.svn/text-base/RigidCylinder.as.svn-base @@ -0,0 +1,32 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.math.Matrix3; + + use namespace altphysics; + + public class RigidCylinder extends Body { + + altphysics var r:Number; + altphysics var h:Number; + + public function RigidCylinder(mass:Number, radius:Number, height:Number) { + super(0, Matrix3.ZERO); + setParams(mass, radius, height); + } + + public function setParams(mass:Number, radius:Number, height:Number):void { + r = radius; + h = height; + invInertiaWorld.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + invInertia.a = invInertia.f = mass*(h*h/12 + r*r/4); + invInertia.k = 0.5*mass*r*r; + invInertia.invert(); + } + } + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/primitives/.svn/text-base/RigidPlane.as.svn-base b/0.0.7.0/src/alternativa/physics/primitives/.svn/text-base/RigidPlane.as.svn-base new file mode 100644 index 0000000..3df7672 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/primitives/.svn/text-base/RigidPlane.as.svn-base @@ -0,0 +1,23 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.math.Matrix3; + import alternativa.physics.math.Vector3; + + use namespace altphysics; + + public class RigidPlane extends Body { + + public var normal:Vector3 = new Vector3(); + public var offset:Number; + + public function RigidPlane() { + super(0, Matrix3.ZERO); + this.normal.vCopy(Vector3.Z_AXIS); + this.offset = 0; + movable = false; + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/primitives/.svn/text-base/RigidRect.as.svn-base b/0.0.7.0/src/alternativa/physics/primitives/.svn/text-base/RigidRect.as.svn-base new file mode 100644 index 0000000..5219666 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/primitives/.svn/text-base/RigidRect.as.svn-base @@ -0,0 +1,20 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.physics.math.Matrix3; + import alternativa.physics.math.Vector3; + + use namespace altphysics; + + public class RigidRect extends Body { + + public function RigidRect(halfWidth:Number, halfLength:Number) { + super(0, Matrix3.ZERO); + movable = false; + addCollisionPrimitive(new CollisionRect(new Vector3(halfWidth, halfLength, 0), 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/primitives/.svn/text-base/RigidSphere.as.svn-base b/0.0.7.0/src/alternativa/physics/primitives/.svn/text-base/RigidSphere.as.svn-base new file mode 100644 index 0000000..63931e4 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/primitives/.svn/text-base/RigidSphere.as.svn-base @@ -0,0 +1,29 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.math.Matrix3; + + use namespace altphysics; + + public class RigidSphere extends Body { + + public function RigidSphere(radius:Number, mass:Number) { + var m:Matrix3; + if (mass == Infinity) { + mass = 0; + m = Matrix3.ZERO; + } else { + mass = 1/mass; + var r2:Number = radius*radius; + m = new Matrix3(); + m.a = m.f = m.k = 2.5*mass/r2; + } + super(mass, m); + if (mass == 0) movable = false; + addCollisionPrimitive(new CollisionSphere(radius, 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/primitives/RigidBox.as b/0.0.7.0/src/alternativa/physics/primitives/RigidBox.as new file mode 100644 index 0000000..bd59d52 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/primitives/RigidBox.as @@ -0,0 +1,56 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.math.Matrix3; + import alternativa.physics.math.Vector3; + + use namespace altphysics; + + /** + * + */ + public class RigidBox extends Body { + + /** + * + * @param halfSize + * @param mass + */ + public function RigidBox(halfSize:Vector3, mass:Number) { + super(0, Matrix3.ZERO); + addCollisionPrimitive(new CollisionBox(halfSize, 1)); + setParams(halfSize, mass); + } + + /** + * @param halfSize + * @param mass + */ + public function setParams(halfSize:Vector3, mass:Number):void { + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + invInertia.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + invInertia.a = 3*invMass/(yy + zz); + invInertia.f = 3*invMass/(zz + xx); + invInertia.k = 3*invMass/(xx + yy); + } + var prim:CollisionBox = CollisionBox(collisionPrimitives.head.primitive); + prim.hs.vCopy(halfSize); + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/primitives/RigidCylinder.as b/0.0.7.0/src/alternativa/physics/primitives/RigidCylinder.as new file mode 100644 index 0000000..a3ef3cd --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/primitives/RigidCylinder.as @@ -0,0 +1,32 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.math.Matrix3; + + use namespace altphysics; + + public class RigidCylinder extends Body { + + altphysics var r:Number; + altphysics var h:Number; + + public function RigidCylinder(mass:Number, radius:Number, height:Number) { + super(0, Matrix3.ZERO); + setParams(mass, radius, height); + } + + public function setParams(mass:Number, radius:Number, height:Number):void { + r = radius; + h = height; + invInertiaWorld.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + invInertia.a = invInertia.f = mass*(h*h/12 + r*r/4); + invInertia.k = 0.5*mass*r*r; + invInertia.invert(); + } + } + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/primitives/RigidPlane.as b/0.0.7.0/src/alternativa/physics/primitives/RigidPlane.as new file mode 100644 index 0000000..3df7672 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/primitives/RigidPlane.as @@ -0,0 +1,23 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.math.Matrix3; + import alternativa.physics.math.Vector3; + + use namespace altphysics; + + public class RigidPlane extends Body { + + public var normal:Vector3 = new Vector3(); + public var offset:Number; + + public function RigidPlane() { + super(0, Matrix3.ZERO); + this.normal.vCopy(Vector3.Z_AXIS); + this.offset = 0; + movable = false; + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/primitives/RigidRect.as b/0.0.7.0/src/alternativa/physics/primitives/RigidRect.as new file mode 100644 index 0000000..5219666 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/primitives/RigidRect.as @@ -0,0 +1,20 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.physics.math.Matrix3; + import alternativa.physics.math.Vector3; + + use namespace altphysics; + + public class RigidRect extends Body { + + public function RigidRect(halfWidth:Number, halfLength:Number) { + super(0, Matrix3.ZERO); + movable = false; + addCollisionPrimitive(new CollisionRect(new Vector3(halfWidth, halfLength, 0), 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.7.0/src/alternativa/physics/primitives/RigidSphere.as b/0.0.7.0/src/alternativa/physics/primitives/RigidSphere.as new file mode 100644 index 0000000..63931e4 --- /dev/null +++ b/0.0.7.0/src/alternativa/physics/primitives/RigidSphere.as @@ -0,0 +1,29 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.math.Matrix3; + + use namespace altphysics; + + public class RigidSphere extends Body { + + public function RigidSphere(radius:Number, mass:Number) { + var m:Matrix3; + if (mass == Infinity) { + mass = 0; + m = Matrix3.ZERO; + } else { + mass = 1/mass; + var r2:Number = radius*radius; + m = new Matrix3(); + m.a = m.f = m.k = 2.5*mass/r2; + } + super(mass, m); + if (mass == 0) movable = false; + addCollisionPrimitive(new CollisionSphere(radius, 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/.actionScriptProperties b/0.0.7.1/.actionScriptProperties new file mode 100644 index 0000000..e5b1b89 --- /dev/null +++ b/0.0.7.1/.actionScriptProperties @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/0.0.7.1/.flexLibProperties b/0.0.7.1/.flexLibProperties new file mode 100644 index 0000000..652bb2a --- /dev/null +++ b/0.0.7.1/.flexLibProperties @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/0.0.7.1/.project b/0.0.7.1/.project new file mode 100644 index 0000000..1f1425f --- /dev/null +++ b/0.0.7.1/.project @@ -0,0 +1,19 @@ + + + AlternativaPhysics + + + + + + com.adobe.flexbuilder.project.flexbuilder + + + + + + org.maven.ide.eclipse.maven2Nature + com.adobe.flexbuilder.project.flexlibnature + com.adobe.flexbuilder.project.actionscriptnature + + diff --git a/0.0.7.1/.settings/.svn/all-wcprops b/0.0.7.1/.settings/.svn/all-wcprops new file mode 100644 index 0000000..7dfdc46 --- /dev/null +++ b/0.0.7.1/.settings/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 89 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/.settings +END +org.eclipse.core.resources.prefs +K 25 +svn:wc:ra_dav:version-url +V 122 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/.settings/org.eclipse.core.resources.prefs +END diff --git a/0.0.7.1/.settings/.svn/entries b/0.0.7.1/.settings/.svn/entries new file mode 100644 index 0000000..b4e6917 --- /dev/null +++ b/0.0.7.1/.settings/.svn/entries @@ -0,0 +1,40 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/.settings +http://svndev.alternativaplatform.com + + + +2009-04-01T12:25:29.638016Z +10301 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +org.eclipse.core.resources.prefs +file + + + + +2010-10-28T04:32:42.000000Z +4f70b476f3e5075e399505021df2f89b +2009-04-01T12:25:29.638016Z +10301 +mike + diff --git a/0.0.7.1/.settings/.svn/format b/0.0.7.1/.settings/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.7.1/.settings/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.7.1/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base b/0.0.7.1/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base new file mode 100644 index 0000000..b3d0d49 --- /dev/null +++ b/0.0.7.1/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base @@ -0,0 +1,3 @@ +#Fri Mar 20 00:19:33 YEKT 2009 +eclipse.preferences.version=1 +encoding/=UTF-8 diff --git a/0.0.7.1/.settings/org.eclipse.core.resources.prefs b/0.0.7.1/.settings/org.eclipse.core.resources.prefs new file mode 100644 index 0000000..b3d0d49 --- /dev/null +++ b/0.0.7.1/.settings/org.eclipse.core.resources.prefs @@ -0,0 +1,3 @@ +#Fri Mar 20 00:19:33 YEKT 2009 +eclipse.preferences.version=1 +encoding/=UTF-8 diff --git a/0.0.7.1/.svn/all-wcprops b/0.0.7.1/.svn/all-wcprops new file mode 100644 index 0000000..4f2f641 --- /dev/null +++ b/0.0.7.1/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 79 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1 +END +.flexLibProperties +K 25 +svn:wc:ra_dav:version-url +V 98 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/.flexLibProperties +END +.project +K 25 +svn:wc:ra_dav:version-url +V 88 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/.project +END +pom.xml +K 25 +svn:wc:ra_dav:version-url +V 87 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/pom.xml +END +.actionScriptProperties +K 25 +svn:wc:ra_dav:version-url +V 103 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/.actionScriptProperties +END diff --git a/0.0.7.1/.svn/dir-prop-base b/0.0.7.1/.svn/dir-prop-base new file mode 100644 index 0000000..c31ca28 --- /dev/null +++ b/0.0.7.1/.svn/dir-prop-base @@ -0,0 +1,7 @@ +K 13 +svn:mergeinfo +V 278 +/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized:19806-19860 +/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.7.0.LinkedList:20360-22217,22226-22252 +/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D:13892-14098 +END diff --git a/0.0.7.1/.svn/entries b/0.0.7.1/.svn/entries new file mode 100644 index 0000000..1bb3e7a --- /dev/null +++ b/0.0.7.1/.svn/entries @@ -0,0 +1,85 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1 +http://svndev.alternativaplatform.com + + + +2010-01-18T13:04:14.097110Z +26572 +mike +has-props + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +META-INF +dir + +.flexLibProperties +file + + + + +2010-10-28T04:32:42.000000Z +c8e034e8e09f239471f9f69abce3a7bb +2009-10-19T07:17:33.113306Z +22253 +mike + +.project +file + + + + +2010-10-28T04:32:42.000000Z +1e90cc68b52b93173b2b5de88eb5c3a3 +2009-04-20T17:15:55.260110Z +11541 +mike + +src +dir + +pom.xml +file + + + + +2010-10-28T04:32:42.000000Z +284be6bfe23eb06ae78d682ef0ab3dd5 +2010-01-18T13:04:14.097110Z +26572 +mike + +.actionScriptProperties +file + + + + +2010-10-28T04:32:42.000000Z +918335cb945c3f8866aeb8e6cfd1b110 +2010-01-14T05:40:59.142095Z +26349 +mike + +.settings +dir + diff --git a/0.0.7.1/.svn/format b/0.0.7.1/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.7.1/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.7.1/.svn/text-base/.actionScriptProperties.svn-base b/0.0.7.1/.svn/text-base/.actionScriptProperties.svn-base new file mode 100644 index 0000000..e5b1b89 --- /dev/null +++ b/0.0.7.1/.svn/text-base/.actionScriptProperties.svn-base @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/0.0.7.1/.svn/text-base/.flexLibProperties.svn-base b/0.0.7.1/.svn/text-base/.flexLibProperties.svn-base new file mode 100644 index 0000000..652bb2a --- /dev/null +++ b/0.0.7.1/.svn/text-base/.flexLibProperties.svn-base @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/0.0.7.1/.svn/text-base/.project.svn-base b/0.0.7.1/.svn/text-base/.project.svn-base new file mode 100644 index 0000000..1f1425f --- /dev/null +++ b/0.0.7.1/.svn/text-base/.project.svn-base @@ -0,0 +1,19 @@ + + + AlternativaPhysics + + + + + + com.adobe.flexbuilder.project.flexbuilder + + + + + + org.maven.ide.eclipse.maven2Nature + com.adobe.flexbuilder.project.flexlibnature + com.adobe.flexbuilder.project.actionscriptnature + + diff --git a/0.0.7.1/.svn/text-base/pom.xml.svn-base b/0.0.7.1/.svn/text-base/pom.xml.svn-base new file mode 100644 index 0000000..1d6d1c6 --- /dev/null +++ b/0.0.7.1/.svn/text-base/pom.xml.svn-base @@ -0,0 +1,17 @@ + + 4.0.0 + platform.clients.fp10.libraries + AlternativaPhysics + swc + 0.0.7.1 + + platform.tools.maven + FlashBasePom + 1.0 + + + scm:svn:http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1 + + + + \ No newline at end of file diff --git a/0.0.7.1/META-INF/.svn/all-wcprops b/0.0.7.1/META-INF/.svn/all-wcprops new file mode 100644 index 0000000..5a0e37e --- /dev/null +++ b/0.0.7.1/META-INF/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 88 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/META-INF +END +MANIFEST.MF +K 25 +svn:wc:ra_dav:version-url +V 100 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/META-INF/MANIFEST.MF +END diff --git a/0.0.7.1/META-INF/.svn/entries b/0.0.7.1/META-INF/.svn/entries new file mode 100644 index 0000000..6fd5163 --- /dev/null +++ b/0.0.7.1/META-INF/.svn/entries @@ -0,0 +1,40 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/META-INF +http://svndev.alternativaplatform.com + + + +2009-04-20T17:15:55.260110Z +11541 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +MANIFEST.MF +file + + + + +2010-10-28T04:32:42.000000Z +7b64dceb50a6905850ff00a0bfbe77eb +2009-04-20T17:15:55.260110Z +11541 +mike + diff --git a/0.0.7.1/META-INF/.svn/format b/0.0.7.1/META-INF/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.7.1/META-INF/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.7.1/META-INF/.svn/text-base/MANIFEST.MF.svn-base b/0.0.7.1/META-INF/.svn/text-base/MANIFEST.MF.svn-base new file mode 100644 index 0000000..5066a55 --- /dev/null +++ b/0.0.7.1/META-INF/.svn/text-base/MANIFEST.MF.svn-base @@ -0,0 +1 @@ +Bundle-Name: platform.clients.fp10.libraries.AlternativaPhysics diff --git a/0.0.7.1/META-INF/MANIFEST.MF b/0.0.7.1/META-INF/MANIFEST.MF new file mode 100644 index 0000000..5066a55 --- /dev/null +++ b/0.0.7.1/META-INF/MANIFEST.MF @@ -0,0 +1 @@ +Bundle-Name: platform.clients.fp10.libraries.AlternativaPhysics diff --git a/0.0.7.1/pom.xml b/0.0.7.1/pom.xml new file mode 100644 index 0000000..1d6d1c6 --- /dev/null +++ b/0.0.7.1/pom.xml @@ -0,0 +1,17 @@ + + 4.0.0 + platform.clients.fp10.libraries + AlternativaPhysics + swc + 0.0.7.1 + + platform.tools.maven + FlashBasePom + 1.0 + + + scm:svn:http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1 + + + + \ No newline at end of file diff --git a/0.0.7.1/src/.svn/all-wcprops b/0.0.7.1/src/.svn/all-wcprops new file mode 100644 index 0000000..adb0701 --- /dev/null +++ b/0.0.7.1/src/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 83 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src +END diff --git a/0.0.7.1/src/.svn/entries b/0.0.7.1/src/.svn/entries new file mode 100644 index 0000000..05df4ff --- /dev/null +++ b/0.0.7.1/src/.svn/entries @@ -0,0 +1,31 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src +http://svndev.alternativaplatform.com + + + +2010-01-15T10:21:05.313154Z +26464 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +alternativa +dir + diff --git a/0.0.7.1/src/.svn/format b/0.0.7.1/src/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.7.1/src/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.7.1/src/alternativa/.svn/all-wcprops b/0.0.7.1/src/alternativa/.svn/all-wcprops new file mode 100644 index 0000000..5d3df3e --- /dev/null +++ b/0.0.7.1/src/alternativa/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 95 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa +END diff --git a/0.0.7.1/src/alternativa/.svn/entries b/0.0.7.1/src/alternativa/.svn/entries new file mode 100644 index 0000000..69b540e --- /dev/null +++ b/0.0.7.1/src/alternativa/.svn/entries @@ -0,0 +1,31 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa +http://svndev.alternativaplatform.com + + + +2010-01-15T10:21:05.313154Z +26464 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +physics +dir + diff --git a/0.0.7.1/src/alternativa/.svn/format b/0.0.7.1/src/alternativa/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.7.1/src/alternativa/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.7.1/src/alternativa/physics/.svn/all-wcprops b/0.0.7.1/src/alternativa/physics/.svn/all-wcprops new file mode 100644 index 0000000..32c12da --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/.svn/all-wcprops @@ -0,0 +1,77 @@ +K 25 +svn:wc:ra_dav:version-url +V 103 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics +END +Body.as +K 25 +svn:wc:ra_dav:version-url +V 111 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/Body.as +END +BodyState.as +K 25 +svn:wc:ra_dav:version-url +V 116 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/BodyState.as +END +CollisionPrimitiveListItem.as +K 25 +svn:wc:ra_dav:version-url +V 133 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/CollisionPrimitiveListItem.as +END +CollisionPrimitiveList.as +K 25 +svn:wc:ra_dav:version-url +V 129 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/CollisionPrimitiveList.as +END +altphysics.as +K 25 +svn:wc:ra_dav:version-url +V 117 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/altphysics.as +END +PhysicsUtils.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/PhysicsUtils.as +END +ContactPoint.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/ContactPoint.as +END +World.as +K 25 +svn:wc:ra_dav:version-url +V 112 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/World.as +END +BodyListItem.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/BodyListItem.as +END +BodyList.as +K 25 +svn:wc:ra_dav:version-url +V 115 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/BodyList.as +END +Contact.as +K 25 +svn:wc:ra_dav:version-url +V 114 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/Contact.as +END +BodyMaterial.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/BodyMaterial.as +END diff --git a/0.0.7.1/src/alternativa/physics/.svn/entries b/0.0.7.1/src/alternativa/physics/.svn/entries new file mode 100644 index 0000000..3467820 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/.svn/entries @@ -0,0 +1,184 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics +http://svndev.alternativaplatform.com + + + +2010-01-15T10:21:05.313154Z +26464 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +CollisionPrimitiveList.as +file + + + + +2010-10-28T04:32:42.000000Z +4a95411d75825cdb04527c284f1708c6 +2009-10-23T05:59:29.567666Z +22444 +mike + +altphysics.as +file + + + + +2010-10-28T04:32:42.000000Z +04fbe40a27502dbbd0dba02a76b2df50 +2009-04-20T20:09:14.715403Z +11579 +mike + +math +dir + +World.as +file + + + + +2010-10-28T04:32:42.000000Z +751ad3f2ae01cf07dbb52dbe10219b75 +2009-10-20T10:42:06.632117Z +22312 +mike + +collision +dir + +primitives +dir + +BodyList.as +file + + + + +2010-10-28T04:32:42.000000Z +08a90332660bd0273728006d1402dd25 +2010-01-14T05:40:59.142095Z +26349 +mike + +Contact.as +file + + + + +2010-10-28T04:32:42.000000Z +675ca7525e2b91ffcb1647978d39eca7 +2009-10-19T07:17:33.113306Z +22253 +mike + +Body.as +file + + + + +2010-10-28T04:32:42.000000Z +99cb2defe02f25078bfd8876dc4ed545 +2009-10-23T05:59:29.567666Z +22444 +mike + +BodyState.as +file + + + + +2010-10-28T04:32:42.000000Z +2ae471fd05b986dce6c19ac37a6beaa8 +2009-10-19T07:17:33.113306Z +22253 +mike + +CollisionPrimitiveListItem.as +file + + + + +2010-10-28T04:32:42.000000Z +cec5dbfefef812d87481478455f367b1 +2009-10-19T07:17:33.113306Z +22253 +mike + +PhysicsUtils.as +file + + + + +2010-10-28T04:32:42.000000Z +b400cdcb00dc64d1dcb1300c95c53fac +2009-10-19T07:17:33.113306Z +22253 +mike + +constraints +dir + +ContactPoint.as +file + + + + +2010-10-28T04:32:42.000000Z +88112c934e6a35adcc6b375d2c6f93b5 +2009-10-19T07:17:33.113306Z +22253 +mike + +BodyListItem.as +file + + + + +2010-10-28T04:32:42.000000Z +2fc2b598310b24aef3f8b90e14a8ba4f +2009-10-19T07:17:33.113306Z +22253 +mike + +BodyMaterial.as +file + + + + +2010-10-28T04:32:42.000000Z +9b700692ebb8017882f3e8cec095e1a9 +2009-10-19T07:17:33.113306Z +22253 +mike + diff --git a/0.0.7.1/src/alternativa/physics/.svn/format b/0.0.7.1/src/alternativa/physics/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.7.1/src/alternativa/physics/.svn/text-base/Body.as.svn-base b/0.0.7.1/src/alternativa/physics/.svn/text-base/Body.as.svn-base new file mode 100644 index 0000000..6b53304 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/.svn/text-base/Body.as.svn-base @@ -0,0 +1,406 @@ +package alternativa.physics { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.IBodyCollisionPredicate; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.math.Matrix3; + import alternativa.physics.math.Matrix4; + import alternativa.physics.math.Quaternion; + import alternativa.physics.math.Vector3; + + use namespace altphysics; + + /** + * + */ + public class Body { + + public static var linDamping:Number = 0.997; + public static var rotDamping:Number = 0.997; + + // Идентификатор тела, уникальный в пределах мира + public var id:int; + // Имя тела + public var name:String; + // Мир, в котором находится тело + public var world:World; + // Флаг подвижности тела + public var movable:Boolean = true; + // Флаг указывает, может ли тело быть заморожено + public var canFreeze:Boolean = false; + + public var freezeCounter:int; + public var frozen:Boolean = false; + // Ограничивающий бокс тела + public var aabb:BoundBox = new BoundBox(); + // Предикат, через который тело получает сообщения о столкновениях + public var postCollisionPredicate:IBodyCollisionPredicate; + + // Текущее состояние тела + public var state:BodyState = new BodyState(); + // Предыдущее состояние тела + public var prevState:BodyState = new BodyState(); + // Линейное ускорение тела на текущем шаге симуляции + public var accel:Vector3 = new Vector3(); + // Угловое ускорение тела на текущем шаге симуляции + public var angleAccel:Vector3 = new Vector3(); + // Физический материал тела + public var material:BodyMaterial = new BodyMaterial(); + // Обратная масса тела + public var invMass:Number = 1; + // Обратная матрица тензора инерции в локальных координатах + public var invInertia:Matrix3 = new Matrix3(); + // Обратная матрица тензора инерции в мировых координатах + public var invInertiaWorld:Matrix3 = new Matrix3(); + // Базисная матрица тела в мировых координатах + public var baseMatrix:Matrix3 = new Matrix3(); + + public const MAX_CONTACTS:int = 20; + public var contacts:Vector. = new Vector.(MAX_CONTACTS); + public var contactsNum:int; + + public var collisionPrimitives:CollisionPrimitiveList; + + // Аккумулятор сил + public var forceAccum:Vector3 = new Vector3(); + // Аккумулятор моментов + public var torqueAccum:Vector3 = new Vector3(); + + // Внутренние переменные для избежания создания экземпляров + private static var _r:Vector3 = new Vector3(); + private static var _f:Vector3 = new Vector3(); + + /** + * + * @param invMass + * @param invInertia + */ + public function Body(invMass:Number, invInertia:Matrix3) { + this.invMass = invMass; + this.invInertia.copy(invInertia); + } + + /** + * @param primitive + * @param localTransform + */ + public function addCollisionPrimitive(primitive:CollisionPrimitive, localTransform:Matrix4 = null):void { + if (primitive == null) { + throw new ArgumentError("Primitive cannot be null"); + } + if (collisionPrimitives == null) { + collisionPrimitives = new CollisionPrimitiveList(); + } + collisionPrimitives.append(primitive); + primitive.setBody(this, localTransform); + } + + /** + * + * @param primitive + */ + public function removeCollisionPrimitive(primitive:CollisionPrimitive):void { + if (collisionPrimitives == null) return; + primitive.setBody(null); + collisionPrimitives.remove(primitive); + if (collisionPrimitives.size == 0) { + collisionPrimitives = null; + } + } + + /** + * @param t + * @param result + */ + public function interpolate(t:Number, pos:Vector3, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.w = prevState.orientation.w*t1 + state.orientation.w*t; + orientation.x = prevState.orientation.x*t1 + state.orientation.x*t; + orientation.y = prevState.orientation.y*t1 + state.orientation.y*t; + orientation.z = prevState.orientation.z*t1 + state.orientation.z*t; + } + + /** + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + state.pos.vCopy(pos); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setPositionXYZ(x:Number, y:Number, z:Number):void { + state.pos.vReset(x, y, z); + } + + /** + * + * @param vel + */ + public function setVelocity(vel:Vector3):void { + state.velocity.vCopy(vel); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setVelocityXYZ(x:Number, y:Number, z:Number):void { + state.velocity.vReset(x, y, z); + } + + /** + * + * @param rot + */ + public function setRotation(rot:Vector3):void { + state.rotation.vCopy(rot); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setRotationXYZ(x:Number, y:Number, z:Number):void { + state.rotation.vReset(x, y, z); + } + + /** + * + * @param q + */ + public function setOrientation(q:Quaternion):void { + state.orientation.copy(q); + } + + /** + * @param r + * @param dir + * @param magnitude + */ + public function applyRelPosWorldImpulse(r:Vector3, dir:Vector3, magnitude:Number):void { + var d:Number = magnitude*invMass; + // Линейная часть + state.velocity.x += d*dir.x; + state.velocity.y += d*dir.y; + state.velocity.z += d*dir.z; + + // Вращательная часть + var x:Number = (r.y*dir.z - r.z*dir.y)*magnitude; + var y:Number = (r.z*dir.x - r.x*dir.z)*magnitude; + var z:Number = (r.x*dir.y - r.y*dir.x)*magnitude; + + state.rotation.x += invInertiaWorld.a*x + invInertiaWorld.b*y + invInertiaWorld.c*z; + state.rotation.y += invInertiaWorld.e*x + invInertiaWorld.f*y + invInertiaWorld.g*z; + state.rotation.z += invInertiaWorld.i*x + invInertiaWorld.j*y + invInertiaWorld.k*z; + } + + /** + * @param f + */ + public function addForce(f:Vector3):void { + forceAccum.vAdd(f); + } + + /** + * + * @param fx + * @param fy + * @param fz + */ + public function addForceXYZ(fx:Number, fy:Number, fz:Number):void { + forceAccum.x += fx; + forceAccum.y += fy; + forceAccum.z += fz; + } + + /** + * @param pos + * @param f + */ + public function addWorldForceXYZ(px:Number, py:Number, pz:Number, fx:Number, fy:Number, fz:Number):void { + forceAccum.x += fx; + forceAccum.y += fy; + forceAccum.z += fz; + + var pos:Vector3 = state.pos; + var rx:Number = px - pos.x; + var ry:Number = py - pos.y; + var rz:Number = pz - pos.z; + +// var x:Number = ry*fz - rz*fy; +// var y:Number = rz*fx - rx*fz; +// var z:Number = rx*fy - ry*fx; + + torqueAccum.x += ry*fz - rz*fy; + torqueAccum.y += rz*fx - rx*fz; + torqueAccum.z += rx*fy - ry*fx; + } + + /** + * @param pos + * @param f + */ + public function addWorldForce(pos:Vector3, force:Vector3):void { + forceAccum.vAdd(force); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(force)); + } + + /** + * @param pos + * @param f + */ + public function addWorldForceScaled(pos:Vector3, force:Vector3, scale:Number):void { + _f.x = scale*force.x; + _f.y = scale*force.y; + _f.z = scale*force.z; + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(_f)); + } + + /** + * @param pos + * @param f + */ + public function addLocalForce(pos:Vector3, force:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(pos, _r); + // Трансформируем вектор силы в мировую систему + baseMatrix.transformVector(force, _f); + // Добавляем силу и момент + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vCross(_f)); + } + + /** + * + * @param localPos + * @param worldForce + */ + public function addWorldForceAtLocalPoint(localPos:Vector3, worldForce:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(localPos, _r); + // Добавляем силу и момент + forceAccum.vAdd(worldForce); + torqueAccum.vAdd(_r.vCross(worldForce)); + } + + /** + * @param dt + */ + public function beforePhysicsStep(dt:Number):void { + } + + /** + * @param t + */ + public function addTorque(t:Vector3):void { + torqueAccum.vAdd(t); + } + + /** + * + */ + altphysics function clearAccumulators():void { + forceAccum.x = forceAccum.y = forceAccum.z = 0; + torqueAccum.x = torqueAccum.y = torqueAccum.z = 0; + } + + /** + * + */ + altphysics function calcAccelerations():void { + accel.x = forceAccum.x*invMass; + accel.y = forceAccum.y*invMass; + accel.z = forceAccum.z*invMass; + angleAccel.x = invInertiaWorld.a*torqueAccum.x + invInertiaWorld.b*torqueAccum.y + invInertiaWorld.c*torqueAccum.z; + angleAccel.y = invInertiaWorld.e*torqueAccum.x + invInertiaWorld.f*torqueAccum.y + invInertiaWorld.g*torqueAccum.z; + angleAccel.z = invInertiaWorld.i*torqueAccum.x + invInertiaWorld.j*torqueAccum.y + invInertiaWorld.k*torqueAccum.z; + } + + /** + * Вычисляет производные данные. + */ + public function calcDerivedData():void { + // Вычисление базисной матрицы и обратного тензора инерции в мировых координатах + state.orientation.toMatrix3(baseMatrix); + invInertiaWorld.copy(invInertia).append(baseMatrix).prependTransposed(baseMatrix); + if (collisionPrimitives != null) { + aabb.infinity(); + var item:CollisionPrimitiveListItem = collisionPrimitives.head; + while (item != null) { + var primitive:CollisionPrimitive = item.primitive; + primitive.transform.setFromMatrix3(baseMatrix, state.pos); + if (primitive.localTransform != null) { + primitive.transform.prepend(primitive.localTransform); + } + primitive.calculateAABB(); + aabb.addBoundBox(primitive.aabb); + item = item.next; + } + } + } + + /** + * + */ + altphysics function saveState():void { + prevState.copy(state); + } + + /** + * + */ + altphysics function restoreState():void { + state.copy(prevState); + } + + /** + * @param dt + */ + altphysics function integrateVelocity(dt:Number):void { + // v = v + a*t + state.velocity.x += accel.x*dt; + state.velocity.y += accel.y*dt; + state.velocity.z += accel.z*dt; + // rot = rot + eps*t + state.rotation.x += angleAccel.x*dt; + state.rotation.y += angleAccel.y*dt; + state.rotation.z += angleAccel.z*dt; + + state.velocity.x *= linDamping; + state.velocity.y *= linDamping; + state.velocity.z *= linDamping; + + state.rotation.x *= rotDamping; + state.rotation.y *= rotDamping; + state.rotation.z *= rotDamping; + } + + /** + * + */ + altphysics function integratePosition(dt:Number):void { + // pos = pos + v*t + state.pos.x += state.velocity.x*dt; + state.pos.y += state.velocity.y*dt; + state.pos.z += state.velocity.z*dt; + // q = q + 0.5*rot*q + state.orientation.addScaledVector(state.rotation, dt); + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/.svn/text-base/BodyList.as.svn-base b/0.0.7.1/src/alternativa/physics/.svn/text-base/BodyList.as.svn-base new file mode 100644 index 0000000..aa8f79f --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/.svn/text-base/BodyList.as.svn-base @@ -0,0 +1,76 @@ +package alternativa.physics { + + /** + * + */ + public class BodyList { + + public var head:BodyListItem; + public var tail:BodyListItem; + public var size:int; + + /** + * + */ + public function BodyList() { + } + + /** + * + * @param body + */ + public function append(body:Body):void { + var item:BodyListItem = BodyListItem.create(body); + if (head == null) { + head = tail = item; + } else { + tail.next = item; + item.prev = tail; + tail = item; + } + size++; + } + + /** + * + * @param body + */ + public function remove(body:Body):Boolean { + var item:BodyListItem = findItem(body); + if (item == null) return false; + if (item == head) { + if (size == 1) { + head = tail = null; + } else { + head = item.next; + head.prev = null; + } + } else { + if (item == tail) { + tail = item.prev; + tail.next = null; + } else { + item.prev.next = item.next; + item.next.prev = item.prev; + } + } + item.dispose(); + size--; + return true; + } + + /** + * + * @param body + * @return + */ + public function findItem(body:Body):BodyListItem { + var item:BodyListItem = head; + while (item != null && item.body != body) { + item = item.next; + } + return item; + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/.svn/text-base/BodyListItem.as.svn-base b/0.0.7.1/src/alternativa/physics/.svn/text-base/BodyListItem.as.svn-base new file mode 100644 index 0000000..382e11c --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/.svn/text-base/BodyListItem.as.svn-base @@ -0,0 +1,74 @@ +package alternativa.physics { + + /** + * + */ + public class BodyListItem { + + // Верхний элемент хранилища + private static var poolTop:BodyListItem; + + /** + * Создаёт новый элемент списка. + * + * @param primitive примитив, содержащийся в элементе + * @return новый элемент списка + */ + public static function create(body:Body):BodyListItem { + var item:BodyListItem; + if (poolTop == null) { + item = new BodyListItem(body); + } else { + item = poolTop; + poolTop = item.next; + item.next = null; + item.body = body; + } + return item; + } + + /** + * Очищает хранилище. + */ + public static function clearPool():void { + var item:BodyListItem = poolTop; + while (item != null) { + poolTop = item.next; + item.next = null; + item = poolTop; + } + } + + /** + * + */ + public var body:Body; + /** + * + */ + public var next:BodyListItem; + /** + * + */ + public var prev:BodyListItem; + + /** + * + * @param body + */ + public function BodyListItem(body:Body) { + this.body = body; + } + + /** + * + */ + public function dispose():void { + body = null; + prev = null; + next = poolTop; + poolTop = this; + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/.svn/text-base/BodyMaterial.as.svn-base b/0.0.7.1/src/alternativa/physics/.svn/text-base/BodyMaterial.as.svn-base new file mode 100644 index 0000000..7a7c47d --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/.svn/text-base/BodyMaterial.as.svn-base @@ -0,0 +1,11 @@ +package alternativa.physics { + + public class BodyMaterial { + + public var restitution:Number = 0; + public var friction:Number = 0.3; +// public var dynamicFriction:Number = 0.2; +// public var dynamicFriction:Number = 0.2; + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/.svn/text-base/BodyState.as.svn-base b/0.0.7.1/src/alternativa/physics/.svn/text-base/BodyState.as.svn-base new file mode 100644 index 0000000..34a1b69 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/.svn/text-base/BodyState.as.svn-base @@ -0,0 +1,39 @@ +package alternativa.physics { + import alternativa.physics.math.Quaternion; + import alternativa.physics.math.Vector3; + + /** + * Класс описывает состояние твёрдого тела. + */ + public class BodyState { + /** + * Положение тела. + */ + public var pos:Vector3 = new Vector3(); + /** + * Ориентация тела. + */ + public var orientation:Quaternion = new Quaternion(); + /** + * Скорость тела. + */ + public var velocity:Vector3 = new Vector3(); + /** + * Угловая скорость тела. + */ + public var rotation:Vector3 = new Vector3(); + + /** + * Копирует значение указанного объекта. + * + * @param state + */ + public function copy(state:BodyState):void { + pos.vCopy(state.pos); + orientation.copy(state.orientation); + velocity.vCopy(state.velocity); + rotation.vCopy(state.rotation); + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/.svn/text-base/CollisionPrimitiveList.as.svn-base b/0.0.7.1/src/alternativa/physics/.svn/text-base/CollisionPrimitiveList.as.svn-base new file mode 100644 index 0000000..7581dee --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/.svn/text-base/CollisionPrimitiveList.as.svn-base @@ -0,0 +1,81 @@ +package alternativa.physics { + import alternativa.physics.collision.CollisionPrimitive; + + /** + * + */ + public class CollisionPrimitiveList { + + public var head:CollisionPrimitiveListItem; + public var tail:CollisionPrimitiveListItem; + public var size:int; + + /** + * + */ + public function CollisionPrimitiveList() { + } + + /** + * + * @param primitive + */ + public function append(primitive:CollisionPrimitive):void { + var item:CollisionPrimitiveListItem = CollisionPrimitiveListItem.create(primitive); + if (head == null) { + head = tail = item; + } else { + tail.next = item; + item.prev = tail; + tail = item; + } + size++; + } + + /** + * + * @param primitve + */ + public function remove(primitve:CollisionPrimitive):void { + if (head == null) return; + var item:CollisionPrimitiveListItem = head; + while (item != null) { + if (item.primitive == primitve) break; + item = item.next; + } + if (item == null) return; + if (item == head) { + if (size == 1) { + head = tail = null; + } else { + head = item.next; + head.prev = null; + } + } else { + if (item == tail) { + tail = tail.prev; + tail.next = null; + } else { + item.prev.next = item.next; + item.next.prev = item.prev; + } + } + item.dispose(); + size--; + } + + /** + * + */ + public function clear():void { + while (head != null) { + var item:CollisionPrimitiveListItem = head; + head = head.next; + item.dispose(); + } + tail = null; + size = 0; + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/.svn/text-base/CollisionPrimitiveListItem.as.svn-base b/0.0.7.1/src/alternativa/physics/.svn/text-base/CollisionPrimitiveListItem.as.svn-base new file mode 100644 index 0000000..d204532 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/.svn/text-base/CollisionPrimitiveListItem.as.svn-base @@ -0,0 +1,76 @@ +package alternativa.physics { + import alternativa.physics.collision.CollisionPrimitive; + + /** + * Элемент списка примитивов. + */ + public class CollisionPrimitiveListItem { + + // Верхний элемент хранилища + private static var poolTop:CollisionPrimitiveListItem; + + /** + * Создаёт новый элемент списка. + * + * @param primitive примитив, содержащийся в элементе + * @return новый элемент списка + */ + public static function create(primitive:CollisionPrimitive):CollisionPrimitiveListItem { + var item:CollisionPrimitiveListItem; + if (poolTop == null) { + item = new CollisionPrimitiveListItem(primitive); + } else { + item = poolTop; + item.primitive = primitive; + poolTop = item.next; + item.next = null; + } + return item; + } + + /** + * Очищает хранилище. + */ + public static function clearPool():void { + var curr:CollisionPrimitiveListItem = poolTop; + while (curr != null) { + poolTop = curr.next; + curr.next = null; + curr = poolTop; + } + } + + /** + * Примитив, хранящийся в элементе списка. + */ + public var primitive:CollisionPrimitive; + /** + * Ссылка не следующий элемент списка. + */ + public var next:CollisionPrimitiveListItem; + /** + * Ссылка не предыдущий элемент списка. + */ + public var prev:CollisionPrimitiveListItem; + + /** + * Создаёт новый экземпляр. + * + * @param primitive примитив, хранящийся в элементе списка + */ + public function CollisionPrimitiveListItem(primitive:CollisionPrimitive) { + this.primitive = primitive; + } + + /** + * Очищает внутренние ссылки и помещает элемент в хранилище для дальнейшего использования. + */ + public function dispose():void { + primitive = null; + prev = null; + next = poolTop; + poolTop = this; + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/.svn/text-base/Contact.as.svn-base b/0.0.7.1/src/alternativa/physics/.svn/text-base/Contact.as.svn-base new file mode 100644 index 0000000..1296996 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/.svn/text-base/Contact.as.svn-base @@ -0,0 +1,69 @@ +package alternativa.physics { + import __AS3__.vec.Vector; + + import alternativa.physics.math.Vector3; + + /** + * Описывает контакт одного или двух тел. + */ + public class Contact { + /** + * Первое тело контакта, не может быть равно null. + */ + public var body1:Body; + /** + * Второе тело контакта, может быть равно null. + */ + public var body2:Body; + /** + * Предрассчитанный взаимный коэффициент отскока. + */ + public var restitution:Number; + /** + * Предрассчитанный взаимный коэффициент трения. + */ + public var friction:Number; + /** + * Нормаль контакта. Направлена к первому телу. + */ + public var normal:Vector3 = new Vector3(); + /** + * Список точек контакта. + */ + public var points:Vector. = new Vector.(MAX_POINTS, true); + /** + * Количество точек контакта. + */ + public var pcount:int; + /** + * Максимальная глубина пересечения. + */ + public var maxPenetration:Number = 0; + /** + * Флаг показывает, разрешён контакт или нет. + */ + public var satisfied:Boolean; + /** + * Следующий контакт в списке. + */ + public var next:Contact; + /** + * Индекс контакта. Первый контакт в списке имеет индекс 0. + */ + public var index:int; + + // Максимальное количество точек контакта + private const MAX_POINTS:int = 8; + + /** + * + */ + public function Contact(index:int) { + this.index = index; + for (var i:int = 0; i < MAX_POINTS; i++) { + points[i] = new ContactPoint(); + } + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/.svn/text-base/ContactPoint.as.svn-base b/0.0.7.1/src/alternativa/physics/.svn/text-base/ContactPoint.as.svn-base new file mode 100644 index 0000000..2136e9a --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/.svn/text-base/ContactPoint.as.svn-base @@ -0,0 +1,55 @@ +package alternativa.physics { + import alternativa.physics.math.Vector3; + + /** + * + */ + public class ContactPoint { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + + // Величины, расчитываемые перед началом фазы решения контактов + + // Требуемая проекция конечной скорости на нормаль для упругого контакта + public var normalVel:Number; + // Минимальная скорость разделения неупругого контакта + public var minSepVel:Number; + // Изменение проекции скорости на единицу нормального импульса + public var velByUnitImpulseN:Number; + + public var angularInertia1:Number; + public var angularInertia2:Number; + + // Радиус-вектор точки контакта относительно центра первого тела + public var r1:Vector3 = new Vector3(); + // Радиус-вектор точки контакта относительно центра второго тела + public var r2:Vector3 = new Vector3(); + + // Величины, накапливаемые во время фазы решения контактов + + // Накопленный импульс, применяемый для получения требуемой относительной скорости в точке контакта + public var accumImpulseN:Number; + // Накопленный импульс, применяемый для разделения тел в точке контакта. Не создаёт момента. +// public var accumSepImpulse:Number; + + public var satisfied:Boolean; + + /** + * + * @param cp + */ + public function copyFrom(cp:ContactPoint):void { + pos.vCopy(cp.pos); + penetration = cp.penetration; + feature1 = cp.feature1; + feature2 = cp.feature2; + r1.vCopy(cp.r1); + r2.vCopy(cp.r2); + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/.svn/text-base/PhysicsUtils.as.svn-base b/0.0.7.1/src/alternativa/physics/.svn/text-base/PhysicsUtils.as.svn-base new file mode 100644 index 0000000..6770b0b --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/.svn/text-base/PhysicsUtils.as.svn-base @@ -0,0 +1,49 @@ +package alternativa.physics { + import alternativa.physics.math.Matrix3; + import alternativa.physics.math.Vector3; + + public class PhysicsUtils { + + /** + * @param mass + * @param halfSize + * @param result + */ + public static function getBoxInvInertia(mass:Number, halfSize:Vector3, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + result.a = 3/(mass*(yy + zz)); + result.f = 3/(mass*(zz + xx)); + result.k = 3/(mass*(xx + yy)); + } + + /** + * @param mass + * @param r + * @param h + * @param result + */ + public static function getCylinderInvInertia(mass:Number, r:Number, h:Number, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + + result.a = result.f = 1/(mass*(h*h/12 + r*r/4)); + result.k = 2/(mass*r*r); + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/.svn/text-base/World.as.svn-base b/0.0.7.1/src/alternativa/physics/.svn/text-base/World.as.svn-base new file mode 100644 index 0000000..9339f20 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/.svn/text-base/World.as.svn-base @@ -0,0 +1,743 @@ +package alternativa.physics { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.ICollisionDetector; + import alternativa.physics.collision.KdTreeCollisionDetector; + import alternativa.physics.constraints.Constraint; + import alternativa.physics.math.Matrix3; + import alternativa.physics.math.Vector3; + + use namespace altphysics; + + /** + * Класс реализует физическую симуляцию поведения твёрдых тел. + */ + public class World { + + private static var lastBodyId:int; + + // Максимальное количество контактов + altphysics const MAX_CONTACTS:int = 1000; + + ////////////////////////////////////////////////////////////////// + // Настроечные параметры симуляции. Могут быть изменены в любой + // момент времени без нарушения корректной работы симуляции. + ////////////////////////////////////////////////////////////////// + // Количество шагов, за которое пересекающиеся тела должны разделиться + public var penResolutionSteps:int = 10; + // Величина допустимой глубины пересечения + public var allowedPenetration:Number = 0.1; + // Максимальная скорость, добавляемая с целью разделения тел + public var maxPenResolutionSpeed:Number = 0.5; + // Количество итераций для обработки упругих контактов + public var collisionIterations:int = 5; + // Количество итераций для обработки неупругих контактов + public var contactIterations:int = 5; + // Флаг использования предсказания состояний + public var usePrediction:Boolean = false; + + public var freezeSteps:int = 10; + public var linSpeedFreezeLimit:Number = 1; + public var angSpeedFreezeLimit:Number = 0.01; + + // Переменные для процедуры разделения тел путём непосредственного их перемещенеия. + // !!!!!!!!!!!!!!!!!!!!!!!! + // !!! Экспериментально !!! + // !!!!!!!!!!!!!!!!!!!!!!!! + public var staticSeparationIterations:int = 10; + public var staticSeparationSteps:int = 10; + public var maxAngleMove:Number = 10; + public var useStaticSeparation:Boolean = false; + + // Вектор гравитации + public var _gravity:Vector3 = new Vector3(0, 0, -9.8); + // Модуль вектора гравитации + public var _gravityMagnitude:Number = 9.8; + + // Использующийся детектор столкновений + public var collisionDetector:ICollisionDetector; + + // Список тел, участвующих в симуляции + public var bodies:BodyList = new BodyList(); + + // Список контактов на текущем шаге симуляции + altphysics var contacts:Contact; + // Количество контактов на текущем шаге симуляции +// altphysics var contactsNum:int; + // Список ограничений + altphysics var constraints:Vector. = new Vector.(); + // Количество ограничений + altphysics var constraintsNum:int; + // Временная метка. Число прошедших шагов с начала симуляции. + public var timeStamp:uint; + // Время с начала симуляции, мс + public var time:uint; + // Первый неиспользованный контакт на текущем шаге симуляции + private var borderContact:Contact; + + // Временные переменные для избежания создания экземпляров + private var _r:Vector3 = new Vector3(); + private var _t:Vector3 = new Vector3(); + private var _v:Vector3 = new Vector3(); + private var _v1:Vector3 = new Vector3(); + private var _v2:Vector3 = new Vector3(); + + /** + * + */ + public function World() { + contacts = new Contact(0); + var contact:Contact = contacts; + for (var i:int = 1; i < MAX_CONTACTS; i++) { + contact.next = new Contact(i); + contact = contact.next; + } + collisionDetector = new KdTreeCollisionDetector(); + } + + /** + * Вектор гравитации. + */ + public function get gravity():Vector3 { + return _gravity.vClone(); + } + + /** + * @private + */ + public function set gravity(value:Vector3):void { + _gravity.vCopy(value); + _gravityMagnitude = _gravity.vLength(); + } + + /** + * Добавляет тело в симуляцию. + * + * @param body + */ + public function addBody(body:Body):void { + body.id = lastBodyId++; + body.world = this; + bodies.append(body); + } + + /** + * Удаляет тело из симуляции. + * @param body + * @return + */ + public function removeBody(body:Body):void { + if (bodies.remove(body)) { + body.world = null; + } + } + + /** + * Добавляет ограничение. + * @param c + */ + public function addConstraint(c:Constraint):void { + constraints[constraintsNum++] = c; + c.world = this; + } + + /** + * Удаляет ограничение. + * @param c + */ + public function removeConstraint(c:Constraint):Boolean { + var idx:int = constraints.indexOf(c); + if (idx < 0) return false; + constraints.splice(idx, 1) + constraintsNum--; + c.world = null; + return true; + } + + /** + * Применяет к телам действующте на них силы. Аккумуляторы сил и моментов тел очищаются после завершения шага + * симуляции, поэтому на момент вызова метода могут уже содержать некоторые значения. + * + * @param dt промежуток времени, в течении которого действуют силы + */ + private function applyForces(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + body.beforePhysicsStep(dt); + body.calcAccelerations(); + // Ускорение свободного падения применяется только к подвижным телам во избежание некорректного изменения + // фиктивной скорости неподвижных тел. + if (body.movable && !body.frozen) { + body.accel.x += _gravity.x; + body.accel.y += _gravity.y; + body.accel.z += _gravity.z; + } + item = item.next; + } + } + + /** + * Определяет все столкновения на текущем шаге симуляции и заполняет список получившихся контактов. + * + * @param dt длительность шага симуляции + */ + private function detectCollisions(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + if (!body.frozen) { + body.contactsNum = 0; + body.saveState(); + // При включённом режиме предсказания состояние тел интегрируется на один шаг вперёд + if (usePrediction) { + body.integrateVelocity(dt); + body.integratePosition(dt); + } + body.calcDerivedData(); + } + item = item.next; + } + + borderContact = collisionDetector.getAllContacts(contacts); + + // Расчёт относительных векторов точки контакта вынесен сюда из-за необходимости учитывать + // положение тел в предсказанном состоянии + var contact:Contact = contacts; + while (contact != borderContact) { + var b1:Body = contact.body1; + var b2:Body = contact.body2; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + var bPos:Vector3 = b1.state.pos; + cp.r1.x = cp.pos.x - bPos.x; + cp.r1.y = cp.pos.y - bPos.y; + cp.r1.z = cp.pos.z - bPos.z; + if (b2 != null) { + bPos = b2.state.pos; + cp.r2.x = cp.pos.x - bPos.x; + cp.r2.y = cp.pos.y - bPos.y; + cp.r2.z = cp.pos.z - bPos.z; + } + } + contact = contact.next; + } + + // Восстановление состояния тел + if (usePrediction) { + item = bodies.head; + while (item != null) { + body = item.body; + if (!body.frozen) { + body.restoreState(); + body.calcDerivedData(); + } + item = item.next; + } + } + } + + /** + * Подготваливает полученные из детектора столкновений контакты, расчитывая значения, не меняющиеся + * в ходе шага симуляции. + */ + private function preProcessContacts(dt:Number):void { + var contact:Contact = contacts; + while (contact != borderContact) { + var b1:Body = contact.body1; + var b2:Body = contact.body2; + // Столкнувшиеся тела размораживаются + if (b1.frozen) { + b1.frozen = false; + b1.freezeCounter = 0; + } + if (b2 != null && b2.frozen) { + b2.frozen = false; + b2.freezeCounter = 0; + } + contact.restitution = b1.material.restitution; + if (b2 != null && b2.material.restitution < contact.restitution) contact.restitution = b2.material.restitution; + contact.friction = b1.material.friction; + if (b2 != null && b2.material.friction < contact.friction) contact.friction = b2.material.friction; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.accumImpulseN = 0; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + cp.velByUnitImpulseN = 0; + if (b1.movable) { + cp.angularInertia1 = _v.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1).vDot(contact.normal); + cp.velByUnitImpulseN += b1.invMass + cp.angularInertia1; + } + if (b2 != null && b2.movable) { + cp.angularInertia2 = _v.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2).vDot(contact.normal); + cp.velByUnitImpulseN += b2.invMass + cp.angularInertia2; + } + // Расчёт требуемой конечной скорости для упругого контакта + calcSepVelocity(b1, b2, cp, _v); + cp.normalVel = _v.vDot(contact.normal); + if (cp.normalVel < 0) cp.normalVel = - contact.restitution*cp.normalVel; + // Скорость разделения неупругого контакта + cp.minSepVel = cp.penetration > allowedPenetration ? (cp.penetration - allowedPenetration)/(penResolutionSteps*dt) : 0; + if (cp.minSepVel > maxPenResolutionSpeed) cp.minSepVel = maxPenResolutionSpeed; + } + contact = contact.next; + } + for (var i:int = 0; i < constraintsNum; i++) { + var constraint:Constraint = constraints[i]; + constraint.preProcess(dt); + } + } + + /** + * + * @param dt + * @param forceInelastic + */ + private function processContacts(dt:Number, forceInelastic:Boolean):void { + var iterNum:int = forceInelastic ? contactIterations : collisionIterations; + var i:int; + var forwardLoop:Boolean = false; + for (var iter:int = 0; iter < iterNum; iter++) { + forwardLoop = !forwardLoop; + var contact:Contact = contacts; + while (contact != borderContact) { + resolveContact(contact, forceInelastic, forwardLoop); + contact = contact.next; + } + // Ограничения + for (i = 0; i < constraintsNum; i++) { + var constraint:Constraint = constraints[i]; + constraint.apply(dt); + } + } + + // Разделение контактов путём непосредственного изменения координат и ориентации. Экспериментально. +// if (forceInelastic && useStaticSeparation) performStaticSeparation(); + } + + /** + * + */ + private function resolveContact(contactInfo:Contact, forceInelastic:Boolean, forwardLoop:Boolean):void { + var b1:Body = contactInfo.body1; + var b2:Body = contactInfo.body2; + var normal:Vector3 = contactInfo.normal; + var i:int; + if (forwardLoop) { + for (i = 0; i < contactInfo.pcount; i++) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } else { + for (i = contactInfo.pcount - 1; i >= 0; i--) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } + } + + /** + * + * @param idx + * @param b1 + * @param b2 + * @param colInfo + * @param normal + * @param forceInelastic + */ + private function resolveContactPoint(idx:int, b1:Body, b2:Body, contact:Contact, normal:Vector3, forceInelastic:Boolean):void { + var cp:ContactPoint = contact.points[idx]; + if (!forceInelastic) { + cp.satisfied = true; + } + + var newVel:Number = 0; + calcSepVelocity(b1, b2, cp, _v); + var cnormal:Vector3 = contact.normal; + var sepVel:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + if (forceInelastic) { + var minSpeVel:Number = useStaticSeparation ? 0 : cp.minSepVel; + if (sepVel < minSpeVel) { + cp.satisfied = false; + } else if (cp.satisfied) return; + newVel = minSpeVel; + } else { + newVel = cp.normalVel; + } + var deltaVel:Number = newVel - sepVel; + var impulse:Number = deltaVel/cp.velByUnitImpulseN; + var accumImpulse:Number = cp.accumImpulseN + impulse; + if (accumImpulse < 0) { + accumImpulse = 0; + } + var deltaImpulse:Number = accumImpulse - cp.accumImpulseN; + cp.accumImpulseN = accumImpulse; + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, normal, deltaImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, normal, -deltaImpulse); + } + + // Учёт силы трения + calcSepVelocity(b1, b2, cp, _v); + // Расчитываем изменение касательной скорости на единицу касательного импульса + var tanSpeedByUnitImpulse:Number = 0; + +// _v.vAddScaled(-_v.vDot(contact.normal), contact.normal); + var dot:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + _v.x -= dot*cnormal.x; + _v.y -= dot*cnormal.y; + _v.z -= dot*cnormal.z; + + var tanSpeed:Number = _v.vLength(); + if (tanSpeed < 0.001) return; + +// _t.vCopy(_v).vNormalize().vReverse(); + _t.x = -_v.x; + _t.y = -_v.y; + _t.z = -_v.z; + _t.vNormalize(); + + var r:Vector3; + var m:Matrix3; + var xx:Number; + var yy:Number; + var zz:Number; + // dV = b.invMass + ((invI * (r % t)) % r) * t + if (b1.movable) { +// _v.vCross2(cp.r1, _t).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1); + r = cp.r1; + m = b1.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b1.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + if (b2 != null && b2.movable) { +// _v.vCross2(cp.r2, _t).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2); + + r = cp.r2; + m = b2.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b2.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + + var tanImpulse:Number = tanSpeed/tanSpeedByUnitImpulse; + var max:Number = contact.friction*cp.accumImpulseN; + if (max < 0) { + if (tanImpulse < max) tanImpulse = max; + } else { + if (tanImpulse > max) tanImpulse = max; + } + + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, _t, tanImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, _t, -tanImpulse); + } + } + + /** + * + * @param cp + * @param normal + * @return + */ + private function calcSepVelocity(body1:Body, body2:Body, cp:ContactPoint, result:Vector3):void { + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 +// result.vCopy(body1.state.velocity).vAdd(_v1.vCross2(body1.state.rotation, cp.r1)); + var rot:Vector3 = body1.state.rotation; + var v:Vector3 = cp.r1; + var x:Number = rot.y*v.z - rot.z*v.y; + var y:Number = rot.z*v.x - rot.x*v.z; + var z:Number = rot.x*v.y - rot.y*v.x; + v = body1.state.velocity; + result.x = v.x + x; + result.y = v.y + y; + result.z = v.z + z; + // V2 = V2_c + w2%r2 + if (body2 != null) { +// result.vSubtract(body2.state.velocity).vSubtract(_v2.vCross2(body2.state.rotation, cp.r2)); + rot = body2.state.rotation; + v = cp.r2; + x = rot.y*v.z - rot.z*v.y; + y = rot.z*v.x - rot.x*v.z; + z = rot.x*v.y - rot.y*v.x; + v = body2.state.velocity; + result.x -= v.x + x; + result.y -= v.y + y; + result.z -= v.z + z; + } + } + + /** + * + * @param dt + */ + private function intergateVelocities(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + item.body.integrateVelocity(dt); + item = item.next; + } + } + + /** + * + * @param dt + */ + private function integratePositions(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + if (body.movable && !body.frozen) { + body.integratePosition(dt); + } + item = item.next; + } + } + + /** + * + */ + private function performStaticSeparation():void { + var iterNum:int = staticSeparationIterations; +// iterNum = 100; + + // 1. В начале каждой итерации для всех контактов сбрасывается флаг satisfied с одновременным поиском наихудшего контакта, т.е. имеющего наибольшую величину пересечения тел. + // 2. Если найденный контакт имеет величину пересечения меньше предельно допустимой процедура прерывается, т.к. разделение контактов не требуется. + // 3. Если процедура продолжается, то выполняется разделение найденного контакта и установка его флага satisfied в true, после чего запускается внутренний цикл, состоящий + // из contactsNum - 1 итераций. На каждой итерации ищется наихудший контакт среди оставшихся (satisfied == false). Если такой контакт найден, то выполняется его разделение + // и переход к следующей итерации внутреннего цикла, иначе внутренний цикл прерывается и выполняется переход к следующей итерации внешнего цикла (шаг 1.). +// for (var iter:int = 0; iter < iterNum; iter++) { +// // Ищем контакт с максимальной величиной пересечения тел, одновременно сбрасывая флаги satisfied в false +// var worstContact:Contact = contacts[0]; +// var i:int; +// for (i = 1; i < contactsNum; i++) { +// var contact:Contact = contacts[i]; +// contact.satisfied = false; +// if (contact.maxPenetration > worstContact.maxPenetration) worstContact = contact; +// } +// if (worstContact.maxPenetration <= allowedPenetration) return; +// +// resolveInterpenetration(worstContact); +// // Внутренний цикл по оставшимся контактам +// for (i = 1; i < contactsNum; i++) { +// worstContact = getWorstContact(); +// if (worstContact == null) break; +// resolveInterpenetration(worstContact); +// } +// } + } + + /** + * + */ +// private function getWorstContact():Contact { +// var maxPen:Number = 0; +// var worst:Contact = null; +// for (var i:int = 0; i < contactsNum; i++) { +// var c:Contact = contacts[i]; +// if (!c.satisfied && c.maxPenetration > maxPen) { +// worst = c; +// maxPen = c.maxPenetration; +// } +// } +// return maxPen > allowedPenetration ? worst : null; +// } + + /** + * Разделяет указанный контакт, выполняя staticSeparationSteps итераций по списку точек контакта. + * 1. В начале каждой итерации ищется точка с наибольшим пересечением, одновременно сбрасываются флаги satisfied у точек. + * 2. Для найденной точки выполняется процедура разделения. + * 3. После выполняется pcount - 1 итерация по списку точек, каждый раз ищется наихудшая среди имеющих satisfied == false, для которой выполняется процедура разделения. + * Если наихудшая точка не найдена, то выполнется переход к следующей итерации внешнего цикла (шаг 1). + * + * В конце процедуры разделения обновляются значения пересечений для остальных точек контакта, а также для всех прочих контактов, относящихся к телам текущего. + * + * @param contact контакт для разделения + */ + private function resolveInterpenetration(contact:Contact):void { + contact.satisfied = true; + + for (var step:int = 0; step < staticSeparationSteps; step++) { + var worstCp:ContactPoint = contact.points[0]; + var cp:ContactPoint; + var i:int; + for (i = 1; i < contact.pcount; i++) { + cp = contact.points[i]; + cp.satisfied = false; + if (cp.penetration > worstCp.penetration) worstCp = cp; + } + if (worstCp.penetration <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + // Разделяем оставшиеся точки + var maxPen:Number = 0; + for (i = 1; i < contact.pcount; i++) { + // Поиск наихудшей точки + for (var j:int = 0; j < contact.pcount; j++) { + cp = contact.points[j]; + if (cp.satisfied) continue; + if (cp.penetration > maxPen) { + maxPen = cp.penetration; + worstCp = cp; + } + } + if (maxPen <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + } + } + } + + /** + * + * @param cp + * @param contact + */ + private function separateContactPoint(cp:ContactPoint, contact:Contact):void { +// cp.satisfied = true; +// +// var b1:Body = contact.body1; +// var b2:Body = contact.body2; +// var totalMove:Number = cp.penetration - allowedPenetration; +// var moveCoeff:Number = totalMove/cp.velByUnitImpulseN; +// var linMove1:Number; +// var angleMove1:Number; +// if (b1.movable) { +// linMove1 = b1.invMass*moveCoeff; +// angleMove1 = cp.angularInertia1*moveCoeff; +// if (angleMove1 > maxAngleMove) { +// linMove1 += angleMove1 - maxAngleMove; +// angleMove1 -= maxAngleMove; +// } +// b1.state.pos.vAddScaled(linMove1, contact.normal); +// _v1.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vScale(angleMove1); +// b1.state.orientation.addScaledVector(_v1, 1); +// } +// var linMove2:Number; +// var angleMove2:Number; +// if (b2 != null && b2.movable) { +// linMove2 = b2.invMass*moveCoeff; +// angleMove2 = cp.angularInertia2*moveCoeff; +// if (angleMove2 > maxAngleMove) { +// linMove2 += angleMove2 - maxAngleMove; +// angleMove2 -= maxAngleMove; +// } +// b2.state.pos.vAddScaled(-linMove2, contact.normal); +// _v2.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vScale(angleMove2); +// _v2.vReverse(); +// b2.state.orientation.addScaledVector(_v2, 1); +// } +// cp.penetration = allowedPenetration; +// // Обновляем пересечения в других точках +// var i:int; +// for (i = 0; i < contact.pcount; i++) { +// var cp1:ContactPoint = contact.points[i]; +// if (cp1 == cp) continue; +// var angularMove:Number; +// if (b1.movable) { +// angularMove = _v.vCross2(_v1, cp1.r1).vDot(contact.normal); +// cp1.penetration -= linMove1 + angularMove; +// } +// if (b2 != null && b2.movable) { +// angularMove = _v.vCross2(_v2, cp1.r2).vDot(contact.normal); +// cp1.penetration -= linMove2 - angularMove; +// } +// // Обновление максимального значения для контакта +// if (cp1.penetration > contact.maxPenetration) contact.maxPenetration = cp1.penetration; +// } +// // Обновляем пересечения для других контактов +// var c:Contact; +// var j:int; +// if (b1.movable) { +// for (i = 0; i < b1.contactsNum; i++) { +// c = b1.contacts[i]; +// if (c == contact) continue; +// for (j = 0; j < c.pcount; j++) { +// cp1 = c.points[j]; +// if (b1 == c.body1) cp1.penetration -= linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r1).vDot(c.normal); +// else cp1.penetration += linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r2).vDot(c.normal); +// if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; +// } +// } +// } +// if (b2 != null && b2.movable) { +// for (i = 0; i < b2.contactsNum; i++) { +// c = b2.contacts[i]; +// if (c == contact) continue; +// for (j = 0; j < c.pcount; j++) { +// cp1 = c.points[j]; +// if (b2 == c.body1) cp1.penetration -= linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r1).vDot(c.normal); +// else cp1.penetration += linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r2).vDot(c.normal); +// if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; +// } +// } +// } + } + + /** + * + */ + private function postPhysics():void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + body.clearAccumulators(); + body.calcDerivedData(); + if (body.canFreeze) { + if (body.state.velocity.vLength() < linSpeedFreezeLimit && body.state.rotation.vLength() < angSpeedFreezeLimit) { + if (!body.frozen) { + body.freezeCounter++; + if (body.freezeCounter >= freezeSteps) body.frozen = true; + } + } else { + body.freezeCounter = 0; + body.frozen = false; + } + } + item = item.next; + } + } + + /** + * + * @param delta + */ + public function runPhysics(delta:uint):void { + timeStamp++; + time += delta; + var dt:Number = 0.001*delta; + applyForces(dt); + detectCollisions(dt); + preProcessContacts(dt); + processContacts(dt, false); + intergateVelocities(dt); + processContacts(dt, true); + integratePositions(dt); + postPhysics(); + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base b/0.0.7.1/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base new file mode 100644 index 0000000..2715307 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base @@ -0,0 +1,3 @@ +package alternativa.physics { + public namespace altphysics = "http://alternativaplatform.com/en/altphysics"; +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/Body.as b/0.0.7.1/src/alternativa/physics/Body.as new file mode 100644 index 0000000..6b53304 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/Body.as @@ -0,0 +1,406 @@ +package alternativa.physics { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.IBodyCollisionPredicate; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.math.Matrix3; + import alternativa.physics.math.Matrix4; + import alternativa.physics.math.Quaternion; + import alternativa.physics.math.Vector3; + + use namespace altphysics; + + /** + * + */ + public class Body { + + public static var linDamping:Number = 0.997; + public static var rotDamping:Number = 0.997; + + // Идентификатор тела, уникальный в пределах мира + public var id:int; + // Имя тела + public var name:String; + // Мир, в котором находится тело + public var world:World; + // Флаг подвижности тела + public var movable:Boolean = true; + // Флаг указывает, может ли тело быть заморожено + public var canFreeze:Boolean = false; + + public var freezeCounter:int; + public var frozen:Boolean = false; + // Ограничивающий бокс тела + public var aabb:BoundBox = new BoundBox(); + // Предикат, через который тело получает сообщения о столкновениях + public var postCollisionPredicate:IBodyCollisionPredicate; + + // Текущее состояние тела + public var state:BodyState = new BodyState(); + // Предыдущее состояние тела + public var prevState:BodyState = new BodyState(); + // Линейное ускорение тела на текущем шаге симуляции + public var accel:Vector3 = new Vector3(); + // Угловое ускорение тела на текущем шаге симуляции + public var angleAccel:Vector3 = new Vector3(); + // Физический материал тела + public var material:BodyMaterial = new BodyMaterial(); + // Обратная масса тела + public var invMass:Number = 1; + // Обратная матрица тензора инерции в локальных координатах + public var invInertia:Matrix3 = new Matrix3(); + // Обратная матрица тензора инерции в мировых координатах + public var invInertiaWorld:Matrix3 = new Matrix3(); + // Базисная матрица тела в мировых координатах + public var baseMatrix:Matrix3 = new Matrix3(); + + public const MAX_CONTACTS:int = 20; + public var contacts:Vector. = new Vector.(MAX_CONTACTS); + public var contactsNum:int; + + public var collisionPrimitives:CollisionPrimitiveList; + + // Аккумулятор сил + public var forceAccum:Vector3 = new Vector3(); + // Аккумулятор моментов + public var torqueAccum:Vector3 = new Vector3(); + + // Внутренние переменные для избежания создания экземпляров + private static var _r:Vector3 = new Vector3(); + private static var _f:Vector3 = new Vector3(); + + /** + * + * @param invMass + * @param invInertia + */ + public function Body(invMass:Number, invInertia:Matrix3) { + this.invMass = invMass; + this.invInertia.copy(invInertia); + } + + /** + * @param primitive + * @param localTransform + */ + public function addCollisionPrimitive(primitive:CollisionPrimitive, localTransform:Matrix4 = null):void { + if (primitive == null) { + throw new ArgumentError("Primitive cannot be null"); + } + if (collisionPrimitives == null) { + collisionPrimitives = new CollisionPrimitiveList(); + } + collisionPrimitives.append(primitive); + primitive.setBody(this, localTransform); + } + + /** + * + * @param primitive + */ + public function removeCollisionPrimitive(primitive:CollisionPrimitive):void { + if (collisionPrimitives == null) return; + primitive.setBody(null); + collisionPrimitives.remove(primitive); + if (collisionPrimitives.size == 0) { + collisionPrimitives = null; + } + } + + /** + * @param t + * @param result + */ + public function interpolate(t:Number, pos:Vector3, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.w = prevState.orientation.w*t1 + state.orientation.w*t; + orientation.x = prevState.orientation.x*t1 + state.orientation.x*t; + orientation.y = prevState.orientation.y*t1 + state.orientation.y*t; + orientation.z = prevState.orientation.z*t1 + state.orientation.z*t; + } + + /** + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + state.pos.vCopy(pos); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setPositionXYZ(x:Number, y:Number, z:Number):void { + state.pos.vReset(x, y, z); + } + + /** + * + * @param vel + */ + public function setVelocity(vel:Vector3):void { + state.velocity.vCopy(vel); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setVelocityXYZ(x:Number, y:Number, z:Number):void { + state.velocity.vReset(x, y, z); + } + + /** + * + * @param rot + */ + public function setRotation(rot:Vector3):void { + state.rotation.vCopy(rot); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setRotationXYZ(x:Number, y:Number, z:Number):void { + state.rotation.vReset(x, y, z); + } + + /** + * + * @param q + */ + public function setOrientation(q:Quaternion):void { + state.orientation.copy(q); + } + + /** + * @param r + * @param dir + * @param magnitude + */ + public function applyRelPosWorldImpulse(r:Vector3, dir:Vector3, magnitude:Number):void { + var d:Number = magnitude*invMass; + // Линейная часть + state.velocity.x += d*dir.x; + state.velocity.y += d*dir.y; + state.velocity.z += d*dir.z; + + // Вращательная часть + var x:Number = (r.y*dir.z - r.z*dir.y)*magnitude; + var y:Number = (r.z*dir.x - r.x*dir.z)*magnitude; + var z:Number = (r.x*dir.y - r.y*dir.x)*magnitude; + + state.rotation.x += invInertiaWorld.a*x + invInertiaWorld.b*y + invInertiaWorld.c*z; + state.rotation.y += invInertiaWorld.e*x + invInertiaWorld.f*y + invInertiaWorld.g*z; + state.rotation.z += invInertiaWorld.i*x + invInertiaWorld.j*y + invInertiaWorld.k*z; + } + + /** + * @param f + */ + public function addForce(f:Vector3):void { + forceAccum.vAdd(f); + } + + /** + * + * @param fx + * @param fy + * @param fz + */ + public function addForceXYZ(fx:Number, fy:Number, fz:Number):void { + forceAccum.x += fx; + forceAccum.y += fy; + forceAccum.z += fz; + } + + /** + * @param pos + * @param f + */ + public function addWorldForceXYZ(px:Number, py:Number, pz:Number, fx:Number, fy:Number, fz:Number):void { + forceAccum.x += fx; + forceAccum.y += fy; + forceAccum.z += fz; + + var pos:Vector3 = state.pos; + var rx:Number = px - pos.x; + var ry:Number = py - pos.y; + var rz:Number = pz - pos.z; + +// var x:Number = ry*fz - rz*fy; +// var y:Number = rz*fx - rx*fz; +// var z:Number = rx*fy - ry*fx; + + torqueAccum.x += ry*fz - rz*fy; + torqueAccum.y += rz*fx - rx*fz; + torqueAccum.z += rx*fy - ry*fx; + } + + /** + * @param pos + * @param f + */ + public function addWorldForce(pos:Vector3, force:Vector3):void { + forceAccum.vAdd(force); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(force)); + } + + /** + * @param pos + * @param f + */ + public function addWorldForceScaled(pos:Vector3, force:Vector3, scale:Number):void { + _f.x = scale*force.x; + _f.y = scale*force.y; + _f.z = scale*force.z; + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(_f)); + } + + /** + * @param pos + * @param f + */ + public function addLocalForce(pos:Vector3, force:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(pos, _r); + // Трансформируем вектор силы в мировую систему + baseMatrix.transformVector(force, _f); + // Добавляем силу и момент + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vCross(_f)); + } + + /** + * + * @param localPos + * @param worldForce + */ + public function addWorldForceAtLocalPoint(localPos:Vector3, worldForce:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(localPos, _r); + // Добавляем силу и момент + forceAccum.vAdd(worldForce); + torqueAccum.vAdd(_r.vCross(worldForce)); + } + + /** + * @param dt + */ + public function beforePhysicsStep(dt:Number):void { + } + + /** + * @param t + */ + public function addTorque(t:Vector3):void { + torqueAccum.vAdd(t); + } + + /** + * + */ + altphysics function clearAccumulators():void { + forceAccum.x = forceAccum.y = forceAccum.z = 0; + torqueAccum.x = torqueAccum.y = torqueAccum.z = 0; + } + + /** + * + */ + altphysics function calcAccelerations():void { + accel.x = forceAccum.x*invMass; + accel.y = forceAccum.y*invMass; + accel.z = forceAccum.z*invMass; + angleAccel.x = invInertiaWorld.a*torqueAccum.x + invInertiaWorld.b*torqueAccum.y + invInertiaWorld.c*torqueAccum.z; + angleAccel.y = invInertiaWorld.e*torqueAccum.x + invInertiaWorld.f*torqueAccum.y + invInertiaWorld.g*torqueAccum.z; + angleAccel.z = invInertiaWorld.i*torqueAccum.x + invInertiaWorld.j*torqueAccum.y + invInertiaWorld.k*torqueAccum.z; + } + + /** + * Вычисляет производные данные. + */ + public function calcDerivedData():void { + // Вычисление базисной матрицы и обратного тензора инерции в мировых координатах + state.orientation.toMatrix3(baseMatrix); + invInertiaWorld.copy(invInertia).append(baseMatrix).prependTransposed(baseMatrix); + if (collisionPrimitives != null) { + aabb.infinity(); + var item:CollisionPrimitiveListItem = collisionPrimitives.head; + while (item != null) { + var primitive:CollisionPrimitive = item.primitive; + primitive.transform.setFromMatrix3(baseMatrix, state.pos); + if (primitive.localTransform != null) { + primitive.transform.prepend(primitive.localTransform); + } + primitive.calculateAABB(); + aabb.addBoundBox(primitive.aabb); + item = item.next; + } + } + } + + /** + * + */ + altphysics function saveState():void { + prevState.copy(state); + } + + /** + * + */ + altphysics function restoreState():void { + state.copy(prevState); + } + + /** + * @param dt + */ + altphysics function integrateVelocity(dt:Number):void { + // v = v + a*t + state.velocity.x += accel.x*dt; + state.velocity.y += accel.y*dt; + state.velocity.z += accel.z*dt; + // rot = rot + eps*t + state.rotation.x += angleAccel.x*dt; + state.rotation.y += angleAccel.y*dt; + state.rotation.z += angleAccel.z*dt; + + state.velocity.x *= linDamping; + state.velocity.y *= linDamping; + state.velocity.z *= linDamping; + + state.rotation.x *= rotDamping; + state.rotation.y *= rotDamping; + state.rotation.z *= rotDamping; + } + + /** + * + */ + altphysics function integratePosition(dt:Number):void { + // pos = pos + v*t + state.pos.x += state.velocity.x*dt; + state.pos.y += state.velocity.y*dt; + state.pos.z += state.velocity.z*dt; + // q = q + 0.5*rot*q + state.orientation.addScaledVector(state.rotation, dt); + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/BodyList.as b/0.0.7.1/src/alternativa/physics/BodyList.as new file mode 100644 index 0000000..aa8f79f --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/BodyList.as @@ -0,0 +1,76 @@ +package alternativa.physics { + + /** + * + */ + public class BodyList { + + public var head:BodyListItem; + public var tail:BodyListItem; + public var size:int; + + /** + * + */ + public function BodyList() { + } + + /** + * + * @param body + */ + public function append(body:Body):void { + var item:BodyListItem = BodyListItem.create(body); + if (head == null) { + head = tail = item; + } else { + tail.next = item; + item.prev = tail; + tail = item; + } + size++; + } + + /** + * + * @param body + */ + public function remove(body:Body):Boolean { + var item:BodyListItem = findItem(body); + if (item == null) return false; + if (item == head) { + if (size == 1) { + head = tail = null; + } else { + head = item.next; + head.prev = null; + } + } else { + if (item == tail) { + tail = item.prev; + tail.next = null; + } else { + item.prev.next = item.next; + item.next.prev = item.prev; + } + } + item.dispose(); + size--; + return true; + } + + /** + * + * @param body + * @return + */ + public function findItem(body:Body):BodyListItem { + var item:BodyListItem = head; + while (item != null && item.body != body) { + item = item.next; + } + return item; + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/BodyListItem.as b/0.0.7.1/src/alternativa/physics/BodyListItem.as new file mode 100644 index 0000000..382e11c --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/BodyListItem.as @@ -0,0 +1,74 @@ +package alternativa.physics { + + /** + * + */ + public class BodyListItem { + + // Верхний элемент хранилища + private static var poolTop:BodyListItem; + + /** + * Создаёт новый элемент списка. + * + * @param primitive примитив, содержащийся в элементе + * @return новый элемент списка + */ + public static function create(body:Body):BodyListItem { + var item:BodyListItem; + if (poolTop == null) { + item = new BodyListItem(body); + } else { + item = poolTop; + poolTop = item.next; + item.next = null; + item.body = body; + } + return item; + } + + /** + * Очищает хранилище. + */ + public static function clearPool():void { + var item:BodyListItem = poolTop; + while (item != null) { + poolTop = item.next; + item.next = null; + item = poolTop; + } + } + + /** + * + */ + public var body:Body; + /** + * + */ + public var next:BodyListItem; + /** + * + */ + public var prev:BodyListItem; + + /** + * + * @param body + */ + public function BodyListItem(body:Body) { + this.body = body; + } + + /** + * + */ + public function dispose():void { + body = null; + prev = null; + next = poolTop; + poolTop = this; + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/BodyMaterial.as b/0.0.7.1/src/alternativa/physics/BodyMaterial.as new file mode 100644 index 0000000..7a7c47d --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/BodyMaterial.as @@ -0,0 +1,11 @@ +package alternativa.physics { + + public class BodyMaterial { + + public var restitution:Number = 0; + public var friction:Number = 0.3; +// public var dynamicFriction:Number = 0.2; +// public var dynamicFriction:Number = 0.2; + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/BodyState.as b/0.0.7.1/src/alternativa/physics/BodyState.as new file mode 100644 index 0000000..34a1b69 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/BodyState.as @@ -0,0 +1,39 @@ +package alternativa.physics { + import alternativa.physics.math.Quaternion; + import alternativa.physics.math.Vector3; + + /** + * Класс описывает состояние твёрдого тела. + */ + public class BodyState { + /** + * Положение тела. + */ + public var pos:Vector3 = new Vector3(); + /** + * Ориентация тела. + */ + public var orientation:Quaternion = new Quaternion(); + /** + * Скорость тела. + */ + public var velocity:Vector3 = new Vector3(); + /** + * Угловая скорость тела. + */ + public var rotation:Vector3 = new Vector3(); + + /** + * Копирует значение указанного объекта. + * + * @param state + */ + public function copy(state:BodyState):void { + pos.vCopy(state.pos); + orientation.copy(state.orientation); + velocity.vCopy(state.velocity); + rotation.vCopy(state.rotation); + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/CollisionPrimitiveList.as b/0.0.7.1/src/alternativa/physics/CollisionPrimitiveList.as new file mode 100644 index 0000000..7581dee --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/CollisionPrimitiveList.as @@ -0,0 +1,81 @@ +package alternativa.physics { + import alternativa.physics.collision.CollisionPrimitive; + + /** + * + */ + public class CollisionPrimitiveList { + + public var head:CollisionPrimitiveListItem; + public var tail:CollisionPrimitiveListItem; + public var size:int; + + /** + * + */ + public function CollisionPrimitiveList() { + } + + /** + * + * @param primitive + */ + public function append(primitive:CollisionPrimitive):void { + var item:CollisionPrimitiveListItem = CollisionPrimitiveListItem.create(primitive); + if (head == null) { + head = tail = item; + } else { + tail.next = item; + item.prev = tail; + tail = item; + } + size++; + } + + /** + * + * @param primitve + */ + public function remove(primitve:CollisionPrimitive):void { + if (head == null) return; + var item:CollisionPrimitiveListItem = head; + while (item != null) { + if (item.primitive == primitve) break; + item = item.next; + } + if (item == null) return; + if (item == head) { + if (size == 1) { + head = tail = null; + } else { + head = item.next; + head.prev = null; + } + } else { + if (item == tail) { + tail = tail.prev; + tail.next = null; + } else { + item.prev.next = item.next; + item.next.prev = item.prev; + } + } + item.dispose(); + size--; + } + + /** + * + */ + public function clear():void { + while (head != null) { + var item:CollisionPrimitiveListItem = head; + head = head.next; + item.dispose(); + } + tail = null; + size = 0; + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/CollisionPrimitiveListItem.as b/0.0.7.1/src/alternativa/physics/CollisionPrimitiveListItem.as new file mode 100644 index 0000000..d204532 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/CollisionPrimitiveListItem.as @@ -0,0 +1,76 @@ +package alternativa.physics { + import alternativa.physics.collision.CollisionPrimitive; + + /** + * Элемент списка примитивов. + */ + public class CollisionPrimitiveListItem { + + // Верхний элемент хранилища + private static var poolTop:CollisionPrimitiveListItem; + + /** + * Создаёт новый элемент списка. + * + * @param primitive примитив, содержащийся в элементе + * @return новый элемент списка + */ + public static function create(primitive:CollisionPrimitive):CollisionPrimitiveListItem { + var item:CollisionPrimitiveListItem; + if (poolTop == null) { + item = new CollisionPrimitiveListItem(primitive); + } else { + item = poolTop; + item.primitive = primitive; + poolTop = item.next; + item.next = null; + } + return item; + } + + /** + * Очищает хранилище. + */ + public static function clearPool():void { + var curr:CollisionPrimitiveListItem = poolTop; + while (curr != null) { + poolTop = curr.next; + curr.next = null; + curr = poolTop; + } + } + + /** + * Примитив, хранящийся в элементе списка. + */ + public var primitive:CollisionPrimitive; + /** + * Ссылка не следующий элемент списка. + */ + public var next:CollisionPrimitiveListItem; + /** + * Ссылка не предыдущий элемент списка. + */ + public var prev:CollisionPrimitiveListItem; + + /** + * Создаёт новый экземпляр. + * + * @param primitive примитив, хранящийся в элементе списка + */ + public function CollisionPrimitiveListItem(primitive:CollisionPrimitive) { + this.primitive = primitive; + } + + /** + * Очищает внутренние ссылки и помещает элемент в хранилище для дальнейшего использования. + */ + public function dispose():void { + primitive = null; + prev = null; + next = poolTop; + poolTop = this; + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/Contact.as b/0.0.7.1/src/alternativa/physics/Contact.as new file mode 100644 index 0000000..1296996 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/Contact.as @@ -0,0 +1,69 @@ +package alternativa.physics { + import __AS3__.vec.Vector; + + import alternativa.physics.math.Vector3; + + /** + * Описывает контакт одного или двух тел. + */ + public class Contact { + /** + * Первое тело контакта, не может быть равно null. + */ + public var body1:Body; + /** + * Второе тело контакта, может быть равно null. + */ + public var body2:Body; + /** + * Предрассчитанный взаимный коэффициент отскока. + */ + public var restitution:Number; + /** + * Предрассчитанный взаимный коэффициент трения. + */ + public var friction:Number; + /** + * Нормаль контакта. Направлена к первому телу. + */ + public var normal:Vector3 = new Vector3(); + /** + * Список точек контакта. + */ + public var points:Vector. = new Vector.(MAX_POINTS, true); + /** + * Количество точек контакта. + */ + public var pcount:int; + /** + * Максимальная глубина пересечения. + */ + public var maxPenetration:Number = 0; + /** + * Флаг показывает, разрешён контакт или нет. + */ + public var satisfied:Boolean; + /** + * Следующий контакт в списке. + */ + public var next:Contact; + /** + * Индекс контакта. Первый контакт в списке имеет индекс 0. + */ + public var index:int; + + // Максимальное количество точек контакта + private const MAX_POINTS:int = 8; + + /** + * + */ + public function Contact(index:int) { + this.index = index; + for (var i:int = 0; i < MAX_POINTS; i++) { + points[i] = new ContactPoint(); + } + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/ContactPoint.as b/0.0.7.1/src/alternativa/physics/ContactPoint.as new file mode 100644 index 0000000..2136e9a --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/ContactPoint.as @@ -0,0 +1,55 @@ +package alternativa.physics { + import alternativa.physics.math.Vector3; + + /** + * + */ + public class ContactPoint { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + + // Величины, расчитываемые перед началом фазы решения контактов + + // Требуемая проекция конечной скорости на нормаль для упругого контакта + public var normalVel:Number; + // Минимальная скорость разделения неупругого контакта + public var minSepVel:Number; + // Изменение проекции скорости на единицу нормального импульса + public var velByUnitImpulseN:Number; + + public var angularInertia1:Number; + public var angularInertia2:Number; + + // Радиус-вектор точки контакта относительно центра первого тела + public var r1:Vector3 = new Vector3(); + // Радиус-вектор точки контакта относительно центра второго тела + public var r2:Vector3 = new Vector3(); + + // Величины, накапливаемые во время фазы решения контактов + + // Накопленный импульс, применяемый для получения требуемой относительной скорости в точке контакта + public var accumImpulseN:Number; + // Накопленный импульс, применяемый для разделения тел в точке контакта. Не создаёт момента. +// public var accumSepImpulse:Number; + + public var satisfied:Boolean; + + /** + * + * @param cp + */ + public function copyFrom(cp:ContactPoint):void { + pos.vCopy(cp.pos); + penetration = cp.penetration; + feature1 = cp.feature1; + feature2 = cp.feature2; + r1.vCopy(cp.r1); + r2.vCopy(cp.r2); + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/PhysicsUtils.as b/0.0.7.1/src/alternativa/physics/PhysicsUtils.as new file mode 100644 index 0000000..6770b0b --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/PhysicsUtils.as @@ -0,0 +1,49 @@ +package alternativa.physics { + import alternativa.physics.math.Matrix3; + import alternativa.physics.math.Vector3; + + public class PhysicsUtils { + + /** + * @param mass + * @param halfSize + * @param result + */ + public static function getBoxInvInertia(mass:Number, halfSize:Vector3, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + result.a = 3/(mass*(yy + zz)); + result.f = 3/(mass*(zz + xx)); + result.k = 3/(mass*(xx + yy)); + } + + /** + * @param mass + * @param r + * @param h + * @param result + */ + public static function getCylinderInvInertia(mass:Number, r:Number, h:Number, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + + result.a = result.f = 1/(mass*(h*h/12 + r*r/4)); + result.k = 2/(mass*r*r); + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/World.as b/0.0.7.1/src/alternativa/physics/World.as new file mode 100644 index 0000000..9339f20 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/World.as @@ -0,0 +1,743 @@ +package alternativa.physics { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.ICollisionDetector; + import alternativa.physics.collision.KdTreeCollisionDetector; + import alternativa.physics.constraints.Constraint; + import alternativa.physics.math.Matrix3; + import alternativa.physics.math.Vector3; + + use namespace altphysics; + + /** + * Класс реализует физическую симуляцию поведения твёрдых тел. + */ + public class World { + + private static var lastBodyId:int; + + // Максимальное количество контактов + altphysics const MAX_CONTACTS:int = 1000; + + ////////////////////////////////////////////////////////////////// + // Настроечные параметры симуляции. Могут быть изменены в любой + // момент времени без нарушения корректной работы симуляции. + ////////////////////////////////////////////////////////////////// + // Количество шагов, за которое пересекающиеся тела должны разделиться + public var penResolutionSteps:int = 10; + // Величина допустимой глубины пересечения + public var allowedPenetration:Number = 0.1; + // Максимальная скорость, добавляемая с целью разделения тел + public var maxPenResolutionSpeed:Number = 0.5; + // Количество итераций для обработки упругих контактов + public var collisionIterations:int = 5; + // Количество итераций для обработки неупругих контактов + public var contactIterations:int = 5; + // Флаг использования предсказания состояний + public var usePrediction:Boolean = false; + + public var freezeSteps:int = 10; + public var linSpeedFreezeLimit:Number = 1; + public var angSpeedFreezeLimit:Number = 0.01; + + // Переменные для процедуры разделения тел путём непосредственного их перемещенеия. + // !!!!!!!!!!!!!!!!!!!!!!!! + // !!! Экспериментально !!! + // !!!!!!!!!!!!!!!!!!!!!!!! + public var staticSeparationIterations:int = 10; + public var staticSeparationSteps:int = 10; + public var maxAngleMove:Number = 10; + public var useStaticSeparation:Boolean = false; + + // Вектор гравитации + public var _gravity:Vector3 = new Vector3(0, 0, -9.8); + // Модуль вектора гравитации + public var _gravityMagnitude:Number = 9.8; + + // Использующийся детектор столкновений + public var collisionDetector:ICollisionDetector; + + // Список тел, участвующих в симуляции + public var bodies:BodyList = new BodyList(); + + // Список контактов на текущем шаге симуляции + altphysics var contacts:Contact; + // Количество контактов на текущем шаге симуляции +// altphysics var contactsNum:int; + // Список ограничений + altphysics var constraints:Vector. = new Vector.(); + // Количество ограничений + altphysics var constraintsNum:int; + // Временная метка. Число прошедших шагов с начала симуляции. + public var timeStamp:uint; + // Время с начала симуляции, мс + public var time:uint; + // Первый неиспользованный контакт на текущем шаге симуляции + private var borderContact:Contact; + + // Временные переменные для избежания создания экземпляров + private var _r:Vector3 = new Vector3(); + private var _t:Vector3 = new Vector3(); + private var _v:Vector3 = new Vector3(); + private var _v1:Vector3 = new Vector3(); + private var _v2:Vector3 = new Vector3(); + + /** + * + */ + public function World() { + contacts = new Contact(0); + var contact:Contact = contacts; + for (var i:int = 1; i < MAX_CONTACTS; i++) { + contact.next = new Contact(i); + contact = contact.next; + } + collisionDetector = new KdTreeCollisionDetector(); + } + + /** + * Вектор гравитации. + */ + public function get gravity():Vector3 { + return _gravity.vClone(); + } + + /** + * @private + */ + public function set gravity(value:Vector3):void { + _gravity.vCopy(value); + _gravityMagnitude = _gravity.vLength(); + } + + /** + * Добавляет тело в симуляцию. + * + * @param body + */ + public function addBody(body:Body):void { + body.id = lastBodyId++; + body.world = this; + bodies.append(body); + } + + /** + * Удаляет тело из симуляции. + * @param body + * @return + */ + public function removeBody(body:Body):void { + if (bodies.remove(body)) { + body.world = null; + } + } + + /** + * Добавляет ограничение. + * @param c + */ + public function addConstraint(c:Constraint):void { + constraints[constraintsNum++] = c; + c.world = this; + } + + /** + * Удаляет ограничение. + * @param c + */ + public function removeConstraint(c:Constraint):Boolean { + var idx:int = constraints.indexOf(c); + if (idx < 0) return false; + constraints.splice(idx, 1) + constraintsNum--; + c.world = null; + return true; + } + + /** + * Применяет к телам действующте на них силы. Аккумуляторы сил и моментов тел очищаются после завершения шага + * симуляции, поэтому на момент вызова метода могут уже содержать некоторые значения. + * + * @param dt промежуток времени, в течении которого действуют силы + */ + private function applyForces(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + body.beforePhysicsStep(dt); + body.calcAccelerations(); + // Ускорение свободного падения применяется только к подвижным телам во избежание некорректного изменения + // фиктивной скорости неподвижных тел. + if (body.movable && !body.frozen) { + body.accel.x += _gravity.x; + body.accel.y += _gravity.y; + body.accel.z += _gravity.z; + } + item = item.next; + } + } + + /** + * Определяет все столкновения на текущем шаге симуляции и заполняет список получившихся контактов. + * + * @param dt длительность шага симуляции + */ + private function detectCollisions(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + if (!body.frozen) { + body.contactsNum = 0; + body.saveState(); + // При включённом режиме предсказания состояние тел интегрируется на один шаг вперёд + if (usePrediction) { + body.integrateVelocity(dt); + body.integratePosition(dt); + } + body.calcDerivedData(); + } + item = item.next; + } + + borderContact = collisionDetector.getAllContacts(contacts); + + // Расчёт относительных векторов точки контакта вынесен сюда из-за необходимости учитывать + // положение тел в предсказанном состоянии + var contact:Contact = contacts; + while (contact != borderContact) { + var b1:Body = contact.body1; + var b2:Body = contact.body2; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + var bPos:Vector3 = b1.state.pos; + cp.r1.x = cp.pos.x - bPos.x; + cp.r1.y = cp.pos.y - bPos.y; + cp.r1.z = cp.pos.z - bPos.z; + if (b2 != null) { + bPos = b2.state.pos; + cp.r2.x = cp.pos.x - bPos.x; + cp.r2.y = cp.pos.y - bPos.y; + cp.r2.z = cp.pos.z - bPos.z; + } + } + contact = contact.next; + } + + // Восстановление состояния тел + if (usePrediction) { + item = bodies.head; + while (item != null) { + body = item.body; + if (!body.frozen) { + body.restoreState(); + body.calcDerivedData(); + } + item = item.next; + } + } + } + + /** + * Подготваливает полученные из детектора столкновений контакты, расчитывая значения, не меняющиеся + * в ходе шага симуляции. + */ + private function preProcessContacts(dt:Number):void { + var contact:Contact = contacts; + while (contact != borderContact) { + var b1:Body = contact.body1; + var b2:Body = contact.body2; + // Столкнувшиеся тела размораживаются + if (b1.frozen) { + b1.frozen = false; + b1.freezeCounter = 0; + } + if (b2 != null && b2.frozen) { + b2.frozen = false; + b2.freezeCounter = 0; + } + contact.restitution = b1.material.restitution; + if (b2 != null && b2.material.restitution < contact.restitution) contact.restitution = b2.material.restitution; + contact.friction = b1.material.friction; + if (b2 != null && b2.material.friction < contact.friction) contact.friction = b2.material.friction; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.accumImpulseN = 0; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + cp.velByUnitImpulseN = 0; + if (b1.movable) { + cp.angularInertia1 = _v.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1).vDot(contact.normal); + cp.velByUnitImpulseN += b1.invMass + cp.angularInertia1; + } + if (b2 != null && b2.movable) { + cp.angularInertia2 = _v.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2).vDot(contact.normal); + cp.velByUnitImpulseN += b2.invMass + cp.angularInertia2; + } + // Расчёт требуемой конечной скорости для упругого контакта + calcSepVelocity(b1, b2, cp, _v); + cp.normalVel = _v.vDot(contact.normal); + if (cp.normalVel < 0) cp.normalVel = - contact.restitution*cp.normalVel; + // Скорость разделения неупругого контакта + cp.minSepVel = cp.penetration > allowedPenetration ? (cp.penetration - allowedPenetration)/(penResolutionSteps*dt) : 0; + if (cp.minSepVel > maxPenResolutionSpeed) cp.minSepVel = maxPenResolutionSpeed; + } + contact = contact.next; + } + for (var i:int = 0; i < constraintsNum; i++) { + var constraint:Constraint = constraints[i]; + constraint.preProcess(dt); + } + } + + /** + * + * @param dt + * @param forceInelastic + */ + private function processContacts(dt:Number, forceInelastic:Boolean):void { + var iterNum:int = forceInelastic ? contactIterations : collisionIterations; + var i:int; + var forwardLoop:Boolean = false; + for (var iter:int = 0; iter < iterNum; iter++) { + forwardLoop = !forwardLoop; + var contact:Contact = contacts; + while (contact != borderContact) { + resolveContact(contact, forceInelastic, forwardLoop); + contact = contact.next; + } + // Ограничения + for (i = 0; i < constraintsNum; i++) { + var constraint:Constraint = constraints[i]; + constraint.apply(dt); + } + } + + // Разделение контактов путём непосредственного изменения координат и ориентации. Экспериментально. +// if (forceInelastic && useStaticSeparation) performStaticSeparation(); + } + + /** + * + */ + private function resolveContact(contactInfo:Contact, forceInelastic:Boolean, forwardLoop:Boolean):void { + var b1:Body = contactInfo.body1; + var b2:Body = contactInfo.body2; + var normal:Vector3 = contactInfo.normal; + var i:int; + if (forwardLoop) { + for (i = 0; i < contactInfo.pcount; i++) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } else { + for (i = contactInfo.pcount - 1; i >= 0; i--) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } + } + + /** + * + * @param idx + * @param b1 + * @param b2 + * @param colInfo + * @param normal + * @param forceInelastic + */ + private function resolveContactPoint(idx:int, b1:Body, b2:Body, contact:Contact, normal:Vector3, forceInelastic:Boolean):void { + var cp:ContactPoint = contact.points[idx]; + if (!forceInelastic) { + cp.satisfied = true; + } + + var newVel:Number = 0; + calcSepVelocity(b1, b2, cp, _v); + var cnormal:Vector3 = contact.normal; + var sepVel:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + if (forceInelastic) { + var minSpeVel:Number = useStaticSeparation ? 0 : cp.minSepVel; + if (sepVel < minSpeVel) { + cp.satisfied = false; + } else if (cp.satisfied) return; + newVel = minSpeVel; + } else { + newVel = cp.normalVel; + } + var deltaVel:Number = newVel - sepVel; + var impulse:Number = deltaVel/cp.velByUnitImpulseN; + var accumImpulse:Number = cp.accumImpulseN + impulse; + if (accumImpulse < 0) { + accumImpulse = 0; + } + var deltaImpulse:Number = accumImpulse - cp.accumImpulseN; + cp.accumImpulseN = accumImpulse; + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, normal, deltaImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, normal, -deltaImpulse); + } + + // Учёт силы трения + calcSepVelocity(b1, b2, cp, _v); + // Расчитываем изменение касательной скорости на единицу касательного импульса + var tanSpeedByUnitImpulse:Number = 0; + +// _v.vAddScaled(-_v.vDot(contact.normal), contact.normal); + var dot:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + _v.x -= dot*cnormal.x; + _v.y -= dot*cnormal.y; + _v.z -= dot*cnormal.z; + + var tanSpeed:Number = _v.vLength(); + if (tanSpeed < 0.001) return; + +// _t.vCopy(_v).vNormalize().vReverse(); + _t.x = -_v.x; + _t.y = -_v.y; + _t.z = -_v.z; + _t.vNormalize(); + + var r:Vector3; + var m:Matrix3; + var xx:Number; + var yy:Number; + var zz:Number; + // dV = b.invMass + ((invI * (r % t)) % r) * t + if (b1.movable) { +// _v.vCross2(cp.r1, _t).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1); + r = cp.r1; + m = b1.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b1.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + if (b2 != null && b2.movable) { +// _v.vCross2(cp.r2, _t).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2); + + r = cp.r2; + m = b2.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b2.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + + var tanImpulse:Number = tanSpeed/tanSpeedByUnitImpulse; + var max:Number = contact.friction*cp.accumImpulseN; + if (max < 0) { + if (tanImpulse < max) tanImpulse = max; + } else { + if (tanImpulse > max) tanImpulse = max; + } + + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, _t, tanImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, _t, -tanImpulse); + } + } + + /** + * + * @param cp + * @param normal + * @return + */ + private function calcSepVelocity(body1:Body, body2:Body, cp:ContactPoint, result:Vector3):void { + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 +// result.vCopy(body1.state.velocity).vAdd(_v1.vCross2(body1.state.rotation, cp.r1)); + var rot:Vector3 = body1.state.rotation; + var v:Vector3 = cp.r1; + var x:Number = rot.y*v.z - rot.z*v.y; + var y:Number = rot.z*v.x - rot.x*v.z; + var z:Number = rot.x*v.y - rot.y*v.x; + v = body1.state.velocity; + result.x = v.x + x; + result.y = v.y + y; + result.z = v.z + z; + // V2 = V2_c + w2%r2 + if (body2 != null) { +// result.vSubtract(body2.state.velocity).vSubtract(_v2.vCross2(body2.state.rotation, cp.r2)); + rot = body2.state.rotation; + v = cp.r2; + x = rot.y*v.z - rot.z*v.y; + y = rot.z*v.x - rot.x*v.z; + z = rot.x*v.y - rot.y*v.x; + v = body2.state.velocity; + result.x -= v.x + x; + result.y -= v.y + y; + result.z -= v.z + z; + } + } + + /** + * + * @param dt + */ + private function intergateVelocities(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + item.body.integrateVelocity(dt); + item = item.next; + } + } + + /** + * + * @param dt + */ + private function integratePositions(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + if (body.movable && !body.frozen) { + body.integratePosition(dt); + } + item = item.next; + } + } + + /** + * + */ + private function performStaticSeparation():void { + var iterNum:int = staticSeparationIterations; +// iterNum = 100; + + // 1. В начале каждой итерации для всех контактов сбрасывается флаг satisfied с одновременным поиском наихудшего контакта, т.е. имеющего наибольшую величину пересечения тел. + // 2. Если найденный контакт имеет величину пересечения меньше предельно допустимой процедура прерывается, т.к. разделение контактов не требуется. + // 3. Если процедура продолжается, то выполняется разделение найденного контакта и установка его флага satisfied в true, после чего запускается внутренний цикл, состоящий + // из contactsNum - 1 итераций. На каждой итерации ищется наихудший контакт среди оставшихся (satisfied == false). Если такой контакт найден, то выполняется его разделение + // и переход к следующей итерации внутреннего цикла, иначе внутренний цикл прерывается и выполняется переход к следующей итерации внешнего цикла (шаг 1.). +// for (var iter:int = 0; iter < iterNum; iter++) { +// // Ищем контакт с максимальной величиной пересечения тел, одновременно сбрасывая флаги satisfied в false +// var worstContact:Contact = contacts[0]; +// var i:int; +// for (i = 1; i < contactsNum; i++) { +// var contact:Contact = contacts[i]; +// contact.satisfied = false; +// if (contact.maxPenetration > worstContact.maxPenetration) worstContact = contact; +// } +// if (worstContact.maxPenetration <= allowedPenetration) return; +// +// resolveInterpenetration(worstContact); +// // Внутренний цикл по оставшимся контактам +// for (i = 1; i < contactsNum; i++) { +// worstContact = getWorstContact(); +// if (worstContact == null) break; +// resolveInterpenetration(worstContact); +// } +// } + } + + /** + * + */ +// private function getWorstContact():Contact { +// var maxPen:Number = 0; +// var worst:Contact = null; +// for (var i:int = 0; i < contactsNum; i++) { +// var c:Contact = contacts[i]; +// if (!c.satisfied && c.maxPenetration > maxPen) { +// worst = c; +// maxPen = c.maxPenetration; +// } +// } +// return maxPen > allowedPenetration ? worst : null; +// } + + /** + * Разделяет указанный контакт, выполняя staticSeparationSteps итераций по списку точек контакта. + * 1. В начале каждой итерации ищется точка с наибольшим пересечением, одновременно сбрасываются флаги satisfied у точек. + * 2. Для найденной точки выполняется процедура разделения. + * 3. После выполняется pcount - 1 итерация по списку точек, каждый раз ищется наихудшая среди имеющих satisfied == false, для которой выполняется процедура разделения. + * Если наихудшая точка не найдена, то выполнется переход к следующей итерации внешнего цикла (шаг 1). + * + * В конце процедуры разделения обновляются значения пересечений для остальных точек контакта, а также для всех прочих контактов, относящихся к телам текущего. + * + * @param contact контакт для разделения + */ + private function resolveInterpenetration(contact:Contact):void { + contact.satisfied = true; + + for (var step:int = 0; step < staticSeparationSteps; step++) { + var worstCp:ContactPoint = contact.points[0]; + var cp:ContactPoint; + var i:int; + for (i = 1; i < contact.pcount; i++) { + cp = contact.points[i]; + cp.satisfied = false; + if (cp.penetration > worstCp.penetration) worstCp = cp; + } + if (worstCp.penetration <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + // Разделяем оставшиеся точки + var maxPen:Number = 0; + for (i = 1; i < contact.pcount; i++) { + // Поиск наихудшей точки + for (var j:int = 0; j < contact.pcount; j++) { + cp = contact.points[j]; + if (cp.satisfied) continue; + if (cp.penetration > maxPen) { + maxPen = cp.penetration; + worstCp = cp; + } + } + if (maxPen <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + } + } + } + + /** + * + * @param cp + * @param contact + */ + private function separateContactPoint(cp:ContactPoint, contact:Contact):void { +// cp.satisfied = true; +// +// var b1:Body = contact.body1; +// var b2:Body = contact.body2; +// var totalMove:Number = cp.penetration - allowedPenetration; +// var moveCoeff:Number = totalMove/cp.velByUnitImpulseN; +// var linMove1:Number; +// var angleMove1:Number; +// if (b1.movable) { +// linMove1 = b1.invMass*moveCoeff; +// angleMove1 = cp.angularInertia1*moveCoeff; +// if (angleMove1 > maxAngleMove) { +// linMove1 += angleMove1 - maxAngleMove; +// angleMove1 -= maxAngleMove; +// } +// b1.state.pos.vAddScaled(linMove1, contact.normal); +// _v1.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vScale(angleMove1); +// b1.state.orientation.addScaledVector(_v1, 1); +// } +// var linMove2:Number; +// var angleMove2:Number; +// if (b2 != null && b2.movable) { +// linMove2 = b2.invMass*moveCoeff; +// angleMove2 = cp.angularInertia2*moveCoeff; +// if (angleMove2 > maxAngleMove) { +// linMove2 += angleMove2 - maxAngleMove; +// angleMove2 -= maxAngleMove; +// } +// b2.state.pos.vAddScaled(-linMove2, contact.normal); +// _v2.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vScale(angleMove2); +// _v2.vReverse(); +// b2.state.orientation.addScaledVector(_v2, 1); +// } +// cp.penetration = allowedPenetration; +// // Обновляем пересечения в других точках +// var i:int; +// for (i = 0; i < contact.pcount; i++) { +// var cp1:ContactPoint = contact.points[i]; +// if (cp1 == cp) continue; +// var angularMove:Number; +// if (b1.movable) { +// angularMove = _v.vCross2(_v1, cp1.r1).vDot(contact.normal); +// cp1.penetration -= linMove1 + angularMove; +// } +// if (b2 != null && b2.movable) { +// angularMove = _v.vCross2(_v2, cp1.r2).vDot(contact.normal); +// cp1.penetration -= linMove2 - angularMove; +// } +// // Обновление максимального значения для контакта +// if (cp1.penetration > contact.maxPenetration) contact.maxPenetration = cp1.penetration; +// } +// // Обновляем пересечения для других контактов +// var c:Contact; +// var j:int; +// if (b1.movable) { +// for (i = 0; i < b1.contactsNum; i++) { +// c = b1.contacts[i]; +// if (c == contact) continue; +// for (j = 0; j < c.pcount; j++) { +// cp1 = c.points[j]; +// if (b1 == c.body1) cp1.penetration -= linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r1).vDot(c.normal); +// else cp1.penetration += linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r2).vDot(c.normal); +// if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; +// } +// } +// } +// if (b2 != null && b2.movable) { +// for (i = 0; i < b2.contactsNum; i++) { +// c = b2.contacts[i]; +// if (c == contact) continue; +// for (j = 0; j < c.pcount; j++) { +// cp1 = c.points[j]; +// if (b2 == c.body1) cp1.penetration -= linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r1).vDot(c.normal); +// else cp1.penetration += linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r2).vDot(c.normal); +// if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; +// } +// } +// } + } + + /** + * + */ + private function postPhysics():void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + body.clearAccumulators(); + body.calcDerivedData(); + if (body.canFreeze) { + if (body.state.velocity.vLength() < linSpeedFreezeLimit && body.state.rotation.vLength() < angSpeedFreezeLimit) { + if (!body.frozen) { + body.freezeCounter++; + if (body.freezeCounter >= freezeSteps) body.frozen = true; + } + } else { + body.freezeCounter = 0; + body.frozen = false; + } + } + item = item.next; + } + } + + /** + * + * @param delta + */ + public function runPhysics(delta:uint):void { + timeStamp++; + time += delta; + var dt:Number = 0.001*delta; + applyForces(dt); + detectCollisions(dt); + preProcessContacts(dt); + processContacts(dt, false); + intergateVelocities(dt); + processContacts(dt, true); + integratePositions(dt); + postPhysics(); + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/altphysics.as b/0.0.7.1/src/alternativa/physics/altphysics.as new file mode 100644 index 0000000..2715307 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/altphysics.as @@ -0,0 +1,3 @@ +package alternativa.physics { + public namespace altphysics = "http://alternativaplatform.com/en/altphysics"; +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/.svn/all-wcprops b/0.0.7.1/src/alternativa/physics/collision/.svn/all-wcprops new file mode 100644 index 0000000..b02215c --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/.svn/all-wcprops @@ -0,0 +1,65 @@ +K 25 +svn:wc:ra_dav:version-url +V 113 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/collision +END +ICollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 135 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/collision/ICollisionDetector.as +END +CollisionKdNode.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/collision/CollisionKdNode.as +END +CollisionKdTree2D.as +K 25 +svn:wc:ra_dav:version-url +V 134 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/collision/CollisionKdTree2D.as +END +IRayCollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 139 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/collision/IRayCollisionPredicate.as +END +IBodyCollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/collision/IBodyCollisionPredicate.as +END +CollisionPrimitive.as +K 25 +svn:wc:ra_dav:version-url +V 135 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/collision/CollisionPrimitive.as +END +KdTreeCollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/collision/KdTreeCollisionDetector.as +END +ICollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 136 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/collision/ICollisionPredicate.as +END +ICollider.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/collision/ICollider.as +END +CollisionKdTree.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/collision/CollisionKdTree.as +END diff --git a/0.0.7.1/src/alternativa/physics/collision/.svn/entries b/0.0.7.1/src/alternativa/physics/collision/.svn/entries new file mode 100644 index 0000000..097a419 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/.svn/entries @@ -0,0 +1,157 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/collision +http://svndev.alternativaplatform.com + + + +2010-01-15T10:21:05.313154Z +26464 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +colliders +dir + +ICollisionDetector.as +file + + + + +2010-10-28T04:32:42.000000Z +04aac68b82def9fa814b66f90a32dbcb +2009-10-19T07:17:33.113306Z +22253 +mike + +CollisionKdNode.as +file + + + + +2010-10-28T04:32:42.000000Z +1d52aba28ce259ca1f9916d70619263d +2009-07-20T03:48:26.517738Z +16522 +mike + +CollisionKdTree2D.as +file + + + + +2010-10-28T04:32:42.000000Z +d8211949ef77817543bc2882480da12f +2009-07-20T03:48:26.517738Z +16522 +mike + +IRayCollisionPredicate.as +file + + + + +2010-10-28T04:32:42.000000Z +32f9b1eaeacfcb40e7ae1927f52b8ab8 +2009-10-19T07:17:33.113306Z +22253 +mike + +types +dir + +IBodyCollisionPredicate.as +file + + + + +2010-10-28T04:32:42.000000Z +618f907a588979a8d06d4f4bcf6618a0 +2009-10-19T07:17:33.113306Z +22253 +mike + +CollisionPrimitive.as +file + + + + +2010-10-28T04:32:42.000000Z +c969fea7a18b12ac1c98faaf9900bf84 +2009-10-20T10:42:06.632117Z +22312 +mike + +KdTreeCollisionDetector.as +file + + + + +2010-10-28T04:32:42.000000Z +fa4965812a9e0e0de2bce66b0de53d44 +2009-10-20T10:42:06.632117Z +22312 +mike + +ICollisionPredicate.as +file + + + + +2010-10-28T04:32:42.000000Z +65f3250d36556fffc457c740fb7d38b3 +2009-10-19T07:17:33.113306Z +22253 +mike + +ICollider.as +file + + + + +2010-10-28T04:32:42.000000Z +128a7d7479e13caeccecb45161b27b22 +2009-10-19T07:17:33.113306Z +22253 +mike + +primitives +dir + +CollisionKdTree.as +file + + + + +2010-10-28T04:32:42.000000Z +1f0db7a1be451e638b7ec88a10219517 +2009-10-19T07:17:33.113306Z +22253 +mike + diff --git a/0.0.7.1/src/alternativa/physics/collision/.svn/format b/0.0.7.1/src/alternativa/physics/collision/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.7.1/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base b/0.0.7.1/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base new file mode 100644 index 0000000..e24922c --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base @@ -0,0 +1,18 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.types.BoundBox; + + public class CollisionKdNode { + public var indices:Vector.; + public var splitIndices:Vector.; + public var boundBox:BoundBox; + public var parent:CollisionKdNode; + public var splitTree:CollisionKdTree2D; + + public var axis:int = -1; // 0 - x, 1 - y, 2 - z + public var coord:Number; + public var positiveNode:CollisionKdNode; + public var negativeNode:CollisionKdNode; + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base b/0.0.7.1/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base new file mode 100644 index 0000000..50ba94a --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base @@ -0,0 +1,261 @@ +package alternativa.physics.collision { + + import __AS3__.vec.Vector; + + import alternativa.physics.collision.types.BoundBox; + + /** + * @author mike + */ + public class CollisionKdTree { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + public var rootNode:CollisionKdNode; + public var staticChildren:Vector.; + public var numStaticChildren:int; + public var staticBoundBoxes:Vector. = new Vector.(); + + private var splitAxis:int; + private var splitCoord:Number; + private var splitCost:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + + /** + * @param boundBox + */ + public function createTree(collisionPrimitives:Vector., boundBox:BoundBox = null):void { + staticChildren = collisionPrimitives.concat(); + numStaticChildren = staticChildren.length; + // Создаём корневую ноду + rootNode = new CollisionKdNode(); + rootNode.indices = new Vector.(); + // Расчитываем баунды объектов и рутовой ноды + var rootNodeBoundBox:BoundBox = rootNode.boundBox = (boundBox != null) ? boundBox : new BoundBox(); + for (var i:int = 0; i < numStaticChildren; ++i) { + var child:CollisionPrimitive = staticChildren[i]; + var childBoundBox:BoundBox = staticBoundBoxes[i] = child.calculateAABB(); + rootNodeBoundBox.addBoundBox(childBoundBox); + rootNode.indices[i] = i; + } + staticBoundBoxes.length = numStaticChildren; + // Разделяем рутовую ноду + splitNode(rootNode); + + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + var indices:Vector. = node.indices; + var numPrimitives:int = indices.length; + if (numPrimitives <= minPrimitivesPerNode) return; + + // Подготовка баунда с погрешностями + var nodeBoundBox:BoundBox = node.boundBox; + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + // Собираем опорные координаты + var i:int; + var j:int; + var numSplitCoordsX:int = 0 + var numSplitCoordsY:int = 0; + var numSplitCoordsZ:int = 0; + for (i = 0; i < numPrimitives; ++i) { + var boundBox:BoundBox = staticBoundBoxes[indices[i]]; + + if (boundBox.maxX - boundBox.minX <= doubleThreshold) { + if (boundBox.minX <= nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.minX; + else if (boundBox.maxX >= nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.maxX; + else splitCoordsX[numSplitCoordsX++] = (boundBox.minX + boundBox.maxX)*0.5; + } else { + if (boundBox.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = boundBox.minX; + if (boundBox.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = boundBox.maxX; + } + + if (boundBox.maxY - boundBox.minY <= doubleThreshold) { + if (boundBox.minY <= nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.minY; + else if (boundBox.maxY >= nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.maxY; + else splitCoordsY[numSplitCoordsY++] = (boundBox.minY + boundBox.maxY)*0.5; + } else { + if (boundBox.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = boundBox.minY; + if (boundBox.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = boundBox.maxY; + } + + if (boundBox.maxZ - boundBox.minZ <= doubleThreshold) { + if (boundBox.minZ <= nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.minZ; + else if (boundBox.maxZ >= nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.maxZ; + else splitCoordsZ[numSplitCoordsZ++] = (boundBox.minZ + boundBox.maxZ)*0.5; + } else { + if (boundBox.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.minZ; + if (boundBox.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numPrimitives; ++i) { + boundBox = staticBoundBoxes[indices[i]]; + var min:Number = axisX ? boundBox.minX : (axisY ? boundBox.minY : boundBox.minZ); + var max:Number = axisX ? boundBox.maxX : (axisY ? boundBox.maxY : boundBox.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(indices[i]); + indices[i] = -1; + } else { + if (node.splitIndices == null) node.splitIndices = new Vector.(); + node.splitIndices.push(indices[i]); + indices[i] = -1; + } + } else { + if (min >= coordMin) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(indices[i]); + indices[i] = -1; + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numPrimitives; ++i) { + if (indices[i] >= 0) indices[j++] = indices[i]; + } + if (j > 0) indices.length = j; + else node.indices = null; + + if (node.splitIndices != null) { + node.splitTree = new CollisionKdTree2D(this, node); + node.splitTree.createTree(); + } + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + /** + * + */ + public function traceTree():void { + traceNode("", rootNode); + } + + /** + * @param str + * @param node + */ + private function traceNode(str:String, node:CollisionKdNode):void { + if (node == null) return; + trace(str, node.axis == -1 ? "end" : ((node.axis == 0) ? "X" : ((node.axis == 1) ? "Y" : "Z")), "splitCoord=" + splitCoord, "bound", node.boundBox, "objs:", node.indices); + traceNode(str + "-", node.negativeNode); + traceNode(str + "+", node.positiveNode); + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base b/0.0.7.1/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base new file mode 100644 index 0000000..78aaa95 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base @@ -0,0 +1,226 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.types.BoundBox; + + /** + * + */ + public class CollisionKdTree2D { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + + public var parentTree:CollisionKdTree; + public var parentNode:CollisionKdNode; + public var rootNode:CollisionKdNode; + + private var splitAxis:int; + private var splitCost:Number; + private var splitCoord:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + /** + * + * @param parentTree + * @param parentNode + */ + public function CollisionKdTree2D(parentTree:CollisionKdTree, parentNode:CollisionKdNode) { + this.parentTree = parentTree; + this.parentNode = parentNode; + } + + /** + * + */ + public function createTree():void { + rootNode = new CollisionKdNode(); + rootNode.boundBox = parentNode.boundBox.clone(); + rootNode.indices = new Vector.(); + var numObjects:int = parentNode.splitIndices.length; + for (var i:int = 0; i < numObjects; ++i) rootNode.indices[i] = parentNode.splitIndices[i]; + + splitNode(rootNode); + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + if (node.indices.length <= minPrimitivesPerNode) return; + + var objects:Vector. = node.indices; + var i:int; + var j:int; + + var nodeBoundBox:BoundBox = node.boundBox; + + // Подготовка баунда с погрешностями + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + // Собираем опорные координаты + var numSplitCoordsX:int; + var numSplitCoordsY:int; + var numSplitCoordsZ:int; + var numObjects:int = objects.length; + for (i = 0; i < numObjects; ++i) { + var bb:BoundBox = staticBoundBoxes[objects[i]]; + + if (parentNode.axis != 0) { + if (bb.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = bb.minX; + if (bb.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = bb.maxX; + } + + if (parentNode.axis != 1) { + if (bb.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = bb.minY; + if (bb.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = bb.maxY; + } + + if (parentNode.axis != 2) { + if (bb.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = bb.minZ; + if (bb.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = bb.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + if (parentNode.axis != 0) checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + if (parentNode.axis != 1) checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + if (parentNode.axis != 2) checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numObjects; ++i) { + bb = staticBoundBoxes[objects[i]]; + var min:Number = axisX ? bb.minX : (axisY ? bb.minY : bb.minZ); + var max:Number = axisX ? bb.maxX : (axisY ? bb.maxY : bb.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(objects[i]); + objects[i] = -1; + } + } else { + if (min >= coordMin) { + if (max > coordMax) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(objects[i]); + objects[i] = -1; + } + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numObjects; ++i) { + if (objects[i] >= 0) objects[j++] = objects[i]; + } + if (j > 0) objects.length = j; + else node.indices = null; + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/.svn/text-base/CollisionPrimitive.as.svn-base b/0.0.7.1/src/alternativa/physics/collision/.svn/text-base/CollisionPrimitive.as.svn-base new file mode 100644 index 0000000..e42dfb3 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/.svn/text-base/CollisionPrimitive.as.svn-base @@ -0,0 +1,140 @@ +package alternativa.physics.collision { + + import alternativa.physics.Body; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.math.Matrix4; + import alternativa.physics.math.Vector3; + + /** + * Базовый класс для примитивов, использующихся детектором столкновений. + */ + public class CollisionPrimitive { + // Константы типов примитива + public static const BOX:int = 1; + public static const PLANE:int = 2; + public static const SPHERE:int = 3; + public static const RECT:int = 4; + public static const TRIANGLE:int = 5; + + // Тип примитива + public var type:int; + // Группы примитива. Каждая группа определяется установленным битом. Столкновения проверяются только для примитивов, + // имеющих хотя бы одну общую группу. + public var collisionGroup:int; + // Предикат примитива, вызывающийся детектором при нахождении столкновения. В зависимости от возвращённого + // предикатом результата столкновение либо регистрируется, либо игнорируется. + // Nullable + public var postCollisionPredicate:ICollisionPredicate; + // Тело, владеющее примитивом. Поле сделано открытым для быстрого доступа на чтение. Установка значения должна выполняться вызовом метода setBody(). + // Nullable + public var body:Body; + // Трансформация примитива в системе координат тела, если оно указано. Не допускается масштабирование матрицы. + public var localTransform:Matrix4; + // Полная трансформация примитива. Не допускается масштабирование матрицы. + public var transform:Matrix4 = new Matrix4(); + // AABB в мировой системе координат. Расчитывается системой вызовом функции calculateBoundBox(). + public var aabb:BoundBox = new BoundBox(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param type тип примитива + * @param collisionGroup группа примитива + */ + public function CollisionPrimitive(type:int, collisionGroup:int) { + this.type = type; + this.collisionGroup = collisionGroup; + } + + /** + * Устанавливает тело, владеющее примитивом. + * + * @param body тело, которое владеет примитивом + * @param localTransform трансформация примитива в системе координат тела. Указание значения null равносильно + * заданию единичной матрицы, однако в первом случае не будет дополнительного умножения матриц при вычислении полной трансформации примитива. + */ + public function setBody(body:Body, localTransform:Matrix4 = null):void { + if (this.body == body) return; + this.body = body; + if (body != null) { + if (localTransform != null) { + if (this.localTransform == null) { + this.localTransform = new Matrix4(); + } + this.localTransform.copy(localTransform); + } else { + this.localTransform = null; + } + } + } + + /** + * Рассчитывает AABB примитива. Наследники должны переопределять этот метод, реализуя в нём корректный рассчёт. + * + * @return ссылка на свой AABB + */ + public function calculateAABB():BoundBox { + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + return -1; + } + + /** + * Клонирует примитив. Переопределять не рекомендуется. Вместо этого переопределяются методы createPrimitive() и copyFrom(). + * + * @return клон примитива + */ + public function clone():CollisionPrimitive { + var p:CollisionPrimitive = createPrimitive(); + return p.copyFrom(this); + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + if (source == null) { + throw new ArgumentError("Parameter source cannot be null"); + } + type = source.type; + transform.copy(source.transform); + collisionGroup = source.collisionGroup; + setBody(source.body, source.localTransform); + aabb.copyFrom(source.aabb); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + public function toString():String { + return "[CollisionPrimitive type=" + type + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + protected function createPrimitive():CollisionPrimitive { + return new CollisionPrimitive(type, collisionGroup); + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base b/0.0.7.1/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base new file mode 100644 index 0000000..4ace681 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.Body; + + public interface IBodyCollisionPredicate { + function considerBodies(body1:Body, body2:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base b/0.0.7.1/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base new file mode 100644 index 0000000..e6aac2a --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base @@ -0,0 +1,29 @@ +package alternativa.physics.collision { + + import alternativa.physics.Contact; + + /** + * Интерфейс определителя столкновений между двумя примитивами. + */ + public interface ICollider { + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base b/0.0.7.1/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base new file mode 100644 index 0000000..25a71d9 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base @@ -0,0 +1,52 @@ +package alternativa.physics.collision { + import alternativa.physics.Contact; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.math.Vector3; + + /** + * Интерфейс детектора столкновений. + */ + public interface ICollisionDetector { + + /** + * Получает все столкновения в текущей конфигурации физической геометрии. + * + * @param contacts список контактов, в кторые будет записана информация о столкновении + * @return количество найденных столкновений + */ + function getAllContacts(contacts:Contact):Contact; + + /** + * Тестирует луч на пересечение с физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param direction направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRay(origin:Vector3, direction:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @return + */ + function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base b/0.0.7.1/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base new file mode 100644 index 0000000..33d30ca --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base @@ -0,0 +1,8 @@ +package alternativa.physics.collision { + + public interface ICollisionPredicate { + + function considerCollision(primitive:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base b/0.0.7.1/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base new file mode 100644 index 0000000..b319f62 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.Body; + + public interface IRayCollisionPredicate { + function considerBody(body:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base b/0.0.7.1/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base new file mode 100644 index 0000000..14202ec --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base @@ -0,0 +1,486 @@ +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.; + 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.(); + + 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.):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. = tree.staticChildren; +// var indices:Vector. = 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., indices:Vector., 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; + } \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/CollisionKdNode.as b/0.0.7.1/src/alternativa/physics/collision/CollisionKdNode.as new file mode 100644 index 0000000..e24922c --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/CollisionKdNode.as @@ -0,0 +1,18 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.types.BoundBox; + + public class CollisionKdNode { + public var indices:Vector.; + public var splitIndices:Vector.; + public var boundBox:BoundBox; + public var parent:CollisionKdNode; + public var splitTree:CollisionKdTree2D; + + public var axis:int = -1; // 0 - x, 1 - y, 2 - z + public var coord:Number; + public var positiveNode:CollisionKdNode; + public var negativeNode:CollisionKdNode; + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/CollisionKdTree.as b/0.0.7.1/src/alternativa/physics/collision/CollisionKdTree.as new file mode 100644 index 0000000..50ba94a --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/CollisionKdTree.as @@ -0,0 +1,261 @@ +package alternativa.physics.collision { + + import __AS3__.vec.Vector; + + import alternativa.physics.collision.types.BoundBox; + + /** + * @author mike + */ + public class CollisionKdTree { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + public var rootNode:CollisionKdNode; + public var staticChildren:Vector.; + public var numStaticChildren:int; + public var staticBoundBoxes:Vector. = new Vector.(); + + private var splitAxis:int; + private var splitCoord:Number; + private var splitCost:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + + /** + * @param boundBox + */ + public function createTree(collisionPrimitives:Vector., boundBox:BoundBox = null):void { + staticChildren = collisionPrimitives.concat(); + numStaticChildren = staticChildren.length; + // Создаём корневую ноду + rootNode = new CollisionKdNode(); + rootNode.indices = new Vector.(); + // Расчитываем баунды объектов и рутовой ноды + var rootNodeBoundBox:BoundBox = rootNode.boundBox = (boundBox != null) ? boundBox : new BoundBox(); + for (var i:int = 0; i < numStaticChildren; ++i) { + var child:CollisionPrimitive = staticChildren[i]; + var childBoundBox:BoundBox = staticBoundBoxes[i] = child.calculateAABB(); + rootNodeBoundBox.addBoundBox(childBoundBox); + rootNode.indices[i] = i; + } + staticBoundBoxes.length = numStaticChildren; + // Разделяем рутовую ноду + splitNode(rootNode); + + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + var indices:Vector. = node.indices; + var numPrimitives:int = indices.length; + if (numPrimitives <= minPrimitivesPerNode) return; + + // Подготовка баунда с погрешностями + var nodeBoundBox:BoundBox = node.boundBox; + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + // Собираем опорные координаты + var i:int; + var j:int; + var numSplitCoordsX:int = 0 + var numSplitCoordsY:int = 0; + var numSplitCoordsZ:int = 0; + for (i = 0; i < numPrimitives; ++i) { + var boundBox:BoundBox = staticBoundBoxes[indices[i]]; + + if (boundBox.maxX - boundBox.minX <= doubleThreshold) { + if (boundBox.minX <= nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.minX; + else if (boundBox.maxX >= nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.maxX; + else splitCoordsX[numSplitCoordsX++] = (boundBox.minX + boundBox.maxX)*0.5; + } else { + if (boundBox.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = boundBox.minX; + if (boundBox.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = boundBox.maxX; + } + + if (boundBox.maxY - boundBox.minY <= doubleThreshold) { + if (boundBox.minY <= nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.minY; + else if (boundBox.maxY >= nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.maxY; + else splitCoordsY[numSplitCoordsY++] = (boundBox.minY + boundBox.maxY)*0.5; + } else { + if (boundBox.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = boundBox.minY; + if (boundBox.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = boundBox.maxY; + } + + if (boundBox.maxZ - boundBox.minZ <= doubleThreshold) { + if (boundBox.minZ <= nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.minZ; + else if (boundBox.maxZ >= nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.maxZ; + else splitCoordsZ[numSplitCoordsZ++] = (boundBox.minZ + boundBox.maxZ)*0.5; + } else { + if (boundBox.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.minZ; + if (boundBox.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numPrimitives; ++i) { + boundBox = staticBoundBoxes[indices[i]]; + var min:Number = axisX ? boundBox.minX : (axisY ? boundBox.minY : boundBox.minZ); + var max:Number = axisX ? boundBox.maxX : (axisY ? boundBox.maxY : boundBox.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(indices[i]); + indices[i] = -1; + } else { + if (node.splitIndices == null) node.splitIndices = new Vector.(); + node.splitIndices.push(indices[i]); + indices[i] = -1; + } + } else { + if (min >= coordMin) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(indices[i]); + indices[i] = -1; + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numPrimitives; ++i) { + if (indices[i] >= 0) indices[j++] = indices[i]; + } + if (j > 0) indices.length = j; + else node.indices = null; + + if (node.splitIndices != null) { + node.splitTree = new CollisionKdTree2D(this, node); + node.splitTree.createTree(); + } + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + /** + * + */ + public function traceTree():void { + traceNode("", rootNode); + } + + /** + * @param str + * @param node + */ + private function traceNode(str:String, node:CollisionKdNode):void { + if (node == null) return; + trace(str, node.axis == -1 ? "end" : ((node.axis == 0) ? "X" : ((node.axis == 1) ? "Y" : "Z")), "splitCoord=" + splitCoord, "bound", node.boundBox, "objs:", node.indices); + traceNode(str + "-", node.negativeNode); + traceNode(str + "+", node.positiveNode); + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/CollisionKdTree2D.as b/0.0.7.1/src/alternativa/physics/collision/CollisionKdTree2D.as new file mode 100644 index 0000000..78aaa95 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/CollisionKdTree2D.as @@ -0,0 +1,226 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.types.BoundBox; + + /** + * + */ + public class CollisionKdTree2D { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + + public var parentTree:CollisionKdTree; + public var parentNode:CollisionKdNode; + public var rootNode:CollisionKdNode; + + private var splitAxis:int; + private var splitCost:Number; + private var splitCoord:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + /** + * + * @param parentTree + * @param parentNode + */ + public function CollisionKdTree2D(parentTree:CollisionKdTree, parentNode:CollisionKdNode) { + this.parentTree = parentTree; + this.parentNode = parentNode; + } + + /** + * + */ + public function createTree():void { + rootNode = new CollisionKdNode(); + rootNode.boundBox = parentNode.boundBox.clone(); + rootNode.indices = new Vector.(); + var numObjects:int = parentNode.splitIndices.length; + for (var i:int = 0; i < numObjects; ++i) rootNode.indices[i] = parentNode.splitIndices[i]; + + splitNode(rootNode); + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + if (node.indices.length <= minPrimitivesPerNode) return; + + var objects:Vector. = node.indices; + var i:int; + var j:int; + + var nodeBoundBox:BoundBox = node.boundBox; + + // Подготовка баунда с погрешностями + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + // Собираем опорные координаты + var numSplitCoordsX:int; + var numSplitCoordsY:int; + var numSplitCoordsZ:int; + var numObjects:int = objects.length; + for (i = 0; i < numObjects; ++i) { + var bb:BoundBox = staticBoundBoxes[objects[i]]; + + if (parentNode.axis != 0) { + if (bb.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = bb.minX; + if (bb.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = bb.maxX; + } + + if (parentNode.axis != 1) { + if (bb.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = bb.minY; + if (bb.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = bb.maxY; + } + + if (parentNode.axis != 2) { + if (bb.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = bb.minZ; + if (bb.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = bb.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + if (parentNode.axis != 0) checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + if (parentNode.axis != 1) checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + if (parentNode.axis != 2) checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numObjects; ++i) { + bb = staticBoundBoxes[objects[i]]; + var min:Number = axisX ? bb.minX : (axisY ? bb.minY : bb.minZ); + var max:Number = axisX ? bb.maxX : (axisY ? bb.maxY : bb.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(objects[i]); + objects[i] = -1; + } + } else { + if (min >= coordMin) { + if (max > coordMax) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(objects[i]); + objects[i] = -1; + } + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numObjects; ++i) { + if (objects[i] >= 0) objects[j++] = objects[i]; + } + if (j > 0) objects.length = j; + else node.indices = null; + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/CollisionPrimitive.as b/0.0.7.1/src/alternativa/physics/collision/CollisionPrimitive.as new file mode 100644 index 0000000..e42dfb3 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/CollisionPrimitive.as @@ -0,0 +1,140 @@ +package alternativa.physics.collision { + + import alternativa.physics.Body; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.math.Matrix4; + import alternativa.physics.math.Vector3; + + /** + * Базовый класс для примитивов, использующихся детектором столкновений. + */ + public class CollisionPrimitive { + // Константы типов примитива + public static const BOX:int = 1; + public static const PLANE:int = 2; + public static const SPHERE:int = 3; + public static const RECT:int = 4; + public static const TRIANGLE:int = 5; + + // Тип примитива + public var type:int; + // Группы примитива. Каждая группа определяется установленным битом. Столкновения проверяются только для примитивов, + // имеющих хотя бы одну общую группу. + public var collisionGroup:int; + // Предикат примитива, вызывающийся детектором при нахождении столкновения. В зависимости от возвращённого + // предикатом результата столкновение либо регистрируется, либо игнорируется. + // Nullable + public var postCollisionPredicate:ICollisionPredicate; + // Тело, владеющее примитивом. Поле сделано открытым для быстрого доступа на чтение. Установка значения должна выполняться вызовом метода setBody(). + // Nullable + public var body:Body; + // Трансформация примитива в системе координат тела, если оно указано. Не допускается масштабирование матрицы. + public var localTransform:Matrix4; + // Полная трансформация примитива. Не допускается масштабирование матрицы. + public var transform:Matrix4 = new Matrix4(); + // AABB в мировой системе координат. Расчитывается системой вызовом функции calculateBoundBox(). + public var aabb:BoundBox = new BoundBox(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param type тип примитива + * @param collisionGroup группа примитива + */ + public function CollisionPrimitive(type:int, collisionGroup:int) { + this.type = type; + this.collisionGroup = collisionGroup; + } + + /** + * Устанавливает тело, владеющее примитивом. + * + * @param body тело, которое владеет примитивом + * @param localTransform трансформация примитива в системе координат тела. Указание значения null равносильно + * заданию единичной матрицы, однако в первом случае не будет дополнительного умножения матриц при вычислении полной трансформации примитива. + */ + public function setBody(body:Body, localTransform:Matrix4 = null):void { + if (this.body == body) return; + this.body = body; + if (body != null) { + if (localTransform != null) { + if (this.localTransform == null) { + this.localTransform = new Matrix4(); + } + this.localTransform.copy(localTransform); + } else { + this.localTransform = null; + } + } + } + + /** + * Рассчитывает AABB примитива. Наследники должны переопределять этот метод, реализуя в нём корректный рассчёт. + * + * @return ссылка на свой AABB + */ + public function calculateAABB():BoundBox { + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + return -1; + } + + /** + * Клонирует примитив. Переопределять не рекомендуется. Вместо этого переопределяются методы createPrimitive() и copyFrom(). + * + * @return клон примитива + */ + public function clone():CollisionPrimitive { + var p:CollisionPrimitive = createPrimitive(); + return p.copyFrom(this); + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + if (source == null) { + throw new ArgumentError("Parameter source cannot be null"); + } + type = source.type; + transform.copy(source.transform); + collisionGroup = source.collisionGroup; + setBody(source.body, source.localTransform); + aabb.copyFrom(source.aabb); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + public function toString():String { + return "[CollisionPrimitive type=" + type + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + protected function createPrimitive():CollisionPrimitive { + return new CollisionPrimitive(type, collisionGroup); + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/IBodyCollisionPredicate.as b/0.0.7.1/src/alternativa/physics/collision/IBodyCollisionPredicate.as new file mode 100644 index 0000000..4ace681 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/IBodyCollisionPredicate.as @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.Body; + + public interface IBodyCollisionPredicate { + function considerBodies(body1:Body, body2:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/ICollider.as b/0.0.7.1/src/alternativa/physics/collision/ICollider.as new file mode 100644 index 0000000..e6aac2a --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/ICollider.as @@ -0,0 +1,29 @@ +package alternativa.physics.collision { + + import alternativa.physics.Contact; + + /** + * Интерфейс определителя столкновений между двумя примитивами. + */ + public interface ICollider { + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/ICollisionDetector.as b/0.0.7.1/src/alternativa/physics/collision/ICollisionDetector.as new file mode 100644 index 0000000..25a71d9 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/ICollisionDetector.as @@ -0,0 +1,52 @@ +package alternativa.physics.collision { + import alternativa.physics.Contact; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.math.Vector3; + + /** + * Интерфейс детектора столкновений. + */ + public interface ICollisionDetector { + + /** + * Получает все столкновения в текущей конфигурации физической геометрии. + * + * @param contacts список контактов, в кторые будет записана информация о столкновении + * @return количество найденных столкновений + */ + function getAllContacts(contacts:Contact):Contact; + + /** + * Тестирует луч на пересечение с физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param direction направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRay(origin:Vector3, direction:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @return + */ + function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/ICollisionPredicate.as b/0.0.7.1/src/alternativa/physics/collision/ICollisionPredicate.as new file mode 100644 index 0000000..33d30ca --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/ICollisionPredicate.as @@ -0,0 +1,8 @@ +package alternativa.physics.collision { + + public interface ICollisionPredicate { + + function considerCollision(primitive:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/IRayCollisionPredicate.as b/0.0.7.1/src/alternativa/physics/collision/IRayCollisionPredicate.as new file mode 100644 index 0000000..b319f62 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/IRayCollisionPredicate.as @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.Body; + + public interface IRayCollisionPredicate { + function considerBody(body:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/KdTreeCollisionDetector.as b/0.0.7.1/src/alternativa/physics/collision/KdTreeCollisionDetector.as new file mode 100644 index 0000000..14202ec --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/KdTreeCollisionDetector.as @@ -0,0 +1,486 @@ +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.; + 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.(); + + 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.):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. = tree.staticChildren; +// var indices:Vector. = 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., indices:Vector., 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; + } \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/colliders/.svn/all-wcprops b/0.0.7.1/src/alternativa/physics/collision/colliders/.svn/all-wcprops new file mode 100644 index 0000000..9229dc0 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/colliders/.svn/all-wcprops @@ -0,0 +1,53 @@ +K 25 +svn:wc:ra_dav:version-url +V 123 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/collision/colliders +END +BoxSphereCollider.as +K 25 +svn:wc:ra_dav:version-url +V 144 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/collision/colliders/BoxSphereCollider.as +END +BoxBoxCollider.as +K 25 +svn:wc:ra_dav:version-url +V 141 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/collision/colliders/BoxBoxCollider.as +END +BoxRectCollider.as +K 25 +svn:wc:ra_dav:version-url +V 142 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/collision/colliders/BoxRectCollider.as +END +SpherePlaneCollider.as +K 25 +svn:wc:ra_dav:version-url +V 146 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/collision/colliders/SpherePlaneCollider.as +END +BoxCollider.as +K 25 +svn:wc:ra_dav:version-url +V 138 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/collision/colliders/BoxCollider.as +END +BoxPlaneCollider.as +K 25 +svn:wc:ra_dav:version-url +V 143 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/collision/colliders/BoxPlaneCollider.as +END +SphereSphereCollider.as +K 25 +svn:wc:ra_dav:version-url +V 147 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/collision/colliders/SphereSphereCollider.as +END +BoxTriangleCollider.as +K 25 +svn:wc:ra_dav:version-url +V 146 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/collision/colliders/BoxTriangleCollider.as +END diff --git a/0.0.7.1/src/alternativa/physics/collision/colliders/.svn/entries b/0.0.7.1/src/alternativa/physics/collision/colliders/.svn/entries new file mode 100644 index 0000000..85aa6d3 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/colliders/.svn/entries @@ -0,0 +1,124 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/collision/colliders +http://svndev.alternativaplatform.com + + + +2010-01-15T10:21:05.313154Z +26464 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +BoxSphereCollider.as +file + + + + +2010-10-28T04:32:42.000000Z +b2478004208a1d598d9f57747d83ccbb +2009-10-19T07:17:33.113306Z +22253 +mike + +BoxBoxCollider.as +file + + + + +2010-10-28T04:32:42.000000Z +543790ceff5b23a0b72bde58c111183d +2009-10-23T05:59:29.567666Z +22444 +mike + +BoxRectCollider.as +file + + + + +2010-10-28T04:32:42.000000Z +651c19524d0018d0236bc9b47f80d524 +2010-01-15T10:21:05.313154Z +26464 +mike + +SpherePlaneCollider.as +file + + + + +2010-10-28T04:32:42.000000Z +e7f53348558f035e2f34a9e1fe1e1e4b +2009-10-19T07:17:33.113306Z +22253 +mike + +BoxCollider.as +file + + + + +2010-10-28T04:32:42.000000Z +5ddcd853975ee9d6a0c776a14f674e0e +2009-10-19T07:17:33.113306Z +22253 +mike + +BoxPlaneCollider.as +file + + + + +2010-10-28T04:32:42.000000Z +71648e823c289cb67b79e7c8af3045d2 +2009-10-19T07:17:33.113306Z +22253 +mike + +SphereSphereCollider.as +file + + + + +2010-10-28T04:32:42.000000Z +b336de4614439b93424863b1ace66c9b +2009-10-19T07:17:33.113306Z +22253 +mike + +BoxTriangleCollider.as +file + + + + +2010-10-28T04:32:42.000000Z +eee22303d4d1e236d4743b6ccb8103f6 +2010-01-15T10:21:05.313154Z +26464 +mike + diff --git a/0.0.7.1/src/alternativa/physics/collision/colliders/.svn/format b/0.0.7.1/src/alternativa/physics/collision/colliders/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/colliders/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.7.1/src/alternativa/physics/collision/colliders/.svn/text-base/BoxBoxCollider.as.svn-base b/0.0.7.1/src/alternativa/physics/collision/colliders/.svn/text-base/BoxBoxCollider.as.svn-base new file mode 100644 index 0000000..b003e98 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/colliders/.svn/text-base/BoxBoxCollider.as.svn-base @@ -0,0 +1,589 @@ +package alternativa.physics.collision.colliders { + import __AS3__.vec.Vector; + + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.math.Matrix4; + import alternativa.physics.math.Vector3; + + use namespace altphysics; + + /** + * Расчитывает точки контакта двух боксов. Нормаль контакта направляется в сторону бокса с меньшим ID. + */ + public class BoxBoxCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + private var vectorToBox1:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var bestAxisIndex:int; + private var minOverlap:Number; + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + private var tmpPoints:Vector. = new Vector.(8, true); + private var pcount:int; + + /** + * + */ + public function BoxBoxCollider() { + for (var i:int = 0; i < 8; i++) { + tmpPoints[i] = new ContactPoint(); + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + + if (bestAxisIndex < 6) { + // Контакт грань-(грань|ребро|вершина) + if (!findFaceContactPoints(box1, box2, vectorToBox1, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 6; + findEdgesIntersection(box1, box2, vectorToBox1, int(bestAxisIndex/3), bestAxisIndex%3, contact); + } + contact.body1 = box1.body; + contact.body2 = box2.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + // Вектор из центра второго бокса в центр первого + vectorToBox1.x = transform1.d - transform2.d; + vectorToBox1.y = transform1.h - transform2.h; + vectorToBox1.z = transform1.l - transform2.l; + + // Проверка пересечения по основным осям + axis10.x = transform1.a; + axis10.y = transform1.e; + axis10.z = transform1.i; + if (!testMainAxis(box1, box2, axis10, 0, vectorToBox1)) return false; + axis11.x = transform1.b; + axis11.y = transform1.f; + axis11.z = transform1.j; + if (!testMainAxis(box1, box2, axis11, 1, vectorToBox1)) return false; + axis12.x = transform1.c; + axis12.y = transform1.g; + axis12.z = transform1.k; + if (!testMainAxis(box1, box2, axis12, 2, vectorToBox1)) return false; + + axis20.x = transform2.a; + axis20.y = transform2.e; + axis20.z = transform2.i; + if (!testMainAxis(box1, box2, axis20, 3, vectorToBox1)) return false; + axis21.x = transform2.b; + axis21.y = transform2.f; + axis21.z = transform2.j; + if (!testMainAxis(box1, box2, axis21, 4, vectorToBox1)) return false; + axis22.x = transform2.c; + axis22.y = transform2.g; + axis22.z = transform2.k; + if (!testMainAxis(box1, box2, axis22, 5, vectorToBox1)) return false; + + // Проверка производных осей + if (!testDerivedAxis(box1, box2, axis10, axis20, 6, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis21, 7, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis22, 8, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis11, axis20, 9, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis21, 10, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis22, 11, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis12, axis20, 12, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis21, 13, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis22, 14, vectorToBox1)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта грани одного бокса с гранью/ребром/вершиной другого. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param faceAxisIdx индекс оси первого бокса, перпендикулярной грани, с которой произошло столкновение + * @param contactInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var swapNormal:Boolean = false; + if (faceAxisIdx > 2) { + // Столкновение с гранью второго бокса. Для дальнейших расчётов боксы меняются местами, + // но нормаль контакта всё равно должна быть направлена в сторону первоначального box1 + var tmpBox:CollisionBox = box1; + box1 = box2; + box2 = tmpBox; + vectorToBox1.x = -vectorToBox1.x; + vectorToBox1.y = -vectorToBox1.y; + vectorToBox1.z = -vectorToBox1.z; + faceAxisIdx -= 3; + swapNormal = true; + } + + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + var colAxis:Vector3 = contact.normal; + + transform1.getAxis(faceAxisIdx, colAxis); + var negativeFace:Boolean = colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z > 0; + + var code:int = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box1.excludedFaces) != 0) return false; + + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + // Ищем ось второго бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + transform2.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisDot = dot; + incidentAxisIdx = axisIdx; + } + } + // Получаем список вершин грани второго бокса, переводим их в систему координат первого бокса и выполняем обрезку + // по грани первого бокса. Таким образом получается список потенциальных точек контакта. + transform2.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box2.hs, incidentAxisIdx, incidentAxisDot < 0, points1); + + // TODO: Вычислить результирующую матрицу, затем преобразовать векторы за один заход + transform2.transformVectorsN(points1, points2, 4); + transform1.transformVectorsInverseN(points2, points1, 4); + + var pnum:int = clip(box1.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points1[i]; + if ((pen = getPointBoxPenetration(box1.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + var cp:ContactPoint = tmpPoints[pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = transform1.a*v.x + transform1.b*v.y + transform1.c*v.z + transform1.d; + cpPos.y = transform1.e*v.x + transform1.f*v.y + transform1.g*v.z + transform1.h; + cpPos.z = transform1.i*v.x + transform1.j*v.y + transform1.k*v.z + transform1.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + + cp.penetration = pen; + } + } + if (swapNormal) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (pcount > 4) { + reducePoints(); + } + for (i = 0; i < pcount; i++) { + cp = contact.points[i]; + cp.copyFrom(tmpPoints[i]); + } + contact.pcount = pcount; + return true; + } + + /** + * + * @param contactInfo + */ + private function reducePoints():void { + var i:int; + var minIdx:int; + var cp1:ContactPoint; + var cp2:ContactPoint; + while (pcount > 4) { + var minLen:Number = 1e10; + var p1:Vector3 = (tmpPoints[int(pcount - 1)] as ContactPoint).pos; + var p2:Vector3; + for (i = 0; i < pcount; i++) { + p2 = (tmpPoints[i] as ContactPoint).pos; + var dx:Number = p2.x - p1.x; + var dy:Number = p2.y - p1.y; + var dz:Number = p2.z - p1.z; + var len:Number = dx*dx + dy*dy + dz*dz; + if (len < minLen) { + minLen = len; + minIdx = i; + } + p1 = p2; + } + var ii:int = minIdx == 0 ? (pcount - 1) : (minIdx - 1); + cp1 = tmpPoints[ii]; + cp2 = tmpPoints[minIdx]; + p1 = cp1.pos; + p2 = cp2.pos; + p2.x = 0.5*(p1.x + p2.x); + p2.y = 0.5*(p1.y + p2.y); + p2.z = 0.5*(p1.z + p2.z); + cp2.penetration = 0.5*(cp1.penetration + cp2.penetration); + if (minIdx > 0) { + for (i = minIdx; i < pcount; i++) { + tmpPoints[int(i - 1)] = tmpPoints[i]; + } + tmpPoints[int(pcount - 1)] = cp1; + } + pcount--; + } + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clip(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param contactInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):void { + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + transform1.getAxis(axisIdx1, axis10); + transform2.getAxis(axisIdx2, axis20); + + var colAxis:Vector3 = contact.normal; + + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону первого бокса + if (colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + + var tx:Number = box1.hs.x; + var ty:Number = box1.hs.y; + var tz:Number = box1.hs.z; + + var x2:Number = box2.hs.x; + var y2:Number = box2.hs.y; + var z2:Number = box2.hs.z; + // x + if (axisIdx1 == 0) { + tx = 0; + halfLen1 = box1.hs.x; + } else { + if (colAxis.x*transform1.a + colAxis.y*transform1.e + colAxis.z*transform1.i > 0) { + tx = -tx; + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = box2.hs.x; + } else { + if (colAxis.x*transform2.a + colAxis.y*transform2.e + colAxis.z*transform2.i < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + ty = 0; + halfLen1 = box1.hs.y; + } else { + if (colAxis.x*transform1.b + colAxis.y*transform1.f + colAxis.z*transform1.j > 0) { + ty = -ty; + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = box2.hs.y; + } else { + if (colAxis.x*transform2.b + colAxis.y*transform2.f + colAxis.z*transform2.j < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + tz = 0; + halfLen1 = box1.hs.z; + } else { + if (colAxis.x*transform1.c + colAxis.y*transform1.g + colAxis.z*transform1.k > 0) { + tz = -tz; + } + } + if (axisIdx2 == 2) { + z2 = 0; + halfLen2 = box2.hs.z; + } else { + if (colAxis.x*transform2.c + colAxis.y*transform2.g + colAxis.z*transform2.k < 0) { + z2 = -z2; + } + } + // Получаем глобальные координаты средних точек рёбер + var x1:Number = transform1.a*tx + transform1.b*ty + transform1.c*tz + transform1.d; + var y1:Number = transform1.e*tx + transform1.f*ty + transform1.g*tz + transform1.h; + var z1:Number = transform1.i*tx + transform1.j*ty + transform1.k*tz + transform1.l; + tx = x2; + ty = y2; + tz = z2; + x2 = transform2.a*tx + transform2.b*ty + transform2.c*tz + transform2.d; + y2 = transform2.e*tx + transform2.f*ty + transform2.g*tz + transform2.h; + z2 = transform2.i*tx + transform2.j*ty + transform2.k*tz + transform2.l; + + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + tx = x2 - x1; + ty = y2 - y1; + tz = z2 - z1; + var c1:Number = axis10.x*tx + axis10.y*ty + axis10.z*tz; + var c2:Number = axis20.x*tx + axis20.y*ty + axis20.z*tz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cp.pos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cp.pos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение боксов вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box1 + * @param box2 + * @param axis + * @param axisIndex + * @param toBox1 + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box1 + * @param box2 + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox1 + * @return + */ + private function testDerivedAxis(box1:CollisionBox, box2:CollisionBox, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия двух боксов вдоль заданной оси. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param axis ось + * @param vectorToBox1 вектор, соединяющий центр второго бокса с центром первого + * @return величина перекрытия боксов вдоль оси + */ + public function overlapOnAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, vectorToBox1:Vector3):Number { + var m:Matrix4 = box1.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box1.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box1.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box1.hs.z; + if (d < 0) d = -d; + projection += d; + + m = box2.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box2.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box2.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box2.hs.z; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox1.x*axis.x + vectorToBox1.y*axis.y + vectorToBox1.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} + import alternativa.physics.math.Vector3; + + class CollisionPointTmp { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + } diff --git a/0.0.7.1/src/alternativa/physics/collision/colliders/.svn/text-base/BoxCollider.as.svn-base b/0.0.7.1/src/alternativa/physics/collision/colliders/.svn/text-base/BoxCollider.as.svn-base new file mode 100644 index 0000000..0021854 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/colliders/.svn/text-base/BoxCollider.as.svn-base @@ -0,0 +1,427 @@ +package alternativa.physics.collision.colliders { + import __AS3__.vec.Vector; + + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.math.Vector3; + + /** + * + */ + public class BoxCollider implements ICollider { + + /** + * + */ + public function BoxCollider() { + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + return false; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + protected function getFaceVertsByAxis(hs:Vector3, axisIdx:int, negativeFace:Boolean, result:Vector.):void { + var v:Vector3; + switch (axisIdx) { + case 0: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + } + break; + case 1: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + } + break; + case 2: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + } else { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } + break; + } + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/colliders/.svn/text-base/BoxPlaneCollider.as.svn-base b/0.0.7.1/src/alternativa/physics/collision/colliders/.svn/text-base/BoxPlaneCollider.as.svn-base new file mode 100644 index 0000000..bb1ed0f --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/colliders/.svn/text-base/BoxPlaneCollider.as.svn-base @@ -0,0 +1,88 @@ +package alternativa.physics.collision.colliders { + import __AS3__.vec.Vector; + + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.math.Vector3; + + public class BoxPlaneCollider implements ICollider { + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var normal:Vector3 = new Vector3(); + + public function BoxPlaneCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var box:RigidBox = body1 as RigidBox; +// var plane:RigidPlane; +// if (box == null) { +// box = body2 as RigidBox; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные координаты вершин бокса +// var sx:Number = box.halfSize.x; +// var sy:Number = box.halfSize.y; +// var sz:Number = box.halfSize.z; +// (verts1[0] as Vector3).reset(-sx, -sy, -sz); +// (verts1[1] as Vector3).reset(sx, -sy, -sz); +// (verts1[2] as Vector3).reset(sx, sy, -sz); +// (verts1[3] as Vector3).reset(-sx, sy, -sz); +// (verts1[4] as Vector3).reset(-sx, -sy, sz); +// (verts1[5] as Vector3).reset(sx, -sy, sz); +// (verts1[6] as Vector3).reset(sx, sy, sz); +// (verts1[7] as Vector3).reset(-sx, sy, sz); +// +// box.transform.transformVectors(verts1, verts2); +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// // Проверяем наличие столкновений с каждой вершиной +// collisionInfo.pcount = 0; +// for (var i:int = 0; i < 8; i++) { +// // Вершина добавляется в список точек столкновения, если лежит под плоскостью +// var dist:Number = (verts2[i] as Vector3).dot(normal); +// if (dist < offset) { +// var cp:ContactPoint; +// if (collisionInfo.pcount == collisionInfo.points.length) { +// cp = new ContactPoint(); +// collisionInfo.points[collisionInfo.pcount] = cp; +// } else { +// cp = collisionInfo.points[collisionInfo.pcount]; +// } +// cp.pos.copy(verts2[i]); +// cp.r1.diff(cp.pos, box.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); +// cp.penetration = offset - dist; +// collisionInfo.pcount++; +// } +// } +// if (collisionInfo.pcount > 0) { +// collisionInfo.body1 = box; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// return true; +// } + return false; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/colliders/.svn/text-base/BoxRectCollider.as.svn-base b/0.0.7.1/src/alternativa/physics/collision/colliders/.svn/text-base/BoxRectCollider.as.svn-base new file mode 100644 index 0000000..7bf8ef8 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/colliders/.svn/text-base/BoxRectCollider.as.svn-base @@ -0,0 +1,575 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.physics.math.Matrix4; + import alternativa.physics.math.Vector3; + use namespace altphysics; + + /** + * + */ + public class BoxRectCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + + private var vectorToBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxRectCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, rect, vectorToBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + if (!findEdgesIntersection(box, rect, vectorToBox, int(bestAxisIndex/2), bestAxisIndex%2, contact)) { + return false; + } + } + contact.body1 = box.body; + contact.body2 = rect.body; + + // Хак для танков, чтобы исключить утыкания танков в стыки статических примитивов + if (rect.transform.k > 0.9999) { + contact.normal.x = rect.transform.c; + contact.normal.y = rect.transform.g; + contact.normal.z = rect.transform.k; + } + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + // Вектор из центра прямоугольника в центр бокса + vectorToBox.x = boxTransform.d - rectTransform.d; + vectorToBox.y = boxTransform.h - rectTransform.h; + vectorToBox.z = boxTransform.l - rectTransform.l; + + // Проверка пересечения по нормали прямоугольника + rectTransform.getAxis(2, axis22); + if (!testMainAxis(box, rect, axis22, 0, vectorToBox)) return false; + + // Проверка пересечения по основным осям бокса + boxTransform.getAxis(0, axis10); + if (!testMainAxis(box, rect, axis10, 1, vectorToBox)) return false; + boxTransform.getAxis(1, axis11); + if (!testMainAxis(box, rect, axis11, 2, vectorToBox)) return false; + boxTransform.getAxis(2, axis12); + if (!testMainAxis(box, rect, axis12, 3, vectorToBox)) return false; + + // Получаем направляющие рёбер прямоугольника + rectTransform.getAxis(0, axis20); + rectTransform.getAxis(1, axis21); + + // Проверка производных осей + if (!testDerivedAxis(box, rect, axis10, axis20, 4, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis10, axis21, 5, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis11, axis20, 6, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis11, axis21, 7, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis12, axis20, 8, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis12, axis21, 9, vectorToBox)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта бокса с прямоугольником. + * + * @param box бокс + * @param rect прямоугольник + * @param vectorToBox вектор, направленный из центра прямоугольника в центр бокса + * @param faceAxisIdx индекс оси, идентифицирующей полскость столкновения (грань бокса или полскость прямоугольника) + * @param colInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var pnum:int; + var i:int; + var v:Vector3; + var cp:ContactPoint; + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + var colAxis:Vector3 = contact.normal; + + var negativeFace:Boolean; + var code:int; + + if (faceAxisIdx == 0) { + // Столкновение с плоскостью прямоугольника + + // Проверим положение бокса относительно плоскости прямоугольника + colAxis.x = rectTransform.c; + colAxis.y = rectTransform.g; + colAxis.z = rectTransform.k; + +// var offset:Number = colAxis.x*rectTransform.d + colAxis.y*rectTransform.h + colAxis.z*rectTransform.l; +// if (bbPos.vDot(colAxis) < offset) return false; + + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisIdx = axisIdx; + incidentAxisDot = dot; + } + } + negativeFace = incidentAxisDot > 0; + + code = 1 << (incidentAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + + // Получаем список вершин грани бокса, переводим их в систему координат прямоугольника и выполняем обрезку + // по прямоугольнику. Таким образом получается список потенциальных точек контакта. + boxTransform.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box.hs, incidentAxisIdx, negativeFace, points1); + boxTransform.transformVectorsN(points1, points2, 4); + rectTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByRect(rect.hs); + // Проверяем каждую потенциальную точку на принадлежность нижней полуплоскости прямоугольника и добавляем такие точки в список контактов + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if (v.z < epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = -v.z; + var cpPos:Vector3 = cp.pos; + cpPos.x = rectTransform.a*v.x + rectTransform.b*v.y + rectTransform.c*v.z + rectTransform.d; + cpPos.y = rectTransform.e*v.x + rectTransform.f*v.y + rectTransform.g*v.z + rectTransform.h; + cpPos.z = rectTransform.i*v.x + rectTransform.j*v.y + rectTransform.k*v.z + rectTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } else { + // Столкновение с гранью бокса + faceAxisIdx--; + boxTransform.getAxis(faceAxisIdx, colAxis); + negativeFace = colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z > 0; + + code = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) { + return false; + } + + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (rectTransform.c*colAxis.x + rectTransform.g*colAxis.y + rectTransform.k*colAxis.z < 0) return false; + + getFaceVertsByAxis(rect.hs, 2, false, points1); + rectTransform.transformVectorsN(points1, points2, 4); + boxTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if ((pen = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = pen; + cpPos = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } + return true; + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * + * @param hs + * @return + */ + private function clipByRect(hs:Vector3):int { + var pnum:int = 4; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box первый бокс + * @param rect второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param colInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + boxTransform.getAxis(axisIdx1, axis10); + rectTransform.getAxis(axisIdx2, axis20); + var colAxis:Vector3 = contact.normal; + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону бокса + if (colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + var vx:Number = box.hs.x; + var vy:Number = box.hs.y; + var vz:Number = box.hs.z; + var x2:Number = rect.hs.x; + var y2:Number = rect.hs.y; + var z2:Number = rect.hs.z; + // x + if (axisIdx1 == 0) { + vx = 0; + halfLen1 = box.hs.x; + } else { + if (boxTransform.a*colAxis.x + boxTransform.e*colAxis.y + boxTransform.i*colAxis.z > 0) { + vx = -vx; + if ((box.excludedFaces & 2) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 1) != 0) { + return false; + } + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = rect.hs.x; + } else { + if (rectTransform.a*colAxis.x + rectTransform.e*colAxis.y + rectTransform.i*colAxis.z < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + vy = 0; + halfLen1 = box.hs.y; + } else { + if (boxTransform.b*colAxis.x + boxTransform.f*colAxis.y + boxTransform.j*colAxis.z > 0) { + vy = -vy; + if ((box.excludedFaces & 8) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 4) != 0) { + return false; + } + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = rect.hs.y; + } else { + if (rectTransform.b*colAxis.x + rectTransform.f*colAxis.y + rectTransform.j*colAxis.z < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + vz = 0; + halfLen1 = box.hs.z; + } else { + if (boxTransform.c*colAxis.x + boxTransform.g*colAxis.y + boxTransform.k*colAxis.z > 0) { + vz = -vz; + if ((box.excludedFaces & 32) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 16) != 0) { + return false; + } + } + } + // Получаем глобальные координаты средних точек рёбер + + var x1:Number = boxTransform.a*vx + boxTransform.b*vy + boxTransform.c*vz + boxTransform.d; + var y1:Number = boxTransform.e*vx + boxTransform.f*vy + boxTransform.g*vz + boxTransform.h; + var z1:Number = boxTransform.i*vx + boxTransform.j*vy + boxTransform.k*vz + boxTransform.l; + vx = x2; + vy = y2; + vz = z2; + x2 = rectTransform.a*vx + rectTransform.b*vy + rectTransform.c*vz + rectTransform.d; + y2 = rectTransform.e*vx + rectTransform.f*vy + rectTransform.g*vz + rectTransform.h; + z2 = rectTransform.i*vx + rectTransform.j*vy + rectTransform.k*vz + rectTransform.l; + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + vx = x2 - x1; + vy = y2 - y1; + vz = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cpPos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cpPos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cpPos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var v:Vector3 = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + return true; + } + + /** + * Проверяет пересечение вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box + * @param rect + * @param axis + * @param axisIndex + * @param vectorToBox + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param rect + * @param axis1 + * @param axis2 + * @param axisIndex + * @param vectorToBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, rect:CollisionRect, axis1:Vector3, axis2:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия вдоль заданной оси. + * + * @param box бокс + * @param rect прямоугольник + * @param axis ось + * @param vectorToBox вектор, соединяющий центр прямоугольника с центром бокса + * @return величина перекрытия вдоль оси + */ + public function overlapOnAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, vectorToBox:Vector3):Number { + var m:Matrix4 = box.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box.hs.z; + if (d < 0) d = -d; + projection += d; + + m = rect.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*rect.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*rect.hs.y; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox.x*axis.x + vectorToBox.y*axis.y + vectorToBox.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/colliders/.svn/text-base/BoxSphereCollider.as.svn-base b/0.0.7.1/src/alternativa/physics/collision/colliders/.svn/text-base/BoxSphereCollider.as.svn-base new file mode 100644 index 0000000..2ba3ce2 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/colliders/.svn/text-base/BoxSphereCollider.as.svn-base @@ -0,0 +1,114 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.math.Vector3; + use namespace altphysics; + /** + * + */ + public class BoxSphereCollider implements ICollider { + + private var center:Vector3 = new Vector3(); + private var closestPt:Vector3 = new Vector3(); + + private var bPos:Vector3 = new Vector3(); + private var sPos:Vector3 = new Vector3(); + + /** + * + */ + public function BoxSphereCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + // Зафиксированно столкновение + contact.body1 = sphere.body; + contact.body2 = box.body; + contact.normal.vCopy(closestPt).vTransformBy4(box.transform).vSubtract(sPos).vNormalize().vReverse(); + contact.pcount = 1; + + var cp:ContactPoint = contact.points[0]; + cp.penetration = sphere.r - Math.sqrt(distSqr); + cp.pos.vCopy(contact.normal).vScale(-sphere.r).vAdd(sPos); + cp.r1.vDiff(cp.pos, sPos); + cp.r2.vDiff(cp.pos, bPos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/colliders/.svn/text-base/BoxTriangleCollider.as.svn-base b/0.0.7.1/src/alternativa/physics/collision/colliders/.svn/text-base/BoxTriangleCollider.as.svn-base new file mode 100644 index 0000000..5be3ae6 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/colliders/.svn/text-base/BoxTriangleCollider.as.svn-base @@ -0,0 +1,725 @@ +package alternativa.physics.collision.colliders { + import __AS3__.vec.Vector; + + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionTriangle; + import alternativa.physics.math.Matrix4; + import alternativa.physics.math.Vector3; + + /** + * + */ + public class BoxTriangleCollider extends BoxCollider { + + public var epsilon:Number = 0.001; + + private var bestAxisIndex:int; + private var minOverlap:Number; + private var toBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var colNormal:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxTriangleCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, tri, toBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + if (!findEdgesIntersection(box, tri, toBox, bestAxisIndex%3, int(bestAxisIndex/3), contact)) return false; + } + + contact.body1 = box.body; + contact.body2 = tri.body; + + // Хак для танков, чтобы исключить утыкания танков в стыки статических примитивов + if (tri.transform.k > 0.9999) { + contact.normal.x = tri.transform.c; + contact.normal.y = tri.transform.g; + contact.normal.z = tri.transform.k; + } + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + toBox.x = boxTransform.d - triTransform.d; + toBox.y = boxTransform.h - triTransform.h; + toBox.z = boxTransform.l - triTransform.l; + + minOverlap = 1e308; + + // Сначала проверяется нормаль треугольника + axis.x = triTransform.c; + axis.y = triTransform.g; + axis.z = triTransform.k; + if (!testMainAxis(box, tri, axis, 0, toBox)) return false; + + // Проверка основных осей бокса + axis10.x = boxTransform.a; + axis10.y = boxTransform.e; + axis10.z = boxTransform.i; + if (!testMainAxis(box, tri, axis10, 1, toBox)) return false; + axis11.x = boxTransform.b; + axis11.y = boxTransform.f; + axis11.z = boxTransform.j; + if (!testMainAxis(box, tri, axis11, 2, toBox)) return false; + axis12.x = boxTransform.c; + axis12.y = boxTransform.g; + axis12.z = boxTransform.k; + if (!testMainAxis(box, tri, axis12, 3, toBox)) return false; + + // Проверка производных осей + // TODO: заменить вычисления векторных произведений инлайнами + var v:Vector3 = tri.e0; + axis20.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis20.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis20.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis20, 4, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis20, 5, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis20, 6, toBox)) return false; + + v = tri.e1; + axis21.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis21.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis21.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis21, 7, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis21, 8, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis21, 9, toBox)) return false; + + v = tri.e2; + axis22.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis22.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis22.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis22, 10, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis22, 11, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis22, 12, toBox)) return false; + + return true; + } + + /** + * Тестирует пересечение примитивов вдоль заданной оси. + * + * @param box бокс + * @param tri треугольник + * @param axis ось + * @param axisIndex индекс оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return true, если примитивы имеют перекрытие проекций вдоль указанной оси, иначе false + */ + private function testMainAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, axisIndex:int, toBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param tri + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, tri:CollisionTriangle, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Рассчитывает величину перекрытия проекций бокса и треугольника на заданную ось. + * + * @param box бокс + * @param tri треугольник + * @param axis единичный направляющий вектор оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return величина перекрытия. Положительное значение указывает на наличие перекрытия, нулевое или отрицательное значение указывает на отсутствие перекрытия. + */ + private function overlapOnAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, toBox:Vector3):Number { + var t:Matrix4 = box.transform; + var projection:Number = (t.a*axis.x + t.e*axis.y + t.i*axis.z)*box.hs.x; + if (projection < 0) projection = -projection; + var d:Number = (t.b*axis.x + t.f*axis.y + t.j*axis.z)*box.hs.y; + projection += d < 0 ? -d : d; + d = (t.c*axis.x + t.g*axis.y + t.k*axis.z)*box.hs.z; + projection += d < 0 ? -d : d; + + var vectorProjection:Number = toBox.x*axis.x + toBox.y*axis.y + toBox.z*axis.z; + // Для оптимизации ось преобразуется в систему треугольника, а не его вершины в мировую систему + t = tri.transform; + var ax:Number = t.a*axis.x + t.e*axis.y + t.i*axis.z; + var ay:Number = t.b*axis.x + t.f*axis.y + t.j*axis.z; + var az:Number = t.c*axis.x + t.g*axis.y + t.k*axis.z; + var max:Number = 0; + if (vectorProjection < 0) { + vectorProjection = -vectorProjection; + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d < max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d < max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d < max) max = d; + max = -max; + } else { + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d > max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d > max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d > max) max = d; + } + + return projection + max - vectorProjection; + } + + /** + * Определяет точки контакта бокса и треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function findFaceContactPoints(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIndex:int, contact:Contact):Boolean { + if (faceAxisIndex == 0) { + // Столкновение с плоскостью треугольника + return getBoxToTriContact(box, tri, toBox, contact); + } else { + // Столкновение с гранью бокса + return getTriToBoxContact(box, tri, toBox, faceAxisIndex, contact); + } + } + + /** + * Определяет точки контакта бокса с плоскостью треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getBoxToTriContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + colNormal.x = triTransform.c; + colNormal.y = triTransform.g; + colNormal.z = triTransform.k; + + var over:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + if (!over) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var incAxisDot:Number = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colNormal.x + axis.y*colNormal.y + axis.z*colNormal.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incAxisDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Обрезка грани + var negativeFace:Boolean = incAxisDot > 0; + + var code:int = 1 << (incFaceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + getFaceVertsByAxis(box.hs, incFaceAxisIdx, negativeFace, points1); + boxTransform.transformVectorsN(points1, points2, 4); + triTransform.transformVectorsInverseN(points2, points1, 4); + var pnum:int = clipByTriangle(tri); + // Среди конечного списка точек определяются лежащие под плоскостью треугольника + var cp:ContactPoint; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points2[i]; + if ((over && v.z < 0) || (!over && v.z > 0)) { + cp = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z + triTransform.d; + cpPos.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z + triTransform.h; + cpPos.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z + triTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = over ? -v.z : v.z; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Оперделяет точки контакта треугольника с гранью бокса. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getTriToBoxContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + faceAxisIdx--; + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + boxTransform.getAxis(faceAxisIdx, colNormal); + var negativeFace:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + + var code:int = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + if (!negativeFace) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + var v:Vector3 = points1[0]; + v.x = tri.v0.x; + v.y = tri.v0.y; + v.z = tri.v0.z; + + v = points1[1]; + v.x = tri.v1.x; + v.y = tri.v1.y; + v.z = tri.v1.z; + + v = points1[2]; + v.x = tri.v2.x; + v.y = tri.v2.y; + v.z = tri.v2.z; + + triTransform.transformVectorsN(points1, points2, 3); + boxTransform.transformVectorsInverseN(points2, points1, 3); + + var pnum:int = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность боксу и добавляем такие точки в список контактов + var penetration:Number; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + v = points1[i]; + penetration = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace); + if (penetration > -epsilon) { + var cp:ContactPoint = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = penetration; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Вычисляет величину проникновения точки в бокс. + * + * @param hs вектор половинных размеров бокса + * @param p точка в системе координат бокса + * @param axisIndex индекс оси + * @param negativeFace если true, проверяется нижняя грань + * @return величина проникновения точки в бокс + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, negativeFace:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (negativeFace) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (negativeFace) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (negativeFace) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, вершины которой заданы в списке points1. Результат сохраняется в этом же списке. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 3; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Обрезает грань прямоугольника сторонами треугольника. Обрезка выполняется в системе координат и в проекции на + * плоскость треугольника. Входные вершины грани в количестве четырёх штук должны находиться в списке points1. + * Конечные вершины сохраняются в списке points2. + * + * @param tri треугольник + * @return количество врешин в конечном списке + */ + private function clipByTriangle(tri:CollisionTriangle):int { + var vnum:int = 4; + vnum = clipByLine(tri.v0, tri.e0, points1, vnum, points2); + if (vnum == 0) return 0; + vnum = clipByLine(tri.v1, tri.e1, points2, vnum, points1); + if (vnum == 0) return 0; + return clipByLine(tri.v2, tri.e2, points1, vnum, points2);; + } + + /** + * Обрезает полигон указанной прямой. + * + * @param linePoint точка на прямой + * @param lineDir единичный направляющий вектор прямой + * @param verticesIn список вершин исходного полигона + * @param vnum количество вершин исходного полигона + * @param verticesOut список, куда будут записаны вершины конечного полигона + * @return количество вершин конечного полигона + */ + private function clipByLine(linePoint:Vector3, lineDir:Vector3, verticesIn:Vector., vnum:int, verticesOut:Vector.):int { + var nx:Number = -lineDir.y; + var ny:Number = lineDir.x; + var offset:Number = linePoint.x*nx + linePoint.y*ny; + var v1:Vector3 = verticesIn[int(vnum - 1)]; + var offset1:Number = v1.x*nx + v1.y*ny; + var t:Number; + var v:Vector3; + var num:int = 0; + for (var i:int = 0; i < vnum; i++) { + var v2:Vector3 = verticesIn[i]; + var offset2:Number = v2.x*nx + v2.y*ny; + if (offset1 < offset) { + // Первая точка ребра во внешней полуплоскости + if (offset2 > offset) { + // Вторая точка ребра во внутренней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } else { + // Первая точка ребра во внутренней полуплоскости. Добавляем её в конечный список. + v = verticesOut[num]; + v.x = v1.x; + v.y = v1.y; + v.z = v1.z; + num++; + if (offset2 < offset) { + // Вторая точка ребра во внешней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } + v1 = v2; + offset1 = offset2; + } + return num; + } + + /** + * + * @param box + * @param tri + * @param toBox + * @param boxAxisIdx + * @param triAxisIdx + * @param contact + */ + private function findEdgesIntersection(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, boxAxisIdx:int, triAxisIdx:int, contact:Contact):Boolean { + // Определение точки и направляющего вектора ребра треугольника + var tmpx1:Number; + var tmpy1:Number; + var tmpz1:Number; + var tmpx2:Number; + var tmpy2:Number; + var tmpz2:Number; + switch (triAxisIdx) { + case 0: + tmpx1 = tri.e0.x; + tmpy1 = tri.e0.y; + tmpz1 = tri.e0.z; + tmpx2 = tri.v0.x; + tmpy2 = tri.v0.y; + tmpz2 = tri.v0.z; + break; + case 1: + tmpx1 = tri.e1.x; + tmpy1 = tri.e1.y; + tmpz1 = tri.e1.z; + tmpx2 = tri.v1.x; + tmpy2 = tri.v1.y; + tmpz2 = tri.v1.z; + break; + case 2: + tmpx1 = tri.e2.x; + tmpy1 = tri.e2.y; + tmpz1 = tri.e2.z; + tmpx2 = tri.v2.x; + tmpy2 = tri.v2.y; + tmpz2 = tri.v2.z; + break; + } + var triTransform:Matrix4 = tri.transform; + axis20.x = triTransform.a*tmpx1 + triTransform.b*tmpy1 + triTransform.c*tmpz1; + axis20.y = triTransform.e*tmpx1 + triTransform.f*tmpy1 + triTransform.g*tmpz1; + axis20.z = triTransform.i*tmpx1 + triTransform.j*tmpy1 + triTransform.k*tmpz1; + var x2:Number = triTransform.a*tmpx2 + triTransform.b*tmpy2 + triTransform.c*tmpz2 + triTransform.d; + var y2:Number = triTransform.e*tmpx2 + triTransform.f*tmpy2 + triTransform.g*tmpz2 + triTransform.h; + var z2:Number = triTransform.i*tmpx2 + triTransform.j*tmpy2 + triTransform.k*tmpz2 + triTransform.l; + + // Определение нормали контакта, точки и направляющего вектора ребра бокса + var boxTransform:Matrix4 = box.transform; + boxTransform.getAxis(boxAxisIdx, axis10); + + // Нормаль контакта + var v:Vector3 = contact.normal; + v.x = axis10.y*axis20.z - axis10.z*axis20.y; + v.y = axis10.z*axis20.x - axis10.x*axis20.z; + v.z = axis10.x*axis20.y - axis10.y*axis20.x; + k = 1/Math.sqrt(v.x*v.x + v.y*v.y + v.z*v.z); + v.x *= k; + v.y *= k; + v.z *= k; + // Разворот нормали в сторону бокса + if (v.x*toBox.x + v.y*toBox.y + v.z*toBox.z < 0) { + v.x = -v.x; + v.y = -v.y; + v.z = -v.z; + } + + var boxHalfLen:Number; + tmpx1 = box.hs.x; + tmpy1 = box.hs.y; + tmpz1 = box.hs.z; + // X + if (boxAxisIdx == 0) { + tmpx1 = 0; + boxHalfLen = box.hs.x; + } else { + if (boxTransform.a*v.x + boxTransform.e*v.y + boxTransform.i*v.z > 0) { + tmpx1 = -tmpx1; + if ((box.excludedFaces & 2) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 1) != 0) { + return false; + } + } + } + // Y + if (boxAxisIdx == 1) { + tmpy1 = 0; + boxHalfLen = box.hs.y; + } else { + if (boxTransform.b*v.x + boxTransform.f*v.y + boxTransform.j*v.z > 0) { + tmpy1 = -tmpy1; + if ((box.excludedFaces & 8) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 4) != 0) { + return false; + } + } + } + // Z + if (boxAxisIdx == 2) { + tmpz1 = 0; + boxHalfLen = box.hs.z; + } else { + if (boxTransform.c*v.x + boxTransform.g*v.y + boxTransform.k*v.z > 0) { + tmpz1 = -tmpz1; + if ((box.excludedFaces & 32) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 16) != 0) { + return false; + } + } + } + var x1:Number = boxTransform.a*tmpx1 + boxTransform.b*tmpy1 + boxTransform.c*tmpz1 + boxTransform.d; + var y1:Number = boxTransform.e*tmpx1 + boxTransform.f*tmpy1 + boxTransform.g*tmpz1 + boxTransform.h; + var z1:Number = boxTransform.i*tmpx1 + boxTransform.j*tmpy1 + boxTransform.k*tmpz1 + boxTransform.l; + + // Находим точку пересечения рёбер, решая систему уравнений + // axis10 - направляющий вектор ребра бокса + // x1, y1, z1 - средняя точка ребра бокса + // axis20 - направляющий вектор ребра треугольника + // x2, y2, z2 - начальная точка ребра треугольника + var k:Number = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + var vx:Number = x2 - x1; + var vy:Number = y2 - y1; + var vz:Number = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + + // Запись данных о контакте + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + v = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + v.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + v.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + v.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = v.x - boxTransform.d; + r.y = v.y - boxTransform.h; + r.z = v.z - boxTransform.l; + r = cp.r2; + r.x = v.x - triTransform.d; + r.y = v.y - triTransform.h; + r.z = v.z - triTransform.l; + return true; + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/colliders/.svn/text-base/SpherePlaneCollider.as.svn-base b/0.0.7.1/src/alternativa/physics/collision/colliders/.svn/text-base/SpherePlaneCollider.as.svn-base new file mode 100644 index 0000000..1ac5d35 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/colliders/.svn/text-base/SpherePlaneCollider.as.svn-base @@ -0,0 +1,68 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.math.Vector3; + + /** + * + */ + public class SpherePlaneCollider implements ICollider { + + private var normal:Vector3 = new Vector3(); + + /** + * + */ + public function SpherePlaneCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var sphere:RigidSphere = body1 as RigidSphere; +// var plane:RigidPlane; +// if (sphere == null) { +// sphere = body2 as RigidSphere; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// +// var dist:Number = sphere.state.pos.dot(normal) - offset; +// if (dist > sphere.r) return false; +// +// collisionInfo.body1 = sphere; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// collisionInfo.pcount = 1; +// +// var cp:ContactPoint = collisionInfo.points[0]; +// cp.penetration = sphere.r - dist; +// cp.pos.copy(normal).reverse().scale(sphere.r).add(sphere.state.pos); +// cp.r1.diff(cp.pos, sphere.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/colliders/.svn/text-base/SphereSphereCollider.as.svn-base b/0.0.7.1/src/alternativa/physics/collision/colliders/.svn/text-base/SphereSphereCollider.as.svn-base new file mode 100644 index 0000000..a90fabb --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/colliders/.svn/text-base/SphereSphereCollider.as.svn-base @@ -0,0 +1,70 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.math.Vector3; + use namespace altphysics; + + public class SphereSphereCollider implements ICollider { + + private var p1:Vector3 = new Vector3(); + private var p2:Vector3 = new Vector3(); + + public function SphereSphereCollider() { + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var s1:CollisionSphere; + var s2:CollisionSphere; + if (prim1.body != null) { + s1 = prim1 as CollisionSphere; + s2 = prim2 as CollisionSphere; + } else { + s1 = prim2 as CollisionSphere; + s2 = prim1 as CollisionSphere; + } + + s1.transform.getAxis(3, p1); + s2.transform.getAxis(3, p2); + var dx:Number = p1.x - p2.x; + var dy:Number = p1.y - p2.y; + var dz:Number = p1.z - p2.z; + var len:Number = dx*dx + dy*dy + dz*dz; + var sum:Number = s1.r + s2.r; + if (len > sum*sum) return false; + len = Math.sqrt(len); + dx /= len; + dy /= len; + dz /= len; + + contact.body1 = s1.body; + contact.body2 = s2.body; + contact.normal.x = dx; + contact.normal.y = dy; + contact.normal.z = dz; + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = sum - len; + cp.pos.x = p1.x - dx*s1.r; + cp.pos.y = p1.y - dy*s1.r; + cp.pos.z = p1.z - dz*s1.r; + cp.r1.vDiff(cp.pos, p1); + cp.r2.vDiff(cp.pos, p2); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/colliders/BoxBoxCollider.as b/0.0.7.1/src/alternativa/physics/collision/colliders/BoxBoxCollider.as new file mode 100644 index 0000000..b003e98 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/colliders/BoxBoxCollider.as @@ -0,0 +1,589 @@ +package alternativa.physics.collision.colliders { + import __AS3__.vec.Vector; + + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.math.Matrix4; + import alternativa.physics.math.Vector3; + + use namespace altphysics; + + /** + * Расчитывает точки контакта двух боксов. Нормаль контакта направляется в сторону бокса с меньшим ID. + */ + public class BoxBoxCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + private var vectorToBox1:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var bestAxisIndex:int; + private var minOverlap:Number; + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + private var tmpPoints:Vector. = new Vector.(8, true); + private var pcount:int; + + /** + * + */ + public function BoxBoxCollider() { + for (var i:int = 0; i < 8; i++) { + tmpPoints[i] = new ContactPoint(); + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + + if (bestAxisIndex < 6) { + // Контакт грань-(грань|ребро|вершина) + if (!findFaceContactPoints(box1, box2, vectorToBox1, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 6; + findEdgesIntersection(box1, box2, vectorToBox1, int(bestAxisIndex/3), bestAxisIndex%3, contact); + } + contact.body1 = box1.body; + contact.body2 = box2.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + // Вектор из центра второго бокса в центр первого + vectorToBox1.x = transform1.d - transform2.d; + vectorToBox1.y = transform1.h - transform2.h; + vectorToBox1.z = transform1.l - transform2.l; + + // Проверка пересечения по основным осям + axis10.x = transform1.a; + axis10.y = transform1.e; + axis10.z = transform1.i; + if (!testMainAxis(box1, box2, axis10, 0, vectorToBox1)) return false; + axis11.x = transform1.b; + axis11.y = transform1.f; + axis11.z = transform1.j; + if (!testMainAxis(box1, box2, axis11, 1, vectorToBox1)) return false; + axis12.x = transform1.c; + axis12.y = transform1.g; + axis12.z = transform1.k; + if (!testMainAxis(box1, box2, axis12, 2, vectorToBox1)) return false; + + axis20.x = transform2.a; + axis20.y = transform2.e; + axis20.z = transform2.i; + if (!testMainAxis(box1, box2, axis20, 3, vectorToBox1)) return false; + axis21.x = transform2.b; + axis21.y = transform2.f; + axis21.z = transform2.j; + if (!testMainAxis(box1, box2, axis21, 4, vectorToBox1)) return false; + axis22.x = transform2.c; + axis22.y = transform2.g; + axis22.z = transform2.k; + if (!testMainAxis(box1, box2, axis22, 5, vectorToBox1)) return false; + + // Проверка производных осей + if (!testDerivedAxis(box1, box2, axis10, axis20, 6, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis21, 7, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis22, 8, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis11, axis20, 9, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis21, 10, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis22, 11, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis12, axis20, 12, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis21, 13, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis22, 14, vectorToBox1)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта грани одного бокса с гранью/ребром/вершиной другого. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param faceAxisIdx индекс оси первого бокса, перпендикулярной грани, с которой произошло столкновение + * @param contactInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var swapNormal:Boolean = false; + if (faceAxisIdx > 2) { + // Столкновение с гранью второго бокса. Для дальнейших расчётов боксы меняются местами, + // но нормаль контакта всё равно должна быть направлена в сторону первоначального box1 + var tmpBox:CollisionBox = box1; + box1 = box2; + box2 = tmpBox; + vectorToBox1.x = -vectorToBox1.x; + vectorToBox1.y = -vectorToBox1.y; + vectorToBox1.z = -vectorToBox1.z; + faceAxisIdx -= 3; + swapNormal = true; + } + + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + var colAxis:Vector3 = contact.normal; + + transform1.getAxis(faceAxisIdx, colAxis); + var negativeFace:Boolean = colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z > 0; + + var code:int = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box1.excludedFaces) != 0) return false; + + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + // Ищем ось второго бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + transform2.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisDot = dot; + incidentAxisIdx = axisIdx; + } + } + // Получаем список вершин грани второго бокса, переводим их в систему координат первого бокса и выполняем обрезку + // по грани первого бокса. Таким образом получается список потенциальных точек контакта. + transform2.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box2.hs, incidentAxisIdx, incidentAxisDot < 0, points1); + + // TODO: Вычислить результирующую матрицу, затем преобразовать векторы за один заход + transform2.transformVectorsN(points1, points2, 4); + transform1.transformVectorsInverseN(points2, points1, 4); + + var pnum:int = clip(box1.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points1[i]; + if ((pen = getPointBoxPenetration(box1.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + var cp:ContactPoint = tmpPoints[pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = transform1.a*v.x + transform1.b*v.y + transform1.c*v.z + transform1.d; + cpPos.y = transform1.e*v.x + transform1.f*v.y + transform1.g*v.z + transform1.h; + cpPos.z = transform1.i*v.x + transform1.j*v.y + transform1.k*v.z + transform1.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + + cp.penetration = pen; + } + } + if (swapNormal) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (pcount > 4) { + reducePoints(); + } + for (i = 0; i < pcount; i++) { + cp = contact.points[i]; + cp.copyFrom(tmpPoints[i]); + } + contact.pcount = pcount; + return true; + } + + /** + * + * @param contactInfo + */ + private function reducePoints():void { + var i:int; + var minIdx:int; + var cp1:ContactPoint; + var cp2:ContactPoint; + while (pcount > 4) { + var minLen:Number = 1e10; + var p1:Vector3 = (tmpPoints[int(pcount - 1)] as ContactPoint).pos; + var p2:Vector3; + for (i = 0; i < pcount; i++) { + p2 = (tmpPoints[i] as ContactPoint).pos; + var dx:Number = p2.x - p1.x; + var dy:Number = p2.y - p1.y; + var dz:Number = p2.z - p1.z; + var len:Number = dx*dx + dy*dy + dz*dz; + if (len < minLen) { + minLen = len; + minIdx = i; + } + p1 = p2; + } + var ii:int = minIdx == 0 ? (pcount - 1) : (minIdx - 1); + cp1 = tmpPoints[ii]; + cp2 = tmpPoints[minIdx]; + p1 = cp1.pos; + p2 = cp2.pos; + p2.x = 0.5*(p1.x + p2.x); + p2.y = 0.5*(p1.y + p2.y); + p2.z = 0.5*(p1.z + p2.z); + cp2.penetration = 0.5*(cp1.penetration + cp2.penetration); + if (minIdx > 0) { + for (i = minIdx; i < pcount; i++) { + tmpPoints[int(i - 1)] = tmpPoints[i]; + } + tmpPoints[int(pcount - 1)] = cp1; + } + pcount--; + } + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clip(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param contactInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):void { + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + transform1.getAxis(axisIdx1, axis10); + transform2.getAxis(axisIdx2, axis20); + + var colAxis:Vector3 = contact.normal; + + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону первого бокса + if (colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + + var tx:Number = box1.hs.x; + var ty:Number = box1.hs.y; + var tz:Number = box1.hs.z; + + var x2:Number = box2.hs.x; + var y2:Number = box2.hs.y; + var z2:Number = box2.hs.z; + // x + if (axisIdx1 == 0) { + tx = 0; + halfLen1 = box1.hs.x; + } else { + if (colAxis.x*transform1.a + colAxis.y*transform1.e + colAxis.z*transform1.i > 0) { + tx = -tx; + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = box2.hs.x; + } else { + if (colAxis.x*transform2.a + colAxis.y*transform2.e + colAxis.z*transform2.i < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + ty = 0; + halfLen1 = box1.hs.y; + } else { + if (colAxis.x*transform1.b + colAxis.y*transform1.f + colAxis.z*transform1.j > 0) { + ty = -ty; + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = box2.hs.y; + } else { + if (colAxis.x*transform2.b + colAxis.y*transform2.f + colAxis.z*transform2.j < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + tz = 0; + halfLen1 = box1.hs.z; + } else { + if (colAxis.x*transform1.c + colAxis.y*transform1.g + colAxis.z*transform1.k > 0) { + tz = -tz; + } + } + if (axisIdx2 == 2) { + z2 = 0; + halfLen2 = box2.hs.z; + } else { + if (colAxis.x*transform2.c + colAxis.y*transform2.g + colAxis.z*transform2.k < 0) { + z2 = -z2; + } + } + // Получаем глобальные координаты средних точек рёбер + var x1:Number = transform1.a*tx + transform1.b*ty + transform1.c*tz + transform1.d; + var y1:Number = transform1.e*tx + transform1.f*ty + transform1.g*tz + transform1.h; + var z1:Number = transform1.i*tx + transform1.j*ty + transform1.k*tz + transform1.l; + tx = x2; + ty = y2; + tz = z2; + x2 = transform2.a*tx + transform2.b*ty + transform2.c*tz + transform2.d; + y2 = transform2.e*tx + transform2.f*ty + transform2.g*tz + transform2.h; + z2 = transform2.i*tx + transform2.j*ty + transform2.k*tz + transform2.l; + + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + tx = x2 - x1; + ty = y2 - y1; + tz = z2 - z1; + var c1:Number = axis10.x*tx + axis10.y*ty + axis10.z*tz; + var c2:Number = axis20.x*tx + axis20.y*ty + axis20.z*tz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cp.pos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cp.pos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение боксов вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box1 + * @param box2 + * @param axis + * @param axisIndex + * @param toBox1 + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box1 + * @param box2 + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox1 + * @return + */ + private function testDerivedAxis(box1:CollisionBox, box2:CollisionBox, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия двух боксов вдоль заданной оси. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param axis ось + * @param vectorToBox1 вектор, соединяющий центр второго бокса с центром первого + * @return величина перекрытия боксов вдоль оси + */ + public function overlapOnAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, vectorToBox1:Vector3):Number { + var m:Matrix4 = box1.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box1.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box1.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box1.hs.z; + if (d < 0) d = -d; + projection += d; + + m = box2.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box2.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box2.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box2.hs.z; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox1.x*axis.x + vectorToBox1.y*axis.y + vectorToBox1.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} + import alternativa.physics.math.Vector3; + + class CollisionPointTmp { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + } diff --git a/0.0.7.1/src/alternativa/physics/collision/colliders/BoxCollider.as b/0.0.7.1/src/alternativa/physics/collision/colliders/BoxCollider.as new file mode 100644 index 0000000..0021854 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/colliders/BoxCollider.as @@ -0,0 +1,427 @@ +package alternativa.physics.collision.colliders { + import __AS3__.vec.Vector; + + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.math.Vector3; + + /** + * + */ + public class BoxCollider implements ICollider { + + /** + * + */ + public function BoxCollider() { + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + return false; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + protected function getFaceVertsByAxis(hs:Vector3, axisIdx:int, negativeFace:Boolean, result:Vector.):void { + var v:Vector3; + switch (axisIdx) { + case 0: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + } + break; + case 1: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + } + break; + case 2: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + } else { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } + break; + } + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/colliders/BoxPlaneCollider.as b/0.0.7.1/src/alternativa/physics/collision/colliders/BoxPlaneCollider.as new file mode 100644 index 0000000..bb1ed0f --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/colliders/BoxPlaneCollider.as @@ -0,0 +1,88 @@ +package alternativa.physics.collision.colliders { + import __AS3__.vec.Vector; + + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.math.Vector3; + + public class BoxPlaneCollider implements ICollider { + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var normal:Vector3 = new Vector3(); + + public function BoxPlaneCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var box:RigidBox = body1 as RigidBox; +// var plane:RigidPlane; +// if (box == null) { +// box = body2 as RigidBox; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные координаты вершин бокса +// var sx:Number = box.halfSize.x; +// var sy:Number = box.halfSize.y; +// var sz:Number = box.halfSize.z; +// (verts1[0] as Vector3).reset(-sx, -sy, -sz); +// (verts1[1] as Vector3).reset(sx, -sy, -sz); +// (verts1[2] as Vector3).reset(sx, sy, -sz); +// (verts1[3] as Vector3).reset(-sx, sy, -sz); +// (verts1[4] as Vector3).reset(-sx, -sy, sz); +// (verts1[5] as Vector3).reset(sx, -sy, sz); +// (verts1[6] as Vector3).reset(sx, sy, sz); +// (verts1[7] as Vector3).reset(-sx, sy, sz); +// +// box.transform.transformVectors(verts1, verts2); +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// // Проверяем наличие столкновений с каждой вершиной +// collisionInfo.pcount = 0; +// for (var i:int = 0; i < 8; i++) { +// // Вершина добавляется в список точек столкновения, если лежит под плоскостью +// var dist:Number = (verts2[i] as Vector3).dot(normal); +// if (dist < offset) { +// var cp:ContactPoint; +// if (collisionInfo.pcount == collisionInfo.points.length) { +// cp = new ContactPoint(); +// collisionInfo.points[collisionInfo.pcount] = cp; +// } else { +// cp = collisionInfo.points[collisionInfo.pcount]; +// } +// cp.pos.copy(verts2[i]); +// cp.r1.diff(cp.pos, box.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); +// cp.penetration = offset - dist; +// collisionInfo.pcount++; +// } +// } +// if (collisionInfo.pcount > 0) { +// collisionInfo.body1 = box; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// return true; +// } + return false; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/colliders/BoxRectCollider.as b/0.0.7.1/src/alternativa/physics/collision/colliders/BoxRectCollider.as new file mode 100644 index 0000000..7bf8ef8 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/colliders/BoxRectCollider.as @@ -0,0 +1,575 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.physics.math.Matrix4; + import alternativa.physics.math.Vector3; + use namespace altphysics; + + /** + * + */ + public class BoxRectCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + + private var vectorToBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxRectCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, rect, vectorToBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + if (!findEdgesIntersection(box, rect, vectorToBox, int(bestAxisIndex/2), bestAxisIndex%2, contact)) { + return false; + } + } + contact.body1 = box.body; + contact.body2 = rect.body; + + // Хак для танков, чтобы исключить утыкания танков в стыки статических примитивов + if (rect.transform.k > 0.9999) { + contact.normal.x = rect.transform.c; + contact.normal.y = rect.transform.g; + contact.normal.z = rect.transform.k; + } + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + // Вектор из центра прямоугольника в центр бокса + vectorToBox.x = boxTransform.d - rectTransform.d; + vectorToBox.y = boxTransform.h - rectTransform.h; + vectorToBox.z = boxTransform.l - rectTransform.l; + + // Проверка пересечения по нормали прямоугольника + rectTransform.getAxis(2, axis22); + if (!testMainAxis(box, rect, axis22, 0, vectorToBox)) return false; + + // Проверка пересечения по основным осям бокса + boxTransform.getAxis(0, axis10); + if (!testMainAxis(box, rect, axis10, 1, vectorToBox)) return false; + boxTransform.getAxis(1, axis11); + if (!testMainAxis(box, rect, axis11, 2, vectorToBox)) return false; + boxTransform.getAxis(2, axis12); + if (!testMainAxis(box, rect, axis12, 3, vectorToBox)) return false; + + // Получаем направляющие рёбер прямоугольника + rectTransform.getAxis(0, axis20); + rectTransform.getAxis(1, axis21); + + // Проверка производных осей + if (!testDerivedAxis(box, rect, axis10, axis20, 4, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis10, axis21, 5, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis11, axis20, 6, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis11, axis21, 7, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis12, axis20, 8, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis12, axis21, 9, vectorToBox)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта бокса с прямоугольником. + * + * @param box бокс + * @param rect прямоугольник + * @param vectorToBox вектор, направленный из центра прямоугольника в центр бокса + * @param faceAxisIdx индекс оси, идентифицирующей полскость столкновения (грань бокса или полскость прямоугольника) + * @param colInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var pnum:int; + var i:int; + var v:Vector3; + var cp:ContactPoint; + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + var colAxis:Vector3 = contact.normal; + + var negativeFace:Boolean; + var code:int; + + if (faceAxisIdx == 0) { + // Столкновение с плоскостью прямоугольника + + // Проверим положение бокса относительно плоскости прямоугольника + colAxis.x = rectTransform.c; + colAxis.y = rectTransform.g; + colAxis.z = rectTransform.k; + +// var offset:Number = colAxis.x*rectTransform.d + colAxis.y*rectTransform.h + colAxis.z*rectTransform.l; +// if (bbPos.vDot(colAxis) < offset) return false; + + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisIdx = axisIdx; + incidentAxisDot = dot; + } + } + negativeFace = incidentAxisDot > 0; + + code = 1 << (incidentAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + + // Получаем список вершин грани бокса, переводим их в систему координат прямоугольника и выполняем обрезку + // по прямоугольнику. Таким образом получается список потенциальных точек контакта. + boxTransform.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box.hs, incidentAxisIdx, negativeFace, points1); + boxTransform.transformVectorsN(points1, points2, 4); + rectTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByRect(rect.hs); + // Проверяем каждую потенциальную точку на принадлежность нижней полуплоскости прямоугольника и добавляем такие точки в список контактов + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if (v.z < epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = -v.z; + var cpPos:Vector3 = cp.pos; + cpPos.x = rectTransform.a*v.x + rectTransform.b*v.y + rectTransform.c*v.z + rectTransform.d; + cpPos.y = rectTransform.e*v.x + rectTransform.f*v.y + rectTransform.g*v.z + rectTransform.h; + cpPos.z = rectTransform.i*v.x + rectTransform.j*v.y + rectTransform.k*v.z + rectTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } else { + // Столкновение с гранью бокса + faceAxisIdx--; + boxTransform.getAxis(faceAxisIdx, colAxis); + negativeFace = colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z > 0; + + code = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) { + return false; + } + + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (rectTransform.c*colAxis.x + rectTransform.g*colAxis.y + rectTransform.k*colAxis.z < 0) return false; + + getFaceVertsByAxis(rect.hs, 2, false, points1); + rectTransform.transformVectorsN(points1, points2, 4); + boxTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if ((pen = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = pen; + cpPos = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } + return true; + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * + * @param hs + * @return + */ + private function clipByRect(hs:Vector3):int { + var pnum:int = 4; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box первый бокс + * @param rect второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param colInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + boxTransform.getAxis(axisIdx1, axis10); + rectTransform.getAxis(axisIdx2, axis20); + var colAxis:Vector3 = contact.normal; + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону бокса + if (colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + var vx:Number = box.hs.x; + var vy:Number = box.hs.y; + var vz:Number = box.hs.z; + var x2:Number = rect.hs.x; + var y2:Number = rect.hs.y; + var z2:Number = rect.hs.z; + // x + if (axisIdx1 == 0) { + vx = 0; + halfLen1 = box.hs.x; + } else { + if (boxTransform.a*colAxis.x + boxTransform.e*colAxis.y + boxTransform.i*colAxis.z > 0) { + vx = -vx; + if ((box.excludedFaces & 2) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 1) != 0) { + return false; + } + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = rect.hs.x; + } else { + if (rectTransform.a*colAxis.x + rectTransform.e*colAxis.y + rectTransform.i*colAxis.z < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + vy = 0; + halfLen1 = box.hs.y; + } else { + if (boxTransform.b*colAxis.x + boxTransform.f*colAxis.y + boxTransform.j*colAxis.z > 0) { + vy = -vy; + if ((box.excludedFaces & 8) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 4) != 0) { + return false; + } + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = rect.hs.y; + } else { + if (rectTransform.b*colAxis.x + rectTransform.f*colAxis.y + rectTransform.j*colAxis.z < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + vz = 0; + halfLen1 = box.hs.z; + } else { + if (boxTransform.c*colAxis.x + boxTransform.g*colAxis.y + boxTransform.k*colAxis.z > 0) { + vz = -vz; + if ((box.excludedFaces & 32) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 16) != 0) { + return false; + } + } + } + // Получаем глобальные координаты средних точек рёбер + + var x1:Number = boxTransform.a*vx + boxTransform.b*vy + boxTransform.c*vz + boxTransform.d; + var y1:Number = boxTransform.e*vx + boxTransform.f*vy + boxTransform.g*vz + boxTransform.h; + var z1:Number = boxTransform.i*vx + boxTransform.j*vy + boxTransform.k*vz + boxTransform.l; + vx = x2; + vy = y2; + vz = z2; + x2 = rectTransform.a*vx + rectTransform.b*vy + rectTransform.c*vz + rectTransform.d; + y2 = rectTransform.e*vx + rectTransform.f*vy + rectTransform.g*vz + rectTransform.h; + z2 = rectTransform.i*vx + rectTransform.j*vy + rectTransform.k*vz + rectTransform.l; + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + vx = x2 - x1; + vy = y2 - y1; + vz = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cpPos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cpPos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cpPos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var v:Vector3 = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + return true; + } + + /** + * Проверяет пересечение вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box + * @param rect + * @param axis + * @param axisIndex + * @param vectorToBox + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param rect + * @param axis1 + * @param axis2 + * @param axisIndex + * @param vectorToBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, rect:CollisionRect, axis1:Vector3, axis2:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия вдоль заданной оси. + * + * @param box бокс + * @param rect прямоугольник + * @param axis ось + * @param vectorToBox вектор, соединяющий центр прямоугольника с центром бокса + * @return величина перекрытия вдоль оси + */ + public function overlapOnAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, vectorToBox:Vector3):Number { + var m:Matrix4 = box.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box.hs.z; + if (d < 0) d = -d; + projection += d; + + m = rect.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*rect.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*rect.hs.y; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox.x*axis.x + vectorToBox.y*axis.y + vectorToBox.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/colliders/BoxSphereCollider.as b/0.0.7.1/src/alternativa/physics/collision/colliders/BoxSphereCollider.as new file mode 100644 index 0000000..2ba3ce2 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/colliders/BoxSphereCollider.as @@ -0,0 +1,114 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.math.Vector3; + use namespace altphysics; + /** + * + */ + public class BoxSphereCollider implements ICollider { + + private var center:Vector3 = new Vector3(); + private var closestPt:Vector3 = new Vector3(); + + private var bPos:Vector3 = new Vector3(); + private var sPos:Vector3 = new Vector3(); + + /** + * + */ + public function BoxSphereCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + // Зафиксированно столкновение + contact.body1 = sphere.body; + contact.body2 = box.body; + contact.normal.vCopy(closestPt).vTransformBy4(box.transform).vSubtract(sPos).vNormalize().vReverse(); + contact.pcount = 1; + + var cp:ContactPoint = contact.points[0]; + cp.penetration = sphere.r - Math.sqrt(distSqr); + cp.pos.vCopy(contact.normal).vScale(-sphere.r).vAdd(sPos); + cp.r1.vDiff(cp.pos, sPos); + cp.r2.vDiff(cp.pos, bPos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/colliders/BoxTriangleCollider.as b/0.0.7.1/src/alternativa/physics/collision/colliders/BoxTriangleCollider.as new file mode 100644 index 0000000..5be3ae6 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/colliders/BoxTriangleCollider.as @@ -0,0 +1,725 @@ +package alternativa.physics.collision.colliders { + import __AS3__.vec.Vector; + + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionTriangle; + import alternativa.physics.math.Matrix4; + import alternativa.physics.math.Vector3; + + /** + * + */ + public class BoxTriangleCollider extends BoxCollider { + + public var epsilon:Number = 0.001; + + private var bestAxisIndex:int; + private var minOverlap:Number; + private var toBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var colNormal:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxTriangleCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, tri, toBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + if (!findEdgesIntersection(box, tri, toBox, bestAxisIndex%3, int(bestAxisIndex/3), contact)) return false; + } + + contact.body1 = box.body; + contact.body2 = tri.body; + + // Хак для танков, чтобы исключить утыкания танков в стыки статических примитивов + if (tri.transform.k > 0.9999) { + contact.normal.x = tri.transform.c; + contact.normal.y = tri.transform.g; + contact.normal.z = tri.transform.k; + } + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + toBox.x = boxTransform.d - triTransform.d; + toBox.y = boxTransform.h - triTransform.h; + toBox.z = boxTransform.l - triTransform.l; + + minOverlap = 1e308; + + // Сначала проверяется нормаль треугольника + axis.x = triTransform.c; + axis.y = triTransform.g; + axis.z = triTransform.k; + if (!testMainAxis(box, tri, axis, 0, toBox)) return false; + + // Проверка основных осей бокса + axis10.x = boxTransform.a; + axis10.y = boxTransform.e; + axis10.z = boxTransform.i; + if (!testMainAxis(box, tri, axis10, 1, toBox)) return false; + axis11.x = boxTransform.b; + axis11.y = boxTransform.f; + axis11.z = boxTransform.j; + if (!testMainAxis(box, tri, axis11, 2, toBox)) return false; + axis12.x = boxTransform.c; + axis12.y = boxTransform.g; + axis12.z = boxTransform.k; + if (!testMainAxis(box, tri, axis12, 3, toBox)) return false; + + // Проверка производных осей + // TODO: заменить вычисления векторных произведений инлайнами + var v:Vector3 = tri.e0; + axis20.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis20.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis20.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis20, 4, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis20, 5, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis20, 6, toBox)) return false; + + v = tri.e1; + axis21.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis21.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis21.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis21, 7, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis21, 8, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis21, 9, toBox)) return false; + + v = tri.e2; + axis22.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis22.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis22.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis22, 10, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis22, 11, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis22, 12, toBox)) return false; + + return true; + } + + /** + * Тестирует пересечение примитивов вдоль заданной оси. + * + * @param box бокс + * @param tri треугольник + * @param axis ось + * @param axisIndex индекс оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return true, если примитивы имеют перекрытие проекций вдоль указанной оси, иначе false + */ + private function testMainAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, axisIndex:int, toBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param tri + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, tri:CollisionTriangle, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Рассчитывает величину перекрытия проекций бокса и треугольника на заданную ось. + * + * @param box бокс + * @param tri треугольник + * @param axis единичный направляющий вектор оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return величина перекрытия. Положительное значение указывает на наличие перекрытия, нулевое или отрицательное значение указывает на отсутствие перекрытия. + */ + private function overlapOnAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, toBox:Vector3):Number { + var t:Matrix4 = box.transform; + var projection:Number = (t.a*axis.x + t.e*axis.y + t.i*axis.z)*box.hs.x; + if (projection < 0) projection = -projection; + var d:Number = (t.b*axis.x + t.f*axis.y + t.j*axis.z)*box.hs.y; + projection += d < 0 ? -d : d; + d = (t.c*axis.x + t.g*axis.y + t.k*axis.z)*box.hs.z; + projection += d < 0 ? -d : d; + + var vectorProjection:Number = toBox.x*axis.x + toBox.y*axis.y + toBox.z*axis.z; + // Для оптимизации ось преобразуется в систему треугольника, а не его вершины в мировую систему + t = tri.transform; + var ax:Number = t.a*axis.x + t.e*axis.y + t.i*axis.z; + var ay:Number = t.b*axis.x + t.f*axis.y + t.j*axis.z; + var az:Number = t.c*axis.x + t.g*axis.y + t.k*axis.z; + var max:Number = 0; + if (vectorProjection < 0) { + vectorProjection = -vectorProjection; + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d < max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d < max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d < max) max = d; + max = -max; + } else { + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d > max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d > max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d > max) max = d; + } + + return projection + max - vectorProjection; + } + + /** + * Определяет точки контакта бокса и треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function findFaceContactPoints(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIndex:int, contact:Contact):Boolean { + if (faceAxisIndex == 0) { + // Столкновение с плоскостью треугольника + return getBoxToTriContact(box, tri, toBox, contact); + } else { + // Столкновение с гранью бокса + return getTriToBoxContact(box, tri, toBox, faceAxisIndex, contact); + } + } + + /** + * Определяет точки контакта бокса с плоскостью треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getBoxToTriContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + colNormal.x = triTransform.c; + colNormal.y = triTransform.g; + colNormal.z = triTransform.k; + + var over:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + if (!over) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var incAxisDot:Number = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colNormal.x + axis.y*colNormal.y + axis.z*colNormal.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incAxisDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Обрезка грани + var negativeFace:Boolean = incAxisDot > 0; + + var code:int = 1 << (incFaceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + getFaceVertsByAxis(box.hs, incFaceAxisIdx, negativeFace, points1); + boxTransform.transformVectorsN(points1, points2, 4); + triTransform.transformVectorsInverseN(points2, points1, 4); + var pnum:int = clipByTriangle(tri); + // Среди конечного списка точек определяются лежащие под плоскостью треугольника + var cp:ContactPoint; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points2[i]; + if ((over && v.z < 0) || (!over && v.z > 0)) { + cp = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z + triTransform.d; + cpPos.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z + triTransform.h; + cpPos.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z + triTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = over ? -v.z : v.z; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Оперделяет точки контакта треугольника с гранью бокса. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getTriToBoxContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + faceAxisIdx--; + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + boxTransform.getAxis(faceAxisIdx, colNormal); + var negativeFace:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + + var code:int = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + if (!negativeFace) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + var v:Vector3 = points1[0]; + v.x = tri.v0.x; + v.y = tri.v0.y; + v.z = tri.v0.z; + + v = points1[1]; + v.x = tri.v1.x; + v.y = tri.v1.y; + v.z = tri.v1.z; + + v = points1[2]; + v.x = tri.v2.x; + v.y = tri.v2.y; + v.z = tri.v2.z; + + triTransform.transformVectorsN(points1, points2, 3); + boxTransform.transformVectorsInverseN(points2, points1, 3); + + var pnum:int = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность боксу и добавляем такие точки в список контактов + var penetration:Number; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + v = points1[i]; + penetration = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace); + if (penetration > -epsilon) { + var cp:ContactPoint = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = penetration; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Вычисляет величину проникновения точки в бокс. + * + * @param hs вектор половинных размеров бокса + * @param p точка в системе координат бокса + * @param axisIndex индекс оси + * @param negativeFace если true, проверяется нижняя грань + * @return величина проникновения точки в бокс + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, negativeFace:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (negativeFace) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (negativeFace) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (negativeFace) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, вершины которой заданы в списке points1. Результат сохраняется в этом же списке. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 3; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Обрезает грань прямоугольника сторонами треугольника. Обрезка выполняется в системе координат и в проекции на + * плоскость треугольника. Входные вершины грани в количестве четырёх штук должны находиться в списке points1. + * Конечные вершины сохраняются в списке points2. + * + * @param tri треугольник + * @return количество врешин в конечном списке + */ + private function clipByTriangle(tri:CollisionTriangle):int { + var vnum:int = 4; + vnum = clipByLine(tri.v0, tri.e0, points1, vnum, points2); + if (vnum == 0) return 0; + vnum = clipByLine(tri.v1, tri.e1, points2, vnum, points1); + if (vnum == 0) return 0; + return clipByLine(tri.v2, tri.e2, points1, vnum, points2);; + } + + /** + * Обрезает полигон указанной прямой. + * + * @param linePoint точка на прямой + * @param lineDir единичный направляющий вектор прямой + * @param verticesIn список вершин исходного полигона + * @param vnum количество вершин исходного полигона + * @param verticesOut список, куда будут записаны вершины конечного полигона + * @return количество вершин конечного полигона + */ + private function clipByLine(linePoint:Vector3, lineDir:Vector3, verticesIn:Vector., vnum:int, verticesOut:Vector.):int { + var nx:Number = -lineDir.y; + var ny:Number = lineDir.x; + var offset:Number = linePoint.x*nx + linePoint.y*ny; + var v1:Vector3 = verticesIn[int(vnum - 1)]; + var offset1:Number = v1.x*nx + v1.y*ny; + var t:Number; + var v:Vector3; + var num:int = 0; + for (var i:int = 0; i < vnum; i++) { + var v2:Vector3 = verticesIn[i]; + var offset2:Number = v2.x*nx + v2.y*ny; + if (offset1 < offset) { + // Первая точка ребра во внешней полуплоскости + if (offset2 > offset) { + // Вторая точка ребра во внутренней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } else { + // Первая точка ребра во внутренней полуплоскости. Добавляем её в конечный список. + v = verticesOut[num]; + v.x = v1.x; + v.y = v1.y; + v.z = v1.z; + num++; + if (offset2 < offset) { + // Вторая точка ребра во внешней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } + v1 = v2; + offset1 = offset2; + } + return num; + } + + /** + * + * @param box + * @param tri + * @param toBox + * @param boxAxisIdx + * @param triAxisIdx + * @param contact + */ + private function findEdgesIntersection(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, boxAxisIdx:int, triAxisIdx:int, contact:Contact):Boolean { + // Определение точки и направляющего вектора ребра треугольника + var tmpx1:Number; + var tmpy1:Number; + var tmpz1:Number; + var tmpx2:Number; + var tmpy2:Number; + var tmpz2:Number; + switch (triAxisIdx) { + case 0: + tmpx1 = tri.e0.x; + tmpy1 = tri.e0.y; + tmpz1 = tri.e0.z; + tmpx2 = tri.v0.x; + tmpy2 = tri.v0.y; + tmpz2 = tri.v0.z; + break; + case 1: + tmpx1 = tri.e1.x; + tmpy1 = tri.e1.y; + tmpz1 = tri.e1.z; + tmpx2 = tri.v1.x; + tmpy2 = tri.v1.y; + tmpz2 = tri.v1.z; + break; + case 2: + tmpx1 = tri.e2.x; + tmpy1 = tri.e2.y; + tmpz1 = tri.e2.z; + tmpx2 = tri.v2.x; + tmpy2 = tri.v2.y; + tmpz2 = tri.v2.z; + break; + } + var triTransform:Matrix4 = tri.transform; + axis20.x = triTransform.a*tmpx1 + triTransform.b*tmpy1 + triTransform.c*tmpz1; + axis20.y = triTransform.e*tmpx1 + triTransform.f*tmpy1 + triTransform.g*tmpz1; + axis20.z = triTransform.i*tmpx1 + triTransform.j*tmpy1 + triTransform.k*tmpz1; + var x2:Number = triTransform.a*tmpx2 + triTransform.b*tmpy2 + triTransform.c*tmpz2 + triTransform.d; + var y2:Number = triTransform.e*tmpx2 + triTransform.f*tmpy2 + triTransform.g*tmpz2 + triTransform.h; + var z2:Number = triTransform.i*tmpx2 + triTransform.j*tmpy2 + triTransform.k*tmpz2 + triTransform.l; + + // Определение нормали контакта, точки и направляющего вектора ребра бокса + var boxTransform:Matrix4 = box.transform; + boxTransform.getAxis(boxAxisIdx, axis10); + + // Нормаль контакта + var v:Vector3 = contact.normal; + v.x = axis10.y*axis20.z - axis10.z*axis20.y; + v.y = axis10.z*axis20.x - axis10.x*axis20.z; + v.z = axis10.x*axis20.y - axis10.y*axis20.x; + k = 1/Math.sqrt(v.x*v.x + v.y*v.y + v.z*v.z); + v.x *= k; + v.y *= k; + v.z *= k; + // Разворот нормали в сторону бокса + if (v.x*toBox.x + v.y*toBox.y + v.z*toBox.z < 0) { + v.x = -v.x; + v.y = -v.y; + v.z = -v.z; + } + + var boxHalfLen:Number; + tmpx1 = box.hs.x; + tmpy1 = box.hs.y; + tmpz1 = box.hs.z; + // X + if (boxAxisIdx == 0) { + tmpx1 = 0; + boxHalfLen = box.hs.x; + } else { + if (boxTransform.a*v.x + boxTransform.e*v.y + boxTransform.i*v.z > 0) { + tmpx1 = -tmpx1; + if ((box.excludedFaces & 2) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 1) != 0) { + return false; + } + } + } + // Y + if (boxAxisIdx == 1) { + tmpy1 = 0; + boxHalfLen = box.hs.y; + } else { + if (boxTransform.b*v.x + boxTransform.f*v.y + boxTransform.j*v.z > 0) { + tmpy1 = -tmpy1; + if ((box.excludedFaces & 8) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 4) != 0) { + return false; + } + } + } + // Z + if (boxAxisIdx == 2) { + tmpz1 = 0; + boxHalfLen = box.hs.z; + } else { + if (boxTransform.c*v.x + boxTransform.g*v.y + boxTransform.k*v.z > 0) { + tmpz1 = -tmpz1; + if ((box.excludedFaces & 32) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 16) != 0) { + return false; + } + } + } + var x1:Number = boxTransform.a*tmpx1 + boxTransform.b*tmpy1 + boxTransform.c*tmpz1 + boxTransform.d; + var y1:Number = boxTransform.e*tmpx1 + boxTransform.f*tmpy1 + boxTransform.g*tmpz1 + boxTransform.h; + var z1:Number = boxTransform.i*tmpx1 + boxTransform.j*tmpy1 + boxTransform.k*tmpz1 + boxTransform.l; + + // Находим точку пересечения рёбер, решая систему уравнений + // axis10 - направляющий вектор ребра бокса + // x1, y1, z1 - средняя точка ребра бокса + // axis20 - направляющий вектор ребра треугольника + // x2, y2, z2 - начальная точка ребра треугольника + var k:Number = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + var vx:Number = x2 - x1; + var vy:Number = y2 - y1; + var vz:Number = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + + // Запись данных о контакте + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + v = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + v.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + v.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + v.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = v.x - boxTransform.d; + r.y = v.y - boxTransform.h; + r.z = v.z - boxTransform.l; + r = cp.r2; + r.x = v.x - triTransform.d; + r.y = v.y - triTransform.h; + r.z = v.z - triTransform.l; + return true; + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/colliders/SpherePlaneCollider.as b/0.0.7.1/src/alternativa/physics/collision/colliders/SpherePlaneCollider.as new file mode 100644 index 0000000..1ac5d35 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/colliders/SpherePlaneCollider.as @@ -0,0 +1,68 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.math.Vector3; + + /** + * + */ + public class SpherePlaneCollider implements ICollider { + + private var normal:Vector3 = new Vector3(); + + /** + * + */ + public function SpherePlaneCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var sphere:RigidSphere = body1 as RigidSphere; +// var plane:RigidPlane; +// if (sphere == null) { +// sphere = body2 as RigidSphere; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// +// var dist:Number = sphere.state.pos.dot(normal) - offset; +// if (dist > sphere.r) return false; +// +// collisionInfo.body1 = sphere; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// collisionInfo.pcount = 1; +// +// var cp:ContactPoint = collisionInfo.points[0]; +// cp.penetration = sphere.r - dist; +// cp.pos.copy(normal).reverse().scale(sphere.r).add(sphere.state.pos); +// cp.r1.diff(cp.pos, sphere.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/colliders/SphereSphereCollider.as b/0.0.7.1/src/alternativa/physics/collision/colliders/SphereSphereCollider.as new file mode 100644 index 0000000..a90fabb --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/colliders/SphereSphereCollider.as @@ -0,0 +1,70 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.math.Vector3; + use namespace altphysics; + + public class SphereSphereCollider implements ICollider { + + private var p1:Vector3 = new Vector3(); + private var p2:Vector3 = new Vector3(); + + public function SphereSphereCollider() { + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var s1:CollisionSphere; + var s2:CollisionSphere; + if (prim1.body != null) { + s1 = prim1 as CollisionSphere; + s2 = prim2 as CollisionSphere; + } else { + s1 = prim2 as CollisionSphere; + s2 = prim1 as CollisionSphere; + } + + s1.transform.getAxis(3, p1); + s2.transform.getAxis(3, p2); + var dx:Number = p1.x - p2.x; + var dy:Number = p1.y - p2.y; + var dz:Number = p1.z - p2.z; + var len:Number = dx*dx + dy*dy + dz*dz; + var sum:Number = s1.r + s2.r; + if (len > sum*sum) return false; + len = Math.sqrt(len); + dx /= len; + dy /= len; + dz /= len; + + contact.body1 = s1.body; + contact.body2 = s2.body; + contact.normal.x = dx; + contact.normal.y = dy; + contact.normal.z = dz; + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = sum - len; + cp.pos.x = p1.x - dx*s1.r; + cp.pos.y = p1.y - dy*s1.r; + cp.pos.z = p1.z - dz*s1.r; + cp.r1.vDiff(cp.pos, p1); + cp.r2.vDiff(cp.pos, p2); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/primitives/.svn/all-wcprops b/0.0.7.1/src/alternativa/physics/collision/primitives/.svn/all-wcprops new file mode 100644 index 0000000..b09c7fa --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/primitives/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 124 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/collision/primitives +END +CollisionTriangle.as +K 25 +svn:wc:ra_dav:version-url +V 145 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/collision/primitives/CollisionTriangle.as +END +CollisionSphere.as +K 25 +svn:wc:ra_dav:version-url +V 143 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/collision/primitives/CollisionSphere.as +END +CollisionBox.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/collision/primitives/CollisionBox.as +END +CollisionRect.as +K 25 +svn:wc:ra_dav:version-url +V 141 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/collision/primitives/CollisionRect.as +END diff --git a/0.0.7.1/src/alternativa/physics/collision/primitives/.svn/entries b/0.0.7.1/src/alternativa/physics/collision/primitives/.svn/entries new file mode 100644 index 0000000..f7324cf --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/primitives/.svn/entries @@ -0,0 +1,76 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/collision/primitives +http://svndev.alternativaplatform.com + + + +2009-10-23T05:59:29.567666Z +22444 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +CollisionTriangle.as +file + + + + +2010-10-28T04:32:42.000000Z +f974b5617d3a5068378d72f540fd4290 +2009-10-19T07:17:33.113306Z +22253 +mike + +CollisionSphere.as +file + + + + +2010-10-28T04:32:42.000000Z +759c8a4e451e224d04fff0bc21d559f2 +2009-10-19T07:17:33.113306Z +22253 +mike + +CollisionBox.as +file + + + + +2010-10-28T04:32:42.000000Z +eaf5410bc4fb8c70444ca57e2a9f4c34 +2009-10-23T05:59:29.567666Z +22444 +mike + +CollisionRect.as +file + + + + +2010-10-28T04:32:42.000000Z +2e5629bb7cf350dd18485fe68675d0f0 +2009-10-19T07:17:33.113306Z +22253 +mike + diff --git a/0.0.7.1/src/alternativa/physics/collision/primitives/.svn/format b/0.0.7.1/src/alternativa/physics/collision/primitives/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/primitives/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.7.1/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base b/0.0.7.1/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base new file mode 100644 index 0000000..3d2f391 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base @@ -0,0 +1,204 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.math.Matrix4; + import alternativa.physics.math.Vector3; + use namespace altphysics; + + /** + * Ориентированный бокс. + */ + public class CollisionBox extends CollisionPrimitive { + + // Половинные размеры вдоль каждой из осей + public var hs:Vector3 = new Vector3(); + + public var excludedFaces:int; + + /** + * @param hs + * @param collisionGroup + */ + public function CollisionBox(hs:Vector3, collisionGroup:int) { + super(BOX, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + var t:Matrix4 = transform; + + var xx:Number; + var yy:Number; + var zz:Number; + + xx = t.a < 0 ? -t.a : t.a; + yy = t.b < 0 ? -t.b : t.b; + zz = t.c < 0 ? -t.c : t.c; + aabb.maxX = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minX = -aabb.maxX; + + xx = t.e < 0 ? -t.e : t.e; + yy = t.f < 0 ? -t.f : t.f; + zz = t.g < 0 ? -t.g : t.g; + aabb.maxY = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minY = -aabb.maxY; + + xx = t.i < 0 ? -t.i : t.i; + yy = t.j < 0 ? -t.j : t.j; + zz = t.k < 0 ? -t.k : t.k; + aabb.maxZ = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var box:CollisionBox = source as CollisionBox; + if (box == null) return this; + super.copyFrom(box); + hs.vCopy(box.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionBox(hs, collisionGroup); + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + var tMin:Number = -1; + var tMax:Number = 1e308; + var t1:Number; + var t2:Number; + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + // X + if (vx < epsilon && vx > -epsilon) { + if (ox < -hs.x || ox > hs.x) return -1; + } else { + t1 = (-hs.x - ox)/vx; + t2 = (hs.x - ox)/vx; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.x = -1; + normal.y = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.x = 1; + normal.y = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Y + if (vy < epsilon && vy > -epsilon) { + if (oy < -hs.y || oy > hs.y) return -1; + } else { + t1 = (-hs.y - oy)/vy; + t2 = (hs.y - oy)/vy; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.y = -1; + normal.x = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.y = 1; + normal.x = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Z + if (vz < epsilon && vz > -epsilon) { + if (oz < -hs.z || oz > hs.z) return -1; + } else { + t1 = (-hs.z - oz)/vz; + t2 = (hs.z - oz)/vz; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.z = -1; + normal.x = normal.y = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.z = 1; + normal.x = normal.y = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Перевод нормали в мировую систему координат + // Inlined normal.vDeltaTransformBy4(transform); + vx = normal.x; + vy = normal.y; + vz = normal.z; + normal.x = transform.a*vx + transform.b*vy + transform.c*vz; + normal.y = transform.e*vx + transform.f*vy + transform.g*vz; + normal.z = transform.i*vx + transform.j*vy + transform.k*vz; + + return tMin; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionBox hs=" + hs + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base b/0.0.7.1/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base new file mode 100644 index 0000000..08001ad --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base @@ -0,0 +1,143 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.math.Matrix4; + import alternativa.physics.math.Vector3; + use namespace altphysics; + + /** + * Ориентированный прямоугольник. Задаётся половинами размеров вдоль осей X (ширина) и Y (длина) локальной системы + * координат примитива. Таким образом, прямоугольник лежит в плоскости XY, его стороны параллельны осям этой + * плоскости, а нормаль направлена вдоль локальной оси Z. + * + * Прямоугольник может быть одно- или двусторонним. В случае одностороннего прямоугольника, + * столкновения с внутренней стороны не регистрируются. + */ + public class CollisionRect extends CollisionPrimitive { + // Половинные размеры прямоугольника вдоль осей X и Y. Нормаль направлена вдоль оси Z. + public var hs:Vector3 = new Vector3(); + // Флаг указывает, является примитив одно- или двусторонним + public var twoSided:Boolean = true; + + // Малое значение. Используется для указания фиктивной высоты примитива и в функции определения пересечения с сегментом. + private static const EPSILON:Number = 0.005; + + /** + * Создаёт новый экземпляр примитива. + * + * @param hs половинные размерв прямоугольника вдоль осей X и Y. Значение z игнорируется. + * @param collisionGroup группа примитива + */ + public function CollisionRect(hs:Vector3, collisionGroup:int) { + super(RECT, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * Расчитывает ограничивающий бокс прямоугольника. Для избежания проблем высота примитива принимается равной + * не нулю, а малому значению. + * + * @return + */ + override public function calculateAABB():BoundBox { + // Баунд бокс прямоугольника имеет минимальную высоту, отличную от нуля во избежание проблем с построением kd-дерева + var t:Matrix4 = transform; + var xx:Number = t.a < 0 ? -t.a : t.a; + var yy:Number = t.b < 0 ? -t.b : t.b; + var zz:Number = t.c < 0 ? -t.c : t.c; + aabb.maxX = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minX = -aabb.maxX; + + xx = t.e < 0 ? -t.e : t.e; + yy = t.f < 0 ? -t.f : t.f; + zz = t.g < 0 ? -t.g : t.g; + aabb.maxY = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minY = -aabb.maxY; + + xx = t.i < 0 ? -t.i : t.i; + yy = t.j < 0 ? -t.j : t.j; + zz = t.k < 0 ? -t.k : t.k; + aabb.maxZ = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var rect:CollisionRect = source as CollisionRect; + if (rect == null) return this; + super.copyFrom(rect); + hs.vCopy(rect.hs); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionRect hs=" + hs + "]"; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionRect(hs, collisionGroup); + } + + /** + * @param origin + * @param vector + * @param collisionGroup + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + + // Проверка параллельности сегмента и плоскости примитива + if (vz > -threshold && vz < threshold) return -1; + var t:Number = -oz/vz; + if (t < 0) return -1; + // Проверка вхождения точки пересечения в прямоугольник + ox += vx*t; + oy += vy*t; + oz = 0; + if (ox < (-hs.x - threshold) || ox > (hs.x + threshold) || oy < (-hs.y - threshold) || oy > (hs.y + threshold)) return -1; + + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + return t; + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base b/0.0.7.1/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base new file mode 100644 index 0000000..887a19a --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base @@ -0,0 +1,81 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.math.Vector3; + use namespace altphysics; + + + /** + * Сфера. + */ + public class CollisionSphere extends CollisionPrimitive { + + // Радиус сферы + public var r:Number = 0; + + /** + * + * @param r + * @param collisionGroup + */ + public function CollisionSphere(r:Number, collisionGroup:int) { + super(SPHERE, collisionGroup); + this.r = r; + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + aabb.maxX = transform.d + r; + aabb.minX = transform.d - r; + + aabb.maxY = transform.h + r; + aabb.minY = transform.h - r; + + aabb.maxZ = transform.l + r; + aabb.minZ = transform.l - r; + + return aabb; + } + + /** + * @param origin + * @param vector + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var px:Number = origin.x - transform.d; + var py:Number = origin.y - transform.h; + var pz:Number = origin.z - transform.l; + var k:Number = vector.x*px + vector.y*py + vector.z*pz; + if (k > 0) return -1; + var a:Number = vector.x*vector.x + vector.y*vector.y + vector.z*vector.z; + var D:Number = k*k - a*(px*px + py*py + pz*pz - r*r); + if (D < 0) return -1; + return -(k + Math.sqrt(D))/a; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var sphere:CollisionSphere = source as CollisionSphere; + if (sphere == null) return this; + super.copyFrom(sphere); + r = sphere.r; + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionSphere(r, collisionGroup); + } + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base b/0.0.7.1/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base new file mode 100644 index 0000000..9cd6d00 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base @@ -0,0 +1,244 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.math.Vector3; + use namespace altphysics; + + /** + * Примитив треугольник. + */ + public class CollisionTriangle extends CollisionPrimitive { + + public var v0:Vector3 = new Vector3(); + public var v1:Vector3 = new Vector3(); + public var v2:Vector3 = new Vector3(); + + public var e0:Vector3 = new Vector3(); + public var e1:Vector3 = new Vector3(); + public var e2:Vector3 = new Vector3(); + +// public var len0:Number; +// public var len1:Number; +// public var len2:Number; +// + /** + * + * @param v0 + * @param v1 + * @param v2 + * @param collisionGroup + */ + public function CollisionTriangle(v0:Vector3, v1:Vector3, v2:Vector3, collisionGroup:int) { + super(TRIANGLE, collisionGroup); + initVertices(v0, v1, v2); + } + + /** + * Рассчитывает AABB примитива. + * + * @return ссылка на свой AABB + */ + override public function calculateAABB():BoundBox { + var epsilon:Number = 0.005; + var a:Number; + var b:Number; + var eps_c:Number = epsilon*transform.c; + var eps_g:Number = epsilon*transform.g; + var eps_k:Number = epsilon*transform.k; + + // Вершина 0 + // Ось X + a = v0.x*transform.a + v0.y*transform.b; + aabb.minX = aabb.maxX = a + eps_c; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v0.x*transform.e + v0.y*transform.f; + aabb.minY = aabb.maxY = a + eps_g; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v0.x*transform.i + v0.y*transform.j; + aabb.minZ = aabb.maxZ = a + eps_k; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 1 + // Ось X + a = v1.x*transform.a + v1.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v1.x*transform.e + v1.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v1.x*transform.i + v1.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 2 + // Ось X + a = v2.x*transform.a + v2.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v2.x*transform.e + v2.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v2.x*transform.i + v2.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + aabb.minX += transform.d; + aabb.maxX += transform.d; + + aabb.minY += transform.h; + aabb.maxY += transform.h; + + aabb.minZ += transform.l; + aabb.maxZ += transform.l; + + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + // Луч трансформируется в систему координат примитива, затем проверяется пересечение + + var vz:Number = vector.x*transform.c + vector.y*transform.g + vector.z*transform.k; + // Если луч параллелен плоскости птимитива, то пересечения нет + if (vz < epsilon && vz > -epsilon) return -1; + + var tx:Number = origin.x - transform.d; + var ty:Number = origin.y - transform.h; + var tz:Number = origin.z - transform.l; + + var oz:Number = tx*transform.c + ty*transform.g + tz*transform.k; + var t:Number = -oz/vz; + if (t < 0) return -1; + + var ox:Number = tx*transform.a + ty*transform.e + tz*transform.i; + var oy:Number = tx*transform.b + ty*transform.f + tz*transform.j; + + tx = ox + t*(vector.x*transform.a + vector.y*transform.e + vector.z*transform.i); + ty = oy + t*(vector.x*transform.b + vector.y*transform.f + vector.z*transform.j); + tz = oz + t*vz; + + // Проверка вхождения точки в треугольник + if ((e0.x*(ty - v0.y) - e0.y*(tx - v0.x) < 0) || (e1.x*(ty - v1.y) - e1.y*(tx - v1.x) < 0) || (e2.x*(ty - v2.y) - e2.y*(tx - v2.x) < 0)) return -1; + + // Запись нормали + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + + return t; + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + super.copyFrom(source); + var tri:CollisionTriangle = source as CollisionTriangle; + if (tri != null) { + v0.vCopy(tri.v0); + v1.vCopy(tri.v1); + v2.vCopy(tri.v2); + + e0.vCopy(tri.e0); + e1.vCopy(tri.e1); + e2.vCopy(tri.e2); + +// len0 = tri.len0; +// len1 = tri.len1; +// len2 = tri.len2; + } + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionTriangle v0=" + v0 + ", v1=" + v1 + ", v2=" + v2 + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionTriangle(v0, v1, v2, collisionGroup); + } + + /** + * + * @param v0 + * @param v1 + * @param v2 + */ + private function initVertices(v0:Vector3, v1:Vector3, v2:Vector3):void { + this.v0.vCopy(v0); + this.v1.vCopy(v1); + this.v2.vCopy(v2); + + e0.vDiff(v1, v0); +// len0 = e0.vLength(); + e0.vNormalize(); + + e1.vDiff(v2, v1); +// len1 = e1.vLength(); + e1.vNormalize(); + + e2.vDiff(v0, v2); +// len2 = e2.vLength(); + e2.vNormalize(); + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/primitives/CollisionBox.as b/0.0.7.1/src/alternativa/physics/collision/primitives/CollisionBox.as new file mode 100644 index 0000000..3d2f391 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/primitives/CollisionBox.as @@ -0,0 +1,204 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.math.Matrix4; + import alternativa.physics.math.Vector3; + use namespace altphysics; + + /** + * Ориентированный бокс. + */ + public class CollisionBox extends CollisionPrimitive { + + // Половинные размеры вдоль каждой из осей + public var hs:Vector3 = new Vector3(); + + public var excludedFaces:int; + + /** + * @param hs + * @param collisionGroup + */ + public function CollisionBox(hs:Vector3, collisionGroup:int) { + super(BOX, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + var t:Matrix4 = transform; + + var xx:Number; + var yy:Number; + var zz:Number; + + xx = t.a < 0 ? -t.a : t.a; + yy = t.b < 0 ? -t.b : t.b; + zz = t.c < 0 ? -t.c : t.c; + aabb.maxX = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minX = -aabb.maxX; + + xx = t.e < 0 ? -t.e : t.e; + yy = t.f < 0 ? -t.f : t.f; + zz = t.g < 0 ? -t.g : t.g; + aabb.maxY = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minY = -aabb.maxY; + + xx = t.i < 0 ? -t.i : t.i; + yy = t.j < 0 ? -t.j : t.j; + zz = t.k < 0 ? -t.k : t.k; + aabb.maxZ = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var box:CollisionBox = source as CollisionBox; + if (box == null) return this; + super.copyFrom(box); + hs.vCopy(box.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionBox(hs, collisionGroup); + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + var tMin:Number = -1; + var tMax:Number = 1e308; + var t1:Number; + var t2:Number; + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + // X + if (vx < epsilon && vx > -epsilon) { + if (ox < -hs.x || ox > hs.x) return -1; + } else { + t1 = (-hs.x - ox)/vx; + t2 = (hs.x - ox)/vx; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.x = -1; + normal.y = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.x = 1; + normal.y = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Y + if (vy < epsilon && vy > -epsilon) { + if (oy < -hs.y || oy > hs.y) return -1; + } else { + t1 = (-hs.y - oy)/vy; + t2 = (hs.y - oy)/vy; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.y = -1; + normal.x = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.y = 1; + normal.x = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Z + if (vz < epsilon && vz > -epsilon) { + if (oz < -hs.z || oz > hs.z) return -1; + } else { + t1 = (-hs.z - oz)/vz; + t2 = (hs.z - oz)/vz; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.z = -1; + normal.x = normal.y = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.z = 1; + normal.x = normal.y = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Перевод нормали в мировую систему координат + // Inlined normal.vDeltaTransformBy4(transform); + vx = normal.x; + vy = normal.y; + vz = normal.z; + normal.x = transform.a*vx + transform.b*vy + transform.c*vz; + normal.y = transform.e*vx + transform.f*vy + transform.g*vz; + normal.z = transform.i*vx + transform.j*vy + transform.k*vz; + + return tMin; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionBox hs=" + hs + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/primitives/CollisionRect.as b/0.0.7.1/src/alternativa/physics/collision/primitives/CollisionRect.as new file mode 100644 index 0000000..08001ad --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/primitives/CollisionRect.as @@ -0,0 +1,143 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.math.Matrix4; + import alternativa.physics.math.Vector3; + use namespace altphysics; + + /** + * Ориентированный прямоугольник. Задаётся половинами размеров вдоль осей X (ширина) и Y (длина) локальной системы + * координат примитива. Таким образом, прямоугольник лежит в плоскости XY, его стороны параллельны осям этой + * плоскости, а нормаль направлена вдоль локальной оси Z. + * + * Прямоугольник может быть одно- или двусторонним. В случае одностороннего прямоугольника, + * столкновения с внутренней стороны не регистрируются. + */ + public class CollisionRect extends CollisionPrimitive { + // Половинные размеры прямоугольника вдоль осей X и Y. Нормаль направлена вдоль оси Z. + public var hs:Vector3 = new Vector3(); + // Флаг указывает, является примитив одно- или двусторонним + public var twoSided:Boolean = true; + + // Малое значение. Используется для указания фиктивной высоты примитива и в функции определения пересечения с сегментом. + private static const EPSILON:Number = 0.005; + + /** + * Создаёт новый экземпляр примитива. + * + * @param hs половинные размерв прямоугольника вдоль осей X и Y. Значение z игнорируется. + * @param collisionGroup группа примитива + */ + public function CollisionRect(hs:Vector3, collisionGroup:int) { + super(RECT, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * Расчитывает ограничивающий бокс прямоугольника. Для избежания проблем высота примитива принимается равной + * не нулю, а малому значению. + * + * @return + */ + override public function calculateAABB():BoundBox { + // Баунд бокс прямоугольника имеет минимальную высоту, отличную от нуля во избежание проблем с построением kd-дерева + var t:Matrix4 = transform; + var xx:Number = t.a < 0 ? -t.a : t.a; + var yy:Number = t.b < 0 ? -t.b : t.b; + var zz:Number = t.c < 0 ? -t.c : t.c; + aabb.maxX = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minX = -aabb.maxX; + + xx = t.e < 0 ? -t.e : t.e; + yy = t.f < 0 ? -t.f : t.f; + zz = t.g < 0 ? -t.g : t.g; + aabb.maxY = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minY = -aabb.maxY; + + xx = t.i < 0 ? -t.i : t.i; + yy = t.j < 0 ? -t.j : t.j; + zz = t.k < 0 ? -t.k : t.k; + aabb.maxZ = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var rect:CollisionRect = source as CollisionRect; + if (rect == null) return this; + super.copyFrom(rect); + hs.vCopy(rect.hs); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionRect hs=" + hs + "]"; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionRect(hs, collisionGroup); + } + + /** + * @param origin + * @param vector + * @param collisionGroup + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + + // Проверка параллельности сегмента и плоскости примитива + if (vz > -threshold && vz < threshold) return -1; + var t:Number = -oz/vz; + if (t < 0) return -1; + // Проверка вхождения точки пересечения в прямоугольник + ox += vx*t; + oy += vy*t; + oz = 0; + if (ox < (-hs.x - threshold) || ox > (hs.x + threshold) || oy < (-hs.y - threshold) || oy > (hs.y + threshold)) return -1; + + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + return t; + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/primitives/CollisionSphere.as b/0.0.7.1/src/alternativa/physics/collision/primitives/CollisionSphere.as new file mode 100644 index 0000000..887a19a --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/primitives/CollisionSphere.as @@ -0,0 +1,81 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.math.Vector3; + use namespace altphysics; + + + /** + * Сфера. + */ + public class CollisionSphere extends CollisionPrimitive { + + // Радиус сферы + public var r:Number = 0; + + /** + * + * @param r + * @param collisionGroup + */ + public function CollisionSphere(r:Number, collisionGroup:int) { + super(SPHERE, collisionGroup); + this.r = r; + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + aabb.maxX = transform.d + r; + aabb.minX = transform.d - r; + + aabb.maxY = transform.h + r; + aabb.minY = transform.h - r; + + aabb.maxZ = transform.l + r; + aabb.minZ = transform.l - r; + + return aabb; + } + + /** + * @param origin + * @param vector + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var px:Number = origin.x - transform.d; + var py:Number = origin.y - transform.h; + var pz:Number = origin.z - transform.l; + var k:Number = vector.x*px + vector.y*py + vector.z*pz; + if (k > 0) return -1; + var a:Number = vector.x*vector.x + vector.y*vector.y + vector.z*vector.z; + var D:Number = k*k - a*(px*px + py*py + pz*pz - r*r); + if (D < 0) return -1; + return -(k + Math.sqrt(D))/a; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var sphere:CollisionSphere = source as CollisionSphere; + if (sphere == null) return this; + super.copyFrom(sphere); + r = sphere.r; + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionSphere(r, collisionGroup); + } + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/primitives/CollisionTriangle.as b/0.0.7.1/src/alternativa/physics/collision/primitives/CollisionTriangle.as new file mode 100644 index 0000000..9cd6d00 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/primitives/CollisionTriangle.as @@ -0,0 +1,244 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.math.Vector3; + use namespace altphysics; + + /** + * Примитив треугольник. + */ + public class CollisionTriangle extends CollisionPrimitive { + + public var v0:Vector3 = new Vector3(); + public var v1:Vector3 = new Vector3(); + public var v2:Vector3 = new Vector3(); + + public var e0:Vector3 = new Vector3(); + public var e1:Vector3 = new Vector3(); + public var e2:Vector3 = new Vector3(); + +// public var len0:Number; +// public var len1:Number; +// public var len2:Number; +// + /** + * + * @param v0 + * @param v1 + * @param v2 + * @param collisionGroup + */ + public function CollisionTriangle(v0:Vector3, v1:Vector3, v2:Vector3, collisionGroup:int) { + super(TRIANGLE, collisionGroup); + initVertices(v0, v1, v2); + } + + /** + * Рассчитывает AABB примитива. + * + * @return ссылка на свой AABB + */ + override public function calculateAABB():BoundBox { + var epsilon:Number = 0.005; + var a:Number; + var b:Number; + var eps_c:Number = epsilon*transform.c; + var eps_g:Number = epsilon*transform.g; + var eps_k:Number = epsilon*transform.k; + + // Вершина 0 + // Ось X + a = v0.x*transform.a + v0.y*transform.b; + aabb.minX = aabb.maxX = a + eps_c; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v0.x*transform.e + v0.y*transform.f; + aabb.minY = aabb.maxY = a + eps_g; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v0.x*transform.i + v0.y*transform.j; + aabb.minZ = aabb.maxZ = a + eps_k; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 1 + // Ось X + a = v1.x*transform.a + v1.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v1.x*transform.e + v1.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v1.x*transform.i + v1.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 2 + // Ось X + a = v2.x*transform.a + v2.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v2.x*transform.e + v2.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v2.x*transform.i + v2.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + aabb.minX += transform.d; + aabb.maxX += transform.d; + + aabb.minY += transform.h; + aabb.maxY += transform.h; + + aabb.minZ += transform.l; + aabb.maxZ += transform.l; + + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + // Луч трансформируется в систему координат примитива, затем проверяется пересечение + + var vz:Number = vector.x*transform.c + vector.y*transform.g + vector.z*transform.k; + // Если луч параллелен плоскости птимитива, то пересечения нет + if (vz < epsilon && vz > -epsilon) return -1; + + var tx:Number = origin.x - transform.d; + var ty:Number = origin.y - transform.h; + var tz:Number = origin.z - transform.l; + + var oz:Number = tx*transform.c + ty*transform.g + tz*transform.k; + var t:Number = -oz/vz; + if (t < 0) return -1; + + var ox:Number = tx*transform.a + ty*transform.e + tz*transform.i; + var oy:Number = tx*transform.b + ty*transform.f + tz*transform.j; + + tx = ox + t*(vector.x*transform.a + vector.y*transform.e + vector.z*transform.i); + ty = oy + t*(vector.x*transform.b + vector.y*transform.f + vector.z*transform.j); + tz = oz + t*vz; + + // Проверка вхождения точки в треугольник + if ((e0.x*(ty - v0.y) - e0.y*(tx - v0.x) < 0) || (e1.x*(ty - v1.y) - e1.y*(tx - v1.x) < 0) || (e2.x*(ty - v2.y) - e2.y*(tx - v2.x) < 0)) return -1; + + // Запись нормали + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + + return t; + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + super.copyFrom(source); + var tri:CollisionTriangle = source as CollisionTriangle; + if (tri != null) { + v0.vCopy(tri.v0); + v1.vCopy(tri.v1); + v2.vCopy(tri.v2); + + e0.vCopy(tri.e0); + e1.vCopy(tri.e1); + e2.vCopy(tri.e2); + +// len0 = tri.len0; +// len1 = tri.len1; +// len2 = tri.len2; + } + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionTriangle v0=" + v0 + ", v1=" + v1 + ", v2=" + v2 + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionTriangle(v0, v1, v2, collisionGroup); + } + + /** + * + * @param v0 + * @param v1 + * @param v2 + */ + private function initVertices(v0:Vector3, v1:Vector3, v2:Vector3):void { + this.v0.vCopy(v0); + this.v1.vCopy(v1); + this.v2.vCopy(v2); + + e0.vDiff(v1, v0); +// len0 = e0.vLength(); + e0.vNormalize(); + + e1.vDiff(v2, v1); +// len1 = e1.vLength(); + e1.vNormalize(); + + e2.vDiff(v0, v2); +// len2 = e2.vLength(); + e2.vNormalize(); + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/types/.svn/all-wcprops b/0.0.7.1/src/alternativa/physics/collision/types/.svn/all-wcprops new file mode 100644 index 0000000..4fcf580 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/types/.svn/all-wcprops @@ -0,0 +1,23 @@ +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/collision/types +END +BoundBox.as +K 25 +svn:wc:ra_dav:version-url +V 131 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/collision/types/BoundBox.as +END +RayIntersection.as +K 25 +svn:wc:ra_dav:version-url +V 138 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/collision/types/RayIntersection.as +END +Ray.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/collision/types/Ray.as +END diff --git a/0.0.7.1/src/alternativa/physics/collision/types/.svn/entries b/0.0.7.1/src/alternativa/physics/collision/types/.svn/entries new file mode 100644 index 0000000..3a353f2 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/types/.svn/entries @@ -0,0 +1,64 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/collision/types +http://svndev.alternativaplatform.com + + + +2009-10-19T07:17:33.113306Z +22253 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +BoundBox.as +file + + + + +2010-10-28T04:32:42.000000Z +d92d4ab3dde698feb674fb9488f59678 +2009-09-09T14:02:48.134744Z +19633 +mike + +RayIntersection.as +file + + + + +2010-10-28T04:32:42.000000Z +4002cb4f3effa05aedf5c38dfb3ade7c +2009-10-19T07:17:33.113306Z +22253 +mike + +Ray.as +file + + + + +2010-10-28T04:32:42.000000Z +77598eacffac4c232aeeaaf83f4b27e5 +2009-10-19T07:17:33.113306Z +22253 +mike + diff --git a/0.0.7.1/src/alternativa/physics/collision/types/.svn/format b/0.0.7.1/src/alternativa/physics/collision/types/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/types/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.7.1/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base b/0.0.7.1/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base new file mode 100644 index 0000000..b4d5278 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base @@ -0,0 +1,75 @@ +package alternativa.physics.collision.types { + + /** + * Ограничивающий параллелепипед. + */ + public class BoundBox { + + public var minX:Number = 1e308; + public var minY:Number = 1e308; + public var minZ:Number = 1e308; + public var maxX:Number = -1e308; + public var maxY:Number = -1e308; + public var maxZ:Number = -1e308; + + public function setSize(minX:Number, minY:Number, minZ:Number, maxX:Number, maxY:Number, maxZ:Number):void { + this.minX = minX; + this.minY = minY; + this.minZ = minZ; + this.maxX = maxX; + this.maxY = maxY; + this.maxZ = maxZ; + } + + public function addBoundBox(boundBox:BoundBox):void { + minX = (boundBox.minX < minX) ? boundBox.minX : minX; + minY = (boundBox.minY < minY) ? boundBox.minY : minY; + minZ = (boundBox.minZ < minZ) ? boundBox.minZ : minZ; + maxX = (boundBox.maxX > maxX) ? boundBox.maxX : maxX; + maxY = (boundBox.maxY > maxY) ? boundBox.maxY : maxY; + maxZ = (boundBox.maxZ > maxZ) ? boundBox.maxZ : maxZ; + } + + public function addPoint(x:Number, y:Number, z:Number):void { + if (x < minX) minX = x; + if (x > maxX) maxX = x; + if (y < minY) minY = y; + if (y > maxY) maxY = y; + if (z < minZ) minZ = z; + if (z > maxZ) maxZ = z; + } + + public function infinity():void { + minX = 1e308; + minY = 1e308; + minZ = 1e308; + maxX = -1e308; + maxY = -1e308; + maxZ = -1e308; + } + + public function intersects(bb:BoundBox, epsilon:Number):Boolean { + return !(minX > bb.maxX + epsilon || maxX < bb.minX - epsilon || minY > bb.maxY + epsilon || maxY < bb.minY - epsilon || minZ > bb.maxZ + epsilon || maxZ < bb.minZ - epsilon); + } + + public function copyFrom(boundBox:BoundBox):void { + minX = boundBox.minX; + minY = boundBox.minY; + minZ = boundBox.minZ; + maxX = boundBox.maxX; + maxY = boundBox.maxY; + maxZ = boundBox.maxZ; + } + + public function clone():BoundBox { + var clone:BoundBox = new BoundBox(); + clone.copyFrom(this); + return clone; + } + + public function toString():String { + return "BoundBox [" + minX + ", " + minY + ", " + minZ + " : " + maxX + ", " + maxY + ", " + maxZ + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base b/0.0.7.1/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base new file mode 100644 index 0000000..01c48e3 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base @@ -0,0 +1,13 @@ +package alternativa.physics.collision.types { + import alternativa.physics.math.Vector3; + + /** + * Структура, описывающая луч и сегмент прямой в пространстве. + */ + public class Ray { + // Начало луча. Начальная точка сегмента на прямой луча. + public var origin:Vector3 = new Vector3(); + // Направляющий вектор луча. Длина вектора задаёт длину сегмента на луче. Конечная точка сегмента вычисляется как origin + dir. + public var dir:Vector3 = new Vector3(); + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base b/0.0.7.1/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base new file mode 100644 index 0000000..cbf3607 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base @@ -0,0 +1,25 @@ +package alternativa.physics.collision.types { + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.math.Vector3; + + /** + * Структура описывает точку пересечения луча с физической геометрией. + */ + public class RayIntersection { + // Физический примитив, с которым пересекается луч + public var primitive:CollisionPrimitive; + // Положенеи точки пересечения + public var pos:Vector3 = new Vector3(); + // Нормаль поверхности физического примитива в точке пересечения + public var normal:Vector3 = new Vector3(); + // Время пересечения (|pos - ray.origin|/|ray.dir|) + public var t:Number = 0; + + public function copy(source:RayIntersection):void { + primitive = source.primitive; + pos.vCopy(source.pos); + normal.vCopy(source.normal); + t = source.t; + } + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/types/BoundBox.as b/0.0.7.1/src/alternativa/physics/collision/types/BoundBox.as new file mode 100644 index 0000000..b4d5278 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/types/BoundBox.as @@ -0,0 +1,75 @@ +package alternativa.physics.collision.types { + + /** + * Ограничивающий параллелепипед. + */ + public class BoundBox { + + public var minX:Number = 1e308; + public var minY:Number = 1e308; + public var minZ:Number = 1e308; + public var maxX:Number = -1e308; + public var maxY:Number = -1e308; + public var maxZ:Number = -1e308; + + public function setSize(minX:Number, minY:Number, minZ:Number, maxX:Number, maxY:Number, maxZ:Number):void { + this.minX = minX; + this.minY = minY; + this.minZ = minZ; + this.maxX = maxX; + this.maxY = maxY; + this.maxZ = maxZ; + } + + public function addBoundBox(boundBox:BoundBox):void { + minX = (boundBox.minX < minX) ? boundBox.minX : minX; + minY = (boundBox.minY < minY) ? boundBox.minY : minY; + minZ = (boundBox.minZ < minZ) ? boundBox.minZ : minZ; + maxX = (boundBox.maxX > maxX) ? boundBox.maxX : maxX; + maxY = (boundBox.maxY > maxY) ? boundBox.maxY : maxY; + maxZ = (boundBox.maxZ > maxZ) ? boundBox.maxZ : maxZ; + } + + public function addPoint(x:Number, y:Number, z:Number):void { + if (x < minX) minX = x; + if (x > maxX) maxX = x; + if (y < minY) minY = y; + if (y > maxY) maxY = y; + if (z < minZ) minZ = z; + if (z > maxZ) maxZ = z; + } + + public function infinity():void { + minX = 1e308; + minY = 1e308; + minZ = 1e308; + maxX = -1e308; + maxY = -1e308; + maxZ = -1e308; + } + + public function intersects(bb:BoundBox, epsilon:Number):Boolean { + return !(minX > bb.maxX + epsilon || maxX < bb.minX - epsilon || minY > bb.maxY + epsilon || maxY < bb.minY - epsilon || minZ > bb.maxZ + epsilon || maxZ < bb.minZ - epsilon); + } + + public function copyFrom(boundBox:BoundBox):void { + minX = boundBox.minX; + minY = boundBox.minY; + minZ = boundBox.minZ; + maxX = boundBox.maxX; + maxY = boundBox.maxY; + maxZ = boundBox.maxZ; + } + + public function clone():BoundBox { + var clone:BoundBox = new BoundBox(); + clone.copyFrom(this); + return clone; + } + + public function toString():String { + return "BoundBox [" + minX + ", " + minY + ", " + minZ + " : " + maxX + ", " + maxY + ", " + maxZ + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/types/Ray.as b/0.0.7.1/src/alternativa/physics/collision/types/Ray.as new file mode 100644 index 0000000..01c48e3 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/types/Ray.as @@ -0,0 +1,13 @@ +package alternativa.physics.collision.types { + import alternativa.physics.math.Vector3; + + /** + * Структура, описывающая луч и сегмент прямой в пространстве. + */ + public class Ray { + // Начало луча. Начальная точка сегмента на прямой луча. + public var origin:Vector3 = new Vector3(); + // Направляющий вектор луча. Длина вектора задаёт длину сегмента на луче. Конечная точка сегмента вычисляется как origin + dir. + public var dir:Vector3 = new Vector3(); + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/collision/types/RayIntersection.as b/0.0.7.1/src/alternativa/physics/collision/types/RayIntersection.as new file mode 100644 index 0000000..cbf3607 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/collision/types/RayIntersection.as @@ -0,0 +1,25 @@ +package alternativa.physics.collision.types { + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.math.Vector3; + + /** + * Структура описывает точку пересечения луча с физической геометрией. + */ + public class RayIntersection { + // Физический примитив, с которым пересекается луч + public var primitive:CollisionPrimitive; + // Положенеи точки пересечения + public var pos:Vector3 = new Vector3(); + // Нормаль поверхности физического примитива в точке пересечения + public var normal:Vector3 = new Vector3(); + // Время пересечения (|pos - ray.origin|/|ray.dir|) + public var t:Number = 0; + + public function copy(source:RayIntersection):void { + primitive = source.primitive; + pos.vCopy(source.pos); + normal.vCopy(source.normal); + t = source.t; + } + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/constraints/.svn/all-wcprops b/0.0.7.1/src/alternativa/physics/constraints/.svn/all-wcprops new file mode 100644 index 0000000..960545e --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/constraints/.svn/all-wcprops @@ -0,0 +1,17 @@ +K 25 +svn:wc:ra_dav:version-url +V 115 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/constraints +END +Constraint.as +K 25 +svn:wc:ra_dav:version-url +V 129 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/constraints/Constraint.as +END +MaxDistanceConstraint.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/constraints/MaxDistanceConstraint.as +END diff --git a/0.0.7.1/src/alternativa/physics/constraints/.svn/entries b/0.0.7.1/src/alternativa/physics/constraints/.svn/entries new file mode 100644 index 0000000..6765842 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/constraints/.svn/entries @@ -0,0 +1,52 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/constraints +http://svndev.alternativaplatform.com + + + +2009-10-19T07:17:33.113306Z +22253 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Constraint.as +file + + + + +2010-10-28T04:32:42.000000Z +2a02ebce2cfc45515549bd41adc8215b +2009-10-19T07:17:33.113306Z +22253 +mike + +MaxDistanceConstraint.as +file + + + + +2010-10-28T04:32:42.000000Z +d6159b2e8bdaa7da4f1b419002c71a09 +2009-10-19T07:17:33.113306Z +22253 +mike + diff --git a/0.0.7.1/src/alternativa/physics/constraints/.svn/format b/0.0.7.1/src/alternativa/physics/constraints/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/constraints/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.7.1/src/alternativa/physics/constraints/.svn/text-base/Constraint.as.svn-base b/0.0.7.1/src/alternativa/physics/constraints/.svn/text-base/Constraint.as.svn-base new file mode 100644 index 0000000..4676d0d --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/constraints/.svn/text-base/Constraint.as.svn-base @@ -0,0 +1,35 @@ +package alternativa.physics.constraints { + + import alternativa.physics.World; + import alternativa.physics.altphysics; + + use namespace altphysics; + + /** + * + */ + public class Constraint { + + altphysics var satisfied:Boolean; + altphysics var world:World; + + /** + * + */ + public function Constraint() { + } + + /** + * @param dt + */ + public function preProcess(dt:Number):void { + } + + /** + * @param dt + */ + public function apply(dt:Number):void { + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base b/0.0.7.1/src/alternativa/physics/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base new file mode 100644 index 0000000..c87e0a3 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base @@ -0,0 +1,183 @@ +package alternativa.physics.constraints { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.math.Matrix3; + import alternativa.physics.math.Vector3; + + use namespace altphysics; + + /** + * + */ + public class MaxDistanceConstraint extends Constraint { + // Первое тело ограничения + altphysics var body1:Body; + // Второе тело ограничения. Может быть null. + altphysics var body2:Body; + // Первая точка крепления в системе координат первого тела + altphysics var r1:Vector3; + // Вторая точка привязки в системе координат второго тела. В случае отсутствия второго тела, координаты задаются в мировой системе. + altphysics var r2:Vector3; + // Максимально допустимое расстояние между точками крепления + altphysics var maxDistance:Number; + // Радиус-вектор первой точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr1:Vector3 = new Vector3(); + // Радиус-вектор второй точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr2:Vector3 = new Vector3(); + // Минимальная скорость сближения для удовлетворения ограничения + private var minClosingVel:Number; + // Изменение относительной скорости на единицу импульса + private var velByUnitImpulseN:Number; + // Направление корректирующего импульса + private var impulseDirection:Vector3 = new Vector3(); + + // Вспомогательные переменные +// private static var _v:Vector3 = new Vector3(); +// private static var _v1:Vector3 = new Vector3(); +// private static var _v2:Vector3 = new Vector3(); + + /** + * @param body1 + * @param body2 + * @param r1 + * @param r2 + * @param maxDistance + */ + public function MaxDistanceConstraint(body1:Body, body2:Body, r1:Vector3, r2:Vector3, maxDistance:Number) { + super(); + this.body1 = body1; + this.body2 = body2; + this.r1 = r1.vClone(); + this.r2 = r2.vClone(); + this.maxDistance = maxDistance; + } + + /** + * @param dt + */ + override public function preProcess(dt:Number):void { + // Вычислим расстояние между точками + var m:Matrix3 = body1.baseMatrix; + wr1.x = m.a*r1.x + m.b*r1.y + m.c*r1.z; + wr1.y = m.e*r1.x + m.f*r1.y + m.g*r1.z; + wr1.z = m.i*r1.x + m.j*r1.y + m.k*r1.z; + if (body2 != null) { + m = body2.baseMatrix; + wr2.x = m.a*r2.x + m.b*r2.y + m.c*r2.z; + wr2.y = m.e*r2.x + m.f*r2.y + m.g*r2.z; + wr2.z = m.i*r2.x + m.j*r2.y + m.k*r2.z; + } else { + wr2.x = r2.x; + wr2.y = r2.y; + wr2.z = r2.z; + } + var p1:Vector3 = body1.state.pos; + impulseDirection.x = wr2.x - wr1.x - p1.x; + impulseDirection.y = wr2.y - wr1.y - p1.y; + impulseDirection.z = wr2.z - wr1.z - p1.z; + if (body2 != null) { + var p2:Vector3 = body2.state.pos; + impulseDirection.x += p2.x; + impulseDirection.y += p2.y; + impulseDirection.z += p2.z; + } + + var len:Number = Math.sqrt(impulseDirection.x*impulseDirection.x + impulseDirection.y*impulseDirection.y + impulseDirection.z*impulseDirection.z); + var delta:Number = len - maxDistance; + if (delta > 0) { + satisfied = false; + if (len < 0.001) { + impulseDirection.x = 1; + } else { + len = 1/len; + impulseDirection.x *= len; + impulseDirection.y *= len; + impulseDirection.z *= len; + } + minClosingVel = delta/(world.penResolutionSteps*dt); + if (minClosingVel > world.maxPenResolutionSpeed) { + minClosingVel = world.maxPenResolutionSpeed; + } + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + var x:Number; + var y:Number; + var z:Number; + var vx:Number; + var vy:Number; + var vz:Number; + velByUnitImpulseN = 0; + if (body1.movable) { +// velByUnitImpulseN += body1.invMass + _v.vCross2(wr1, impulseDirection).vTransformBy3(body1.invInertiaWorld).vCross(wr1).vDot(impulseDirection); + vx = wr1.y*impulseDirection.z - wr1.z*impulseDirection.y; + vy = wr1.z*impulseDirection.x - wr1.x*impulseDirection.z; + vz = wr1.x*impulseDirection.y - wr1.y*impulseDirection.x; + + m = body1.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr1.z - z*wr1.y; + vy = z*wr1.x - x*wr1.z; + vz = x*wr1.y - y*wr1.x; + velByUnitImpulseN += body1.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + if (body2 != null && body2.movable) { +// velByUnitImpulseN += body2.invMass + _v.vCross2(wr2, impulseDirection).vTransformBy3(body2.invInertiaWorld).vCross(wr2).vDot(impulseDirection); + vx = wr2.y*impulseDirection.z - wr2.z*impulseDirection.y; + vy = wr2.z*impulseDirection.x - wr2.x*impulseDirection.z; + vz = wr2.x*impulseDirection.y - wr2.y*impulseDirection.x; + + m = body2.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr2.z - z*wr2.y; + vy = z*wr2.x - x*wr2.z; + vz = x*wr2.y - y*wr2.x; + velByUnitImpulseN += body2.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + } else { + satisfied = true; + } + } + + /** + * @param dt + */ + override public function apply(dt:Number):void { + if (satisfied) return; + // Расчитываем проекцию скорости относительной скорости + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 + var vel:Vector3 = body1.state.velocity; + var rot:Vector3 = body1.state.rotation; +// v1Cross2(state.rotation, wr1); + var vx:Number = vel.x + rot.y*wr1.z - rot.z*wr1.y; + var vy:Number = vel.y + rot.z*wr1.x - rot.x*wr1.z; + var vz:Number = vel.z + rot.x*wr1.y - rot.y*wr1.x; + // V2 = V2_c + w2%r2 + if (body2 != null) { + vel = body2.state.velocity; + rot = body2.state.rotation; +// _v2.vCross2(state.rotation, wr2); + vx -= vel.x + rot.y*wr2.z - rot.z*wr2.y; + vy -= vel.y + rot.z*wr2.x - rot.x*wr2.z; + vz -= vel.z + rot.x*wr2.y - rot.y*wr2.x; + } + var closingVel:Number = vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + if (closingVel > minClosingVel) return; + var impulse:Number = (minClosingVel - closingVel)/velByUnitImpulseN; + if (body1.movable) { + body1.applyRelPosWorldImpulse(wr1, impulseDirection, impulse); + } + if (body2 != null && body2.movable) { + body2.applyRelPosWorldImpulse(wr2, impulseDirection, -impulse); + } + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/constraints/Constraint.as b/0.0.7.1/src/alternativa/physics/constraints/Constraint.as new file mode 100644 index 0000000..4676d0d --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/constraints/Constraint.as @@ -0,0 +1,35 @@ +package alternativa.physics.constraints { + + import alternativa.physics.World; + import alternativa.physics.altphysics; + + use namespace altphysics; + + /** + * + */ + public class Constraint { + + altphysics var satisfied:Boolean; + altphysics var world:World; + + /** + * + */ + public function Constraint() { + } + + /** + * @param dt + */ + public function preProcess(dt:Number):void { + } + + /** + * @param dt + */ + public function apply(dt:Number):void { + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/constraints/MaxDistanceConstraint.as b/0.0.7.1/src/alternativa/physics/constraints/MaxDistanceConstraint.as new file mode 100644 index 0000000..c87e0a3 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/constraints/MaxDistanceConstraint.as @@ -0,0 +1,183 @@ +package alternativa.physics.constraints { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.math.Matrix3; + import alternativa.physics.math.Vector3; + + use namespace altphysics; + + /** + * + */ + public class MaxDistanceConstraint extends Constraint { + // Первое тело ограничения + altphysics var body1:Body; + // Второе тело ограничения. Может быть null. + altphysics var body2:Body; + // Первая точка крепления в системе координат первого тела + altphysics var r1:Vector3; + // Вторая точка привязки в системе координат второго тела. В случае отсутствия второго тела, координаты задаются в мировой системе. + altphysics var r2:Vector3; + // Максимально допустимое расстояние между точками крепления + altphysics var maxDistance:Number; + // Радиус-вектор первой точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr1:Vector3 = new Vector3(); + // Радиус-вектор второй точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr2:Vector3 = new Vector3(); + // Минимальная скорость сближения для удовлетворения ограничения + private var minClosingVel:Number; + // Изменение относительной скорости на единицу импульса + private var velByUnitImpulseN:Number; + // Направление корректирующего импульса + private var impulseDirection:Vector3 = new Vector3(); + + // Вспомогательные переменные +// private static var _v:Vector3 = new Vector3(); +// private static var _v1:Vector3 = new Vector3(); +// private static var _v2:Vector3 = new Vector3(); + + /** + * @param body1 + * @param body2 + * @param r1 + * @param r2 + * @param maxDistance + */ + public function MaxDistanceConstraint(body1:Body, body2:Body, r1:Vector3, r2:Vector3, maxDistance:Number) { + super(); + this.body1 = body1; + this.body2 = body2; + this.r1 = r1.vClone(); + this.r2 = r2.vClone(); + this.maxDistance = maxDistance; + } + + /** + * @param dt + */ + override public function preProcess(dt:Number):void { + // Вычислим расстояние между точками + var m:Matrix3 = body1.baseMatrix; + wr1.x = m.a*r1.x + m.b*r1.y + m.c*r1.z; + wr1.y = m.e*r1.x + m.f*r1.y + m.g*r1.z; + wr1.z = m.i*r1.x + m.j*r1.y + m.k*r1.z; + if (body2 != null) { + m = body2.baseMatrix; + wr2.x = m.a*r2.x + m.b*r2.y + m.c*r2.z; + wr2.y = m.e*r2.x + m.f*r2.y + m.g*r2.z; + wr2.z = m.i*r2.x + m.j*r2.y + m.k*r2.z; + } else { + wr2.x = r2.x; + wr2.y = r2.y; + wr2.z = r2.z; + } + var p1:Vector3 = body1.state.pos; + impulseDirection.x = wr2.x - wr1.x - p1.x; + impulseDirection.y = wr2.y - wr1.y - p1.y; + impulseDirection.z = wr2.z - wr1.z - p1.z; + if (body2 != null) { + var p2:Vector3 = body2.state.pos; + impulseDirection.x += p2.x; + impulseDirection.y += p2.y; + impulseDirection.z += p2.z; + } + + var len:Number = Math.sqrt(impulseDirection.x*impulseDirection.x + impulseDirection.y*impulseDirection.y + impulseDirection.z*impulseDirection.z); + var delta:Number = len - maxDistance; + if (delta > 0) { + satisfied = false; + if (len < 0.001) { + impulseDirection.x = 1; + } else { + len = 1/len; + impulseDirection.x *= len; + impulseDirection.y *= len; + impulseDirection.z *= len; + } + minClosingVel = delta/(world.penResolutionSteps*dt); + if (minClosingVel > world.maxPenResolutionSpeed) { + minClosingVel = world.maxPenResolutionSpeed; + } + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + var x:Number; + var y:Number; + var z:Number; + var vx:Number; + var vy:Number; + var vz:Number; + velByUnitImpulseN = 0; + if (body1.movable) { +// velByUnitImpulseN += body1.invMass + _v.vCross2(wr1, impulseDirection).vTransformBy3(body1.invInertiaWorld).vCross(wr1).vDot(impulseDirection); + vx = wr1.y*impulseDirection.z - wr1.z*impulseDirection.y; + vy = wr1.z*impulseDirection.x - wr1.x*impulseDirection.z; + vz = wr1.x*impulseDirection.y - wr1.y*impulseDirection.x; + + m = body1.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr1.z - z*wr1.y; + vy = z*wr1.x - x*wr1.z; + vz = x*wr1.y - y*wr1.x; + velByUnitImpulseN += body1.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + if (body2 != null && body2.movable) { +// velByUnitImpulseN += body2.invMass + _v.vCross2(wr2, impulseDirection).vTransformBy3(body2.invInertiaWorld).vCross(wr2).vDot(impulseDirection); + vx = wr2.y*impulseDirection.z - wr2.z*impulseDirection.y; + vy = wr2.z*impulseDirection.x - wr2.x*impulseDirection.z; + vz = wr2.x*impulseDirection.y - wr2.y*impulseDirection.x; + + m = body2.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr2.z - z*wr2.y; + vy = z*wr2.x - x*wr2.z; + vz = x*wr2.y - y*wr2.x; + velByUnitImpulseN += body2.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + } else { + satisfied = true; + } + } + + /** + * @param dt + */ + override public function apply(dt:Number):void { + if (satisfied) return; + // Расчитываем проекцию скорости относительной скорости + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 + var vel:Vector3 = body1.state.velocity; + var rot:Vector3 = body1.state.rotation; +// v1Cross2(state.rotation, wr1); + var vx:Number = vel.x + rot.y*wr1.z - rot.z*wr1.y; + var vy:Number = vel.y + rot.z*wr1.x - rot.x*wr1.z; + var vz:Number = vel.z + rot.x*wr1.y - rot.y*wr1.x; + // V2 = V2_c + w2%r2 + if (body2 != null) { + vel = body2.state.velocity; + rot = body2.state.rotation; +// _v2.vCross2(state.rotation, wr2); + vx -= vel.x + rot.y*wr2.z - rot.z*wr2.y; + vy -= vel.y + rot.z*wr2.x - rot.x*wr2.z; + vz -= vel.z + rot.x*wr2.y - rot.y*wr2.x; + } + var closingVel:Number = vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + if (closingVel > minClosingVel) return; + var impulse:Number = (minClosingVel - closingVel)/velByUnitImpulseN; + if (body1.movable) { + body1.applyRelPosWorldImpulse(wr1, impulseDirection, impulse); + } + if (body2 != null && body2.movable) { + body2.applyRelPosWorldImpulse(wr2, impulseDirection, -impulse); + } + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/math/.svn/all-wcprops b/0.0.7.1/src/alternativa/physics/math/.svn/all-wcprops new file mode 100644 index 0000000..5155369 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/math/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 108 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/math +END +Quaternion.as +K 25 +svn:wc:ra_dav:version-url +V 122 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/math/Quaternion.as +END +Vector3.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/math/Vector3.as +END +Matrix3.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/math/Matrix3.as +END +Matrix4.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/math/Matrix4.as +END diff --git a/0.0.7.1/src/alternativa/physics/math/.svn/entries b/0.0.7.1/src/alternativa/physics/math/.svn/entries new file mode 100644 index 0000000..0425fbe --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/math/.svn/entries @@ -0,0 +1,76 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/math +http://svndev.alternativaplatform.com + + + +2009-10-19T07:17:33.113306Z +22253 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Quaternion.as +file + + + + +2010-10-28T04:32:42.000000Z +813c4e874ea2dae729983537e27d5648 +2009-10-19T07:17:33.113306Z +22253 +mike + +Vector3.as +file + + + + +2010-10-28T04:32:42.000000Z +72a5b87dac5c32096c716639488b92e2 +2009-10-19T07:17:33.113306Z +22253 +mike + +Matrix3.as +file + + + + +2010-10-28T04:32:42.000000Z +c99517d0c388915e157464d4b31a0b86 +2009-10-19T07:17:33.113306Z +22253 +mike + +Matrix4.as +file + + + + +2010-10-28T04:32:42.000000Z +3f37b8a6a7e9f111a35e6ee1fe63dfbc +2009-10-19T07:17:33.113306Z +22253 +mike + diff --git a/0.0.7.1/src/alternativa/physics/math/.svn/format b/0.0.7.1/src/alternativa/physics/math/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/math/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.7.1/src/alternativa/physics/math/.svn/text-base/Matrix3.as.svn-base b/0.0.7.1/src/alternativa/physics/math/.svn/text-base/Matrix3.as.svn-base new file mode 100644 index 0000000..0b4dcd0 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/math/.svn/text-base/Matrix3.as.svn-base @@ -0,0 +1,391 @@ +package alternativa.physics.math { + import flash.geom.Vector3D; + + + /** + * + */ + public class Matrix3 { + + public static const ZERO:Matrix3 = new Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0); + public static const IDENTITY:Matrix3 = new Matrix3(); + + public var a:Number; + public var b:Number; + public var c:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix3(a:Number = 1, b:Number = 0, c:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1) { + this.a = a; + this.b = b; + this.c = c; + + this.e = e; + this.f = f; + this.g = g; + + this.i = i; + this.j = j; + this.k = k; + } + + /** + * + * @return + */ + public function toIdentity():Matrix3 { + a = f = k = 1; + b = c = e = g = i = j = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + var det:Number = 1/(-cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk); + + a = (ff*kk - gg*jj)*det; + b = (cc*jj - bb*kk)*det; + c = (bb*gg - cc*ff)*det; + e = (gg*ii - ee*kk)*det; + f = (aa*kk - cc*ii)*det; + g = (cc*ee - aa*gg)*det; + i = (ee*jj - ff*ii)*det; + j = (bb*ii - aa*jj)*det; + k = (aa*ff - bb*ee)*det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prependTransposed(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.b + cc*m.c; + b = aa*m.e + bb*m.f + cc*m.g; + c = aa*m.i + bb*m.j + cc*m.k; + e = ee*m.a + ff*m.b + gg*m.c; + f = ee*m.e + ff*m.f + gg*m.g; + g = ee*m.i + ff*m.j + gg*m.k; + i = ii*m.a + jj*m.b + kk*m.c; + j = ii*m.e + jj*m.f + kk*m.g; + k = ii*m.i + jj*m.j + kk*m.k; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix3):Matrix3 { + a += m.a; + b += m.b; + c += m.c; + e += m.e; + f += m.f; + g += m.g; + i += m.i; + j += m.j; + k += m.k; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix3):Matrix3 { + a -= m.a; + b -= m.b; + c -= m.c; + e -= m.e; + f -= m.f; + g -= m.g; + i -= m.i; + j -= m.j; + k -= m.k; + + return this; + } + + /** + * + * @return + */ + public function transpose():Matrix3 { + var tmp:Number = b; + b = e; + e = tmp; + tmp = c; + c = i; + i = tmp; + tmp = g; + g = j; + j = tmp; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector3To3D(vin:Vector3, vout:Vector3D):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * + * @param matrix + */ + public function createSkewSymmetric(v:Vector3):Matrix3 { + a = f = k = 0; + b = -v.z; + c = v.y; + e = v.z; + g = -v.x; + i = -v.y; + j = v.x; + return this; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix3):Matrix3 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @return + */ + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):Matrix3 { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + + return this; + } + + /** + * Метод формирует матрицу поворота на заданный угол относительно заданной оси. + * + * @param axis нормализованный вектор, задающий ось, относительно которой выполняется поворот + * @param angle угол поворота в радианах + */ + public function fromAxisAngle(axis:Vector3, angle:Number):void { + var c1:Number = Math.cos(angle); + var s:Number = Math.sin(angle); + var t:Number = 1 - c1; + var x:Number = axis.x; + var y:Number = axis.y; + var z:Number = axis.z; + + a = t*x*x + c1; + b = t*x*y - z*s; + c = t*x*z + y*s; + + e = t*x*y + z*s; + f = t*y*y + c1; + g = t*y*z - x*s; + + i = t*x*z - y*s; + j = t*y*z + x*s; + k = t*z*z + c1; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix3 { + return new Matrix3(a, b, c, e, f, g, i, j, k); + } + + /** + * + * @return + */ + public function toString():String { + return "[Matrix3 (" + a + ", " + b + ", " + c + "), (" + e + ", " + f + ", " + g + "), (" + i + ", " + j + ", " + k + ")]"; + } + + /** + * @param angles + */ + public function getEulerAngles(angles:Vector3):void { + if (-1 < i && i < 1) { + angles.x = Math.atan2(j, k); + angles.y = -Math.asin(i); + angles.z = Math.atan2(e, a); + } else { + angles.x = 0; + angles.y = (i <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-b, f); + } + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/math/.svn/text-base/Matrix4.as.svn-base b/0.0.7.1/src/alternativa/physics/math/.svn/text-base/Matrix4.as.svn-base new file mode 100644 index 0000000..939d46a --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/math/.svn/text-base/Matrix4.as.svn-base @@ -0,0 +1,461 @@ +package alternativa.physics.math { + import __AS3__.vec.Vector; + + /** + * + */ + public class Matrix4 { + + public static const IDENTITY:Matrix4 = new Matrix4(); + + public var a:Number; + public var b:Number; + public var c:Number; + public var d:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + public var h:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + public var l:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix4(a:Number = 1, b:Number = 0, c:Number = 0, d:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, h:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1, l:Number = 0) { + this.a = a; + this.b = b; + this.c = c; + this.d = d; + + this.e = e; + this.f = f; + this.g = g; + this.h = h; + + this.i = i; + this.j = j; + this.k = k; + this.l = l; + } + + /** + * + * @return + */ + public function toIdentity():Matrix4 { + a = f = k = 1; + b = c = e = g = i = j = d = h = l = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + var det:Number = -cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk; + + a = (-gg*jj + ff*kk)/det; + b = (cc*jj - bb*kk)/det; + c = (-cc*ff + bb*gg)/det; + d = (dd*gg*jj - cc*hh*jj - dd*ff*kk + bb*hh*kk + cc*ff*ll - bb*gg*ll)/det; + e = (gg*ii - ee*kk)/det; + f = (-cc*ii + aa*kk)/det; + g = (cc*ee - aa*gg)/det; + h = (cc*hh*ii - dd*gg*ii + dd*ee*kk - aa*hh*kk - cc*ee*ll + aa*gg*ll)/det; + i = (-ff*ii + ee*jj)/det; + j = (bb*ii - aa*jj)/det; + k = (-bb*ee + aa*ff)/det; + l = (dd*ff*ii - bb*hh*ii - dd*ee*jj + aa*hh*jj + bb*ee*ll - aa*ff*ll)/det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + d = m.a*dd + m.b*hh + m.c*ll + m.d; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + h = m.e*dd + m.f*hh + m.g*ll + m.h; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + l = m.i*dd + m.j*hh + m.k*ll + m.l; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + d = aa*m.d + bb*m.h + cc*m.l + dd; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + h = ee*m.d + ff*m.h + gg*m.l + hh; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + l = ii*m.d + jj*m.h + kk*m.l + ll; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix4):Matrix4 { + a += m.a; + b += m.b; + c += m.c; + d += m.d; + e += m.e; + f += m.f; + g += m.g; + h += m.h; + i += m.i; + j += m.j; + k += m.k; + l += m.l; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix4):Matrix4 { + a -= m.a; + b -= m.b; + c -= m.c; + d -= m.d; + e -= m.e; + f -= m.f; + g -= m.g; + h -= m.h; + i -= m.i; + j -= m.j; + k -= m.k; + l -= m.l; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectors(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + * @param len + */ + public function transformVectorsN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverse(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverseN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(idx:int, axis:Vector3):void { + switch (idx) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + case 3: + axis.x = d; + axis.y = h; + axis.z = l; + return; + } + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix4):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = m.d; + e = m.e; + f = m.f; + g = m.g; + h = m.h; + i = m.i; + j = m.j; + k = m.k; + l = m.l; + + return this; + } + + public function setFromMatrix3(m:Matrix3, offset:Vector3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = offset.x; + e = m.e; + f = m.f; + g = m.g; + h = offset.y; + i = m.i; + j = m.j; + k = m.k; + l = offset.z; + + return this; + } + + public function setOrientationFromMatrix3(m:Matrix3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @param angles + */ + public function getEulerAngles(angles:Vector3):void { + if (-1 < i && i < 1) { + angles.x = Math.atan2(j, k); + angles.y = -Math.asin(i); + angles.z = Math.atan2(e, a); + } else { + angles.x = 0; + angles.y = (i <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-b, f); + } + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix4 { + return new Matrix4(a, b, c, d, e, f, g, h, i, j, k, l); + } + + public function toString():String { + return "[Matrix4 [" + a.toFixed(3) + " " + b.toFixed(3) + " " + c.toFixed(3) + " " + d.toFixed(3) + "] [" + e.toFixed(3) + " " + f.toFixed(3) + " " + g.toFixed(3) + " " + h.toFixed(3) + "] [" + i.toFixed(3) + " " + j.toFixed(3) + " " + k.toFixed(3) + " " + l.toFixed(3) + "]]"; + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/math/.svn/text-base/Quaternion.as.svn-base b/0.0.7.1/src/alternativa/physics/math/.svn/text-base/Quaternion.as.svn-base new file mode 100644 index 0000000..cf39d7b --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/math/.svn/text-base/Quaternion.as.svn-base @@ -0,0 +1,397 @@ +package alternativa.physics.math { + import flash.geom.Vector3D; + + + public class Quaternion { + + public var w:Number; + public var x:Number; + public var y:Number; + public var z:Number; + + public static function multiply(q1:Quaternion, q2:Quaternion, result:Quaternion):void { + result.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z; + result.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y; + result.y = q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z; + result.z = q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x; + } + + public static function createFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngle(axis, angle); + return q; + } + + public static function createFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngleComponents(x, y, z, angle); + return q; + } + + public function Quaternion(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0) { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + } + + public function reset(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0):Quaternion { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + return this; + } + + public function normalize():Quaternion { + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * Умножает на указанный кватернион слева: this * q + * + * @param q множитель + */ + public function prepend(q:Quaternion):Quaternion { + var ww:Number = w*q.w - x*q.x - y*q.y - z*q.z; + var xx:Number = w*q.x + x*q.w + y*q.z - z*q.y; + var yy:Number = w*q.y + y*q.w + z*q.x - x*q.z; + var zz:Number = w*q.z + z*q.w + x*q.y - y*q.x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Умножает на указанный кватернион справа: q * this + * + * @param q множитель + */ + public function append(q:Quaternion):Quaternion { + var ww:Number = q.w*w - q.x*x - q.y*y - q.z*z; + var xx:Number = q.w*x + q.x*w + q.y*z - q.z*y; + var yy:Number = q.w*y + q.y*w + q.z*x - q.x*z; + var zz:Number = q.w*z + q.z*w + q.x*y - q.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * + * @param vector + */ + public function rotateByVector(v:Vector3):Quaternion { + var ww:Number = -v.x*x - v.y*y - v.z*z; + var xx:Number = v.x*w + v.y*z - v.z*y; + var yy:Number = v.y*w + v.z*x - v.x*z; + var zz:Number = v.z*w + v.x*y - v.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Добавляет вращение, приданное вектором угловой скорости за указанное время. + * + * @param v + * @param scale + */ + public function addScaledVector(v:Vector3, scale:Number):Quaternion { + var vx:Number = v.x*scale; + var vy:Number = v.y*scale; + var vz:Number = v.z*scale; + var ww:Number = -x*vx - y*vy - z*vz; + var xx:Number = vx*w + vy*z - vz*y; + var yy:Number = vy*w + vz*x - vx*z; + var zz:Number = vz*w + vx*y - vy*x; + w += 0.5*ww; + x += 0.5*xx; + y += 0.5*yy; + z += 0.5*zz; + // inlined normalize + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param m + * @return + */ + public function toMatrix3(m:Matrix3):Quaternion { + var xx2:Number = 2.0*x*x; + var yy2:Number = 2.0*y*y; + var zz2:Number = 2.0*z*z; + var xy2:Number = 2.0*x*y; + var yz2:Number = 2.0*y*z; + var zx2:Number = 2.0*z*x; + var wx2:Number = 2.0*w*x; + var wy2:Number = 2.0*w*y; + var wz2:Number = 2.0*w*z; + + m.a = 1.0 - yy2 - zz2; + m.b = xy2 - wz2; + m.c = zx2 + wy2; + + m.e = xy2 + wz2; + m.f = 1.0 - xx2 - zz2; + m.g = yz2 - wx2; + + m.i = zx2 - wy2; + m.j = yz2 + wx2; + m.k = 1.0 - xx2 - yy2; + return this; + } + + /** + * + * @return + */ + public function length():Number { + return Math.sqrt(w*w + x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function lengthSqr():Number { + return w*w + x*x + y*y + z*z; + } + + /** + * + * @param axis + * @param angle + * @return + */ + public function setFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z); + x = axis.x*k; + y = axis.y*k; + z = axis.z*k; + return this; + } + + /** + * + * @param x + * @param y + * @param z + * @param angle + */ + public function setFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(x*x + y*y + z*z); + this.x = x*k; + this.y = y*k; + this.z = z*k; + return this; + } + + /** + * + * @param vector + */ + public function toAxisVector(v:Vector3 = null):Vector3 { + if (w < -1 || w > 1) { + normalize(); + } + if (v == null) { + v = new Vector3(); + } + if (w > -1 && w < 1) { + if (w == 0) { + v.x = x; + v.y = y; + v.z = z; + } else { + var angle:Number = 2*Math.acos(w); + var coeff:Number = 1/Math.sqrt(1 - w*w); + v.x = x*coeff*angle; + v.y = y*coeff*angle; + v.z = z*coeff*angle; + } + } else { + v.x = 0; + v.y = 0; + v.z = 0; + } + return v; + } + + /** + * + * @param rotations + */ + public function getEulerAngles(angles:Vector3):Vector3 { + var qi2:Number = 2*x*x; + var qj2:Number = 2*y*y; + var qk2:Number = 2*z*z; + var qij:Number = 2*x*y; + var qjk:Number = 2*y*z; + var qki:Number = 2*z*x; + var qri:Number = 2*w*x; + var qrj:Number = 2*w*y; + var qrk:Number = 2*w*z; + + var aa:Number = 1 - qj2 - qk2; + var bb:Number = qij - qrk; + var ee:Number = qij + qrk; + var ff:Number = 1 - qi2 - qk2; + var ii:Number = qki - qrj; + var jj:Number = qjk + qri; + var kk:Number = 1 - qi2 - qj2; + + if (-1 < ii && ii < 1) { + if (angles == null) { + angles = new Vector3(Math.atan2(jj, kk), -Math.asin(ii), Math.atan2(ee, aa)); + } else { + angles.x = Math.atan2(jj, kk); + angles.y = -Math.asin(ii); + angles.z = Math.atan2(ee, aa); + } + } else { + if (angles == null) { + angles = new Vector3(0, 0.5*((ii <= -1) ? Math.PI : -Math.PI), Math.atan2(-bb, ff)); + } else { + angles.x = 0; + angles.y = (ii <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-bb, ff); + } + } + return angles; + } + + /** + * + */ + public function conjugate():void { + x = -x; + y = -y; + z = -z; + } + + /** + * Выполняет линейную интерполяцию. + * + * @param q1 начало отрезка + * @param q2 конец отрезка + * @param t время, обычно задаётся в интервале [0, 1] + */ + public function nlerp(q1:Quaternion, q2:Quaternion, t:Number):Quaternion { + var d:Number = 1 - t; + w = q1.w*d + q2.w*t; + x = q1.x*d + q2.x*t; + y = q1.y*d + q2.y*t; + z = q1.z*d + q2.z*t; + // inlined normalize + d = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param q + * @return + */ + public function subtract(q:Quaternion):Quaternion { + w -= q.w; + x -= q.x; + y -= q.y; + z -= q.z; + return this; + } + + /** + * + * @param q1 + * @param q2 + * @return + */ + public function diff(q1:Quaternion, q2:Quaternion):Quaternion { + w = q2.w - q1.w; + x = q2.x - q1.x; + y = q2.y - q1.y; + z = q2.z - q1.z; + return this; + } + + /** + * + * @param q + * @return + */ + public function copy(q:Quaternion):Quaternion { + w = q.w; + x = q.x; + y = q.y; + z = q.z; + return this; + } + + /** + * @param reslult + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + result.w = w; + return result; + } + + /** + * + * @return + */ + public function clone():Quaternion { + return new Quaternion(w, x, y, z); + } + + /** + * + * @return + */ + public function toString():String { + return "[" + w + ", " + x + ", " + y + ", " + z + "]"; + } + } +} diff --git a/0.0.7.1/src/alternativa/physics/math/.svn/text-base/Vector3.as.svn-base b/0.0.7.1/src/alternativa/physics/math/.svn/text-base/Vector3.as.svn-base new file mode 100644 index 0000000..232e499 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/math/.svn/text-base/Vector3.as.svn-base @@ -0,0 +1,335 @@ +package alternativa.physics.math { + import flash.geom.Vector3D; + + + /** + * + */ + public class Vector3 extends Vector3D { + + public static const ZERO:Vector3 = new Vector3(0, 0, 0); + public static const X_AXIS:Vector3 = new Vector3(1, 0, 0); + public static const Y_AXIS:Vector3 = new Vector3(0, 1, 0); + public static const Z_AXIS:Vector3 = new Vector3(0, 0, 1); + + /** + * + * @param x + * @param y + * @param z + */ + public function Vector3(x:Number = 0, y:Number = 0, z:Number = 0) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * + * @return + */ + public function vLength():Number { + return Math.sqrt(x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function vLengthSqr():Number { + return x*x + y*y + z*z; + } + + /** + * + * @param length + * @return + */ + public function vSetLength(length:Number):Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = length; + } else { + var k:Number = length/Math.sqrt(x*x + y*y + z*z); + x *= k; + y *= k; + z *= k; + } + return this; + } + + /** + * + * @return + */ + public function vNormalize():Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = 1; + } else { + d = Math.sqrt(d); + x /= d; + y /= d; + z /= d; + } + return this; + } + + /** + * Сложение координат. + * + * @param v точка, координаты которой прибавляются к собственным + */ + public function vAdd(v:Vector3):Vector3 { + x += v.x; + y += v.y; + z += v.z; + return this; + } + + /** + * + * @param k + * @param v + * @return + */ + public function vAddScaled(k:Number, v:Vector3):Vector3 { + x += k*v.x; + y += k*v.y; + z += k*v.z; + return this; + } + + /** + * Вычитание координат. + * + * @param v точка, координаты которой вычитаются из собственных + */ + public function vSubtract(v:Vector3):Vector3 { + x -= v.x; + y -= v.y; + z -= v.z; + return this; + } + + /** + * Вычисляет сумму векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + * @return this + */ + public function vSum(a:Vector3, b:Vector3):Vector3 { + x = a.x + b.x; + y = a.y + b.y; + z = a.z + b.z; + return this; + } + + /** + * Вычисление разности векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + */ + public function vDiff(a:Vector3, b:Vector3):Vector3 { + x = a.x - b.x; + y = a.y - b.y; + z = a.z - b.z; + return this; + } + + /** + * Умножение на скаляр. + * + * @param k число, на которое умножаются координаты + */ + public function vScale(k:Number):Vector3 { + x *= k; + y *= k; + z *= k; + return this; + } + + /** + * Инвертирование вектора. + */ + public function vReverse():Vector3 { + x = -x; + y = -y; + z = -z; + return this; + } + + /** + * + * @param v + * @return + */ + public function vDot(v:Vector3):Number { + return x*v.x + y*v.y + z*v.z; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross(v:Vector3):Vector3 { + var xx:Number = y*v.z - z*v.y; + var yy:Number = z*v.x - x*v.z; + var zz:Number = x*v.y - y*v.x; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross2(a:Vector3, b:Vector3):Vector3 { + x = a.y*b.z - a.z*b.y; + y = a.z*b.x - a.x*b.z; + z = a.x*b.y - a.y*b.x; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy3(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * + * @param matrix + */ + public function vTransformBy3Tr(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz + m.d; + y = m.e*xx + m.f*yy + m.g*zz + m.h; + z = m.i*xx + m.j*yy + m.k*zz + m.l; + return this; + } + + /** + * + * @param m + * @return + */ + public function vTransformInverseBy4(m:Matrix4):Vector3 { + var xx:Number = x - m.d; + var yy:Number = y - m.h; + var zz:Number = z - m.l; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * + * @param m + */ + public function vDeltaTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * Установка координат. + */ + public function vReset(x:Number = 0, y:Number = 0, z:Number = 0):Vector3 { + this.x = x; + this.y = y; + this.z = z; + return this; + } + + /** + * Копирование координат точки. + * + * @param v точка, координаты которой копируются + */ + public function vCopy(v:Vector3):Vector3 { + x = v.x; + y = v.y; + z = v.z; + return this; + } + + /** + * Клонирование точки. + * + * @return клонированная точка + */ + public function vClone():Vector3 { + return new Vector3(x, y, z); + } + + /** + * Копирует координаты вектора в экземпляр класса Vector3D. + * + * @param result экземпляр, в который копируются координаты + * @return переданный параметр + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + return result; + } + + /** + * @param source + * @return + */ + public function copyFromVector3D(source:Vector3D):Vector3 { + x = source.x; + y = source.y; + z = source.z; + return this; + } + + override public function toString():String { + return "Vector3(" + x + "," + y + ", " + z + ")"; + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/math/Matrix3.as b/0.0.7.1/src/alternativa/physics/math/Matrix3.as new file mode 100644 index 0000000..0b4dcd0 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/math/Matrix3.as @@ -0,0 +1,391 @@ +package alternativa.physics.math { + import flash.geom.Vector3D; + + + /** + * + */ + public class Matrix3 { + + public static const ZERO:Matrix3 = new Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0); + public static const IDENTITY:Matrix3 = new Matrix3(); + + public var a:Number; + public var b:Number; + public var c:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix3(a:Number = 1, b:Number = 0, c:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1) { + this.a = a; + this.b = b; + this.c = c; + + this.e = e; + this.f = f; + this.g = g; + + this.i = i; + this.j = j; + this.k = k; + } + + /** + * + * @return + */ + public function toIdentity():Matrix3 { + a = f = k = 1; + b = c = e = g = i = j = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + var det:Number = 1/(-cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk); + + a = (ff*kk - gg*jj)*det; + b = (cc*jj - bb*kk)*det; + c = (bb*gg - cc*ff)*det; + e = (gg*ii - ee*kk)*det; + f = (aa*kk - cc*ii)*det; + g = (cc*ee - aa*gg)*det; + i = (ee*jj - ff*ii)*det; + j = (bb*ii - aa*jj)*det; + k = (aa*ff - bb*ee)*det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prependTransposed(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.b + cc*m.c; + b = aa*m.e + bb*m.f + cc*m.g; + c = aa*m.i + bb*m.j + cc*m.k; + e = ee*m.a + ff*m.b + gg*m.c; + f = ee*m.e + ff*m.f + gg*m.g; + g = ee*m.i + ff*m.j + gg*m.k; + i = ii*m.a + jj*m.b + kk*m.c; + j = ii*m.e + jj*m.f + kk*m.g; + k = ii*m.i + jj*m.j + kk*m.k; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix3):Matrix3 { + a += m.a; + b += m.b; + c += m.c; + e += m.e; + f += m.f; + g += m.g; + i += m.i; + j += m.j; + k += m.k; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix3):Matrix3 { + a -= m.a; + b -= m.b; + c -= m.c; + e -= m.e; + f -= m.f; + g -= m.g; + i -= m.i; + j -= m.j; + k -= m.k; + + return this; + } + + /** + * + * @return + */ + public function transpose():Matrix3 { + var tmp:Number = b; + b = e; + e = tmp; + tmp = c; + c = i; + i = tmp; + tmp = g; + g = j; + j = tmp; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector3To3D(vin:Vector3, vout:Vector3D):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * + * @param matrix + */ + public function createSkewSymmetric(v:Vector3):Matrix3 { + a = f = k = 0; + b = -v.z; + c = v.y; + e = v.z; + g = -v.x; + i = -v.y; + j = v.x; + return this; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix3):Matrix3 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @return + */ + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):Matrix3 { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + + return this; + } + + /** + * Метод формирует матрицу поворота на заданный угол относительно заданной оси. + * + * @param axis нормализованный вектор, задающий ось, относительно которой выполняется поворот + * @param angle угол поворота в радианах + */ + public function fromAxisAngle(axis:Vector3, angle:Number):void { + var c1:Number = Math.cos(angle); + var s:Number = Math.sin(angle); + var t:Number = 1 - c1; + var x:Number = axis.x; + var y:Number = axis.y; + var z:Number = axis.z; + + a = t*x*x + c1; + b = t*x*y - z*s; + c = t*x*z + y*s; + + e = t*x*y + z*s; + f = t*y*y + c1; + g = t*y*z - x*s; + + i = t*x*z - y*s; + j = t*y*z + x*s; + k = t*z*z + c1; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix3 { + return new Matrix3(a, b, c, e, f, g, i, j, k); + } + + /** + * + * @return + */ + public function toString():String { + return "[Matrix3 (" + a + ", " + b + ", " + c + "), (" + e + ", " + f + ", " + g + "), (" + i + ", " + j + ", " + k + ")]"; + } + + /** + * @param angles + */ + public function getEulerAngles(angles:Vector3):void { + if (-1 < i && i < 1) { + angles.x = Math.atan2(j, k); + angles.y = -Math.asin(i); + angles.z = Math.atan2(e, a); + } else { + angles.x = 0; + angles.y = (i <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-b, f); + } + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/math/Matrix4.as b/0.0.7.1/src/alternativa/physics/math/Matrix4.as new file mode 100644 index 0000000..939d46a --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/math/Matrix4.as @@ -0,0 +1,461 @@ +package alternativa.physics.math { + import __AS3__.vec.Vector; + + /** + * + */ + public class Matrix4 { + + public static const IDENTITY:Matrix4 = new Matrix4(); + + public var a:Number; + public var b:Number; + public var c:Number; + public var d:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + public var h:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + public var l:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix4(a:Number = 1, b:Number = 0, c:Number = 0, d:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, h:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1, l:Number = 0) { + this.a = a; + this.b = b; + this.c = c; + this.d = d; + + this.e = e; + this.f = f; + this.g = g; + this.h = h; + + this.i = i; + this.j = j; + this.k = k; + this.l = l; + } + + /** + * + * @return + */ + public function toIdentity():Matrix4 { + a = f = k = 1; + b = c = e = g = i = j = d = h = l = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + var det:Number = -cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk; + + a = (-gg*jj + ff*kk)/det; + b = (cc*jj - bb*kk)/det; + c = (-cc*ff + bb*gg)/det; + d = (dd*gg*jj - cc*hh*jj - dd*ff*kk + bb*hh*kk + cc*ff*ll - bb*gg*ll)/det; + e = (gg*ii - ee*kk)/det; + f = (-cc*ii + aa*kk)/det; + g = (cc*ee - aa*gg)/det; + h = (cc*hh*ii - dd*gg*ii + dd*ee*kk - aa*hh*kk - cc*ee*ll + aa*gg*ll)/det; + i = (-ff*ii + ee*jj)/det; + j = (bb*ii - aa*jj)/det; + k = (-bb*ee + aa*ff)/det; + l = (dd*ff*ii - bb*hh*ii - dd*ee*jj + aa*hh*jj + bb*ee*ll - aa*ff*ll)/det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + d = m.a*dd + m.b*hh + m.c*ll + m.d; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + h = m.e*dd + m.f*hh + m.g*ll + m.h; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + l = m.i*dd + m.j*hh + m.k*ll + m.l; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + d = aa*m.d + bb*m.h + cc*m.l + dd; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + h = ee*m.d + ff*m.h + gg*m.l + hh; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + l = ii*m.d + jj*m.h + kk*m.l + ll; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix4):Matrix4 { + a += m.a; + b += m.b; + c += m.c; + d += m.d; + e += m.e; + f += m.f; + g += m.g; + h += m.h; + i += m.i; + j += m.j; + k += m.k; + l += m.l; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix4):Matrix4 { + a -= m.a; + b -= m.b; + c -= m.c; + d -= m.d; + e -= m.e; + f -= m.f; + g -= m.g; + h -= m.h; + i -= m.i; + j -= m.j; + k -= m.k; + l -= m.l; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectors(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + * @param len + */ + public function transformVectorsN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverse(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverseN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(idx:int, axis:Vector3):void { + switch (idx) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + case 3: + axis.x = d; + axis.y = h; + axis.z = l; + return; + } + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix4):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = m.d; + e = m.e; + f = m.f; + g = m.g; + h = m.h; + i = m.i; + j = m.j; + k = m.k; + l = m.l; + + return this; + } + + public function setFromMatrix3(m:Matrix3, offset:Vector3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = offset.x; + e = m.e; + f = m.f; + g = m.g; + h = offset.y; + i = m.i; + j = m.j; + k = m.k; + l = offset.z; + + return this; + } + + public function setOrientationFromMatrix3(m:Matrix3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @param angles + */ + public function getEulerAngles(angles:Vector3):void { + if (-1 < i && i < 1) { + angles.x = Math.atan2(j, k); + angles.y = -Math.asin(i); + angles.z = Math.atan2(e, a); + } else { + angles.x = 0; + angles.y = (i <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-b, f); + } + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix4 { + return new Matrix4(a, b, c, d, e, f, g, h, i, j, k, l); + } + + public function toString():String { + return "[Matrix4 [" + a.toFixed(3) + " " + b.toFixed(3) + " " + c.toFixed(3) + " " + d.toFixed(3) + "] [" + e.toFixed(3) + " " + f.toFixed(3) + " " + g.toFixed(3) + " " + h.toFixed(3) + "] [" + i.toFixed(3) + " " + j.toFixed(3) + " " + k.toFixed(3) + " " + l.toFixed(3) + "]]"; + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/math/Quaternion.as b/0.0.7.1/src/alternativa/physics/math/Quaternion.as new file mode 100644 index 0000000..cf39d7b --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/math/Quaternion.as @@ -0,0 +1,397 @@ +package alternativa.physics.math { + import flash.geom.Vector3D; + + + public class Quaternion { + + public var w:Number; + public var x:Number; + public var y:Number; + public var z:Number; + + public static function multiply(q1:Quaternion, q2:Quaternion, result:Quaternion):void { + result.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z; + result.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y; + result.y = q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z; + result.z = q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x; + } + + public static function createFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngle(axis, angle); + return q; + } + + public static function createFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngleComponents(x, y, z, angle); + return q; + } + + public function Quaternion(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0) { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + } + + public function reset(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0):Quaternion { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + return this; + } + + public function normalize():Quaternion { + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * Умножает на указанный кватернион слева: this * q + * + * @param q множитель + */ + public function prepend(q:Quaternion):Quaternion { + var ww:Number = w*q.w - x*q.x - y*q.y - z*q.z; + var xx:Number = w*q.x + x*q.w + y*q.z - z*q.y; + var yy:Number = w*q.y + y*q.w + z*q.x - x*q.z; + var zz:Number = w*q.z + z*q.w + x*q.y - y*q.x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Умножает на указанный кватернион справа: q * this + * + * @param q множитель + */ + public function append(q:Quaternion):Quaternion { + var ww:Number = q.w*w - q.x*x - q.y*y - q.z*z; + var xx:Number = q.w*x + q.x*w + q.y*z - q.z*y; + var yy:Number = q.w*y + q.y*w + q.z*x - q.x*z; + var zz:Number = q.w*z + q.z*w + q.x*y - q.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * + * @param vector + */ + public function rotateByVector(v:Vector3):Quaternion { + var ww:Number = -v.x*x - v.y*y - v.z*z; + var xx:Number = v.x*w + v.y*z - v.z*y; + var yy:Number = v.y*w + v.z*x - v.x*z; + var zz:Number = v.z*w + v.x*y - v.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Добавляет вращение, приданное вектором угловой скорости за указанное время. + * + * @param v + * @param scale + */ + public function addScaledVector(v:Vector3, scale:Number):Quaternion { + var vx:Number = v.x*scale; + var vy:Number = v.y*scale; + var vz:Number = v.z*scale; + var ww:Number = -x*vx - y*vy - z*vz; + var xx:Number = vx*w + vy*z - vz*y; + var yy:Number = vy*w + vz*x - vx*z; + var zz:Number = vz*w + vx*y - vy*x; + w += 0.5*ww; + x += 0.5*xx; + y += 0.5*yy; + z += 0.5*zz; + // inlined normalize + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param m + * @return + */ + public function toMatrix3(m:Matrix3):Quaternion { + var xx2:Number = 2.0*x*x; + var yy2:Number = 2.0*y*y; + var zz2:Number = 2.0*z*z; + var xy2:Number = 2.0*x*y; + var yz2:Number = 2.0*y*z; + var zx2:Number = 2.0*z*x; + var wx2:Number = 2.0*w*x; + var wy2:Number = 2.0*w*y; + var wz2:Number = 2.0*w*z; + + m.a = 1.0 - yy2 - zz2; + m.b = xy2 - wz2; + m.c = zx2 + wy2; + + m.e = xy2 + wz2; + m.f = 1.0 - xx2 - zz2; + m.g = yz2 - wx2; + + m.i = zx2 - wy2; + m.j = yz2 + wx2; + m.k = 1.0 - xx2 - yy2; + return this; + } + + /** + * + * @return + */ + public function length():Number { + return Math.sqrt(w*w + x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function lengthSqr():Number { + return w*w + x*x + y*y + z*z; + } + + /** + * + * @param axis + * @param angle + * @return + */ + public function setFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z); + x = axis.x*k; + y = axis.y*k; + z = axis.z*k; + return this; + } + + /** + * + * @param x + * @param y + * @param z + * @param angle + */ + public function setFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(x*x + y*y + z*z); + this.x = x*k; + this.y = y*k; + this.z = z*k; + return this; + } + + /** + * + * @param vector + */ + public function toAxisVector(v:Vector3 = null):Vector3 { + if (w < -1 || w > 1) { + normalize(); + } + if (v == null) { + v = new Vector3(); + } + if (w > -1 && w < 1) { + if (w == 0) { + v.x = x; + v.y = y; + v.z = z; + } else { + var angle:Number = 2*Math.acos(w); + var coeff:Number = 1/Math.sqrt(1 - w*w); + v.x = x*coeff*angle; + v.y = y*coeff*angle; + v.z = z*coeff*angle; + } + } else { + v.x = 0; + v.y = 0; + v.z = 0; + } + return v; + } + + /** + * + * @param rotations + */ + public function getEulerAngles(angles:Vector3):Vector3 { + var qi2:Number = 2*x*x; + var qj2:Number = 2*y*y; + var qk2:Number = 2*z*z; + var qij:Number = 2*x*y; + var qjk:Number = 2*y*z; + var qki:Number = 2*z*x; + var qri:Number = 2*w*x; + var qrj:Number = 2*w*y; + var qrk:Number = 2*w*z; + + var aa:Number = 1 - qj2 - qk2; + var bb:Number = qij - qrk; + var ee:Number = qij + qrk; + var ff:Number = 1 - qi2 - qk2; + var ii:Number = qki - qrj; + var jj:Number = qjk + qri; + var kk:Number = 1 - qi2 - qj2; + + if (-1 < ii && ii < 1) { + if (angles == null) { + angles = new Vector3(Math.atan2(jj, kk), -Math.asin(ii), Math.atan2(ee, aa)); + } else { + angles.x = Math.atan2(jj, kk); + angles.y = -Math.asin(ii); + angles.z = Math.atan2(ee, aa); + } + } else { + if (angles == null) { + angles = new Vector3(0, 0.5*((ii <= -1) ? Math.PI : -Math.PI), Math.atan2(-bb, ff)); + } else { + angles.x = 0; + angles.y = (ii <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-bb, ff); + } + } + return angles; + } + + /** + * + */ + public function conjugate():void { + x = -x; + y = -y; + z = -z; + } + + /** + * Выполняет линейную интерполяцию. + * + * @param q1 начало отрезка + * @param q2 конец отрезка + * @param t время, обычно задаётся в интервале [0, 1] + */ + public function nlerp(q1:Quaternion, q2:Quaternion, t:Number):Quaternion { + var d:Number = 1 - t; + w = q1.w*d + q2.w*t; + x = q1.x*d + q2.x*t; + y = q1.y*d + q2.y*t; + z = q1.z*d + q2.z*t; + // inlined normalize + d = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param q + * @return + */ + public function subtract(q:Quaternion):Quaternion { + w -= q.w; + x -= q.x; + y -= q.y; + z -= q.z; + return this; + } + + /** + * + * @param q1 + * @param q2 + * @return + */ + public function diff(q1:Quaternion, q2:Quaternion):Quaternion { + w = q2.w - q1.w; + x = q2.x - q1.x; + y = q2.y - q1.y; + z = q2.z - q1.z; + return this; + } + + /** + * + * @param q + * @return + */ + public function copy(q:Quaternion):Quaternion { + w = q.w; + x = q.x; + y = q.y; + z = q.z; + return this; + } + + /** + * @param reslult + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + result.w = w; + return result; + } + + /** + * + * @return + */ + public function clone():Quaternion { + return new Quaternion(w, x, y, z); + } + + /** + * + * @return + */ + public function toString():String { + return "[" + w + ", " + x + ", " + y + ", " + z + "]"; + } + } +} diff --git a/0.0.7.1/src/alternativa/physics/math/Vector3.as b/0.0.7.1/src/alternativa/physics/math/Vector3.as new file mode 100644 index 0000000..232e499 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/math/Vector3.as @@ -0,0 +1,335 @@ +package alternativa.physics.math { + import flash.geom.Vector3D; + + + /** + * + */ + public class Vector3 extends Vector3D { + + public static const ZERO:Vector3 = new Vector3(0, 0, 0); + public static const X_AXIS:Vector3 = new Vector3(1, 0, 0); + public static const Y_AXIS:Vector3 = new Vector3(0, 1, 0); + public static const Z_AXIS:Vector3 = new Vector3(0, 0, 1); + + /** + * + * @param x + * @param y + * @param z + */ + public function Vector3(x:Number = 0, y:Number = 0, z:Number = 0) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * + * @return + */ + public function vLength():Number { + return Math.sqrt(x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function vLengthSqr():Number { + return x*x + y*y + z*z; + } + + /** + * + * @param length + * @return + */ + public function vSetLength(length:Number):Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = length; + } else { + var k:Number = length/Math.sqrt(x*x + y*y + z*z); + x *= k; + y *= k; + z *= k; + } + return this; + } + + /** + * + * @return + */ + public function vNormalize():Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = 1; + } else { + d = Math.sqrt(d); + x /= d; + y /= d; + z /= d; + } + return this; + } + + /** + * Сложение координат. + * + * @param v точка, координаты которой прибавляются к собственным + */ + public function vAdd(v:Vector3):Vector3 { + x += v.x; + y += v.y; + z += v.z; + return this; + } + + /** + * + * @param k + * @param v + * @return + */ + public function vAddScaled(k:Number, v:Vector3):Vector3 { + x += k*v.x; + y += k*v.y; + z += k*v.z; + return this; + } + + /** + * Вычитание координат. + * + * @param v точка, координаты которой вычитаются из собственных + */ + public function vSubtract(v:Vector3):Vector3 { + x -= v.x; + y -= v.y; + z -= v.z; + return this; + } + + /** + * Вычисляет сумму векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + * @return this + */ + public function vSum(a:Vector3, b:Vector3):Vector3 { + x = a.x + b.x; + y = a.y + b.y; + z = a.z + b.z; + return this; + } + + /** + * Вычисление разности векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + */ + public function vDiff(a:Vector3, b:Vector3):Vector3 { + x = a.x - b.x; + y = a.y - b.y; + z = a.z - b.z; + return this; + } + + /** + * Умножение на скаляр. + * + * @param k число, на которое умножаются координаты + */ + public function vScale(k:Number):Vector3 { + x *= k; + y *= k; + z *= k; + return this; + } + + /** + * Инвертирование вектора. + */ + public function vReverse():Vector3 { + x = -x; + y = -y; + z = -z; + return this; + } + + /** + * + * @param v + * @return + */ + public function vDot(v:Vector3):Number { + return x*v.x + y*v.y + z*v.z; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross(v:Vector3):Vector3 { + var xx:Number = y*v.z - z*v.y; + var yy:Number = z*v.x - x*v.z; + var zz:Number = x*v.y - y*v.x; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross2(a:Vector3, b:Vector3):Vector3 { + x = a.y*b.z - a.z*b.y; + y = a.z*b.x - a.x*b.z; + z = a.x*b.y - a.y*b.x; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy3(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * + * @param matrix + */ + public function vTransformBy3Tr(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz + m.d; + y = m.e*xx + m.f*yy + m.g*zz + m.h; + z = m.i*xx + m.j*yy + m.k*zz + m.l; + return this; + } + + /** + * + * @param m + * @return + */ + public function vTransformInverseBy4(m:Matrix4):Vector3 { + var xx:Number = x - m.d; + var yy:Number = y - m.h; + var zz:Number = z - m.l; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * + * @param m + */ + public function vDeltaTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * Установка координат. + */ + public function vReset(x:Number = 0, y:Number = 0, z:Number = 0):Vector3 { + this.x = x; + this.y = y; + this.z = z; + return this; + } + + /** + * Копирование координат точки. + * + * @param v точка, координаты которой копируются + */ + public function vCopy(v:Vector3):Vector3 { + x = v.x; + y = v.y; + z = v.z; + return this; + } + + /** + * Клонирование точки. + * + * @return клонированная точка + */ + public function vClone():Vector3 { + return new Vector3(x, y, z); + } + + /** + * Копирует координаты вектора в экземпляр класса Vector3D. + * + * @param result экземпляр, в который копируются координаты + * @return переданный параметр + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + return result; + } + + /** + * @param source + * @return + */ + public function copyFromVector3D(source:Vector3D):Vector3 { + x = source.x; + y = source.y; + z = source.z; + return this; + } + + override public function toString():String { + return "Vector3(" + x + "," + y + ", " + z + ")"; + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/primitives/.svn/all-wcprops b/0.0.7.1/src/alternativa/physics/primitives/.svn/all-wcprops new file mode 100644 index 0000000..b76e733 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/primitives/.svn/all-wcprops @@ -0,0 +1,35 @@ +K 25 +svn:wc:ra_dav:version-url +V 114 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/primitives +END +RigidPlane.as +K 25 +svn:wc:ra_dav:version-url +V 128 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/primitives/RigidPlane.as +END +RigidSphere.as +K 25 +svn:wc:ra_dav:version-url +V 129 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/primitives/RigidSphere.as +END +RigidBox.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/primitives/RigidBox.as +END +RigidCylinder.as +K 25 +svn:wc:ra_dav:version-url +V 131 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/primitives/RigidCylinder.as +END +RigidRect.as +K 25 +svn:wc:ra_dav:version-url +V 127 +/!svn/ver/26572/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/primitives/RigidRect.as +END diff --git a/0.0.7.1/src/alternativa/physics/primitives/.svn/entries b/0.0.7.1/src/alternativa/physics/primitives/.svn/entries new file mode 100644 index 0000000..2ec4b13 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/primitives/.svn/entries @@ -0,0 +1,88 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.7.1/src/alternativa/physics/primitives +http://svndev.alternativaplatform.com + + + +2009-10-19T07:17:33.113306Z +22253 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +RigidPlane.as +file + + + + +2010-10-28T04:32:42.000000Z +3a8402814cf99a68d018a90077f62746 +2009-10-19T07:17:33.113306Z +22253 +mike + +RigidSphere.as +file + + + + +2010-10-28T04:32:42.000000Z +1a898ad68fc203a75a157cbf21f7dc92 +2009-10-19T07:17:33.113306Z +22253 +mike + +RigidBox.as +file + + + + +2010-10-28T04:32:42.000000Z +ae534a73f34403a99de94e649d45e238 +2009-10-19T07:17:33.113306Z +22253 +mike + +RigidCylinder.as +file + + + + +2010-10-28T04:32:42.000000Z +86388b2dc5c35fd45cc1b0bcbd09c517 +2009-10-19T07:17:33.113306Z +22253 +mike + +RigidRect.as +file + + + + +2010-10-28T04:32:42.000000Z +bdc2ecbdfebbd0417a19519c338ef54c +2009-10-19T07:17:33.113306Z +22253 +mike + diff --git a/0.0.7.1/src/alternativa/physics/primitives/.svn/format b/0.0.7.1/src/alternativa/physics/primitives/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/primitives/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.7.1/src/alternativa/physics/primitives/.svn/text-base/RigidBox.as.svn-base b/0.0.7.1/src/alternativa/physics/primitives/.svn/text-base/RigidBox.as.svn-base new file mode 100644 index 0000000..bd59d52 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/primitives/.svn/text-base/RigidBox.as.svn-base @@ -0,0 +1,56 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.math.Matrix3; + import alternativa.physics.math.Vector3; + + use namespace altphysics; + + /** + * + */ + public class RigidBox extends Body { + + /** + * + * @param halfSize + * @param mass + */ + public function RigidBox(halfSize:Vector3, mass:Number) { + super(0, Matrix3.ZERO); + addCollisionPrimitive(new CollisionBox(halfSize, 1)); + setParams(halfSize, mass); + } + + /** + * @param halfSize + * @param mass + */ + public function setParams(halfSize:Vector3, mass:Number):void { + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + invInertia.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + invInertia.a = 3*invMass/(yy + zz); + invInertia.f = 3*invMass/(zz + xx); + invInertia.k = 3*invMass/(xx + yy); + } + var prim:CollisionBox = CollisionBox(collisionPrimitives.head.primitive); + prim.hs.vCopy(halfSize); + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/primitives/.svn/text-base/RigidCylinder.as.svn-base b/0.0.7.1/src/alternativa/physics/primitives/.svn/text-base/RigidCylinder.as.svn-base new file mode 100644 index 0000000..a3ef3cd --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/primitives/.svn/text-base/RigidCylinder.as.svn-base @@ -0,0 +1,32 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.math.Matrix3; + + use namespace altphysics; + + public class RigidCylinder extends Body { + + altphysics var r:Number; + altphysics var h:Number; + + public function RigidCylinder(mass:Number, radius:Number, height:Number) { + super(0, Matrix3.ZERO); + setParams(mass, radius, height); + } + + public function setParams(mass:Number, radius:Number, height:Number):void { + r = radius; + h = height; + invInertiaWorld.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + invInertia.a = invInertia.f = mass*(h*h/12 + r*r/4); + invInertia.k = 0.5*mass*r*r; + invInertia.invert(); + } + } + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/primitives/.svn/text-base/RigidPlane.as.svn-base b/0.0.7.1/src/alternativa/physics/primitives/.svn/text-base/RigidPlane.as.svn-base new file mode 100644 index 0000000..3df7672 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/primitives/.svn/text-base/RigidPlane.as.svn-base @@ -0,0 +1,23 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.math.Matrix3; + import alternativa.physics.math.Vector3; + + use namespace altphysics; + + public class RigidPlane extends Body { + + public var normal:Vector3 = new Vector3(); + public var offset:Number; + + public function RigidPlane() { + super(0, Matrix3.ZERO); + this.normal.vCopy(Vector3.Z_AXIS); + this.offset = 0; + movable = false; + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/primitives/.svn/text-base/RigidRect.as.svn-base b/0.0.7.1/src/alternativa/physics/primitives/.svn/text-base/RigidRect.as.svn-base new file mode 100644 index 0000000..5219666 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/primitives/.svn/text-base/RigidRect.as.svn-base @@ -0,0 +1,20 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.physics.math.Matrix3; + import alternativa.physics.math.Vector3; + + use namespace altphysics; + + public class RigidRect extends Body { + + public function RigidRect(halfWidth:Number, halfLength:Number) { + super(0, Matrix3.ZERO); + movable = false; + addCollisionPrimitive(new CollisionRect(new Vector3(halfWidth, halfLength, 0), 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/primitives/.svn/text-base/RigidSphere.as.svn-base b/0.0.7.1/src/alternativa/physics/primitives/.svn/text-base/RigidSphere.as.svn-base new file mode 100644 index 0000000..63931e4 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/primitives/.svn/text-base/RigidSphere.as.svn-base @@ -0,0 +1,29 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.math.Matrix3; + + use namespace altphysics; + + public class RigidSphere extends Body { + + public function RigidSphere(radius:Number, mass:Number) { + var m:Matrix3; + if (mass == Infinity) { + mass = 0; + m = Matrix3.ZERO; + } else { + mass = 1/mass; + var r2:Number = radius*radius; + m = new Matrix3(); + m.a = m.f = m.k = 2.5*mass/r2; + } + super(mass, m); + if (mass == 0) movable = false; + addCollisionPrimitive(new CollisionSphere(radius, 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/primitives/RigidBox.as b/0.0.7.1/src/alternativa/physics/primitives/RigidBox.as new file mode 100644 index 0000000..bd59d52 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/primitives/RigidBox.as @@ -0,0 +1,56 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.math.Matrix3; + import alternativa.physics.math.Vector3; + + use namespace altphysics; + + /** + * + */ + public class RigidBox extends Body { + + /** + * + * @param halfSize + * @param mass + */ + public function RigidBox(halfSize:Vector3, mass:Number) { + super(0, Matrix3.ZERO); + addCollisionPrimitive(new CollisionBox(halfSize, 1)); + setParams(halfSize, mass); + } + + /** + * @param halfSize + * @param mass + */ + public function setParams(halfSize:Vector3, mass:Number):void { + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + invInertia.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + invInertia.a = 3*invMass/(yy + zz); + invInertia.f = 3*invMass/(zz + xx); + invInertia.k = 3*invMass/(xx + yy); + } + var prim:CollisionBox = CollisionBox(collisionPrimitives.head.primitive); + prim.hs.vCopy(halfSize); + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/primitives/RigidCylinder.as b/0.0.7.1/src/alternativa/physics/primitives/RigidCylinder.as new file mode 100644 index 0000000..a3ef3cd --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/primitives/RigidCylinder.as @@ -0,0 +1,32 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.math.Matrix3; + + use namespace altphysics; + + public class RigidCylinder extends Body { + + altphysics var r:Number; + altphysics var h:Number; + + public function RigidCylinder(mass:Number, radius:Number, height:Number) { + super(0, Matrix3.ZERO); + setParams(mass, radius, height); + } + + public function setParams(mass:Number, radius:Number, height:Number):void { + r = radius; + h = height; + invInertiaWorld.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + invInertia.a = invInertia.f = mass*(h*h/12 + r*r/4); + invInertia.k = 0.5*mass*r*r; + invInertia.invert(); + } + } + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/primitives/RigidPlane.as b/0.0.7.1/src/alternativa/physics/primitives/RigidPlane.as new file mode 100644 index 0000000..3df7672 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/primitives/RigidPlane.as @@ -0,0 +1,23 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.math.Matrix3; + import alternativa.physics.math.Vector3; + + use namespace altphysics; + + public class RigidPlane extends Body { + + public var normal:Vector3 = new Vector3(); + public var offset:Number; + + public function RigidPlane() { + super(0, Matrix3.ZERO); + this.normal.vCopy(Vector3.Z_AXIS); + this.offset = 0; + movable = false; + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/primitives/RigidRect.as b/0.0.7.1/src/alternativa/physics/primitives/RigidRect.as new file mode 100644 index 0000000..5219666 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/primitives/RigidRect.as @@ -0,0 +1,20 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.physics.math.Matrix3; + import alternativa.physics.math.Vector3; + + use namespace altphysics; + + public class RigidRect extends Body { + + public function RigidRect(halfWidth:Number, halfLength:Number) { + super(0, Matrix3.ZERO); + movable = false; + addCollisionPrimitive(new CollisionRect(new Vector3(halfWidth, halfLength, 0), 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.7.1/src/alternativa/physics/primitives/RigidSphere.as b/0.0.7.1/src/alternativa/physics/primitives/RigidSphere.as new file mode 100644 index 0000000..63931e4 --- /dev/null +++ b/0.0.7.1/src/alternativa/physics/primitives/RigidSphere.as @@ -0,0 +1,29 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.math.Matrix3; + + use namespace altphysics; + + public class RigidSphere extends Body { + + public function RigidSphere(radius:Number, mass:Number) { + var m:Matrix3; + if (mass == Infinity) { + mass = 0; + m = Matrix3.ZERO; + } else { + mass = 1/mass; + var r2:Number = radius*radius; + m = new Matrix3(); + m.a = m.f = m.k = 2.5*mass/r2; + } + super(mass, m); + if (mass == 0) movable = false; + addCollisionPrimitive(new CollisionSphere(radius, 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/.actionScriptProperties b/0.0.8.0/.actionScriptProperties new file mode 100644 index 0000000..f73adc8 --- /dev/null +++ b/0.0.8.0/.actionScriptProperties @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/0.0.8.0/.flexLibProperties b/0.0.8.0/.flexLibProperties new file mode 100644 index 0000000..652bb2a --- /dev/null +++ b/0.0.8.0/.flexLibProperties @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/0.0.8.0/.project b/0.0.8.0/.project new file mode 100644 index 0000000..1f1425f --- /dev/null +++ b/0.0.8.0/.project @@ -0,0 +1,19 @@ + + + AlternativaPhysics + + + + + + com.adobe.flexbuilder.project.flexbuilder + + + + + + org.maven.ide.eclipse.maven2Nature + com.adobe.flexbuilder.project.flexlibnature + com.adobe.flexbuilder.project.actionscriptnature + + diff --git a/0.0.8.0/.settings/.svn/all-wcprops b/0.0.8.0/.settings/.svn/all-wcprops new file mode 100644 index 0000000..f2b3181 --- /dev/null +++ b/0.0.8.0/.settings/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 89 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/.settings +END +org.eclipse.core.resources.prefs +K 25 +svn:wc:ra_dav:version-url +V 122 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/.settings/org.eclipse.core.resources.prefs +END diff --git a/0.0.8.0/.settings/.svn/entries b/0.0.8.0/.settings/.svn/entries new file mode 100644 index 0000000..f6d3f6a --- /dev/null +++ b/0.0.8.0/.settings/.svn/entries @@ -0,0 +1,40 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/.settings +http://svndev.alternativaplatform.com + + + +2009-04-01T12:25:29.638016Z +10301 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +org.eclipse.core.resources.prefs +file + + + + +2010-10-28T04:32:43.000000Z +4f70b476f3e5075e399505021df2f89b +2009-04-01T12:25:29.638016Z +10301 +mike + diff --git a/0.0.8.0/.settings/.svn/format b/0.0.8.0/.settings/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.8.0/.settings/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.8.0/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base b/0.0.8.0/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base new file mode 100644 index 0000000..b3d0d49 --- /dev/null +++ b/0.0.8.0/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base @@ -0,0 +1,3 @@ +#Fri Mar 20 00:19:33 YEKT 2009 +eclipse.preferences.version=1 +encoding/=UTF-8 diff --git a/0.0.8.0/.settings/org.eclipse.core.resources.prefs b/0.0.8.0/.settings/org.eclipse.core.resources.prefs new file mode 100644 index 0000000..b3d0d49 --- /dev/null +++ b/0.0.8.0/.settings/org.eclipse.core.resources.prefs @@ -0,0 +1,3 @@ +#Fri Mar 20 00:19:33 YEKT 2009 +eclipse.preferences.version=1 +encoding/=UTF-8 diff --git a/0.0.8.0/.svn/all-wcprops b/0.0.8.0/.svn/all-wcprops new file mode 100644 index 0000000..9c9706f --- /dev/null +++ b/0.0.8.0/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 79 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0 +END +.flexLibProperties +K 25 +svn:wc:ra_dav:version-url +V 98 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/.flexLibProperties +END +.project +K 25 +svn:wc:ra_dav:version-url +V 88 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/.project +END +pom.xml +K 25 +svn:wc:ra_dav:version-url +V 87 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/pom.xml +END +.actionScriptProperties +K 25 +svn:wc:ra_dav:version-url +V 103 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/.actionScriptProperties +END diff --git a/0.0.8.0/.svn/dir-prop-base b/0.0.8.0/.svn/dir-prop-base new file mode 100644 index 0000000..c31ca28 --- /dev/null +++ b/0.0.8.0/.svn/dir-prop-base @@ -0,0 +1,7 @@ +K 13 +svn:mergeinfo +V 278 +/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized:19806-19860 +/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.7.0.LinkedList:20360-22217,22226-22252 +/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D:13892-14098 +END diff --git a/0.0.8.0/.svn/entries b/0.0.8.0/.svn/entries new file mode 100644 index 0000000..65a0114 --- /dev/null +++ b/0.0.8.0/.svn/entries @@ -0,0 +1,85 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0 +http://svndev.alternativaplatform.com + + + +2009-11-13T09:43:18.278323Z +23340 +mike +has-props + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +META-INF +dir + +.flexLibProperties +file + + + + +2010-10-28T04:32:43.000000Z +c8e034e8e09f239471f9f69abce3a7bb +2009-10-19T07:17:33.113306Z +22253 +mike + +.project +file + + + + +2010-10-28T04:32:43.000000Z +1e90cc68b52b93173b2b5de88eb5c3a3 +2009-04-20T17:15:55.260110Z +11541 +mike + +src +dir + +pom.xml +file + + + + +2010-10-28T04:32:43.000000Z +03d56a45edddbda4f480dd2a69e5d8f8 +2009-11-13T09:43:18.278323Z +23340 +mike + +.actionScriptProperties +file + + + + +2010-10-28T04:32:43.000000Z +b8656f882fff2baedd57a92db86fcf8c +2009-10-18T11:19:50.210229Z +22219 +mike + +.settings +dir + diff --git a/0.0.8.0/.svn/format b/0.0.8.0/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.8.0/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.8.0/.svn/text-base/.actionScriptProperties.svn-base b/0.0.8.0/.svn/text-base/.actionScriptProperties.svn-base new file mode 100644 index 0000000..f73adc8 --- /dev/null +++ b/0.0.8.0/.svn/text-base/.actionScriptProperties.svn-base @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/0.0.8.0/.svn/text-base/.flexLibProperties.svn-base b/0.0.8.0/.svn/text-base/.flexLibProperties.svn-base new file mode 100644 index 0000000..652bb2a --- /dev/null +++ b/0.0.8.0/.svn/text-base/.flexLibProperties.svn-base @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/0.0.8.0/.svn/text-base/.project.svn-base b/0.0.8.0/.svn/text-base/.project.svn-base new file mode 100644 index 0000000..1f1425f --- /dev/null +++ b/0.0.8.0/.svn/text-base/.project.svn-base @@ -0,0 +1,19 @@ + + + AlternativaPhysics + + + + + + com.adobe.flexbuilder.project.flexbuilder + + + + + + org.maven.ide.eclipse.maven2Nature + com.adobe.flexbuilder.project.flexlibnature + com.adobe.flexbuilder.project.actionscriptnature + + diff --git a/0.0.8.0/.svn/text-base/pom.xml.svn-base b/0.0.8.0/.svn/text-base/pom.xml.svn-base new file mode 100644 index 0000000..6e15235 --- /dev/null +++ b/0.0.8.0/.svn/text-base/pom.xml.svn-base @@ -0,0 +1,17 @@ + + 4.0.0 + platform.clients.fp10.libraries + AlternativaPhysics + swc + 0.0.8.0 + + platform.tools.maven + FlashBasePom + 1.0 + + + scm:svn:http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0 + + + + \ No newline at end of file diff --git a/0.0.8.0/META-INF/.svn/all-wcprops b/0.0.8.0/META-INF/.svn/all-wcprops new file mode 100644 index 0000000..67146e9 --- /dev/null +++ b/0.0.8.0/META-INF/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 88 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/META-INF +END +MANIFEST.MF +K 25 +svn:wc:ra_dav:version-url +V 100 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/META-INF/MANIFEST.MF +END diff --git a/0.0.8.0/META-INF/.svn/entries b/0.0.8.0/META-INF/.svn/entries new file mode 100644 index 0000000..6c6f081 --- /dev/null +++ b/0.0.8.0/META-INF/.svn/entries @@ -0,0 +1,40 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/META-INF +http://svndev.alternativaplatform.com + + + +2009-04-20T17:15:55.260110Z +11541 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +MANIFEST.MF +file + + + + +2010-10-28T04:32:42.000000Z +7b64dceb50a6905850ff00a0bfbe77eb +2009-04-20T17:15:55.260110Z +11541 +mike + diff --git a/0.0.8.0/META-INF/.svn/format b/0.0.8.0/META-INF/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.8.0/META-INF/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.8.0/META-INF/.svn/text-base/MANIFEST.MF.svn-base b/0.0.8.0/META-INF/.svn/text-base/MANIFEST.MF.svn-base new file mode 100644 index 0000000..5066a55 --- /dev/null +++ b/0.0.8.0/META-INF/.svn/text-base/MANIFEST.MF.svn-base @@ -0,0 +1 @@ +Bundle-Name: platform.clients.fp10.libraries.AlternativaPhysics diff --git a/0.0.8.0/META-INF/MANIFEST.MF b/0.0.8.0/META-INF/MANIFEST.MF new file mode 100644 index 0000000..5066a55 --- /dev/null +++ b/0.0.8.0/META-INF/MANIFEST.MF @@ -0,0 +1 @@ +Bundle-Name: platform.clients.fp10.libraries.AlternativaPhysics diff --git a/0.0.8.0/pom.xml b/0.0.8.0/pom.xml new file mode 100644 index 0000000..6e15235 --- /dev/null +++ b/0.0.8.0/pom.xml @@ -0,0 +1,17 @@ + + 4.0.0 + platform.clients.fp10.libraries + AlternativaPhysics + swc + 0.0.8.0 + + platform.tools.maven + FlashBasePom + 1.0 + + + scm:svn:http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0 + + + + \ No newline at end of file diff --git a/0.0.8.0/src/.svn/all-wcprops b/0.0.8.0/src/.svn/all-wcprops new file mode 100644 index 0000000..7b71c71 --- /dev/null +++ b/0.0.8.0/src/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 83 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src +END diff --git a/0.0.8.0/src/.svn/entries b/0.0.8.0/src/.svn/entries new file mode 100644 index 0000000..69150e7 --- /dev/null +++ b/0.0.8.0/src/.svn/entries @@ -0,0 +1,31 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src +http://svndev.alternativaplatform.com + + + +2009-11-01T12:35:52.270571Z +22955 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +alternativa +dir + diff --git a/0.0.8.0/src/.svn/format b/0.0.8.0/src/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.8.0/src/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.8.0/src/alternativa/.svn/all-wcprops b/0.0.8.0/src/alternativa/.svn/all-wcprops new file mode 100644 index 0000000..c24fbac --- /dev/null +++ b/0.0.8.0/src/alternativa/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 95 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa +END diff --git a/0.0.8.0/src/alternativa/.svn/entries b/0.0.8.0/src/alternativa/.svn/entries new file mode 100644 index 0000000..93b3b26 --- /dev/null +++ b/0.0.8.0/src/alternativa/.svn/entries @@ -0,0 +1,31 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa +http://svndev.alternativaplatform.com + + + +2009-11-01T12:35:52.270571Z +22955 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +physics +dir + diff --git a/0.0.8.0/src/alternativa/.svn/format b/0.0.8.0/src/alternativa/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.8.0/src/alternativa/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.8.0/src/alternativa/physics/.svn/all-wcprops b/0.0.8.0/src/alternativa/physics/.svn/all-wcprops new file mode 100644 index 0000000..5a6f6b5 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/.svn/all-wcprops @@ -0,0 +1,77 @@ +K 25 +svn:wc:ra_dav:version-url +V 103 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics +END +Body.as +K 25 +svn:wc:ra_dav:version-url +V 111 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/Body.as +END +BodyState.as +K 25 +svn:wc:ra_dav:version-url +V 116 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/BodyState.as +END +CollisionPrimitiveListItem.as +K 25 +svn:wc:ra_dav:version-url +V 133 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/CollisionPrimitiveListItem.as +END +CollisionPrimitiveList.as +K 25 +svn:wc:ra_dav:version-url +V 129 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/CollisionPrimitiveList.as +END +altphysics.as +K 25 +svn:wc:ra_dav:version-url +V 117 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/altphysics.as +END +PhysicsUtils.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/PhysicsUtils.as +END +ContactPoint.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/ContactPoint.as +END +World.as +K 25 +svn:wc:ra_dav:version-url +V 112 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/World.as +END +BodyListItem.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/BodyListItem.as +END +BodyList.as +K 25 +svn:wc:ra_dav:version-url +V 115 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/BodyList.as +END +Contact.as +K 25 +svn:wc:ra_dav:version-url +V 114 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/Contact.as +END +BodyMaterial.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/BodyMaterial.as +END diff --git a/0.0.8.0/src/alternativa/physics/.svn/entries b/0.0.8.0/src/alternativa/physics/.svn/entries new file mode 100644 index 0000000..6c3fa5c --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/.svn/entries @@ -0,0 +1,184 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics +http://svndev.alternativaplatform.com + + + +2009-11-01T12:35:52.270571Z +22955 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +CollisionPrimitiveList.as +file + + + + +2010-10-28T04:32:43.000000Z +5752b1f4909c5146720b58fbfffed468 +2009-11-01T12:35:52.270571Z +22955 +mike + +altphysics.as +file + + + + +2010-10-28T04:32:43.000000Z +04fbe40a27502dbbd0dba02a76b2df50 +2009-04-20T20:09:14.715403Z +11579 +mike + +math +dir + +World.as +file + + + + +2010-10-28T04:32:43.000000Z +751ad3f2ae01cf07dbb52dbe10219b75 +2009-10-20T10:42:06.632117Z +22312 +mike + +collision +dir + +primitives +dir + +BodyList.as +file + + + + +2010-10-28T04:32:43.000000Z +08a90332660bd0273728006d1402dd25 +2009-11-01T12:35:52.270571Z +22955 +mike + +Contact.as +file + + + + +2010-10-28T04:32:43.000000Z +675ca7525e2b91ffcb1647978d39eca7 +2009-10-19T07:17:33.113306Z +22253 +mike + +Body.as +file + + + + +2010-10-28T04:32:43.000000Z +99cb2defe02f25078bfd8876dc4ed545 +2009-10-23T05:59:29.567666Z +22444 +mike + +BodyState.as +file + + + + +2010-10-28T04:32:43.000000Z +2ae471fd05b986dce6c19ac37a6beaa8 +2009-10-19T07:17:33.113306Z +22253 +mike + +CollisionPrimitiveListItem.as +file + + + + +2010-10-28T04:32:43.000000Z +cec5dbfefef812d87481478455f367b1 +2009-10-19T07:17:33.113306Z +22253 +mike + +PhysicsUtils.as +file + + + + +2010-10-28T04:32:43.000000Z +b400cdcb00dc64d1dcb1300c95c53fac +2009-10-19T07:17:33.113306Z +22253 +mike + +constraints +dir + +ContactPoint.as +file + + + + +2010-10-28T04:32:43.000000Z +88112c934e6a35adcc6b375d2c6f93b5 +2009-10-19T07:17:33.113306Z +22253 +mike + +BodyListItem.as +file + + + + +2010-10-28T04:32:43.000000Z +2fc2b598310b24aef3f8b90e14a8ba4f +2009-10-19T07:17:33.113306Z +22253 +mike + +BodyMaterial.as +file + + + + +2010-10-28T04:32:43.000000Z +9b700692ebb8017882f3e8cec095e1a9 +2009-10-19T07:17:33.113306Z +22253 +mike + diff --git a/0.0.8.0/src/alternativa/physics/.svn/format b/0.0.8.0/src/alternativa/physics/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.8.0/src/alternativa/physics/.svn/text-base/Body.as.svn-base b/0.0.8.0/src/alternativa/physics/.svn/text-base/Body.as.svn-base new file mode 100644 index 0000000..6b53304 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/.svn/text-base/Body.as.svn-base @@ -0,0 +1,406 @@ +package alternativa.physics { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.IBodyCollisionPredicate; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.math.Matrix3; + import alternativa.physics.math.Matrix4; + import alternativa.physics.math.Quaternion; + import alternativa.physics.math.Vector3; + + use namespace altphysics; + + /** + * + */ + public class Body { + + public static var linDamping:Number = 0.997; + public static var rotDamping:Number = 0.997; + + // Идентификатор тела, уникальный в пределах мира + public var id:int; + // Имя тела + public var name:String; + // Мир, в котором находится тело + public var world:World; + // Флаг подвижности тела + public var movable:Boolean = true; + // Флаг указывает, может ли тело быть заморожено + public var canFreeze:Boolean = false; + + public var freezeCounter:int; + public var frozen:Boolean = false; + // Ограничивающий бокс тела + public var aabb:BoundBox = new BoundBox(); + // Предикат, через который тело получает сообщения о столкновениях + public var postCollisionPredicate:IBodyCollisionPredicate; + + // Текущее состояние тела + public var state:BodyState = new BodyState(); + // Предыдущее состояние тела + public var prevState:BodyState = new BodyState(); + // Линейное ускорение тела на текущем шаге симуляции + public var accel:Vector3 = new Vector3(); + // Угловое ускорение тела на текущем шаге симуляции + public var angleAccel:Vector3 = new Vector3(); + // Физический материал тела + public var material:BodyMaterial = new BodyMaterial(); + // Обратная масса тела + public var invMass:Number = 1; + // Обратная матрица тензора инерции в локальных координатах + public var invInertia:Matrix3 = new Matrix3(); + // Обратная матрица тензора инерции в мировых координатах + public var invInertiaWorld:Matrix3 = new Matrix3(); + // Базисная матрица тела в мировых координатах + public var baseMatrix:Matrix3 = new Matrix3(); + + public const MAX_CONTACTS:int = 20; + public var contacts:Vector. = new Vector.(MAX_CONTACTS); + public var contactsNum:int; + + public var collisionPrimitives:CollisionPrimitiveList; + + // Аккумулятор сил + public var forceAccum:Vector3 = new Vector3(); + // Аккумулятор моментов + public var torqueAccum:Vector3 = new Vector3(); + + // Внутренние переменные для избежания создания экземпляров + private static var _r:Vector3 = new Vector3(); + private static var _f:Vector3 = new Vector3(); + + /** + * + * @param invMass + * @param invInertia + */ + public function Body(invMass:Number, invInertia:Matrix3) { + this.invMass = invMass; + this.invInertia.copy(invInertia); + } + + /** + * @param primitive + * @param localTransform + */ + public function addCollisionPrimitive(primitive:CollisionPrimitive, localTransform:Matrix4 = null):void { + if (primitive == null) { + throw new ArgumentError("Primitive cannot be null"); + } + if (collisionPrimitives == null) { + collisionPrimitives = new CollisionPrimitiveList(); + } + collisionPrimitives.append(primitive); + primitive.setBody(this, localTransform); + } + + /** + * + * @param primitive + */ + public function removeCollisionPrimitive(primitive:CollisionPrimitive):void { + if (collisionPrimitives == null) return; + primitive.setBody(null); + collisionPrimitives.remove(primitive); + if (collisionPrimitives.size == 0) { + collisionPrimitives = null; + } + } + + /** + * @param t + * @param result + */ + public function interpolate(t:Number, pos:Vector3, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.w = prevState.orientation.w*t1 + state.orientation.w*t; + orientation.x = prevState.orientation.x*t1 + state.orientation.x*t; + orientation.y = prevState.orientation.y*t1 + state.orientation.y*t; + orientation.z = prevState.orientation.z*t1 + state.orientation.z*t; + } + + /** + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + state.pos.vCopy(pos); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setPositionXYZ(x:Number, y:Number, z:Number):void { + state.pos.vReset(x, y, z); + } + + /** + * + * @param vel + */ + public function setVelocity(vel:Vector3):void { + state.velocity.vCopy(vel); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setVelocityXYZ(x:Number, y:Number, z:Number):void { + state.velocity.vReset(x, y, z); + } + + /** + * + * @param rot + */ + public function setRotation(rot:Vector3):void { + state.rotation.vCopy(rot); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setRotationXYZ(x:Number, y:Number, z:Number):void { + state.rotation.vReset(x, y, z); + } + + /** + * + * @param q + */ + public function setOrientation(q:Quaternion):void { + state.orientation.copy(q); + } + + /** + * @param r + * @param dir + * @param magnitude + */ + public function applyRelPosWorldImpulse(r:Vector3, dir:Vector3, magnitude:Number):void { + var d:Number = magnitude*invMass; + // Линейная часть + state.velocity.x += d*dir.x; + state.velocity.y += d*dir.y; + state.velocity.z += d*dir.z; + + // Вращательная часть + var x:Number = (r.y*dir.z - r.z*dir.y)*magnitude; + var y:Number = (r.z*dir.x - r.x*dir.z)*magnitude; + var z:Number = (r.x*dir.y - r.y*dir.x)*magnitude; + + state.rotation.x += invInertiaWorld.a*x + invInertiaWorld.b*y + invInertiaWorld.c*z; + state.rotation.y += invInertiaWorld.e*x + invInertiaWorld.f*y + invInertiaWorld.g*z; + state.rotation.z += invInertiaWorld.i*x + invInertiaWorld.j*y + invInertiaWorld.k*z; + } + + /** + * @param f + */ + public function addForce(f:Vector3):void { + forceAccum.vAdd(f); + } + + /** + * + * @param fx + * @param fy + * @param fz + */ + public function addForceXYZ(fx:Number, fy:Number, fz:Number):void { + forceAccum.x += fx; + forceAccum.y += fy; + forceAccum.z += fz; + } + + /** + * @param pos + * @param f + */ + public function addWorldForceXYZ(px:Number, py:Number, pz:Number, fx:Number, fy:Number, fz:Number):void { + forceAccum.x += fx; + forceAccum.y += fy; + forceAccum.z += fz; + + var pos:Vector3 = state.pos; + var rx:Number = px - pos.x; + var ry:Number = py - pos.y; + var rz:Number = pz - pos.z; + +// var x:Number = ry*fz - rz*fy; +// var y:Number = rz*fx - rx*fz; +// var z:Number = rx*fy - ry*fx; + + torqueAccum.x += ry*fz - rz*fy; + torqueAccum.y += rz*fx - rx*fz; + torqueAccum.z += rx*fy - ry*fx; + } + + /** + * @param pos + * @param f + */ + public function addWorldForce(pos:Vector3, force:Vector3):void { + forceAccum.vAdd(force); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(force)); + } + + /** + * @param pos + * @param f + */ + public function addWorldForceScaled(pos:Vector3, force:Vector3, scale:Number):void { + _f.x = scale*force.x; + _f.y = scale*force.y; + _f.z = scale*force.z; + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(_f)); + } + + /** + * @param pos + * @param f + */ + public function addLocalForce(pos:Vector3, force:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(pos, _r); + // Трансформируем вектор силы в мировую систему + baseMatrix.transformVector(force, _f); + // Добавляем силу и момент + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vCross(_f)); + } + + /** + * + * @param localPos + * @param worldForce + */ + public function addWorldForceAtLocalPoint(localPos:Vector3, worldForce:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(localPos, _r); + // Добавляем силу и момент + forceAccum.vAdd(worldForce); + torqueAccum.vAdd(_r.vCross(worldForce)); + } + + /** + * @param dt + */ + public function beforePhysicsStep(dt:Number):void { + } + + /** + * @param t + */ + public function addTorque(t:Vector3):void { + torqueAccum.vAdd(t); + } + + /** + * + */ + altphysics function clearAccumulators():void { + forceAccum.x = forceAccum.y = forceAccum.z = 0; + torqueAccum.x = torqueAccum.y = torqueAccum.z = 0; + } + + /** + * + */ + altphysics function calcAccelerations():void { + accel.x = forceAccum.x*invMass; + accel.y = forceAccum.y*invMass; + accel.z = forceAccum.z*invMass; + angleAccel.x = invInertiaWorld.a*torqueAccum.x + invInertiaWorld.b*torqueAccum.y + invInertiaWorld.c*torqueAccum.z; + angleAccel.y = invInertiaWorld.e*torqueAccum.x + invInertiaWorld.f*torqueAccum.y + invInertiaWorld.g*torqueAccum.z; + angleAccel.z = invInertiaWorld.i*torqueAccum.x + invInertiaWorld.j*torqueAccum.y + invInertiaWorld.k*torqueAccum.z; + } + + /** + * Вычисляет производные данные. + */ + public function calcDerivedData():void { + // Вычисление базисной матрицы и обратного тензора инерции в мировых координатах + state.orientation.toMatrix3(baseMatrix); + invInertiaWorld.copy(invInertia).append(baseMatrix).prependTransposed(baseMatrix); + if (collisionPrimitives != null) { + aabb.infinity(); + var item:CollisionPrimitiveListItem = collisionPrimitives.head; + while (item != null) { + var primitive:CollisionPrimitive = item.primitive; + primitive.transform.setFromMatrix3(baseMatrix, state.pos); + if (primitive.localTransform != null) { + primitive.transform.prepend(primitive.localTransform); + } + primitive.calculateAABB(); + aabb.addBoundBox(primitive.aabb); + item = item.next; + } + } + } + + /** + * + */ + altphysics function saveState():void { + prevState.copy(state); + } + + /** + * + */ + altphysics function restoreState():void { + state.copy(prevState); + } + + /** + * @param dt + */ + altphysics function integrateVelocity(dt:Number):void { + // v = v + a*t + state.velocity.x += accel.x*dt; + state.velocity.y += accel.y*dt; + state.velocity.z += accel.z*dt; + // rot = rot + eps*t + state.rotation.x += angleAccel.x*dt; + state.rotation.y += angleAccel.y*dt; + state.rotation.z += angleAccel.z*dt; + + state.velocity.x *= linDamping; + state.velocity.y *= linDamping; + state.velocity.z *= linDamping; + + state.rotation.x *= rotDamping; + state.rotation.y *= rotDamping; + state.rotation.z *= rotDamping; + } + + /** + * + */ + altphysics function integratePosition(dt:Number):void { + // pos = pos + v*t + state.pos.x += state.velocity.x*dt; + state.pos.y += state.velocity.y*dt; + state.pos.z += state.velocity.z*dt; + // q = q + 0.5*rot*q + state.orientation.addScaledVector(state.rotation, dt); + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/.svn/text-base/BodyList.as.svn-base b/0.0.8.0/src/alternativa/physics/.svn/text-base/BodyList.as.svn-base new file mode 100644 index 0000000..aa8f79f --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/.svn/text-base/BodyList.as.svn-base @@ -0,0 +1,76 @@ +package alternativa.physics { + + /** + * + */ + public class BodyList { + + public var head:BodyListItem; + public var tail:BodyListItem; + public var size:int; + + /** + * + */ + public function BodyList() { + } + + /** + * + * @param body + */ + public function append(body:Body):void { + var item:BodyListItem = BodyListItem.create(body); + if (head == null) { + head = tail = item; + } else { + tail.next = item; + item.prev = tail; + tail = item; + } + size++; + } + + /** + * + * @param body + */ + public function remove(body:Body):Boolean { + var item:BodyListItem = findItem(body); + if (item == null) return false; + if (item == head) { + if (size == 1) { + head = tail = null; + } else { + head = item.next; + head.prev = null; + } + } else { + if (item == tail) { + tail = item.prev; + tail.next = null; + } else { + item.prev.next = item.next; + item.next.prev = item.prev; + } + } + item.dispose(); + size--; + return true; + } + + /** + * + * @param body + * @return + */ + public function findItem(body:Body):BodyListItem { + var item:BodyListItem = head; + while (item != null && item.body != body) { + item = item.next; + } + return item; + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/.svn/text-base/BodyListItem.as.svn-base b/0.0.8.0/src/alternativa/physics/.svn/text-base/BodyListItem.as.svn-base new file mode 100644 index 0000000..382e11c --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/.svn/text-base/BodyListItem.as.svn-base @@ -0,0 +1,74 @@ +package alternativa.physics { + + /** + * + */ + public class BodyListItem { + + // Верхний элемент хранилища + private static var poolTop:BodyListItem; + + /** + * Создаёт новый элемент списка. + * + * @param primitive примитив, содержащийся в элементе + * @return новый элемент списка + */ + public static function create(body:Body):BodyListItem { + var item:BodyListItem; + if (poolTop == null) { + item = new BodyListItem(body); + } else { + item = poolTop; + poolTop = item.next; + item.next = null; + item.body = body; + } + return item; + } + + /** + * Очищает хранилище. + */ + public static function clearPool():void { + var item:BodyListItem = poolTop; + while (item != null) { + poolTop = item.next; + item.next = null; + item = poolTop; + } + } + + /** + * + */ + public var body:Body; + /** + * + */ + public var next:BodyListItem; + /** + * + */ + public var prev:BodyListItem; + + /** + * + * @param body + */ + public function BodyListItem(body:Body) { + this.body = body; + } + + /** + * + */ + public function dispose():void { + body = null; + prev = null; + next = poolTop; + poolTop = this; + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/.svn/text-base/BodyMaterial.as.svn-base b/0.0.8.0/src/alternativa/physics/.svn/text-base/BodyMaterial.as.svn-base new file mode 100644 index 0000000..7a7c47d --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/.svn/text-base/BodyMaterial.as.svn-base @@ -0,0 +1,11 @@ +package alternativa.physics { + + public class BodyMaterial { + + public var restitution:Number = 0; + public var friction:Number = 0.3; +// public var dynamicFriction:Number = 0.2; +// public var dynamicFriction:Number = 0.2; + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/.svn/text-base/BodyState.as.svn-base b/0.0.8.0/src/alternativa/physics/.svn/text-base/BodyState.as.svn-base new file mode 100644 index 0000000..34a1b69 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/.svn/text-base/BodyState.as.svn-base @@ -0,0 +1,39 @@ +package alternativa.physics { + import alternativa.physics.math.Quaternion; + import alternativa.physics.math.Vector3; + + /** + * Класс описывает состояние твёрдого тела. + */ + public class BodyState { + /** + * Положение тела. + */ + public var pos:Vector3 = new Vector3(); + /** + * Ориентация тела. + */ + public var orientation:Quaternion = new Quaternion(); + /** + * Скорость тела. + */ + public var velocity:Vector3 = new Vector3(); + /** + * Угловая скорость тела. + */ + public var rotation:Vector3 = new Vector3(); + + /** + * Копирует значение указанного объекта. + * + * @param state + */ + public function copy(state:BodyState):void { + pos.vCopy(state.pos); + orientation.copy(state.orientation); + velocity.vCopy(state.velocity); + rotation.vCopy(state.rotation); + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/.svn/text-base/CollisionPrimitiveList.as.svn-base b/0.0.8.0/src/alternativa/physics/.svn/text-base/CollisionPrimitiveList.as.svn-base new file mode 100644 index 0000000..5d5304f --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/.svn/text-base/CollisionPrimitiveList.as.svn-base @@ -0,0 +1,89 @@ +package alternativa.physics { + import alternativa.physics.collision.CollisionPrimitive; + + /** + * + */ + public class CollisionPrimitiveList { + + public var head:CollisionPrimitiveListItem; + public var tail:CollisionPrimitiveListItem; + public var size:int; + + /** + * + */ + public function CollisionPrimitiveList() { + } + + /** + * + * @param primitive + */ + public function append(primitive:CollisionPrimitive):void { + var item:CollisionPrimitiveListItem = CollisionPrimitiveListItem.create(primitive); + if (head == null) { + head = tail = item; + } else { + tail.next = item; + item.prev = tail; + tail = item; + } + size++; + } + + /** + * + * @param primitve + */ + public function remove(primitve:CollisionPrimitive):void { + var item:CollisionPrimitiveListItem = findItem(primitve); + if (item == null) return; + if (item == head) { + if (size == 1) { + head = tail = null; + } else { + head = item.next; + head.prev = null; + } + } else { + if (item == tail) { + tail = tail.prev; + tail.next = null; + } else { + item.prev.next = item.next; + item.next.prev = item.prev; + } + } + item.dispose(); + size--; + } + + /** + * + * @param primitive + * @return + */ + public function findItem(primitive:CollisionPrimitive):CollisionPrimitiveListItem { + var item:CollisionPrimitiveListItem = head; + while (item != null && item.primitive != primitive) { + item = item.next; + } + return item; + } + + /** + * + */ + public function clear():void { + while (head != null) { + var item:CollisionPrimitiveListItem = head; + head = head.next; + item.dispose(); + } + tail = null; + size = 0; + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/.svn/text-base/CollisionPrimitiveListItem.as.svn-base b/0.0.8.0/src/alternativa/physics/.svn/text-base/CollisionPrimitiveListItem.as.svn-base new file mode 100644 index 0000000..d204532 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/.svn/text-base/CollisionPrimitiveListItem.as.svn-base @@ -0,0 +1,76 @@ +package alternativa.physics { + import alternativa.physics.collision.CollisionPrimitive; + + /** + * Элемент списка примитивов. + */ + public class CollisionPrimitiveListItem { + + // Верхний элемент хранилища + private static var poolTop:CollisionPrimitiveListItem; + + /** + * Создаёт новый элемент списка. + * + * @param primitive примитив, содержащийся в элементе + * @return новый элемент списка + */ + public static function create(primitive:CollisionPrimitive):CollisionPrimitiveListItem { + var item:CollisionPrimitiveListItem; + if (poolTop == null) { + item = new CollisionPrimitiveListItem(primitive); + } else { + item = poolTop; + item.primitive = primitive; + poolTop = item.next; + item.next = null; + } + return item; + } + + /** + * Очищает хранилище. + */ + public static function clearPool():void { + var curr:CollisionPrimitiveListItem = poolTop; + while (curr != null) { + poolTop = curr.next; + curr.next = null; + curr = poolTop; + } + } + + /** + * Примитив, хранящийся в элементе списка. + */ + public var primitive:CollisionPrimitive; + /** + * Ссылка не следующий элемент списка. + */ + public var next:CollisionPrimitiveListItem; + /** + * Ссылка не предыдущий элемент списка. + */ + public var prev:CollisionPrimitiveListItem; + + /** + * Создаёт новый экземпляр. + * + * @param primitive примитив, хранящийся в элементе списка + */ + public function CollisionPrimitiveListItem(primitive:CollisionPrimitive) { + this.primitive = primitive; + } + + /** + * Очищает внутренние ссылки и помещает элемент в хранилище для дальнейшего использования. + */ + public function dispose():void { + primitive = null; + prev = null; + next = poolTop; + poolTop = this; + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/.svn/text-base/Contact.as.svn-base b/0.0.8.0/src/alternativa/physics/.svn/text-base/Contact.as.svn-base new file mode 100644 index 0000000..1296996 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/.svn/text-base/Contact.as.svn-base @@ -0,0 +1,69 @@ +package alternativa.physics { + import __AS3__.vec.Vector; + + import alternativa.physics.math.Vector3; + + /** + * Описывает контакт одного или двух тел. + */ + public class Contact { + /** + * Первое тело контакта, не может быть равно null. + */ + public var body1:Body; + /** + * Второе тело контакта, может быть равно null. + */ + public var body2:Body; + /** + * Предрассчитанный взаимный коэффициент отскока. + */ + public var restitution:Number; + /** + * Предрассчитанный взаимный коэффициент трения. + */ + public var friction:Number; + /** + * Нормаль контакта. Направлена к первому телу. + */ + public var normal:Vector3 = new Vector3(); + /** + * Список точек контакта. + */ + public var points:Vector. = new Vector.(MAX_POINTS, true); + /** + * Количество точек контакта. + */ + public var pcount:int; + /** + * Максимальная глубина пересечения. + */ + public var maxPenetration:Number = 0; + /** + * Флаг показывает, разрешён контакт или нет. + */ + public var satisfied:Boolean; + /** + * Следующий контакт в списке. + */ + public var next:Contact; + /** + * Индекс контакта. Первый контакт в списке имеет индекс 0. + */ + public var index:int; + + // Максимальное количество точек контакта + private const MAX_POINTS:int = 8; + + /** + * + */ + public function Contact(index:int) { + this.index = index; + for (var i:int = 0; i < MAX_POINTS; i++) { + points[i] = new ContactPoint(); + } + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/.svn/text-base/ContactPoint.as.svn-base b/0.0.8.0/src/alternativa/physics/.svn/text-base/ContactPoint.as.svn-base new file mode 100644 index 0000000..2136e9a --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/.svn/text-base/ContactPoint.as.svn-base @@ -0,0 +1,55 @@ +package alternativa.physics { + import alternativa.physics.math.Vector3; + + /** + * + */ + public class ContactPoint { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + + // Величины, расчитываемые перед началом фазы решения контактов + + // Требуемая проекция конечной скорости на нормаль для упругого контакта + public var normalVel:Number; + // Минимальная скорость разделения неупругого контакта + public var minSepVel:Number; + // Изменение проекции скорости на единицу нормального импульса + public var velByUnitImpulseN:Number; + + public var angularInertia1:Number; + public var angularInertia2:Number; + + // Радиус-вектор точки контакта относительно центра первого тела + public var r1:Vector3 = new Vector3(); + // Радиус-вектор точки контакта относительно центра второго тела + public var r2:Vector3 = new Vector3(); + + // Величины, накапливаемые во время фазы решения контактов + + // Накопленный импульс, применяемый для получения требуемой относительной скорости в точке контакта + public var accumImpulseN:Number; + // Накопленный импульс, применяемый для разделения тел в точке контакта. Не создаёт момента. +// public var accumSepImpulse:Number; + + public var satisfied:Boolean; + + /** + * + * @param cp + */ + public function copyFrom(cp:ContactPoint):void { + pos.vCopy(cp.pos); + penetration = cp.penetration; + feature1 = cp.feature1; + feature2 = cp.feature2; + r1.vCopy(cp.r1); + r2.vCopy(cp.r2); + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/.svn/text-base/PhysicsUtils.as.svn-base b/0.0.8.0/src/alternativa/physics/.svn/text-base/PhysicsUtils.as.svn-base new file mode 100644 index 0000000..6770b0b --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/.svn/text-base/PhysicsUtils.as.svn-base @@ -0,0 +1,49 @@ +package alternativa.physics { + import alternativa.physics.math.Matrix3; + import alternativa.physics.math.Vector3; + + public class PhysicsUtils { + + /** + * @param mass + * @param halfSize + * @param result + */ + public static function getBoxInvInertia(mass:Number, halfSize:Vector3, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + result.a = 3/(mass*(yy + zz)); + result.f = 3/(mass*(zz + xx)); + result.k = 3/(mass*(xx + yy)); + } + + /** + * @param mass + * @param r + * @param h + * @param result + */ + public static function getCylinderInvInertia(mass:Number, r:Number, h:Number, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + + result.a = result.f = 1/(mass*(h*h/12 + r*r/4)); + result.k = 2/(mass*r*r); + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/.svn/text-base/World.as.svn-base b/0.0.8.0/src/alternativa/physics/.svn/text-base/World.as.svn-base new file mode 100644 index 0000000..9339f20 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/.svn/text-base/World.as.svn-base @@ -0,0 +1,743 @@ +package alternativa.physics { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.ICollisionDetector; + import alternativa.physics.collision.KdTreeCollisionDetector; + import alternativa.physics.constraints.Constraint; + import alternativa.physics.math.Matrix3; + import alternativa.physics.math.Vector3; + + use namespace altphysics; + + /** + * Класс реализует физическую симуляцию поведения твёрдых тел. + */ + public class World { + + private static var lastBodyId:int; + + // Максимальное количество контактов + altphysics const MAX_CONTACTS:int = 1000; + + ////////////////////////////////////////////////////////////////// + // Настроечные параметры симуляции. Могут быть изменены в любой + // момент времени без нарушения корректной работы симуляции. + ////////////////////////////////////////////////////////////////// + // Количество шагов, за которое пересекающиеся тела должны разделиться + public var penResolutionSteps:int = 10; + // Величина допустимой глубины пересечения + public var allowedPenetration:Number = 0.1; + // Максимальная скорость, добавляемая с целью разделения тел + public var maxPenResolutionSpeed:Number = 0.5; + // Количество итераций для обработки упругих контактов + public var collisionIterations:int = 5; + // Количество итераций для обработки неупругих контактов + public var contactIterations:int = 5; + // Флаг использования предсказания состояний + public var usePrediction:Boolean = false; + + public var freezeSteps:int = 10; + public var linSpeedFreezeLimit:Number = 1; + public var angSpeedFreezeLimit:Number = 0.01; + + // Переменные для процедуры разделения тел путём непосредственного их перемещенеия. + // !!!!!!!!!!!!!!!!!!!!!!!! + // !!! Экспериментально !!! + // !!!!!!!!!!!!!!!!!!!!!!!! + public var staticSeparationIterations:int = 10; + public var staticSeparationSteps:int = 10; + public var maxAngleMove:Number = 10; + public var useStaticSeparation:Boolean = false; + + // Вектор гравитации + public var _gravity:Vector3 = new Vector3(0, 0, -9.8); + // Модуль вектора гравитации + public var _gravityMagnitude:Number = 9.8; + + // Использующийся детектор столкновений + public var collisionDetector:ICollisionDetector; + + // Список тел, участвующих в симуляции + public var bodies:BodyList = new BodyList(); + + // Список контактов на текущем шаге симуляции + altphysics var contacts:Contact; + // Количество контактов на текущем шаге симуляции +// altphysics var contactsNum:int; + // Список ограничений + altphysics var constraints:Vector. = new Vector.(); + // Количество ограничений + altphysics var constraintsNum:int; + // Временная метка. Число прошедших шагов с начала симуляции. + public var timeStamp:uint; + // Время с начала симуляции, мс + public var time:uint; + // Первый неиспользованный контакт на текущем шаге симуляции + private var borderContact:Contact; + + // Временные переменные для избежания создания экземпляров + private var _r:Vector3 = new Vector3(); + private var _t:Vector3 = new Vector3(); + private var _v:Vector3 = new Vector3(); + private var _v1:Vector3 = new Vector3(); + private var _v2:Vector3 = new Vector3(); + + /** + * + */ + public function World() { + contacts = new Contact(0); + var contact:Contact = contacts; + for (var i:int = 1; i < MAX_CONTACTS; i++) { + contact.next = new Contact(i); + contact = contact.next; + } + collisionDetector = new KdTreeCollisionDetector(); + } + + /** + * Вектор гравитации. + */ + public function get gravity():Vector3 { + return _gravity.vClone(); + } + + /** + * @private + */ + public function set gravity(value:Vector3):void { + _gravity.vCopy(value); + _gravityMagnitude = _gravity.vLength(); + } + + /** + * Добавляет тело в симуляцию. + * + * @param body + */ + public function addBody(body:Body):void { + body.id = lastBodyId++; + body.world = this; + bodies.append(body); + } + + /** + * Удаляет тело из симуляции. + * @param body + * @return + */ + public function removeBody(body:Body):void { + if (bodies.remove(body)) { + body.world = null; + } + } + + /** + * Добавляет ограничение. + * @param c + */ + public function addConstraint(c:Constraint):void { + constraints[constraintsNum++] = c; + c.world = this; + } + + /** + * Удаляет ограничение. + * @param c + */ + public function removeConstraint(c:Constraint):Boolean { + var idx:int = constraints.indexOf(c); + if (idx < 0) return false; + constraints.splice(idx, 1) + constraintsNum--; + c.world = null; + return true; + } + + /** + * Применяет к телам действующте на них силы. Аккумуляторы сил и моментов тел очищаются после завершения шага + * симуляции, поэтому на момент вызова метода могут уже содержать некоторые значения. + * + * @param dt промежуток времени, в течении которого действуют силы + */ + private function applyForces(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + body.beforePhysicsStep(dt); + body.calcAccelerations(); + // Ускорение свободного падения применяется только к подвижным телам во избежание некорректного изменения + // фиктивной скорости неподвижных тел. + if (body.movable && !body.frozen) { + body.accel.x += _gravity.x; + body.accel.y += _gravity.y; + body.accel.z += _gravity.z; + } + item = item.next; + } + } + + /** + * Определяет все столкновения на текущем шаге симуляции и заполняет список получившихся контактов. + * + * @param dt длительность шага симуляции + */ + private function detectCollisions(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + if (!body.frozen) { + body.contactsNum = 0; + body.saveState(); + // При включённом режиме предсказания состояние тел интегрируется на один шаг вперёд + if (usePrediction) { + body.integrateVelocity(dt); + body.integratePosition(dt); + } + body.calcDerivedData(); + } + item = item.next; + } + + borderContact = collisionDetector.getAllContacts(contacts); + + // Расчёт относительных векторов точки контакта вынесен сюда из-за необходимости учитывать + // положение тел в предсказанном состоянии + var contact:Contact = contacts; + while (contact != borderContact) { + var b1:Body = contact.body1; + var b2:Body = contact.body2; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + var bPos:Vector3 = b1.state.pos; + cp.r1.x = cp.pos.x - bPos.x; + cp.r1.y = cp.pos.y - bPos.y; + cp.r1.z = cp.pos.z - bPos.z; + if (b2 != null) { + bPos = b2.state.pos; + cp.r2.x = cp.pos.x - bPos.x; + cp.r2.y = cp.pos.y - bPos.y; + cp.r2.z = cp.pos.z - bPos.z; + } + } + contact = contact.next; + } + + // Восстановление состояния тел + if (usePrediction) { + item = bodies.head; + while (item != null) { + body = item.body; + if (!body.frozen) { + body.restoreState(); + body.calcDerivedData(); + } + item = item.next; + } + } + } + + /** + * Подготваливает полученные из детектора столкновений контакты, расчитывая значения, не меняющиеся + * в ходе шага симуляции. + */ + private function preProcessContacts(dt:Number):void { + var contact:Contact = contacts; + while (contact != borderContact) { + var b1:Body = contact.body1; + var b2:Body = contact.body2; + // Столкнувшиеся тела размораживаются + if (b1.frozen) { + b1.frozen = false; + b1.freezeCounter = 0; + } + if (b2 != null && b2.frozen) { + b2.frozen = false; + b2.freezeCounter = 0; + } + contact.restitution = b1.material.restitution; + if (b2 != null && b2.material.restitution < contact.restitution) contact.restitution = b2.material.restitution; + contact.friction = b1.material.friction; + if (b2 != null && b2.material.friction < contact.friction) contact.friction = b2.material.friction; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.accumImpulseN = 0; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + cp.velByUnitImpulseN = 0; + if (b1.movable) { + cp.angularInertia1 = _v.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1).vDot(contact.normal); + cp.velByUnitImpulseN += b1.invMass + cp.angularInertia1; + } + if (b2 != null && b2.movable) { + cp.angularInertia2 = _v.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2).vDot(contact.normal); + cp.velByUnitImpulseN += b2.invMass + cp.angularInertia2; + } + // Расчёт требуемой конечной скорости для упругого контакта + calcSepVelocity(b1, b2, cp, _v); + cp.normalVel = _v.vDot(contact.normal); + if (cp.normalVel < 0) cp.normalVel = - contact.restitution*cp.normalVel; + // Скорость разделения неупругого контакта + cp.minSepVel = cp.penetration > allowedPenetration ? (cp.penetration - allowedPenetration)/(penResolutionSteps*dt) : 0; + if (cp.minSepVel > maxPenResolutionSpeed) cp.minSepVel = maxPenResolutionSpeed; + } + contact = contact.next; + } + for (var i:int = 0; i < constraintsNum; i++) { + var constraint:Constraint = constraints[i]; + constraint.preProcess(dt); + } + } + + /** + * + * @param dt + * @param forceInelastic + */ + private function processContacts(dt:Number, forceInelastic:Boolean):void { + var iterNum:int = forceInelastic ? contactIterations : collisionIterations; + var i:int; + var forwardLoop:Boolean = false; + for (var iter:int = 0; iter < iterNum; iter++) { + forwardLoop = !forwardLoop; + var contact:Contact = contacts; + while (contact != borderContact) { + resolveContact(contact, forceInelastic, forwardLoop); + contact = contact.next; + } + // Ограничения + for (i = 0; i < constraintsNum; i++) { + var constraint:Constraint = constraints[i]; + constraint.apply(dt); + } + } + + // Разделение контактов путём непосредственного изменения координат и ориентации. Экспериментально. +// if (forceInelastic && useStaticSeparation) performStaticSeparation(); + } + + /** + * + */ + private function resolveContact(contactInfo:Contact, forceInelastic:Boolean, forwardLoop:Boolean):void { + var b1:Body = contactInfo.body1; + var b2:Body = contactInfo.body2; + var normal:Vector3 = contactInfo.normal; + var i:int; + if (forwardLoop) { + for (i = 0; i < contactInfo.pcount; i++) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } else { + for (i = contactInfo.pcount - 1; i >= 0; i--) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } + } + + /** + * + * @param idx + * @param b1 + * @param b2 + * @param colInfo + * @param normal + * @param forceInelastic + */ + private function resolveContactPoint(idx:int, b1:Body, b2:Body, contact:Contact, normal:Vector3, forceInelastic:Boolean):void { + var cp:ContactPoint = contact.points[idx]; + if (!forceInelastic) { + cp.satisfied = true; + } + + var newVel:Number = 0; + calcSepVelocity(b1, b2, cp, _v); + var cnormal:Vector3 = contact.normal; + var sepVel:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + if (forceInelastic) { + var minSpeVel:Number = useStaticSeparation ? 0 : cp.minSepVel; + if (sepVel < minSpeVel) { + cp.satisfied = false; + } else if (cp.satisfied) return; + newVel = minSpeVel; + } else { + newVel = cp.normalVel; + } + var deltaVel:Number = newVel - sepVel; + var impulse:Number = deltaVel/cp.velByUnitImpulseN; + var accumImpulse:Number = cp.accumImpulseN + impulse; + if (accumImpulse < 0) { + accumImpulse = 0; + } + var deltaImpulse:Number = accumImpulse - cp.accumImpulseN; + cp.accumImpulseN = accumImpulse; + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, normal, deltaImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, normal, -deltaImpulse); + } + + // Учёт силы трения + calcSepVelocity(b1, b2, cp, _v); + // Расчитываем изменение касательной скорости на единицу касательного импульса + var tanSpeedByUnitImpulse:Number = 0; + +// _v.vAddScaled(-_v.vDot(contact.normal), contact.normal); + var dot:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + _v.x -= dot*cnormal.x; + _v.y -= dot*cnormal.y; + _v.z -= dot*cnormal.z; + + var tanSpeed:Number = _v.vLength(); + if (tanSpeed < 0.001) return; + +// _t.vCopy(_v).vNormalize().vReverse(); + _t.x = -_v.x; + _t.y = -_v.y; + _t.z = -_v.z; + _t.vNormalize(); + + var r:Vector3; + var m:Matrix3; + var xx:Number; + var yy:Number; + var zz:Number; + // dV = b.invMass + ((invI * (r % t)) % r) * t + if (b1.movable) { +// _v.vCross2(cp.r1, _t).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1); + r = cp.r1; + m = b1.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b1.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + if (b2 != null && b2.movable) { +// _v.vCross2(cp.r2, _t).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2); + + r = cp.r2; + m = b2.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b2.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + + var tanImpulse:Number = tanSpeed/tanSpeedByUnitImpulse; + var max:Number = contact.friction*cp.accumImpulseN; + if (max < 0) { + if (tanImpulse < max) tanImpulse = max; + } else { + if (tanImpulse > max) tanImpulse = max; + } + + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, _t, tanImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, _t, -tanImpulse); + } + } + + /** + * + * @param cp + * @param normal + * @return + */ + private function calcSepVelocity(body1:Body, body2:Body, cp:ContactPoint, result:Vector3):void { + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 +// result.vCopy(body1.state.velocity).vAdd(_v1.vCross2(body1.state.rotation, cp.r1)); + var rot:Vector3 = body1.state.rotation; + var v:Vector3 = cp.r1; + var x:Number = rot.y*v.z - rot.z*v.y; + var y:Number = rot.z*v.x - rot.x*v.z; + var z:Number = rot.x*v.y - rot.y*v.x; + v = body1.state.velocity; + result.x = v.x + x; + result.y = v.y + y; + result.z = v.z + z; + // V2 = V2_c + w2%r2 + if (body2 != null) { +// result.vSubtract(body2.state.velocity).vSubtract(_v2.vCross2(body2.state.rotation, cp.r2)); + rot = body2.state.rotation; + v = cp.r2; + x = rot.y*v.z - rot.z*v.y; + y = rot.z*v.x - rot.x*v.z; + z = rot.x*v.y - rot.y*v.x; + v = body2.state.velocity; + result.x -= v.x + x; + result.y -= v.y + y; + result.z -= v.z + z; + } + } + + /** + * + * @param dt + */ + private function intergateVelocities(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + item.body.integrateVelocity(dt); + item = item.next; + } + } + + /** + * + * @param dt + */ + private function integratePositions(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + if (body.movable && !body.frozen) { + body.integratePosition(dt); + } + item = item.next; + } + } + + /** + * + */ + private function performStaticSeparation():void { + var iterNum:int = staticSeparationIterations; +// iterNum = 100; + + // 1. В начале каждой итерации для всех контактов сбрасывается флаг satisfied с одновременным поиском наихудшего контакта, т.е. имеющего наибольшую величину пересечения тел. + // 2. Если найденный контакт имеет величину пересечения меньше предельно допустимой процедура прерывается, т.к. разделение контактов не требуется. + // 3. Если процедура продолжается, то выполняется разделение найденного контакта и установка его флага satisfied в true, после чего запускается внутренний цикл, состоящий + // из contactsNum - 1 итераций. На каждой итерации ищется наихудший контакт среди оставшихся (satisfied == false). Если такой контакт найден, то выполняется его разделение + // и переход к следующей итерации внутреннего цикла, иначе внутренний цикл прерывается и выполняется переход к следующей итерации внешнего цикла (шаг 1.). +// for (var iter:int = 0; iter < iterNum; iter++) { +// // Ищем контакт с максимальной величиной пересечения тел, одновременно сбрасывая флаги satisfied в false +// var worstContact:Contact = contacts[0]; +// var i:int; +// for (i = 1; i < contactsNum; i++) { +// var contact:Contact = contacts[i]; +// contact.satisfied = false; +// if (contact.maxPenetration > worstContact.maxPenetration) worstContact = contact; +// } +// if (worstContact.maxPenetration <= allowedPenetration) return; +// +// resolveInterpenetration(worstContact); +// // Внутренний цикл по оставшимся контактам +// for (i = 1; i < contactsNum; i++) { +// worstContact = getWorstContact(); +// if (worstContact == null) break; +// resolveInterpenetration(worstContact); +// } +// } + } + + /** + * + */ +// private function getWorstContact():Contact { +// var maxPen:Number = 0; +// var worst:Contact = null; +// for (var i:int = 0; i < contactsNum; i++) { +// var c:Contact = contacts[i]; +// if (!c.satisfied && c.maxPenetration > maxPen) { +// worst = c; +// maxPen = c.maxPenetration; +// } +// } +// return maxPen > allowedPenetration ? worst : null; +// } + + /** + * Разделяет указанный контакт, выполняя staticSeparationSteps итераций по списку точек контакта. + * 1. В начале каждой итерации ищется точка с наибольшим пересечением, одновременно сбрасываются флаги satisfied у точек. + * 2. Для найденной точки выполняется процедура разделения. + * 3. После выполняется pcount - 1 итерация по списку точек, каждый раз ищется наихудшая среди имеющих satisfied == false, для которой выполняется процедура разделения. + * Если наихудшая точка не найдена, то выполнется переход к следующей итерации внешнего цикла (шаг 1). + * + * В конце процедуры разделения обновляются значения пересечений для остальных точек контакта, а также для всех прочих контактов, относящихся к телам текущего. + * + * @param contact контакт для разделения + */ + private function resolveInterpenetration(contact:Contact):void { + contact.satisfied = true; + + for (var step:int = 0; step < staticSeparationSteps; step++) { + var worstCp:ContactPoint = contact.points[0]; + var cp:ContactPoint; + var i:int; + for (i = 1; i < contact.pcount; i++) { + cp = contact.points[i]; + cp.satisfied = false; + if (cp.penetration > worstCp.penetration) worstCp = cp; + } + if (worstCp.penetration <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + // Разделяем оставшиеся точки + var maxPen:Number = 0; + for (i = 1; i < contact.pcount; i++) { + // Поиск наихудшей точки + for (var j:int = 0; j < contact.pcount; j++) { + cp = contact.points[j]; + if (cp.satisfied) continue; + if (cp.penetration > maxPen) { + maxPen = cp.penetration; + worstCp = cp; + } + } + if (maxPen <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + } + } + } + + /** + * + * @param cp + * @param contact + */ + private function separateContactPoint(cp:ContactPoint, contact:Contact):void { +// cp.satisfied = true; +// +// var b1:Body = contact.body1; +// var b2:Body = contact.body2; +// var totalMove:Number = cp.penetration - allowedPenetration; +// var moveCoeff:Number = totalMove/cp.velByUnitImpulseN; +// var linMove1:Number; +// var angleMove1:Number; +// if (b1.movable) { +// linMove1 = b1.invMass*moveCoeff; +// angleMove1 = cp.angularInertia1*moveCoeff; +// if (angleMove1 > maxAngleMove) { +// linMove1 += angleMove1 - maxAngleMove; +// angleMove1 -= maxAngleMove; +// } +// b1.state.pos.vAddScaled(linMove1, contact.normal); +// _v1.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vScale(angleMove1); +// b1.state.orientation.addScaledVector(_v1, 1); +// } +// var linMove2:Number; +// var angleMove2:Number; +// if (b2 != null && b2.movable) { +// linMove2 = b2.invMass*moveCoeff; +// angleMove2 = cp.angularInertia2*moveCoeff; +// if (angleMove2 > maxAngleMove) { +// linMove2 += angleMove2 - maxAngleMove; +// angleMove2 -= maxAngleMove; +// } +// b2.state.pos.vAddScaled(-linMove2, contact.normal); +// _v2.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vScale(angleMove2); +// _v2.vReverse(); +// b2.state.orientation.addScaledVector(_v2, 1); +// } +// cp.penetration = allowedPenetration; +// // Обновляем пересечения в других точках +// var i:int; +// for (i = 0; i < contact.pcount; i++) { +// var cp1:ContactPoint = contact.points[i]; +// if (cp1 == cp) continue; +// var angularMove:Number; +// if (b1.movable) { +// angularMove = _v.vCross2(_v1, cp1.r1).vDot(contact.normal); +// cp1.penetration -= linMove1 + angularMove; +// } +// if (b2 != null && b2.movable) { +// angularMove = _v.vCross2(_v2, cp1.r2).vDot(contact.normal); +// cp1.penetration -= linMove2 - angularMove; +// } +// // Обновление максимального значения для контакта +// if (cp1.penetration > contact.maxPenetration) contact.maxPenetration = cp1.penetration; +// } +// // Обновляем пересечения для других контактов +// var c:Contact; +// var j:int; +// if (b1.movable) { +// for (i = 0; i < b1.contactsNum; i++) { +// c = b1.contacts[i]; +// if (c == contact) continue; +// for (j = 0; j < c.pcount; j++) { +// cp1 = c.points[j]; +// if (b1 == c.body1) cp1.penetration -= linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r1).vDot(c.normal); +// else cp1.penetration += linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r2).vDot(c.normal); +// if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; +// } +// } +// } +// if (b2 != null && b2.movable) { +// for (i = 0; i < b2.contactsNum; i++) { +// c = b2.contacts[i]; +// if (c == contact) continue; +// for (j = 0; j < c.pcount; j++) { +// cp1 = c.points[j]; +// if (b2 == c.body1) cp1.penetration -= linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r1).vDot(c.normal); +// else cp1.penetration += linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r2).vDot(c.normal); +// if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; +// } +// } +// } + } + + /** + * + */ + private function postPhysics():void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + body.clearAccumulators(); + body.calcDerivedData(); + if (body.canFreeze) { + if (body.state.velocity.vLength() < linSpeedFreezeLimit && body.state.rotation.vLength() < angSpeedFreezeLimit) { + if (!body.frozen) { + body.freezeCounter++; + if (body.freezeCounter >= freezeSteps) body.frozen = true; + } + } else { + body.freezeCounter = 0; + body.frozen = false; + } + } + item = item.next; + } + } + + /** + * + * @param delta + */ + public function runPhysics(delta:uint):void { + timeStamp++; + time += delta; + var dt:Number = 0.001*delta; + applyForces(dt); + detectCollisions(dt); + preProcessContacts(dt); + processContacts(dt, false); + intergateVelocities(dt); + processContacts(dt, true); + integratePositions(dt); + postPhysics(); + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base b/0.0.8.0/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base new file mode 100644 index 0000000..2715307 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base @@ -0,0 +1,3 @@ +package alternativa.physics { + public namespace altphysics = "http://alternativaplatform.com/en/altphysics"; +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/Body.as b/0.0.8.0/src/alternativa/physics/Body.as new file mode 100644 index 0000000..6b53304 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/Body.as @@ -0,0 +1,406 @@ +package alternativa.physics { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.IBodyCollisionPredicate; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.math.Matrix3; + import alternativa.physics.math.Matrix4; + import alternativa.physics.math.Quaternion; + import alternativa.physics.math.Vector3; + + use namespace altphysics; + + /** + * + */ + public class Body { + + public static var linDamping:Number = 0.997; + public static var rotDamping:Number = 0.997; + + // Идентификатор тела, уникальный в пределах мира + public var id:int; + // Имя тела + public var name:String; + // Мир, в котором находится тело + public var world:World; + // Флаг подвижности тела + public var movable:Boolean = true; + // Флаг указывает, может ли тело быть заморожено + public var canFreeze:Boolean = false; + + public var freezeCounter:int; + public var frozen:Boolean = false; + // Ограничивающий бокс тела + public var aabb:BoundBox = new BoundBox(); + // Предикат, через который тело получает сообщения о столкновениях + public var postCollisionPredicate:IBodyCollisionPredicate; + + // Текущее состояние тела + public var state:BodyState = new BodyState(); + // Предыдущее состояние тела + public var prevState:BodyState = new BodyState(); + // Линейное ускорение тела на текущем шаге симуляции + public var accel:Vector3 = new Vector3(); + // Угловое ускорение тела на текущем шаге симуляции + public var angleAccel:Vector3 = new Vector3(); + // Физический материал тела + public var material:BodyMaterial = new BodyMaterial(); + // Обратная масса тела + public var invMass:Number = 1; + // Обратная матрица тензора инерции в локальных координатах + public var invInertia:Matrix3 = new Matrix3(); + // Обратная матрица тензора инерции в мировых координатах + public var invInertiaWorld:Matrix3 = new Matrix3(); + // Базисная матрица тела в мировых координатах + public var baseMatrix:Matrix3 = new Matrix3(); + + public const MAX_CONTACTS:int = 20; + public var contacts:Vector. = new Vector.(MAX_CONTACTS); + public var contactsNum:int; + + public var collisionPrimitives:CollisionPrimitiveList; + + // Аккумулятор сил + public var forceAccum:Vector3 = new Vector3(); + // Аккумулятор моментов + public var torqueAccum:Vector3 = new Vector3(); + + // Внутренние переменные для избежания создания экземпляров + private static var _r:Vector3 = new Vector3(); + private static var _f:Vector3 = new Vector3(); + + /** + * + * @param invMass + * @param invInertia + */ + public function Body(invMass:Number, invInertia:Matrix3) { + this.invMass = invMass; + this.invInertia.copy(invInertia); + } + + /** + * @param primitive + * @param localTransform + */ + public function addCollisionPrimitive(primitive:CollisionPrimitive, localTransform:Matrix4 = null):void { + if (primitive == null) { + throw new ArgumentError("Primitive cannot be null"); + } + if (collisionPrimitives == null) { + collisionPrimitives = new CollisionPrimitiveList(); + } + collisionPrimitives.append(primitive); + primitive.setBody(this, localTransform); + } + + /** + * + * @param primitive + */ + public function removeCollisionPrimitive(primitive:CollisionPrimitive):void { + if (collisionPrimitives == null) return; + primitive.setBody(null); + collisionPrimitives.remove(primitive); + if (collisionPrimitives.size == 0) { + collisionPrimitives = null; + } + } + + /** + * @param t + * @param result + */ + public function interpolate(t:Number, pos:Vector3, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.w = prevState.orientation.w*t1 + state.orientation.w*t; + orientation.x = prevState.orientation.x*t1 + state.orientation.x*t; + orientation.y = prevState.orientation.y*t1 + state.orientation.y*t; + orientation.z = prevState.orientation.z*t1 + state.orientation.z*t; + } + + /** + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + state.pos.vCopy(pos); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setPositionXYZ(x:Number, y:Number, z:Number):void { + state.pos.vReset(x, y, z); + } + + /** + * + * @param vel + */ + public function setVelocity(vel:Vector3):void { + state.velocity.vCopy(vel); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setVelocityXYZ(x:Number, y:Number, z:Number):void { + state.velocity.vReset(x, y, z); + } + + /** + * + * @param rot + */ + public function setRotation(rot:Vector3):void { + state.rotation.vCopy(rot); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setRotationXYZ(x:Number, y:Number, z:Number):void { + state.rotation.vReset(x, y, z); + } + + /** + * + * @param q + */ + public function setOrientation(q:Quaternion):void { + state.orientation.copy(q); + } + + /** + * @param r + * @param dir + * @param magnitude + */ + public function applyRelPosWorldImpulse(r:Vector3, dir:Vector3, magnitude:Number):void { + var d:Number = magnitude*invMass; + // Линейная часть + state.velocity.x += d*dir.x; + state.velocity.y += d*dir.y; + state.velocity.z += d*dir.z; + + // Вращательная часть + var x:Number = (r.y*dir.z - r.z*dir.y)*magnitude; + var y:Number = (r.z*dir.x - r.x*dir.z)*magnitude; + var z:Number = (r.x*dir.y - r.y*dir.x)*magnitude; + + state.rotation.x += invInertiaWorld.a*x + invInertiaWorld.b*y + invInertiaWorld.c*z; + state.rotation.y += invInertiaWorld.e*x + invInertiaWorld.f*y + invInertiaWorld.g*z; + state.rotation.z += invInertiaWorld.i*x + invInertiaWorld.j*y + invInertiaWorld.k*z; + } + + /** + * @param f + */ + public function addForce(f:Vector3):void { + forceAccum.vAdd(f); + } + + /** + * + * @param fx + * @param fy + * @param fz + */ + public function addForceXYZ(fx:Number, fy:Number, fz:Number):void { + forceAccum.x += fx; + forceAccum.y += fy; + forceAccum.z += fz; + } + + /** + * @param pos + * @param f + */ + public function addWorldForceXYZ(px:Number, py:Number, pz:Number, fx:Number, fy:Number, fz:Number):void { + forceAccum.x += fx; + forceAccum.y += fy; + forceAccum.z += fz; + + var pos:Vector3 = state.pos; + var rx:Number = px - pos.x; + var ry:Number = py - pos.y; + var rz:Number = pz - pos.z; + +// var x:Number = ry*fz - rz*fy; +// var y:Number = rz*fx - rx*fz; +// var z:Number = rx*fy - ry*fx; + + torqueAccum.x += ry*fz - rz*fy; + torqueAccum.y += rz*fx - rx*fz; + torqueAccum.z += rx*fy - ry*fx; + } + + /** + * @param pos + * @param f + */ + public function addWorldForce(pos:Vector3, force:Vector3):void { + forceAccum.vAdd(force); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(force)); + } + + /** + * @param pos + * @param f + */ + public function addWorldForceScaled(pos:Vector3, force:Vector3, scale:Number):void { + _f.x = scale*force.x; + _f.y = scale*force.y; + _f.z = scale*force.z; + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(_f)); + } + + /** + * @param pos + * @param f + */ + public function addLocalForce(pos:Vector3, force:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(pos, _r); + // Трансформируем вектор силы в мировую систему + baseMatrix.transformVector(force, _f); + // Добавляем силу и момент + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vCross(_f)); + } + + /** + * + * @param localPos + * @param worldForce + */ + public function addWorldForceAtLocalPoint(localPos:Vector3, worldForce:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(localPos, _r); + // Добавляем силу и момент + forceAccum.vAdd(worldForce); + torqueAccum.vAdd(_r.vCross(worldForce)); + } + + /** + * @param dt + */ + public function beforePhysicsStep(dt:Number):void { + } + + /** + * @param t + */ + public function addTorque(t:Vector3):void { + torqueAccum.vAdd(t); + } + + /** + * + */ + altphysics function clearAccumulators():void { + forceAccum.x = forceAccum.y = forceAccum.z = 0; + torqueAccum.x = torqueAccum.y = torqueAccum.z = 0; + } + + /** + * + */ + altphysics function calcAccelerations():void { + accel.x = forceAccum.x*invMass; + accel.y = forceAccum.y*invMass; + accel.z = forceAccum.z*invMass; + angleAccel.x = invInertiaWorld.a*torqueAccum.x + invInertiaWorld.b*torqueAccum.y + invInertiaWorld.c*torqueAccum.z; + angleAccel.y = invInertiaWorld.e*torqueAccum.x + invInertiaWorld.f*torqueAccum.y + invInertiaWorld.g*torqueAccum.z; + angleAccel.z = invInertiaWorld.i*torqueAccum.x + invInertiaWorld.j*torqueAccum.y + invInertiaWorld.k*torqueAccum.z; + } + + /** + * Вычисляет производные данные. + */ + public function calcDerivedData():void { + // Вычисление базисной матрицы и обратного тензора инерции в мировых координатах + state.orientation.toMatrix3(baseMatrix); + invInertiaWorld.copy(invInertia).append(baseMatrix).prependTransposed(baseMatrix); + if (collisionPrimitives != null) { + aabb.infinity(); + var item:CollisionPrimitiveListItem = collisionPrimitives.head; + while (item != null) { + var primitive:CollisionPrimitive = item.primitive; + primitive.transform.setFromMatrix3(baseMatrix, state.pos); + if (primitive.localTransform != null) { + primitive.transform.prepend(primitive.localTransform); + } + primitive.calculateAABB(); + aabb.addBoundBox(primitive.aabb); + item = item.next; + } + } + } + + /** + * + */ + altphysics function saveState():void { + prevState.copy(state); + } + + /** + * + */ + altphysics function restoreState():void { + state.copy(prevState); + } + + /** + * @param dt + */ + altphysics function integrateVelocity(dt:Number):void { + // v = v + a*t + state.velocity.x += accel.x*dt; + state.velocity.y += accel.y*dt; + state.velocity.z += accel.z*dt; + // rot = rot + eps*t + state.rotation.x += angleAccel.x*dt; + state.rotation.y += angleAccel.y*dt; + state.rotation.z += angleAccel.z*dt; + + state.velocity.x *= linDamping; + state.velocity.y *= linDamping; + state.velocity.z *= linDamping; + + state.rotation.x *= rotDamping; + state.rotation.y *= rotDamping; + state.rotation.z *= rotDamping; + } + + /** + * + */ + altphysics function integratePosition(dt:Number):void { + // pos = pos + v*t + state.pos.x += state.velocity.x*dt; + state.pos.y += state.velocity.y*dt; + state.pos.z += state.velocity.z*dt; + // q = q + 0.5*rot*q + state.orientation.addScaledVector(state.rotation, dt); + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/BodyList.as b/0.0.8.0/src/alternativa/physics/BodyList.as new file mode 100644 index 0000000..aa8f79f --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/BodyList.as @@ -0,0 +1,76 @@ +package alternativa.physics { + + /** + * + */ + public class BodyList { + + public var head:BodyListItem; + public var tail:BodyListItem; + public var size:int; + + /** + * + */ + public function BodyList() { + } + + /** + * + * @param body + */ + public function append(body:Body):void { + var item:BodyListItem = BodyListItem.create(body); + if (head == null) { + head = tail = item; + } else { + tail.next = item; + item.prev = tail; + tail = item; + } + size++; + } + + /** + * + * @param body + */ + public function remove(body:Body):Boolean { + var item:BodyListItem = findItem(body); + if (item == null) return false; + if (item == head) { + if (size == 1) { + head = tail = null; + } else { + head = item.next; + head.prev = null; + } + } else { + if (item == tail) { + tail = item.prev; + tail.next = null; + } else { + item.prev.next = item.next; + item.next.prev = item.prev; + } + } + item.dispose(); + size--; + return true; + } + + /** + * + * @param body + * @return + */ + public function findItem(body:Body):BodyListItem { + var item:BodyListItem = head; + while (item != null && item.body != body) { + item = item.next; + } + return item; + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/BodyListItem.as b/0.0.8.0/src/alternativa/physics/BodyListItem.as new file mode 100644 index 0000000..382e11c --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/BodyListItem.as @@ -0,0 +1,74 @@ +package alternativa.physics { + + /** + * + */ + public class BodyListItem { + + // Верхний элемент хранилища + private static var poolTop:BodyListItem; + + /** + * Создаёт новый элемент списка. + * + * @param primitive примитив, содержащийся в элементе + * @return новый элемент списка + */ + public static function create(body:Body):BodyListItem { + var item:BodyListItem; + if (poolTop == null) { + item = new BodyListItem(body); + } else { + item = poolTop; + poolTop = item.next; + item.next = null; + item.body = body; + } + return item; + } + + /** + * Очищает хранилище. + */ + public static function clearPool():void { + var item:BodyListItem = poolTop; + while (item != null) { + poolTop = item.next; + item.next = null; + item = poolTop; + } + } + + /** + * + */ + public var body:Body; + /** + * + */ + public var next:BodyListItem; + /** + * + */ + public var prev:BodyListItem; + + /** + * + * @param body + */ + public function BodyListItem(body:Body) { + this.body = body; + } + + /** + * + */ + public function dispose():void { + body = null; + prev = null; + next = poolTop; + poolTop = this; + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/BodyMaterial.as b/0.0.8.0/src/alternativa/physics/BodyMaterial.as new file mode 100644 index 0000000..7a7c47d --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/BodyMaterial.as @@ -0,0 +1,11 @@ +package alternativa.physics { + + public class BodyMaterial { + + public var restitution:Number = 0; + public var friction:Number = 0.3; +// public var dynamicFriction:Number = 0.2; +// public var dynamicFriction:Number = 0.2; + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/BodyState.as b/0.0.8.0/src/alternativa/physics/BodyState.as new file mode 100644 index 0000000..34a1b69 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/BodyState.as @@ -0,0 +1,39 @@ +package alternativa.physics { + import alternativa.physics.math.Quaternion; + import alternativa.physics.math.Vector3; + + /** + * Класс описывает состояние твёрдого тела. + */ + public class BodyState { + /** + * Положение тела. + */ + public var pos:Vector3 = new Vector3(); + /** + * Ориентация тела. + */ + public var orientation:Quaternion = new Quaternion(); + /** + * Скорость тела. + */ + public var velocity:Vector3 = new Vector3(); + /** + * Угловая скорость тела. + */ + public var rotation:Vector3 = new Vector3(); + + /** + * Копирует значение указанного объекта. + * + * @param state + */ + public function copy(state:BodyState):void { + pos.vCopy(state.pos); + orientation.copy(state.orientation); + velocity.vCopy(state.velocity); + rotation.vCopy(state.rotation); + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/CollisionPrimitiveList.as b/0.0.8.0/src/alternativa/physics/CollisionPrimitiveList.as new file mode 100644 index 0000000..5d5304f --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/CollisionPrimitiveList.as @@ -0,0 +1,89 @@ +package alternativa.physics { + import alternativa.physics.collision.CollisionPrimitive; + + /** + * + */ + public class CollisionPrimitiveList { + + public var head:CollisionPrimitiveListItem; + public var tail:CollisionPrimitiveListItem; + public var size:int; + + /** + * + */ + public function CollisionPrimitiveList() { + } + + /** + * + * @param primitive + */ + public function append(primitive:CollisionPrimitive):void { + var item:CollisionPrimitiveListItem = CollisionPrimitiveListItem.create(primitive); + if (head == null) { + head = tail = item; + } else { + tail.next = item; + item.prev = tail; + tail = item; + } + size++; + } + + /** + * + * @param primitve + */ + public function remove(primitve:CollisionPrimitive):void { + var item:CollisionPrimitiveListItem = findItem(primitve); + if (item == null) return; + if (item == head) { + if (size == 1) { + head = tail = null; + } else { + head = item.next; + head.prev = null; + } + } else { + if (item == tail) { + tail = tail.prev; + tail.next = null; + } else { + item.prev.next = item.next; + item.next.prev = item.prev; + } + } + item.dispose(); + size--; + } + + /** + * + * @param primitive + * @return + */ + public function findItem(primitive:CollisionPrimitive):CollisionPrimitiveListItem { + var item:CollisionPrimitiveListItem = head; + while (item != null && item.primitive != primitive) { + item = item.next; + } + return item; + } + + /** + * + */ + public function clear():void { + while (head != null) { + var item:CollisionPrimitiveListItem = head; + head = head.next; + item.dispose(); + } + tail = null; + size = 0; + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/CollisionPrimitiveListItem.as b/0.0.8.0/src/alternativa/physics/CollisionPrimitiveListItem.as new file mode 100644 index 0000000..d204532 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/CollisionPrimitiveListItem.as @@ -0,0 +1,76 @@ +package alternativa.physics { + import alternativa.physics.collision.CollisionPrimitive; + + /** + * Элемент списка примитивов. + */ + public class CollisionPrimitiveListItem { + + // Верхний элемент хранилища + private static var poolTop:CollisionPrimitiveListItem; + + /** + * Создаёт новый элемент списка. + * + * @param primitive примитив, содержащийся в элементе + * @return новый элемент списка + */ + public static function create(primitive:CollisionPrimitive):CollisionPrimitiveListItem { + var item:CollisionPrimitiveListItem; + if (poolTop == null) { + item = new CollisionPrimitiveListItem(primitive); + } else { + item = poolTop; + item.primitive = primitive; + poolTop = item.next; + item.next = null; + } + return item; + } + + /** + * Очищает хранилище. + */ + public static function clearPool():void { + var curr:CollisionPrimitiveListItem = poolTop; + while (curr != null) { + poolTop = curr.next; + curr.next = null; + curr = poolTop; + } + } + + /** + * Примитив, хранящийся в элементе списка. + */ + public var primitive:CollisionPrimitive; + /** + * Ссылка не следующий элемент списка. + */ + public var next:CollisionPrimitiveListItem; + /** + * Ссылка не предыдущий элемент списка. + */ + public var prev:CollisionPrimitiveListItem; + + /** + * Создаёт новый экземпляр. + * + * @param primitive примитив, хранящийся в элементе списка + */ + public function CollisionPrimitiveListItem(primitive:CollisionPrimitive) { + this.primitive = primitive; + } + + /** + * Очищает внутренние ссылки и помещает элемент в хранилище для дальнейшего использования. + */ + public function dispose():void { + primitive = null; + prev = null; + next = poolTop; + poolTop = this; + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/Contact.as b/0.0.8.0/src/alternativa/physics/Contact.as new file mode 100644 index 0000000..1296996 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/Contact.as @@ -0,0 +1,69 @@ +package alternativa.physics { + import __AS3__.vec.Vector; + + import alternativa.physics.math.Vector3; + + /** + * Описывает контакт одного или двух тел. + */ + public class Contact { + /** + * Первое тело контакта, не может быть равно null. + */ + public var body1:Body; + /** + * Второе тело контакта, может быть равно null. + */ + public var body2:Body; + /** + * Предрассчитанный взаимный коэффициент отскока. + */ + public var restitution:Number; + /** + * Предрассчитанный взаимный коэффициент трения. + */ + public var friction:Number; + /** + * Нормаль контакта. Направлена к первому телу. + */ + public var normal:Vector3 = new Vector3(); + /** + * Список точек контакта. + */ + public var points:Vector. = new Vector.(MAX_POINTS, true); + /** + * Количество точек контакта. + */ + public var pcount:int; + /** + * Максимальная глубина пересечения. + */ + public var maxPenetration:Number = 0; + /** + * Флаг показывает, разрешён контакт или нет. + */ + public var satisfied:Boolean; + /** + * Следующий контакт в списке. + */ + public var next:Contact; + /** + * Индекс контакта. Первый контакт в списке имеет индекс 0. + */ + public var index:int; + + // Максимальное количество точек контакта + private const MAX_POINTS:int = 8; + + /** + * + */ + public function Contact(index:int) { + this.index = index; + for (var i:int = 0; i < MAX_POINTS; i++) { + points[i] = new ContactPoint(); + } + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/ContactPoint.as b/0.0.8.0/src/alternativa/physics/ContactPoint.as new file mode 100644 index 0000000..2136e9a --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/ContactPoint.as @@ -0,0 +1,55 @@ +package alternativa.physics { + import alternativa.physics.math.Vector3; + + /** + * + */ + public class ContactPoint { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + + // Величины, расчитываемые перед началом фазы решения контактов + + // Требуемая проекция конечной скорости на нормаль для упругого контакта + public var normalVel:Number; + // Минимальная скорость разделения неупругого контакта + public var minSepVel:Number; + // Изменение проекции скорости на единицу нормального импульса + public var velByUnitImpulseN:Number; + + public var angularInertia1:Number; + public var angularInertia2:Number; + + // Радиус-вектор точки контакта относительно центра первого тела + public var r1:Vector3 = new Vector3(); + // Радиус-вектор точки контакта относительно центра второго тела + public var r2:Vector3 = new Vector3(); + + // Величины, накапливаемые во время фазы решения контактов + + // Накопленный импульс, применяемый для получения требуемой относительной скорости в точке контакта + public var accumImpulseN:Number; + // Накопленный импульс, применяемый для разделения тел в точке контакта. Не создаёт момента. +// public var accumSepImpulse:Number; + + public var satisfied:Boolean; + + /** + * + * @param cp + */ + public function copyFrom(cp:ContactPoint):void { + pos.vCopy(cp.pos); + penetration = cp.penetration; + feature1 = cp.feature1; + feature2 = cp.feature2; + r1.vCopy(cp.r1); + r2.vCopy(cp.r2); + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/PhysicsUtils.as b/0.0.8.0/src/alternativa/physics/PhysicsUtils.as new file mode 100644 index 0000000..6770b0b --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/PhysicsUtils.as @@ -0,0 +1,49 @@ +package alternativa.physics { + import alternativa.physics.math.Matrix3; + import alternativa.physics.math.Vector3; + + public class PhysicsUtils { + + /** + * @param mass + * @param halfSize + * @param result + */ + public static function getBoxInvInertia(mass:Number, halfSize:Vector3, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + result.a = 3/(mass*(yy + zz)); + result.f = 3/(mass*(zz + xx)); + result.k = 3/(mass*(xx + yy)); + } + + /** + * @param mass + * @param r + * @param h + * @param result + */ + public static function getCylinderInvInertia(mass:Number, r:Number, h:Number, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + + result.a = result.f = 1/(mass*(h*h/12 + r*r/4)); + result.k = 2/(mass*r*r); + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/World.as b/0.0.8.0/src/alternativa/physics/World.as new file mode 100644 index 0000000..9339f20 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/World.as @@ -0,0 +1,743 @@ +package alternativa.physics { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.ICollisionDetector; + import alternativa.physics.collision.KdTreeCollisionDetector; + import alternativa.physics.constraints.Constraint; + import alternativa.physics.math.Matrix3; + import alternativa.physics.math.Vector3; + + use namespace altphysics; + + /** + * Класс реализует физическую симуляцию поведения твёрдых тел. + */ + public class World { + + private static var lastBodyId:int; + + // Максимальное количество контактов + altphysics const MAX_CONTACTS:int = 1000; + + ////////////////////////////////////////////////////////////////// + // Настроечные параметры симуляции. Могут быть изменены в любой + // момент времени без нарушения корректной работы симуляции. + ////////////////////////////////////////////////////////////////// + // Количество шагов, за которое пересекающиеся тела должны разделиться + public var penResolutionSteps:int = 10; + // Величина допустимой глубины пересечения + public var allowedPenetration:Number = 0.1; + // Максимальная скорость, добавляемая с целью разделения тел + public var maxPenResolutionSpeed:Number = 0.5; + // Количество итераций для обработки упругих контактов + public var collisionIterations:int = 5; + // Количество итераций для обработки неупругих контактов + public var contactIterations:int = 5; + // Флаг использования предсказания состояний + public var usePrediction:Boolean = false; + + public var freezeSteps:int = 10; + public var linSpeedFreezeLimit:Number = 1; + public var angSpeedFreezeLimit:Number = 0.01; + + // Переменные для процедуры разделения тел путём непосредственного их перемещенеия. + // !!!!!!!!!!!!!!!!!!!!!!!! + // !!! Экспериментально !!! + // !!!!!!!!!!!!!!!!!!!!!!!! + public var staticSeparationIterations:int = 10; + public var staticSeparationSteps:int = 10; + public var maxAngleMove:Number = 10; + public var useStaticSeparation:Boolean = false; + + // Вектор гравитации + public var _gravity:Vector3 = new Vector3(0, 0, -9.8); + // Модуль вектора гравитации + public var _gravityMagnitude:Number = 9.8; + + // Использующийся детектор столкновений + public var collisionDetector:ICollisionDetector; + + // Список тел, участвующих в симуляции + public var bodies:BodyList = new BodyList(); + + // Список контактов на текущем шаге симуляции + altphysics var contacts:Contact; + // Количество контактов на текущем шаге симуляции +// altphysics var contactsNum:int; + // Список ограничений + altphysics var constraints:Vector. = new Vector.(); + // Количество ограничений + altphysics var constraintsNum:int; + // Временная метка. Число прошедших шагов с начала симуляции. + public var timeStamp:uint; + // Время с начала симуляции, мс + public var time:uint; + // Первый неиспользованный контакт на текущем шаге симуляции + private var borderContact:Contact; + + // Временные переменные для избежания создания экземпляров + private var _r:Vector3 = new Vector3(); + private var _t:Vector3 = new Vector3(); + private var _v:Vector3 = new Vector3(); + private var _v1:Vector3 = new Vector3(); + private var _v2:Vector3 = new Vector3(); + + /** + * + */ + public function World() { + contacts = new Contact(0); + var contact:Contact = contacts; + for (var i:int = 1; i < MAX_CONTACTS; i++) { + contact.next = new Contact(i); + contact = contact.next; + } + collisionDetector = new KdTreeCollisionDetector(); + } + + /** + * Вектор гравитации. + */ + public function get gravity():Vector3 { + return _gravity.vClone(); + } + + /** + * @private + */ + public function set gravity(value:Vector3):void { + _gravity.vCopy(value); + _gravityMagnitude = _gravity.vLength(); + } + + /** + * Добавляет тело в симуляцию. + * + * @param body + */ + public function addBody(body:Body):void { + body.id = lastBodyId++; + body.world = this; + bodies.append(body); + } + + /** + * Удаляет тело из симуляции. + * @param body + * @return + */ + public function removeBody(body:Body):void { + if (bodies.remove(body)) { + body.world = null; + } + } + + /** + * Добавляет ограничение. + * @param c + */ + public function addConstraint(c:Constraint):void { + constraints[constraintsNum++] = c; + c.world = this; + } + + /** + * Удаляет ограничение. + * @param c + */ + public function removeConstraint(c:Constraint):Boolean { + var idx:int = constraints.indexOf(c); + if (idx < 0) return false; + constraints.splice(idx, 1) + constraintsNum--; + c.world = null; + return true; + } + + /** + * Применяет к телам действующте на них силы. Аккумуляторы сил и моментов тел очищаются после завершения шага + * симуляции, поэтому на момент вызова метода могут уже содержать некоторые значения. + * + * @param dt промежуток времени, в течении которого действуют силы + */ + private function applyForces(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + body.beforePhysicsStep(dt); + body.calcAccelerations(); + // Ускорение свободного падения применяется только к подвижным телам во избежание некорректного изменения + // фиктивной скорости неподвижных тел. + if (body.movable && !body.frozen) { + body.accel.x += _gravity.x; + body.accel.y += _gravity.y; + body.accel.z += _gravity.z; + } + item = item.next; + } + } + + /** + * Определяет все столкновения на текущем шаге симуляции и заполняет список получившихся контактов. + * + * @param dt длительность шага симуляции + */ + private function detectCollisions(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + if (!body.frozen) { + body.contactsNum = 0; + body.saveState(); + // При включённом режиме предсказания состояние тел интегрируется на один шаг вперёд + if (usePrediction) { + body.integrateVelocity(dt); + body.integratePosition(dt); + } + body.calcDerivedData(); + } + item = item.next; + } + + borderContact = collisionDetector.getAllContacts(contacts); + + // Расчёт относительных векторов точки контакта вынесен сюда из-за необходимости учитывать + // положение тел в предсказанном состоянии + var contact:Contact = contacts; + while (contact != borderContact) { + var b1:Body = contact.body1; + var b2:Body = contact.body2; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + var bPos:Vector3 = b1.state.pos; + cp.r1.x = cp.pos.x - bPos.x; + cp.r1.y = cp.pos.y - bPos.y; + cp.r1.z = cp.pos.z - bPos.z; + if (b2 != null) { + bPos = b2.state.pos; + cp.r2.x = cp.pos.x - bPos.x; + cp.r2.y = cp.pos.y - bPos.y; + cp.r2.z = cp.pos.z - bPos.z; + } + } + contact = contact.next; + } + + // Восстановление состояния тел + if (usePrediction) { + item = bodies.head; + while (item != null) { + body = item.body; + if (!body.frozen) { + body.restoreState(); + body.calcDerivedData(); + } + item = item.next; + } + } + } + + /** + * Подготваливает полученные из детектора столкновений контакты, расчитывая значения, не меняющиеся + * в ходе шага симуляции. + */ + private function preProcessContacts(dt:Number):void { + var contact:Contact = contacts; + while (contact != borderContact) { + var b1:Body = contact.body1; + var b2:Body = contact.body2; + // Столкнувшиеся тела размораживаются + if (b1.frozen) { + b1.frozen = false; + b1.freezeCounter = 0; + } + if (b2 != null && b2.frozen) { + b2.frozen = false; + b2.freezeCounter = 0; + } + contact.restitution = b1.material.restitution; + if (b2 != null && b2.material.restitution < contact.restitution) contact.restitution = b2.material.restitution; + contact.friction = b1.material.friction; + if (b2 != null && b2.material.friction < contact.friction) contact.friction = b2.material.friction; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.accumImpulseN = 0; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + cp.velByUnitImpulseN = 0; + if (b1.movable) { + cp.angularInertia1 = _v.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1).vDot(contact.normal); + cp.velByUnitImpulseN += b1.invMass + cp.angularInertia1; + } + if (b2 != null && b2.movable) { + cp.angularInertia2 = _v.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2).vDot(contact.normal); + cp.velByUnitImpulseN += b2.invMass + cp.angularInertia2; + } + // Расчёт требуемой конечной скорости для упругого контакта + calcSepVelocity(b1, b2, cp, _v); + cp.normalVel = _v.vDot(contact.normal); + if (cp.normalVel < 0) cp.normalVel = - contact.restitution*cp.normalVel; + // Скорость разделения неупругого контакта + cp.minSepVel = cp.penetration > allowedPenetration ? (cp.penetration - allowedPenetration)/(penResolutionSteps*dt) : 0; + if (cp.minSepVel > maxPenResolutionSpeed) cp.minSepVel = maxPenResolutionSpeed; + } + contact = contact.next; + } + for (var i:int = 0; i < constraintsNum; i++) { + var constraint:Constraint = constraints[i]; + constraint.preProcess(dt); + } + } + + /** + * + * @param dt + * @param forceInelastic + */ + private function processContacts(dt:Number, forceInelastic:Boolean):void { + var iterNum:int = forceInelastic ? contactIterations : collisionIterations; + var i:int; + var forwardLoop:Boolean = false; + for (var iter:int = 0; iter < iterNum; iter++) { + forwardLoop = !forwardLoop; + var contact:Contact = contacts; + while (contact != borderContact) { + resolveContact(contact, forceInelastic, forwardLoop); + contact = contact.next; + } + // Ограничения + for (i = 0; i < constraintsNum; i++) { + var constraint:Constraint = constraints[i]; + constraint.apply(dt); + } + } + + // Разделение контактов путём непосредственного изменения координат и ориентации. Экспериментально. +// if (forceInelastic && useStaticSeparation) performStaticSeparation(); + } + + /** + * + */ + private function resolveContact(contactInfo:Contact, forceInelastic:Boolean, forwardLoop:Boolean):void { + var b1:Body = contactInfo.body1; + var b2:Body = contactInfo.body2; + var normal:Vector3 = contactInfo.normal; + var i:int; + if (forwardLoop) { + for (i = 0; i < contactInfo.pcount; i++) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } else { + for (i = contactInfo.pcount - 1; i >= 0; i--) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } + } + + /** + * + * @param idx + * @param b1 + * @param b2 + * @param colInfo + * @param normal + * @param forceInelastic + */ + private function resolveContactPoint(idx:int, b1:Body, b2:Body, contact:Contact, normal:Vector3, forceInelastic:Boolean):void { + var cp:ContactPoint = contact.points[idx]; + if (!forceInelastic) { + cp.satisfied = true; + } + + var newVel:Number = 0; + calcSepVelocity(b1, b2, cp, _v); + var cnormal:Vector3 = contact.normal; + var sepVel:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + if (forceInelastic) { + var minSpeVel:Number = useStaticSeparation ? 0 : cp.minSepVel; + if (sepVel < minSpeVel) { + cp.satisfied = false; + } else if (cp.satisfied) return; + newVel = minSpeVel; + } else { + newVel = cp.normalVel; + } + var deltaVel:Number = newVel - sepVel; + var impulse:Number = deltaVel/cp.velByUnitImpulseN; + var accumImpulse:Number = cp.accumImpulseN + impulse; + if (accumImpulse < 0) { + accumImpulse = 0; + } + var deltaImpulse:Number = accumImpulse - cp.accumImpulseN; + cp.accumImpulseN = accumImpulse; + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, normal, deltaImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, normal, -deltaImpulse); + } + + // Учёт силы трения + calcSepVelocity(b1, b2, cp, _v); + // Расчитываем изменение касательной скорости на единицу касательного импульса + var tanSpeedByUnitImpulse:Number = 0; + +// _v.vAddScaled(-_v.vDot(contact.normal), contact.normal); + var dot:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + _v.x -= dot*cnormal.x; + _v.y -= dot*cnormal.y; + _v.z -= dot*cnormal.z; + + var tanSpeed:Number = _v.vLength(); + if (tanSpeed < 0.001) return; + +// _t.vCopy(_v).vNormalize().vReverse(); + _t.x = -_v.x; + _t.y = -_v.y; + _t.z = -_v.z; + _t.vNormalize(); + + var r:Vector3; + var m:Matrix3; + var xx:Number; + var yy:Number; + var zz:Number; + // dV = b.invMass + ((invI * (r % t)) % r) * t + if (b1.movable) { +// _v.vCross2(cp.r1, _t).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1); + r = cp.r1; + m = b1.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b1.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + if (b2 != null && b2.movable) { +// _v.vCross2(cp.r2, _t).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2); + + r = cp.r2; + m = b2.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b2.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + + var tanImpulse:Number = tanSpeed/tanSpeedByUnitImpulse; + var max:Number = contact.friction*cp.accumImpulseN; + if (max < 0) { + if (tanImpulse < max) tanImpulse = max; + } else { + if (tanImpulse > max) tanImpulse = max; + } + + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, _t, tanImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, _t, -tanImpulse); + } + } + + /** + * + * @param cp + * @param normal + * @return + */ + private function calcSepVelocity(body1:Body, body2:Body, cp:ContactPoint, result:Vector3):void { + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 +// result.vCopy(body1.state.velocity).vAdd(_v1.vCross2(body1.state.rotation, cp.r1)); + var rot:Vector3 = body1.state.rotation; + var v:Vector3 = cp.r1; + var x:Number = rot.y*v.z - rot.z*v.y; + var y:Number = rot.z*v.x - rot.x*v.z; + var z:Number = rot.x*v.y - rot.y*v.x; + v = body1.state.velocity; + result.x = v.x + x; + result.y = v.y + y; + result.z = v.z + z; + // V2 = V2_c + w2%r2 + if (body2 != null) { +// result.vSubtract(body2.state.velocity).vSubtract(_v2.vCross2(body2.state.rotation, cp.r2)); + rot = body2.state.rotation; + v = cp.r2; + x = rot.y*v.z - rot.z*v.y; + y = rot.z*v.x - rot.x*v.z; + z = rot.x*v.y - rot.y*v.x; + v = body2.state.velocity; + result.x -= v.x + x; + result.y -= v.y + y; + result.z -= v.z + z; + } + } + + /** + * + * @param dt + */ + private function intergateVelocities(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + item.body.integrateVelocity(dt); + item = item.next; + } + } + + /** + * + * @param dt + */ + private function integratePositions(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + if (body.movable && !body.frozen) { + body.integratePosition(dt); + } + item = item.next; + } + } + + /** + * + */ + private function performStaticSeparation():void { + var iterNum:int = staticSeparationIterations; +// iterNum = 100; + + // 1. В начале каждой итерации для всех контактов сбрасывается флаг satisfied с одновременным поиском наихудшего контакта, т.е. имеющего наибольшую величину пересечения тел. + // 2. Если найденный контакт имеет величину пересечения меньше предельно допустимой процедура прерывается, т.к. разделение контактов не требуется. + // 3. Если процедура продолжается, то выполняется разделение найденного контакта и установка его флага satisfied в true, после чего запускается внутренний цикл, состоящий + // из contactsNum - 1 итераций. На каждой итерации ищется наихудший контакт среди оставшихся (satisfied == false). Если такой контакт найден, то выполняется его разделение + // и переход к следующей итерации внутреннего цикла, иначе внутренний цикл прерывается и выполняется переход к следующей итерации внешнего цикла (шаг 1.). +// for (var iter:int = 0; iter < iterNum; iter++) { +// // Ищем контакт с максимальной величиной пересечения тел, одновременно сбрасывая флаги satisfied в false +// var worstContact:Contact = contacts[0]; +// var i:int; +// for (i = 1; i < contactsNum; i++) { +// var contact:Contact = contacts[i]; +// contact.satisfied = false; +// if (contact.maxPenetration > worstContact.maxPenetration) worstContact = contact; +// } +// if (worstContact.maxPenetration <= allowedPenetration) return; +// +// resolveInterpenetration(worstContact); +// // Внутренний цикл по оставшимся контактам +// for (i = 1; i < contactsNum; i++) { +// worstContact = getWorstContact(); +// if (worstContact == null) break; +// resolveInterpenetration(worstContact); +// } +// } + } + + /** + * + */ +// private function getWorstContact():Contact { +// var maxPen:Number = 0; +// var worst:Contact = null; +// for (var i:int = 0; i < contactsNum; i++) { +// var c:Contact = contacts[i]; +// if (!c.satisfied && c.maxPenetration > maxPen) { +// worst = c; +// maxPen = c.maxPenetration; +// } +// } +// return maxPen > allowedPenetration ? worst : null; +// } + + /** + * Разделяет указанный контакт, выполняя staticSeparationSteps итераций по списку точек контакта. + * 1. В начале каждой итерации ищется точка с наибольшим пересечением, одновременно сбрасываются флаги satisfied у точек. + * 2. Для найденной точки выполняется процедура разделения. + * 3. После выполняется pcount - 1 итерация по списку точек, каждый раз ищется наихудшая среди имеющих satisfied == false, для которой выполняется процедура разделения. + * Если наихудшая точка не найдена, то выполнется переход к следующей итерации внешнего цикла (шаг 1). + * + * В конце процедуры разделения обновляются значения пересечений для остальных точек контакта, а также для всех прочих контактов, относящихся к телам текущего. + * + * @param contact контакт для разделения + */ + private function resolveInterpenetration(contact:Contact):void { + contact.satisfied = true; + + for (var step:int = 0; step < staticSeparationSteps; step++) { + var worstCp:ContactPoint = contact.points[0]; + var cp:ContactPoint; + var i:int; + for (i = 1; i < contact.pcount; i++) { + cp = contact.points[i]; + cp.satisfied = false; + if (cp.penetration > worstCp.penetration) worstCp = cp; + } + if (worstCp.penetration <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + // Разделяем оставшиеся точки + var maxPen:Number = 0; + for (i = 1; i < contact.pcount; i++) { + // Поиск наихудшей точки + for (var j:int = 0; j < contact.pcount; j++) { + cp = contact.points[j]; + if (cp.satisfied) continue; + if (cp.penetration > maxPen) { + maxPen = cp.penetration; + worstCp = cp; + } + } + if (maxPen <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + } + } + } + + /** + * + * @param cp + * @param contact + */ + private function separateContactPoint(cp:ContactPoint, contact:Contact):void { +// cp.satisfied = true; +// +// var b1:Body = contact.body1; +// var b2:Body = contact.body2; +// var totalMove:Number = cp.penetration - allowedPenetration; +// var moveCoeff:Number = totalMove/cp.velByUnitImpulseN; +// var linMove1:Number; +// var angleMove1:Number; +// if (b1.movable) { +// linMove1 = b1.invMass*moveCoeff; +// angleMove1 = cp.angularInertia1*moveCoeff; +// if (angleMove1 > maxAngleMove) { +// linMove1 += angleMove1 - maxAngleMove; +// angleMove1 -= maxAngleMove; +// } +// b1.state.pos.vAddScaled(linMove1, contact.normal); +// _v1.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vScale(angleMove1); +// b1.state.orientation.addScaledVector(_v1, 1); +// } +// var linMove2:Number; +// var angleMove2:Number; +// if (b2 != null && b2.movable) { +// linMove2 = b2.invMass*moveCoeff; +// angleMove2 = cp.angularInertia2*moveCoeff; +// if (angleMove2 > maxAngleMove) { +// linMove2 += angleMove2 - maxAngleMove; +// angleMove2 -= maxAngleMove; +// } +// b2.state.pos.vAddScaled(-linMove2, contact.normal); +// _v2.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vScale(angleMove2); +// _v2.vReverse(); +// b2.state.orientation.addScaledVector(_v2, 1); +// } +// cp.penetration = allowedPenetration; +// // Обновляем пересечения в других точках +// var i:int; +// for (i = 0; i < contact.pcount; i++) { +// var cp1:ContactPoint = contact.points[i]; +// if (cp1 == cp) continue; +// var angularMove:Number; +// if (b1.movable) { +// angularMove = _v.vCross2(_v1, cp1.r1).vDot(contact.normal); +// cp1.penetration -= linMove1 + angularMove; +// } +// if (b2 != null && b2.movable) { +// angularMove = _v.vCross2(_v2, cp1.r2).vDot(contact.normal); +// cp1.penetration -= linMove2 - angularMove; +// } +// // Обновление максимального значения для контакта +// if (cp1.penetration > contact.maxPenetration) contact.maxPenetration = cp1.penetration; +// } +// // Обновляем пересечения для других контактов +// var c:Contact; +// var j:int; +// if (b1.movable) { +// for (i = 0; i < b1.contactsNum; i++) { +// c = b1.contacts[i]; +// if (c == contact) continue; +// for (j = 0; j < c.pcount; j++) { +// cp1 = c.points[j]; +// if (b1 == c.body1) cp1.penetration -= linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r1).vDot(c.normal); +// else cp1.penetration += linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r2).vDot(c.normal); +// if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; +// } +// } +// } +// if (b2 != null && b2.movable) { +// for (i = 0; i < b2.contactsNum; i++) { +// c = b2.contacts[i]; +// if (c == contact) continue; +// for (j = 0; j < c.pcount; j++) { +// cp1 = c.points[j]; +// if (b2 == c.body1) cp1.penetration -= linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r1).vDot(c.normal); +// else cp1.penetration += linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r2).vDot(c.normal); +// if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; +// } +// } +// } + } + + /** + * + */ + private function postPhysics():void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + body.clearAccumulators(); + body.calcDerivedData(); + if (body.canFreeze) { + if (body.state.velocity.vLength() < linSpeedFreezeLimit && body.state.rotation.vLength() < angSpeedFreezeLimit) { + if (!body.frozen) { + body.freezeCounter++; + if (body.freezeCounter >= freezeSteps) body.frozen = true; + } + } else { + body.freezeCounter = 0; + body.frozen = false; + } + } + item = item.next; + } + } + + /** + * + * @param delta + */ + public function runPhysics(delta:uint):void { + timeStamp++; + time += delta; + var dt:Number = 0.001*delta; + applyForces(dt); + detectCollisions(dt); + preProcessContacts(dt); + processContacts(dt, false); + intergateVelocities(dt); + processContacts(dt, true); + integratePositions(dt); + postPhysics(); + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/altphysics.as b/0.0.8.0/src/alternativa/physics/altphysics.as new file mode 100644 index 0000000..2715307 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/altphysics.as @@ -0,0 +1,3 @@ +package alternativa.physics { + public namespace altphysics = "http://alternativaplatform.com/en/altphysics"; +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/.svn/all-wcprops b/0.0.8.0/src/alternativa/physics/collision/.svn/all-wcprops new file mode 100644 index 0000000..3b56746 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/.svn/all-wcprops @@ -0,0 +1,65 @@ +K 25 +svn:wc:ra_dav:version-url +V 113 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/collision +END +ICollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 135 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/collision/ICollisionDetector.as +END +CollisionKdNode.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/collision/CollisionKdNode.as +END +CollisionKdTree2D.as +K 25 +svn:wc:ra_dav:version-url +V 134 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/collision/CollisionKdTree2D.as +END +IRayCollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 139 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/collision/IRayCollisionPredicate.as +END +IBodyCollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/collision/IBodyCollisionPredicate.as +END +CollisionPrimitive.as +K 25 +svn:wc:ra_dav:version-url +V 135 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/collision/CollisionPrimitive.as +END +KdTreeCollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/collision/KdTreeCollisionDetector.as +END +ICollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 136 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/collision/ICollisionPredicate.as +END +ICollider.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/collision/ICollider.as +END +CollisionKdTree.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/collision/CollisionKdTree.as +END diff --git a/0.0.8.0/src/alternativa/physics/collision/.svn/entries b/0.0.8.0/src/alternativa/physics/collision/.svn/entries new file mode 100644 index 0000000..23cda68 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/.svn/entries @@ -0,0 +1,157 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/collision +http://svndev.alternativaplatform.com + + + +2009-10-23T05:59:29.567666Z +22444 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +colliders +dir + +ICollisionDetector.as +file + + + + +2010-10-28T04:32:42.000000Z +04aac68b82def9fa814b66f90a32dbcb +2009-10-19T07:17:33.113306Z +22253 +mike + +CollisionKdNode.as +file + + + + +2010-10-28T04:32:42.000000Z +1d52aba28ce259ca1f9916d70619263d +2009-07-20T03:48:26.517738Z +16522 +mike + +CollisionKdTree2D.as +file + + + + +2010-10-28T04:32:42.000000Z +d8211949ef77817543bc2882480da12f +2009-07-20T03:48:26.517738Z +16522 +mike + +IRayCollisionPredicate.as +file + + + + +2010-10-28T04:32:42.000000Z +32f9b1eaeacfcb40e7ae1927f52b8ab8 +2009-10-19T07:17:33.113306Z +22253 +mike + +types +dir + +IBodyCollisionPredicate.as +file + + + + +2010-10-28T04:32:42.000000Z +618f907a588979a8d06d4f4bcf6618a0 +2009-10-19T07:17:33.113306Z +22253 +mike + +CollisionPrimitive.as +file + + + + +2010-10-28T04:32:42.000000Z +c969fea7a18b12ac1c98faaf9900bf84 +2009-10-20T10:42:06.632117Z +22312 +mike + +KdTreeCollisionDetector.as +file + + + + +2010-10-28T04:32:42.000000Z +fa4965812a9e0e0de2bce66b0de53d44 +2009-10-20T10:42:06.632117Z +22312 +mike + +ICollisionPredicate.as +file + + + + +2010-10-28T04:32:42.000000Z +65f3250d36556fffc457c740fb7d38b3 +2009-10-19T07:17:33.113306Z +22253 +mike + +ICollider.as +file + + + + +2010-10-28T04:32:42.000000Z +128a7d7479e13caeccecb45161b27b22 +2009-10-19T07:17:33.113306Z +22253 +mike + +primitives +dir + +CollisionKdTree.as +file + + + + +2010-10-28T04:32:42.000000Z +1f0db7a1be451e638b7ec88a10219517 +2009-10-19T07:17:33.113306Z +22253 +mike + diff --git a/0.0.8.0/src/alternativa/physics/collision/.svn/format b/0.0.8.0/src/alternativa/physics/collision/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.8.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base b/0.0.8.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base new file mode 100644 index 0000000..e24922c --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base @@ -0,0 +1,18 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.types.BoundBox; + + public class CollisionKdNode { + public var indices:Vector.; + public var splitIndices:Vector.; + public var boundBox:BoundBox; + public var parent:CollisionKdNode; + public var splitTree:CollisionKdTree2D; + + public var axis:int = -1; // 0 - x, 1 - y, 2 - z + public var coord:Number; + public var positiveNode:CollisionKdNode; + public var negativeNode:CollisionKdNode; + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base b/0.0.8.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base new file mode 100644 index 0000000..50ba94a --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base @@ -0,0 +1,261 @@ +package alternativa.physics.collision { + + import __AS3__.vec.Vector; + + import alternativa.physics.collision.types.BoundBox; + + /** + * @author mike + */ + public class CollisionKdTree { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + public var rootNode:CollisionKdNode; + public var staticChildren:Vector.; + public var numStaticChildren:int; + public var staticBoundBoxes:Vector. = new Vector.(); + + private var splitAxis:int; + private var splitCoord:Number; + private var splitCost:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + + /** + * @param boundBox + */ + public function createTree(collisionPrimitives:Vector., boundBox:BoundBox = null):void { + staticChildren = collisionPrimitives.concat(); + numStaticChildren = staticChildren.length; + // Создаём корневую ноду + rootNode = new CollisionKdNode(); + rootNode.indices = new Vector.(); + // Расчитываем баунды объектов и рутовой ноды + var rootNodeBoundBox:BoundBox = rootNode.boundBox = (boundBox != null) ? boundBox : new BoundBox(); + for (var i:int = 0; i < numStaticChildren; ++i) { + var child:CollisionPrimitive = staticChildren[i]; + var childBoundBox:BoundBox = staticBoundBoxes[i] = child.calculateAABB(); + rootNodeBoundBox.addBoundBox(childBoundBox); + rootNode.indices[i] = i; + } + staticBoundBoxes.length = numStaticChildren; + // Разделяем рутовую ноду + splitNode(rootNode); + + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + var indices:Vector. = node.indices; + var numPrimitives:int = indices.length; + if (numPrimitives <= minPrimitivesPerNode) return; + + // Подготовка баунда с погрешностями + var nodeBoundBox:BoundBox = node.boundBox; + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + // Собираем опорные координаты + var i:int; + var j:int; + var numSplitCoordsX:int = 0 + var numSplitCoordsY:int = 0; + var numSplitCoordsZ:int = 0; + for (i = 0; i < numPrimitives; ++i) { + var boundBox:BoundBox = staticBoundBoxes[indices[i]]; + + if (boundBox.maxX - boundBox.minX <= doubleThreshold) { + if (boundBox.minX <= nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.minX; + else if (boundBox.maxX >= nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.maxX; + else splitCoordsX[numSplitCoordsX++] = (boundBox.minX + boundBox.maxX)*0.5; + } else { + if (boundBox.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = boundBox.minX; + if (boundBox.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = boundBox.maxX; + } + + if (boundBox.maxY - boundBox.minY <= doubleThreshold) { + if (boundBox.minY <= nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.minY; + else if (boundBox.maxY >= nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.maxY; + else splitCoordsY[numSplitCoordsY++] = (boundBox.minY + boundBox.maxY)*0.5; + } else { + if (boundBox.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = boundBox.minY; + if (boundBox.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = boundBox.maxY; + } + + if (boundBox.maxZ - boundBox.minZ <= doubleThreshold) { + if (boundBox.minZ <= nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.minZ; + else if (boundBox.maxZ >= nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.maxZ; + else splitCoordsZ[numSplitCoordsZ++] = (boundBox.minZ + boundBox.maxZ)*0.5; + } else { + if (boundBox.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.minZ; + if (boundBox.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numPrimitives; ++i) { + boundBox = staticBoundBoxes[indices[i]]; + var min:Number = axisX ? boundBox.minX : (axisY ? boundBox.minY : boundBox.minZ); + var max:Number = axisX ? boundBox.maxX : (axisY ? boundBox.maxY : boundBox.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(indices[i]); + indices[i] = -1; + } else { + if (node.splitIndices == null) node.splitIndices = new Vector.(); + node.splitIndices.push(indices[i]); + indices[i] = -1; + } + } else { + if (min >= coordMin) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(indices[i]); + indices[i] = -1; + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numPrimitives; ++i) { + if (indices[i] >= 0) indices[j++] = indices[i]; + } + if (j > 0) indices.length = j; + else node.indices = null; + + if (node.splitIndices != null) { + node.splitTree = new CollisionKdTree2D(this, node); + node.splitTree.createTree(); + } + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + /** + * + */ + public function traceTree():void { + traceNode("", rootNode); + } + + /** + * @param str + * @param node + */ + private function traceNode(str:String, node:CollisionKdNode):void { + if (node == null) return; + trace(str, node.axis == -1 ? "end" : ((node.axis == 0) ? "X" : ((node.axis == 1) ? "Y" : "Z")), "splitCoord=" + splitCoord, "bound", node.boundBox, "objs:", node.indices); + traceNode(str + "-", node.negativeNode); + traceNode(str + "+", node.positiveNode); + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base b/0.0.8.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base new file mode 100644 index 0000000..78aaa95 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base @@ -0,0 +1,226 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.types.BoundBox; + + /** + * + */ + public class CollisionKdTree2D { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + + public var parentTree:CollisionKdTree; + public var parentNode:CollisionKdNode; + public var rootNode:CollisionKdNode; + + private var splitAxis:int; + private var splitCost:Number; + private var splitCoord:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + /** + * + * @param parentTree + * @param parentNode + */ + public function CollisionKdTree2D(parentTree:CollisionKdTree, parentNode:CollisionKdNode) { + this.parentTree = parentTree; + this.parentNode = parentNode; + } + + /** + * + */ + public function createTree():void { + rootNode = new CollisionKdNode(); + rootNode.boundBox = parentNode.boundBox.clone(); + rootNode.indices = new Vector.(); + var numObjects:int = parentNode.splitIndices.length; + for (var i:int = 0; i < numObjects; ++i) rootNode.indices[i] = parentNode.splitIndices[i]; + + splitNode(rootNode); + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + if (node.indices.length <= minPrimitivesPerNode) return; + + var objects:Vector. = node.indices; + var i:int; + var j:int; + + var nodeBoundBox:BoundBox = node.boundBox; + + // Подготовка баунда с погрешностями + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + // Собираем опорные координаты + var numSplitCoordsX:int; + var numSplitCoordsY:int; + var numSplitCoordsZ:int; + var numObjects:int = objects.length; + for (i = 0; i < numObjects; ++i) { + var bb:BoundBox = staticBoundBoxes[objects[i]]; + + if (parentNode.axis != 0) { + if (bb.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = bb.minX; + if (bb.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = bb.maxX; + } + + if (parentNode.axis != 1) { + if (bb.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = bb.minY; + if (bb.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = bb.maxY; + } + + if (parentNode.axis != 2) { + if (bb.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = bb.minZ; + if (bb.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = bb.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + if (parentNode.axis != 0) checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + if (parentNode.axis != 1) checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + if (parentNode.axis != 2) checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numObjects; ++i) { + bb = staticBoundBoxes[objects[i]]; + var min:Number = axisX ? bb.minX : (axisY ? bb.minY : bb.minZ); + var max:Number = axisX ? bb.maxX : (axisY ? bb.maxY : bb.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(objects[i]); + objects[i] = -1; + } + } else { + if (min >= coordMin) { + if (max > coordMax) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(objects[i]); + objects[i] = -1; + } + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numObjects; ++i) { + if (objects[i] >= 0) objects[j++] = objects[i]; + } + if (j > 0) objects.length = j; + else node.indices = null; + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/.svn/text-base/CollisionPrimitive.as.svn-base b/0.0.8.0/src/alternativa/physics/collision/.svn/text-base/CollisionPrimitive.as.svn-base new file mode 100644 index 0000000..e42dfb3 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/.svn/text-base/CollisionPrimitive.as.svn-base @@ -0,0 +1,140 @@ +package alternativa.physics.collision { + + import alternativa.physics.Body; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.math.Matrix4; + import alternativa.physics.math.Vector3; + + /** + * Базовый класс для примитивов, использующихся детектором столкновений. + */ + public class CollisionPrimitive { + // Константы типов примитива + public static const BOX:int = 1; + public static const PLANE:int = 2; + public static const SPHERE:int = 3; + public static const RECT:int = 4; + public static const TRIANGLE:int = 5; + + // Тип примитива + public var type:int; + // Группы примитива. Каждая группа определяется установленным битом. Столкновения проверяются только для примитивов, + // имеющих хотя бы одну общую группу. + public var collisionGroup:int; + // Предикат примитива, вызывающийся детектором при нахождении столкновения. В зависимости от возвращённого + // предикатом результата столкновение либо регистрируется, либо игнорируется. + // Nullable + public var postCollisionPredicate:ICollisionPredicate; + // Тело, владеющее примитивом. Поле сделано открытым для быстрого доступа на чтение. Установка значения должна выполняться вызовом метода setBody(). + // Nullable + public var body:Body; + // Трансформация примитива в системе координат тела, если оно указано. Не допускается масштабирование матрицы. + public var localTransform:Matrix4; + // Полная трансформация примитива. Не допускается масштабирование матрицы. + public var transform:Matrix4 = new Matrix4(); + // AABB в мировой системе координат. Расчитывается системой вызовом функции calculateBoundBox(). + public var aabb:BoundBox = new BoundBox(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param type тип примитива + * @param collisionGroup группа примитива + */ + public function CollisionPrimitive(type:int, collisionGroup:int) { + this.type = type; + this.collisionGroup = collisionGroup; + } + + /** + * Устанавливает тело, владеющее примитивом. + * + * @param body тело, которое владеет примитивом + * @param localTransform трансформация примитива в системе координат тела. Указание значения null равносильно + * заданию единичной матрицы, однако в первом случае не будет дополнительного умножения матриц при вычислении полной трансформации примитива. + */ + public function setBody(body:Body, localTransform:Matrix4 = null):void { + if (this.body == body) return; + this.body = body; + if (body != null) { + if (localTransform != null) { + if (this.localTransform == null) { + this.localTransform = new Matrix4(); + } + this.localTransform.copy(localTransform); + } else { + this.localTransform = null; + } + } + } + + /** + * Рассчитывает AABB примитива. Наследники должны переопределять этот метод, реализуя в нём корректный рассчёт. + * + * @return ссылка на свой AABB + */ + public function calculateAABB():BoundBox { + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + return -1; + } + + /** + * Клонирует примитив. Переопределять не рекомендуется. Вместо этого переопределяются методы createPrimitive() и copyFrom(). + * + * @return клон примитива + */ + public function clone():CollisionPrimitive { + var p:CollisionPrimitive = createPrimitive(); + return p.copyFrom(this); + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + if (source == null) { + throw new ArgumentError("Parameter source cannot be null"); + } + type = source.type; + transform.copy(source.transform); + collisionGroup = source.collisionGroup; + setBody(source.body, source.localTransform); + aabb.copyFrom(source.aabb); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + public function toString():String { + return "[CollisionPrimitive type=" + type + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + protected function createPrimitive():CollisionPrimitive { + return new CollisionPrimitive(type, collisionGroup); + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base b/0.0.8.0/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base new file mode 100644 index 0000000..4ace681 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.Body; + + public interface IBodyCollisionPredicate { + function considerBodies(body1:Body, body2:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base b/0.0.8.0/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base new file mode 100644 index 0000000..e6aac2a --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base @@ -0,0 +1,29 @@ +package alternativa.physics.collision { + + import alternativa.physics.Contact; + + /** + * Интерфейс определителя столкновений между двумя примитивами. + */ + public interface ICollider { + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base b/0.0.8.0/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base new file mode 100644 index 0000000..25a71d9 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base @@ -0,0 +1,52 @@ +package alternativa.physics.collision { + import alternativa.physics.Contact; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.math.Vector3; + + /** + * Интерфейс детектора столкновений. + */ + public interface ICollisionDetector { + + /** + * Получает все столкновения в текущей конфигурации физической геометрии. + * + * @param contacts список контактов, в кторые будет записана информация о столкновении + * @return количество найденных столкновений + */ + function getAllContacts(contacts:Contact):Contact; + + /** + * Тестирует луч на пересечение с физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param direction направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRay(origin:Vector3, direction:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @return + */ + function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base b/0.0.8.0/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base new file mode 100644 index 0000000..33d30ca --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base @@ -0,0 +1,8 @@ +package alternativa.physics.collision { + + public interface ICollisionPredicate { + + function considerCollision(primitive:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base b/0.0.8.0/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base new file mode 100644 index 0000000..b319f62 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.Body; + + public interface IRayCollisionPredicate { + function considerBody(body:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base b/0.0.8.0/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base new file mode 100644 index 0000000..14202ec --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base @@ -0,0 +1,486 @@ +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.; + 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.(); + + 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.):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. = tree.staticChildren; +// var indices:Vector. = 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., indices:Vector., 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; + } \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/CollisionKdNode.as b/0.0.8.0/src/alternativa/physics/collision/CollisionKdNode.as new file mode 100644 index 0000000..e24922c --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/CollisionKdNode.as @@ -0,0 +1,18 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.types.BoundBox; + + public class CollisionKdNode { + public var indices:Vector.; + public var splitIndices:Vector.; + public var boundBox:BoundBox; + public var parent:CollisionKdNode; + public var splitTree:CollisionKdTree2D; + + public var axis:int = -1; // 0 - x, 1 - y, 2 - z + public var coord:Number; + public var positiveNode:CollisionKdNode; + public var negativeNode:CollisionKdNode; + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/CollisionKdTree.as b/0.0.8.0/src/alternativa/physics/collision/CollisionKdTree.as new file mode 100644 index 0000000..50ba94a --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/CollisionKdTree.as @@ -0,0 +1,261 @@ +package alternativa.physics.collision { + + import __AS3__.vec.Vector; + + import alternativa.physics.collision.types.BoundBox; + + /** + * @author mike + */ + public class CollisionKdTree { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + public var rootNode:CollisionKdNode; + public var staticChildren:Vector.; + public var numStaticChildren:int; + public var staticBoundBoxes:Vector. = new Vector.(); + + private var splitAxis:int; + private var splitCoord:Number; + private var splitCost:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + + /** + * @param boundBox + */ + public function createTree(collisionPrimitives:Vector., boundBox:BoundBox = null):void { + staticChildren = collisionPrimitives.concat(); + numStaticChildren = staticChildren.length; + // Создаём корневую ноду + rootNode = new CollisionKdNode(); + rootNode.indices = new Vector.(); + // Расчитываем баунды объектов и рутовой ноды + var rootNodeBoundBox:BoundBox = rootNode.boundBox = (boundBox != null) ? boundBox : new BoundBox(); + for (var i:int = 0; i < numStaticChildren; ++i) { + var child:CollisionPrimitive = staticChildren[i]; + var childBoundBox:BoundBox = staticBoundBoxes[i] = child.calculateAABB(); + rootNodeBoundBox.addBoundBox(childBoundBox); + rootNode.indices[i] = i; + } + staticBoundBoxes.length = numStaticChildren; + // Разделяем рутовую ноду + splitNode(rootNode); + + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + var indices:Vector. = node.indices; + var numPrimitives:int = indices.length; + if (numPrimitives <= minPrimitivesPerNode) return; + + // Подготовка баунда с погрешностями + var nodeBoundBox:BoundBox = node.boundBox; + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + // Собираем опорные координаты + var i:int; + var j:int; + var numSplitCoordsX:int = 0 + var numSplitCoordsY:int = 0; + var numSplitCoordsZ:int = 0; + for (i = 0; i < numPrimitives; ++i) { + var boundBox:BoundBox = staticBoundBoxes[indices[i]]; + + if (boundBox.maxX - boundBox.minX <= doubleThreshold) { + if (boundBox.minX <= nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.minX; + else if (boundBox.maxX >= nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.maxX; + else splitCoordsX[numSplitCoordsX++] = (boundBox.minX + boundBox.maxX)*0.5; + } else { + if (boundBox.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = boundBox.minX; + if (boundBox.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = boundBox.maxX; + } + + if (boundBox.maxY - boundBox.minY <= doubleThreshold) { + if (boundBox.minY <= nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.minY; + else if (boundBox.maxY >= nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.maxY; + else splitCoordsY[numSplitCoordsY++] = (boundBox.minY + boundBox.maxY)*0.5; + } else { + if (boundBox.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = boundBox.minY; + if (boundBox.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = boundBox.maxY; + } + + if (boundBox.maxZ - boundBox.minZ <= doubleThreshold) { + if (boundBox.minZ <= nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.minZ; + else if (boundBox.maxZ >= nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.maxZ; + else splitCoordsZ[numSplitCoordsZ++] = (boundBox.minZ + boundBox.maxZ)*0.5; + } else { + if (boundBox.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.minZ; + if (boundBox.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numPrimitives; ++i) { + boundBox = staticBoundBoxes[indices[i]]; + var min:Number = axisX ? boundBox.minX : (axisY ? boundBox.minY : boundBox.minZ); + var max:Number = axisX ? boundBox.maxX : (axisY ? boundBox.maxY : boundBox.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(indices[i]); + indices[i] = -1; + } else { + if (node.splitIndices == null) node.splitIndices = new Vector.(); + node.splitIndices.push(indices[i]); + indices[i] = -1; + } + } else { + if (min >= coordMin) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(indices[i]); + indices[i] = -1; + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numPrimitives; ++i) { + if (indices[i] >= 0) indices[j++] = indices[i]; + } + if (j > 0) indices.length = j; + else node.indices = null; + + if (node.splitIndices != null) { + node.splitTree = new CollisionKdTree2D(this, node); + node.splitTree.createTree(); + } + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + /** + * + */ + public function traceTree():void { + traceNode("", rootNode); + } + + /** + * @param str + * @param node + */ + private function traceNode(str:String, node:CollisionKdNode):void { + if (node == null) return; + trace(str, node.axis == -1 ? "end" : ((node.axis == 0) ? "X" : ((node.axis == 1) ? "Y" : "Z")), "splitCoord=" + splitCoord, "bound", node.boundBox, "objs:", node.indices); + traceNode(str + "-", node.negativeNode); + traceNode(str + "+", node.positiveNode); + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/CollisionKdTree2D.as b/0.0.8.0/src/alternativa/physics/collision/CollisionKdTree2D.as new file mode 100644 index 0000000..78aaa95 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/CollisionKdTree2D.as @@ -0,0 +1,226 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.types.BoundBox; + + /** + * + */ + public class CollisionKdTree2D { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + + public var parentTree:CollisionKdTree; + public var parentNode:CollisionKdNode; + public var rootNode:CollisionKdNode; + + private var splitAxis:int; + private var splitCost:Number; + private var splitCoord:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + /** + * + * @param parentTree + * @param parentNode + */ + public function CollisionKdTree2D(parentTree:CollisionKdTree, parentNode:CollisionKdNode) { + this.parentTree = parentTree; + this.parentNode = parentNode; + } + + /** + * + */ + public function createTree():void { + rootNode = new CollisionKdNode(); + rootNode.boundBox = parentNode.boundBox.clone(); + rootNode.indices = new Vector.(); + var numObjects:int = parentNode.splitIndices.length; + for (var i:int = 0; i < numObjects; ++i) rootNode.indices[i] = parentNode.splitIndices[i]; + + splitNode(rootNode); + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + if (node.indices.length <= minPrimitivesPerNode) return; + + var objects:Vector. = node.indices; + var i:int; + var j:int; + + var nodeBoundBox:BoundBox = node.boundBox; + + // Подготовка баунда с погрешностями + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + // Собираем опорные координаты + var numSplitCoordsX:int; + var numSplitCoordsY:int; + var numSplitCoordsZ:int; + var numObjects:int = objects.length; + for (i = 0; i < numObjects; ++i) { + var bb:BoundBox = staticBoundBoxes[objects[i]]; + + if (parentNode.axis != 0) { + if (bb.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = bb.minX; + if (bb.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = bb.maxX; + } + + if (parentNode.axis != 1) { + if (bb.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = bb.minY; + if (bb.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = bb.maxY; + } + + if (parentNode.axis != 2) { + if (bb.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = bb.minZ; + if (bb.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = bb.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + if (parentNode.axis != 0) checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + if (parentNode.axis != 1) checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + if (parentNode.axis != 2) checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numObjects; ++i) { + bb = staticBoundBoxes[objects[i]]; + var min:Number = axisX ? bb.minX : (axisY ? bb.minY : bb.minZ); + var max:Number = axisX ? bb.maxX : (axisY ? bb.maxY : bb.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(objects[i]); + objects[i] = -1; + } + } else { + if (min >= coordMin) { + if (max > coordMax) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(objects[i]); + objects[i] = -1; + } + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numObjects; ++i) { + if (objects[i] >= 0) objects[j++] = objects[i]; + } + if (j > 0) objects.length = j; + else node.indices = null; + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/CollisionPrimitive.as b/0.0.8.0/src/alternativa/physics/collision/CollisionPrimitive.as new file mode 100644 index 0000000..e42dfb3 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/CollisionPrimitive.as @@ -0,0 +1,140 @@ +package alternativa.physics.collision { + + import alternativa.physics.Body; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.math.Matrix4; + import alternativa.physics.math.Vector3; + + /** + * Базовый класс для примитивов, использующихся детектором столкновений. + */ + public class CollisionPrimitive { + // Константы типов примитива + public static const BOX:int = 1; + public static const PLANE:int = 2; + public static const SPHERE:int = 3; + public static const RECT:int = 4; + public static const TRIANGLE:int = 5; + + // Тип примитива + public var type:int; + // Группы примитива. Каждая группа определяется установленным битом. Столкновения проверяются только для примитивов, + // имеющих хотя бы одну общую группу. + public var collisionGroup:int; + // Предикат примитива, вызывающийся детектором при нахождении столкновения. В зависимости от возвращённого + // предикатом результата столкновение либо регистрируется, либо игнорируется. + // Nullable + public var postCollisionPredicate:ICollisionPredicate; + // Тело, владеющее примитивом. Поле сделано открытым для быстрого доступа на чтение. Установка значения должна выполняться вызовом метода setBody(). + // Nullable + public var body:Body; + // Трансформация примитива в системе координат тела, если оно указано. Не допускается масштабирование матрицы. + public var localTransform:Matrix4; + // Полная трансформация примитива. Не допускается масштабирование матрицы. + public var transform:Matrix4 = new Matrix4(); + // AABB в мировой системе координат. Расчитывается системой вызовом функции calculateBoundBox(). + public var aabb:BoundBox = new BoundBox(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param type тип примитива + * @param collisionGroup группа примитива + */ + public function CollisionPrimitive(type:int, collisionGroup:int) { + this.type = type; + this.collisionGroup = collisionGroup; + } + + /** + * Устанавливает тело, владеющее примитивом. + * + * @param body тело, которое владеет примитивом + * @param localTransform трансформация примитива в системе координат тела. Указание значения null равносильно + * заданию единичной матрицы, однако в первом случае не будет дополнительного умножения матриц при вычислении полной трансформации примитива. + */ + public function setBody(body:Body, localTransform:Matrix4 = null):void { + if (this.body == body) return; + this.body = body; + if (body != null) { + if (localTransform != null) { + if (this.localTransform == null) { + this.localTransform = new Matrix4(); + } + this.localTransform.copy(localTransform); + } else { + this.localTransform = null; + } + } + } + + /** + * Рассчитывает AABB примитива. Наследники должны переопределять этот метод, реализуя в нём корректный рассчёт. + * + * @return ссылка на свой AABB + */ + public function calculateAABB():BoundBox { + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + return -1; + } + + /** + * Клонирует примитив. Переопределять не рекомендуется. Вместо этого переопределяются методы createPrimitive() и copyFrom(). + * + * @return клон примитива + */ + public function clone():CollisionPrimitive { + var p:CollisionPrimitive = createPrimitive(); + return p.copyFrom(this); + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + if (source == null) { + throw new ArgumentError("Parameter source cannot be null"); + } + type = source.type; + transform.copy(source.transform); + collisionGroup = source.collisionGroup; + setBody(source.body, source.localTransform); + aabb.copyFrom(source.aabb); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + public function toString():String { + return "[CollisionPrimitive type=" + type + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + protected function createPrimitive():CollisionPrimitive { + return new CollisionPrimitive(type, collisionGroup); + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/IBodyCollisionPredicate.as b/0.0.8.0/src/alternativa/physics/collision/IBodyCollisionPredicate.as new file mode 100644 index 0000000..4ace681 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/IBodyCollisionPredicate.as @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.Body; + + public interface IBodyCollisionPredicate { + function considerBodies(body1:Body, body2:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/ICollider.as b/0.0.8.0/src/alternativa/physics/collision/ICollider.as new file mode 100644 index 0000000..e6aac2a --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/ICollider.as @@ -0,0 +1,29 @@ +package alternativa.physics.collision { + + import alternativa.physics.Contact; + + /** + * Интерфейс определителя столкновений между двумя примитивами. + */ + public interface ICollider { + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/ICollisionDetector.as b/0.0.8.0/src/alternativa/physics/collision/ICollisionDetector.as new file mode 100644 index 0000000..25a71d9 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/ICollisionDetector.as @@ -0,0 +1,52 @@ +package alternativa.physics.collision { + import alternativa.physics.Contact; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.math.Vector3; + + /** + * Интерфейс детектора столкновений. + */ + public interface ICollisionDetector { + + /** + * Получает все столкновения в текущей конфигурации физической геометрии. + * + * @param contacts список контактов, в кторые будет записана информация о столкновении + * @return количество найденных столкновений + */ + function getAllContacts(contacts:Contact):Contact; + + /** + * Тестирует луч на пересечение с физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param direction направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRay(origin:Vector3, direction:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @return + */ + function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/ICollisionPredicate.as b/0.0.8.0/src/alternativa/physics/collision/ICollisionPredicate.as new file mode 100644 index 0000000..33d30ca --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/ICollisionPredicate.as @@ -0,0 +1,8 @@ +package alternativa.physics.collision { + + public interface ICollisionPredicate { + + function considerCollision(primitive:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/IRayCollisionPredicate.as b/0.0.8.0/src/alternativa/physics/collision/IRayCollisionPredicate.as new file mode 100644 index 0000000..b319f62 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/IRayCollisionPredicate.as @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.Body; + + public interface IRayCollisionPredicate { + function considerBody(body:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/KdTreeCollisionDetector.as b/0.0.8.0/src/alternativa/physics/collision/KdTreeCollisionDetector.as new file mode 100644 index 0000000..14202ec --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/KdTreeCollisionDetector.as @@ -0,0 +1,486 @@ +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.; + 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.(); + + 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.):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. = tree.staticChildren; +// var indices:Vector. = 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., indices:Vector., 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; + } \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/colliders/.svn/all-wcprops b/0.0.8.0/src/alternativa/physics/collision/colliders/.svn/all-wcprops new file mode 100644 index 0000000..dca2aef --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/colliders/.svn/all-wcprops @@ -0,0 +1,53 @@ +K 25 +svn:wc:ra_dav:version-url +V 123 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/collision/colliders +END +BoxSphereCollider.as +K 25 +svn:wc:ra_dav:version-url +V 144 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/collision/colliders/BoxSphereCollider.as +END +BoxBoxCollider.as +K 25 +svn:wc:ra_dav:version-url +V 141 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/collision/colliders/BoxBoxCollider.as +END +BoxRectCollider.as +K 25 +svn:wc:ra_dav:version-url +V 142 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/collision/colliders/BoxRectCollider.as +END +SpherePlaneCollider.as +K 25 +svn:wc:ra_dav:version-url +V 146 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/collision/colliders/SpherePlaneCollider.as +END +BoxCollider.as +K 25 +svn:wc:ra_dav:version-url +V 138 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/collision/colliders/BoxCollider.as +END +BoxPlaneCollider.as +K 25 +svn:wc:ra_dav:version-url +V 143 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/collision/colliders/BoxPlaneCollider.as +END +SphereSphereCollider.as +K 25 +svn:wc:ra_dav:version-url +V 147 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/collision/colliders/SphereSphereCollider.as +END +BoxTriangleCollider.as +K 25 +svn:wc:ra_dav:version-url +V 146 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/collision/colliders/BoxTriangleCollider.as +END diff --git a/0.0.8.0/src/alternativa/physics/collision/colliders/.svn/entries b/0.0.8.0/src/alternativa/physics/collision/colliders/.svn/entries new file mode 100644 index 0000000..c94e0ef --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/colliders/.svn/entries @@ -0,0 +1,124 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/collision/colliders +http://svndev.alternativaplatform.com + + + +2009-10-23T05:59:29.567666Z +22444 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +BoxSphereCollider.as +file + + + + +2010-10-28T04:32:42.000000Z +b2478004208a1d598d9f57747d83ccbb +2009-10-19T07:17:33.113306Z +22253 +mike + +BoxBoxCollider.as +file + + + + +2010-10-28T04:32:42.000000Z +543790ceff5b23a0b72bde58c111183d +2009-10-23T05:59:29.567666Z +22444 +mike + +BoxRectCollider.as +file + + + + +2010-10-28T04:32:42.000000Z +7ebc58be651f18d7e0374e6a936f36e0 +2009-10-23T05:59:29.567666Z +22444 +mike + +SpherePlaneCollider.as +file + + + + +2010-10-28T04:32:42.000000Z +e7f53348558f035e2f34a9e1fe1e1e4b +2009-10-19T07:17:33.113306Z +22253 +mike + +BoxCollider.as +file + + + + +2010-10-28T04:32:42.000000Z +5ddcd853975ee9d6a0c776a14f674e0e +2009-10-19T07:17:33.113306Z +22253 +mike + +BoxPlaneCollider.as +file + + + + +2010-10-28T04:32:42.000000Z +71648e823c289cb67b79e7c8af3045d2 +2009-10-19T07:17:33.113306Z +22253 +mike + +SphereSphereCollider.as +file + + + + +2010-10-28T04:32:42.000000Z +b336de4614439b93424863b1ace66c9b +2009-10-19T07:17:33.113306Z +22253 +mike + +BoxTriangleCollider.as +file + + + + +2010-10-28T04:32:42.000000Z +72a48b0dde7454a9c2f2b76345d77de2 +2009-10-23T05:59:29.567666Z +22444 +mike + diff --git a/0.0.8.0/src/alternativa/physics/collision/colliders/.svn/format b/0.0.8.0/src/alternativa/physics/collision/colliders/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/colliders/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.8.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxBoxCollider.as.svn-base b/0.0.8.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxBoxCollider.as.svn-base new file mode 100644 index 0000000..b003e98 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxBoxCollider.as.svn-base @@ -0,0 +1,589 @@ +package alternativa.physics.collision.colliders { + import __AS3__.vec.Vector; + + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.math.Matrix4; + import alternativa.physics.math.Vector3; + + use namespace altphysics; + + /** + * Расчитывает точки контакта двух боксов. Нормаль контакта направляется в сторону бокса с меньшим ID. + */ + public class BoxBoxCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + private var vectorToBox1:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var bestAxisIndex:int; + private var minOverlap:Number; + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + private var tmpPoints:Vector. = new Vector.(8, true); + private var pcount:int; + + /** + * + */ + public function BoxBoxCollider() { + for (var i:int = 0; i < 8; i++) { + tmpPoints[i] = new ContactPoint(); + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + + if (bestAxisIndex < 6) { + // Контакт грань-(грань|ребро|вершина) + if (!findFaceContactPoints(box1, box2, vectorToBox1, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 6; + findEdgesIntersection(box1, box2, vectorToBox1, int(bestAxisIndex/3), bestAxisIndex%3, contact); + } + contact.body1 = box1.body; + contact.body2 = box2.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + // Вектор из центра второго бокса в центр первого + vectorToBox1.x = transform1.d - transform2.d; + vectorToBox1.y = transform1.h - transform2.h; + vectorToBox1.z = transform1.l - transform2.l; + + // Проверка пересечения по основным осям + axis10.x = transform1.a; + axis10.y = transform1.e; + axis10.z = transform1.i; + if (!testMainAxis(box1, box2, axis10, 0, vectorToBox1)) return false; + axis11.x = transform1.b; + axis11.y = transform1.f; + axis11.z = transform1.j; + if (!testMainAxis(box1, box2, axis11, 1, vectorToBox1)) return false; + axis12.x = transform1.c; + axis12.y = transform1.g; + axis12.z = transform1.k; + if (!testMainAxis(box1, box2, axis12, 2, vectorToBox1)) return false; + + axis20.x = transform2.a; + axis20.y = transform2.e; + axis20.z = transform2.i; + if (!testMainAxis(box1, box2, axis20, 3, vectorToBox1)) return false; + axis21.x = transform2.b; + axis21.y = transform2.f; + axis21.z = transform2.j; + if (!testMainAxis(box1, box2, axis21, 4, vectorToBox1)) return false; + axis22.x = transform2.c; + axis22.y = transform2.g; + axis22.z = transform2.k; + if (!testMainAxis(box1, box2, axis22, 5, vectorToBox1)) return false; + + // Проверка производных осей + if (!testDerivedAxis(box1, box2, axis10, axis20, 6, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis21, 7, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis22, 8, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis11, axis20, 9, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis21, 10, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis22, 11, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis12, axis20, 12, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis21, 13, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis22, 14, vectorToBox1)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта грани одного бокса с гранью/ребром/вершиной другого. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param faceAxisIdx индекс оси первого бокса, перпендикулярной грани, с которой произошло столкновение + * @param contactInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var swapNormal:Boolean = false; + if (faceAxisIdx > 2) { + // Столкновение с гранью второго бокса. Для дальнейших расчётов боксы меняются местами, + // но нормаль контакта всё равно должна быть направлена в сторону первоначального box1 + var tmpBox:CollisionBox = box1; + box1 = box2; + box2 = tmpBox; + vectorToBox1.x = -vectorToBox1.x; + vectorToBox1.y = -vectorToBox1.y; + vectorToBox1.z = -vectorToBox1.z; + faceAxisIdx -= 3; + swapNormal = true; + } + + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + var colAxis:Vector3 = contact.normal; + + transform1.getAxis(faceAxisIdx, colAxis); + var negativeFace:Boolean = colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z > 0; + + var code:int = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box1.excludedFaces) != 0) return false; + + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + // Ищем ось второго бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + transform2.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisDot = dot; + incidentAxisIdx = axisIdx; + } + } + // Получаем список вершин грани второго бокса, переводим их в систему координат первого бокса и выполняем обрезку + // по грани первого бокса. Таким образом получается список потенциальных точек контакта. + transform2.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box2.hs, incidentAxisIdx, incidentAxisDot < 0, points1); + + // TODO: Вычислить результирующую матрицу, затем преобразовать векторы за один заход + transform2.transformVectorsN(points1, points2, 4); + transform1.transformVectorsInverseN(points2, points1, 4); + + var pnum:int = clip(box1.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points1[i]; + if ((pen = getPointBoxPenetration(box1.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + var cp:ContactPoint = tmpPoints[pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = transform1.a*v.x + transform1.b*v.y + transform1.c*v.z + transform1.d; + cpPos.y = transform1.e*v.x + transform1.f*v.y + transform1.g*v.z + transform1.h; + cpPos.z = transform1.i*v.x + transform1.j*v.y + transform1.k*v.z + transform1.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + + cp.penetration = pen; + } + } + if (swapNormal) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (pcount > 4) { + reducePoints(); + } + for (i = 0; i < pcount; i++) { + cp = contact.points[i]; + cp.copyFrom(tmpPoints[i]); + } + contact.pcount = pcount; + return true; + } + + /** + * + * @param contactInfo + */ + private function reducePoints():void { + var i:int; + var minIdx:int; + var cp1:ContactPoint; + var cp2:ContactPoint; + while (pcount > 4) { + var minLen:Number = 1e10; + var p1:Vector3 = (tmpPoints[int(pcount - 1)] as ContactPoint).pos; + var p2:Vector3; + for (i = 0; i < pcount; i++) { + p2 = (tmpPoints[i] as ContactPoint).pos; + var dx:Number = p2.x - p1.x; + var dy:Number = p2.y - p1.y; + var dz:Number = p2.z - p1.z; + var len:Number = dx*dx + dy*dy + dz*dz; + if (len < minLen) { + minLen = len; + minIdx = i; + } + p1 = p2; + } + var ii:int = minIdx == 0 ? (pcount - 1) : (minIdx - 1); + cp1 = tmpPoints[ii]; + cp2 = tmpPoints[minIdx]; + p1 = cp1.pos; + p2 = cp2.pos; + p2.x = 0.5*(p1.x + p2.x); + p2.y = 0.5*(p1.y + p2.y); + p2.z = 0.5*(p1.z + p2.z); + cp2.penetration = 0.5*(cp1.penetration + cp2.penetration); + if (minIdx > 0) { + for (i = minIdx; i < pcount; i++) { + tmpPoints[int(i - 1)] = tmpPoints[i]; + } + tmpPoints[int(pcount - 1)] = cp1; + } + pcount--; + } + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clip(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param contactInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):void { + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + transform1.getAxis(axisIdx1, axis10); + transform2.getAxis(axisIdx2, axis20); + + var colAxis:Vector3 = contact.normal; + + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону первого бокса + if (colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + + var tx:Number = box1.hs.x; + var ty:Number = box1.hs.y; + var tz:Number = box1.hs.z; + + var x2:Number = box2.hs.x; + var y2:Number = box2.hs.y; + var z2:Number = box2.hs.z; + // x + if (axisIdx1 == 0) { + tx = 0; + halfLen1 = box1.hs.x; + } else { + if (colAxis.x*transform1.a + colAxis.y*transform1.e + colAxis.z*transform1.i > 0) { + tx = -tx; + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = box2.hs.x; + } else { + if (colAxis.x*transform2.a + colAxis.y*transform2.e + colAxis.z*transform2.i < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + ty = 0; + halfLen1 = box1.hs.y; + } else { + if (colAxis.x*transform1.b + colAxis.y*transform1.f + colAxis.z*transform1.j > 0) { + ty = -ty; + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = box2.hs.y; + } else { + if (colAxis.x*transform2.b + colAxis.y*transform2.f + colAxis.z*transform2.j < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + tz = 0; + halfLen1 = box1.hs.z; + } else { + if (colAxis.x*transform1.c + colAxis.y*transform1.g + colAxis.z*transform1.k > 0) { + tz = -tz; + } + } + if (axisIdx2 == 2) { + z2 = 0; + halfLen2 = box2.hs.z; + } else { + if (colAxis.x*transform2.c + colAxis.y*transform2.g + colAxis.z*transform2.k < 0) { + z2 = -z2; + } + } + // Получаем глобальные координаты средних точек рёбер + var x1:Number = transform1.a*tx + transform1.b*ty + transform1.c*tz + transform1.d; + var y1:Number = transform1.e*tx + transform1.f*ty + transform1.g*tz + transform1.h; + var z1:Number = transform1.i*tx + transform1.j*ty + transform1.k*tz + transform1.l; + tx = x2; + ty = y2; + tz = z2; + x2 = transform2.a*tx + transform2.b*ty + transform2.c*tz + transform2.d; + y2 = transform2.e*tx + transform2.f*ty + transform2.g*tz + transform2.h; + z2 = transform2.i*tx + transform2.j*ty + transform2.k*tz + transform2.l; + + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + tx = x2 - x1; + ty = y2 - y1; + tz = z2 - z1; + var c1:Number = axis10.x*tx + axis10.y*ty + axis10.z*tz; + var c2:Number = axis20.x*tx + axis20.y*ty + axis20.z*tz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cp.pos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cp.pos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение боксов вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box1 + * @param box2 + * @param axis + * @param axisIndex + * @param toBox1 + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box1 + * @param box2 + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox1 + * @return + */ + private function testDerivedAxis(box1:CollisionBox, box2:CollisionBox, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия двух боксов вдоль заданной оси. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param axis ось + * @param vectorToBox1 вектор, соединяющий центр второго бокса с центром первого + * @return величина перекрытия боксов вдоль оси + */ + public function overlapOnAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, vectorToBox1:Vector3):Number { + var m:Matrix4 = box1.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box1.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box1.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box1.hs.z; + if (d < 0) d = -d; + projection += d; + + m = box2.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box2.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box2.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box2.hs.z; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox1.x*axis.x + vectorToBox1.y*axis.y + vectorToBox1.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} + import alternativa.physics.math.Vector3; + + class CollisionPointTmp { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + } diff --git a/0.0.8.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxCollider.as.svn-base b/0.0.8.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxCollider.as.svn-base new file mode 100644 index 0000000..0021854 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxCollider.as.svn-base @@ -0,0 +1,427 @@ +package alternativa.physics.collision.colliders { + import __AS3__.vec.Vector; + + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.math.Vector3; + + /** + * + */ + public class BoxCollider implements ICollider { + + /** + * + */ + public function BoxCollider() { + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + return false; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + protected function getFaceVertsByAxis(hs:Vector3, axisIdx:int, negativeFace:Boolean, result:Vector.):void { + var v:Vector3; + switch (axisIdx) { + case 0: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + } + break; + case 1: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + } + break; + case 2: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + } else { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } + break; + } + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxPlaneCollider.as.svn-base b/0.0.8.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxPlaneCollider.as.svn-base new file mode 100644 index 0000000..bb1ed0f --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxPlaneCollider.as.svn-base @@ -0,0 +1,88 @@ +package alternativa.physics.collision.colliders { + import __AS3__.vec.Vector; + + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.math.Vector3; + + public class BoxPlaneCollider implements ICollider { + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var normal:Vector3 = new Vector3(); + + public function BoxPlaneCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var box:RigidBox = body1 as RigidBox; +// var plane:RigidPlane; +// if (box == null) { +// box = body2 as RigidBox; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные координаты вершин бокса +// var sx:Number = box.halfSize.x; +// var sy:Number = box.halfSize.y; +// var sz:Number = box.halfSize.z; +// (verts1[0] as Vector3).reset(-sx, -sy, -sz); +// (verts1[1] as Vector3).reset(sx, -sy, -sz); +// (verts1[2] as Vector3).reset(sx, sy, -sz); +// (verts1[3] as Vector3).reset(-sx, sy, -sz); +// (verts1[4] as Vector3).reset(-sx, -sy, sz); +// (verts1[5] as Vector3).reset(sx, -sy, sz); +// (verts1[6] as Vector3).reset(sx, sy, sz); +// (verts1[7] as Vector3).reset(-sx, sy, sz); +// +// box.transform.transformVectors(verts1, verts2); +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// // Проверяем наличие столкновений с каждой вершиной +// collisionInfo.pcount = 0; +// for (var i:int = 0; i < 8; i++) { +// // Вершина добавляется в список точек столкновения, если лежит под плоскостью +// var dist:Number = (verts2[i] as Vector3).dot(normal); +// if (dist < offset) { +// var cp:ContactPoint; +// if (collisionInfo.pcount == collisionInfo.points.length) { +// cp = new ContactPoint(); +// collisionInfo.points[collisionInfo.pcount] = cp; +// } else { +// cp = collisionInfo.points[collisionInfo.pcount]; +// } +// cp.pos.copy(verts2[i]); +// cp.r1.diff(cp.pos, box.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); +// cp.penetration = offset - dist; +// collisionInfo.pcount++; +// } +// } +// if (collisionInfo.pcount > 0) { +// collisionInfo.body1 = box; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// return true; +// } + return false; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxRectCollider.as.svn-base b/0.0.8.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxRectCollider.as.svn-base new file mode 100644 index 0000000..35f904e --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxRectCollider.as.svn-base @@ -0,0 +1,568 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.physics.math.Matrix4; + import alternativa.physics.math.Vector3; + use namespace altphysics; + + /** + * + */ + public class BoxRectCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + + private var vectorToBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxRectCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, rect, vectorToBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + if (!findEdgesIntersection(box, rect, vectorToBox, int(bestAxisIndex/2), bestAxisIndex%2, contact)) { + return false; + } + } + contact.body1 = box.body; + contact.body2 = rect.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + // Вектор из центра прямоугольника в центр бокса + vectorToBox.x = boxTransform.d - rectTransform.d; + vectorToBox.y = boxTransform.h - rectTransform.h; + vectorToBox.z = boxTransform.l - rectTransform.l; + + // Проверка пересечения по нормали прямоугольника + rectTransform.getAxis(2, axis22); + if (!testMainAxis(box, rect, axis22, 0, vectorToBox)) return false; + + // Проверка пересечения по основным осям бокса + boxTransform.getAxis(0, axis10); + if (!testMainAxis(box, rect, axis10, 1, vectorToBox)) return false; + boxTransform.getAxis(1, axis11); + if (!testMainAxis(box, rect, axis11, 2, vectorToBox)) return false; + boxTransform.getAxis(2, axis12); + if (!testMainAxis(box, rect, axis12, 3, vectorToBox)) return false; + + // Получаем направляющие рёбер прямоугольника + rectTransform.getAxis(0, axis20); + rectTransform.getAxis(1, axis21); + + // Проверка производных осей + if (!testDerivedAxis(box, rect, axis10, axis20, 4, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis10, axis21, 5, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis11, axis20, 6, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis11, axis21, 7, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis12, axis20, 8, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis12, axis21, 9, vectorToBox)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта бокса с прямоугольником. + * + * @param box бокс + * @param rect прямоугольник + * @param vectorToBox вектор, направленный из центра прямоугольника в центр бокса + * @param faceAxisIdx индекс оси, идентифицирующей полскость столкновения (грань бокса или полскость прямоугольника) + * @param colInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var pnum:int; + var i:int; + var v:Vector3; + var cp:ContactPoint; + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + var colAxis:Vector3 = contact.normal; + + var negativeFace:Boolean; + var code:int; + + if (faceAxisIdx == 0) { + // Столкновение с плоскостью прямоугольника + + // Проверим положение бокса относительно плоскости прямоугольника + colAxis.x = rectTransform.c; + colAxis.y = rectTransform.g; + colAxis.z = rectTransform.k; + +// var offset:Number = colAxis.x*rectTransform.d + colAxis.y*rectTransform.h + colAxis.z*rectTransform.l; +// if (bbPos.vDot(colAxis) < offset) return false; + + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisIdx = axisIdx; + incidentAxisDot = dot; + } + } + negativeFace = incidentAxisDot > 0; + + code = 1 << (incidentAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + + // Получаем список вершин грани бокса, переводим их в систему координат прямоугольника и выполняем обрезку + // по прямоугольнику. Таким образом получается список потенциальных точек контакта. + boxTransform.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box.hs, incidentAxisIdx, negativeFace, points1); + boxTransform.transformVectorsN(points1, points2, 4); + rectTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByRect(rect.hs); + // Проверяем каждую потенциальную точку на принадлежность нижней полуплоскости прямоугольника и добавляем такие точки в список контактов + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if (v.z < epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = -v.z; + var cpPos:Vector3 = cp.pos; + cpPos.x = rectTransform.a*v.x + rectTransform.b*v.y + rectTransform.c*v.z + rectTransform.d; + cpPos.y = rectTransform.e*v.x + rectTransform.f*v.y + rectTransform.g*v.z + rectTransform.h; + cpPos.z = rectTransform.i*v.x + rectTransform.j*v.y + rectTransform.k*v.z + rectTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } else { + // Столкновение с гранью бокса + faceAxisIdx--; + boxTransform.getAxis(faceAxisIdx, colAxis); + negativeFace = colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z > 0; + + code = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) { + return false; + } + + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (rectTransform.c*colAxis.x + rectTransform.g*colAxis.y + rectTransform.k*colAxis.z < 0) return false; + + getFaceVertsByAxis(rect.hs, 2, false, points1); + rectTransform.transformVectorsN(points1, points2, 4); + boxTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if ((pen = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = pen; + cpPos = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } + return true; + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * + * @param hs + * @return + */ + private function clipByRect(hs:Vector3):int { + var pnum:int = 4; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box первый бокс + * @param rect второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param colInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + boxTransform.getAxis(axisIdx1, axis10); + rectTransform.getAxis(axisIdx2, axis20); + var colAxis:Vector3 = contact.normal; + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону бокса + if (colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + var vx:Number = box.hs.x; + var vy:Number = box.hs.y; + var vz:Number = box.hs.z; + var x2:Number = rect.hs.x; + var y2:Number = rect.hs.y; + var z2:Number = rect.hs.z; + // x + if (axisIdx1 == 0) { + vx = 0; + halfLen1 = box.hs.x; + } else { + if (boxTransform.a*colAxis.x + boxTransform.e*colAxis.y + boxTransform.i*colAxis.z > 0) { + vx = -vx; + if ((box.excludedFaces & 2) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 1) != 0) { + return false; + } + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = rect.hs.x; + } else { + if (rectTransform.a*colAxis.x + rectTransform.e*colAxis.y + rectTransform.i*colAxis.z < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + vy = 0; + halfLen1 = box.hs.y; + } else { + if (boxTransform.b*colAxis.x + boxTransform.f*colAxis.y + boxTransform.j*colAxis.z > 0) { + vy = -vy; + if ((box.excludedFaces & 8) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 4) != 0) { + return false; + } + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = rect.hs.y; + } else { + if (rectTransform.b*colAxis.x + rectTransform.f*colAxis.y + rectTransform.j*colAxis.z < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + vz = 0; + halfLen1 = box.hs.z; + } else { + if (boxTransform.c*colAxis.x + boxTransform.g*colAxis.y + boxTransform.k*colAxis.z > 0) { + vz = -vz; + if ((box.excludedFaces & 32) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 16) != 0) { + return false; + } + } + } + // Получаем глобальные координаты средних точек рёбер + + var x1:Number = boxTransform.a*vx + boxTransform.b*vy + boxTransform.c*vz + boxTransform.d; + var y1:Number = boxTransform.e*vx + boxTransform.f*vy + boxTransform.g*vz + boxTransform.h; + var z1:Number = boxTransform.i*vx + boxTransform.j*vy + boxTransform.k*vz + boxTransform.l; + vx = x2; + vy = y2; + vz = z2; + x2 = rectTransform.a*vx + rectTransform.b*vy + rectTransform.c*vz + rectTransform.d; + y2 = rectTransform.e*vx + rectTransform.f*vy + rectTransform.g*vz + rectTransform.h; + z2 = rectTransform.i*vx + rectTransform.j*vy + rectTransform.k*vz + rectTransform.l; + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + vx = x2 - x1; + vy = y2 - y1; + vz = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cpPos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cpPos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cpPos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var v:Vector3 = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + return true; + } + + /** + * Проверяет пересечение вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box + * @param rect + * @param axis + * @param axisIndex + * @param vectorToBox + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param rect + * @param axis1 + * @param axis2 + * @param axisIndex + * @param vectorToBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, rect:CollisionRect, axis1:Vector3, axis2:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия вдоль заданной оси. + * + * @param box бокс + * @param rect прямоугольник + * @param axis ось + * @param vectorToBox вектор, соединяющий центр прямоугольника с центром бокса + * @return величина перекрытия вдоль оси + */ + public function overlapOnAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, vectorToBox:Vector3):Number { + var m:Matrix4 = box.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box.hs.z; + if (d < 0) d = -d; + projection += d; + + m = rect.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*rect.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*rect.hs.y; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox.x*axis.x + vectorToBox.y*axis.y + vectorToBox.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxSphereCollider.as.svn-base b/0.0.8.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxSphereCollider.as.svn-base new file mode 100644 index 0000000..2ba3ce2 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxSphereCollider.as.svn-base @@ -0,0 +1,114 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.math.Vector3; + use namespace altphysics; + /** + * + */ + public class BoxSphereCollider implements ICollider { + + private var center:Vector3 = new Vector3(); + private var closestPt:Vector3 = new Vector3(); + + private var bPos:Vector3 = new Vector3(); + private var sPos:Vector3 = new Vector3(); + + /** + * + */ + public function BoxSphereCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + // Зафиксированно столкновение + contact.body1 = sphere.body; + contact.body2 = box.body; + contact.normal.vCopy(closestPt).vTransformBy4(box.transform).vSubtract(sPos).vNormalize().vReverse(); + contact.pcount = 1; + + var cp:ContactPoint = contact.points[0]; + cp.penetration = sphere.r - Math.sqrt(distSqr); + cp.pos.vCopy(contact.normal).vScale(-sphere.r).vAdd(sPos); + cp.r1.vDiff(cp.pos, sPos); + cp.r2.vDiff(cp.pos, bPos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxTriangleCollider.as.svn-base b/0.0.8.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxTriangleCollider.as.svn-base new file mode 100644 index 0000000..b22db8e --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxTriangleCollider.as.svn-base @@ -0,0 +1,718 @@ +package alternativa.physics.collision.colliders { + import __AS3__.vec.Vector; + + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionTriangle; + import alternativa.physics.math.Matrix4; + import alternativa.physics.math.Vector3; + + /** + * + */ + public class BoxTriangleCollider extends BoxCollider { + + public var epsilon:Number = 0.001; + + private var bestAxisIndex:int; + private var minOverlap:Number; + private var toBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var colNormal:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxTriangleCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, tri, toBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + if (!findEdgesIntersection(box, tri, toBox, bestAxisIndex%3, int(bestAxisIndex/3), contact)) return false; + } + + contact.body1 = box.body; + contact.body2 = tri.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + toBox.x = boxTransform.d - triTransform.d; + toBox.y = boxTransform.h - triTransform.h; + toBox.z = boxTransform.l - triTransform.l; + + minOverlap = 1e308; + + // Сначала проверяется нормаль треугольника + axis.x = triTransform.c; + axis.y = triTransform.g; + axis.z = triTransform.k; + if (!testMainAxis(box, tri, axis, 0, toBox)) return false; + + // Проверка основных осей бокса + axis10.x = boxTransform.a; + axis10.y = boxTransform.e; + axis10.z = boxTransform.i; + if (!testMainAxis(box, tri, axis10, 1, toBox)) return false; + axis11.x = boxTransform.b; + axis11.y = boxTransform.f; + axis11.z = boxTransform.j; + if (!testMainAxis(box, tri, axis11, 2, toBox)) return false; + axis12.x = boxTransform.c; + axis12.y = boxTransform.g; + axis12.z = boxTransform.k; + if (!testMainAxis(box, tri, axis12, 3, toBox)) return false; + + // Проверка производных осей + // TODO: заменить вычисления векторных произведений инлайнами + var v:Vector3 = tri.e0; + axis20.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis20.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis20.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis20, 4, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis20, 5, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis20, 6, toBox)) return false; + + v = tri.e1; + axis21.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis21.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis21.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis21, 7, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis21, 8, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis21, 9, toBox)) return false; + + v = tri.e2; + axis22.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis22.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis22.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis22, 10, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis22, 11, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis22, 12, toBox)) return false; + + return true; + } + + /** + * Тестирует пересечение примитивов вдоль заданной оси. + * + * @param box бокс + * @param tri треугольник + * @param axis ось + * @param axisIndex индекс оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return true, если примитивы имеют перекрытие проекций вдоль указанной оси, иначе false + */ + private function testMainAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, axisIndex:int, toBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param tri + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, tri:CollisionTriangle, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Рассчитывает величину перекрытия проекций бокса и треугольника на заданную ось. + * + * @param box бокс + * @param tri треугольник + * @param axis единичный направляющий вектор оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return величина перекрытия. Положительное значение указывает на наличие перекрытия, нулевое или отрицательное значение указывает на отсутствие перекрытия. + */ + private function overlapOnAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, toBox:Vector3):Number { + var t:Matrix4 = box.transform; + var projection:Number = (t.a*axis.x + t.e*axis.y + t.i*axis.z)*box.hs.x; + if (projection < 0) projection = -projection; + var d:Number = (t.b*axis.x + t.f*axis.y + t.j*axis.z)*box.hs.y; + projection += d < 0 ? -d : d; + d = (t.c*axis.x + t.g*axis.y + t.k*axis.z)*box.hs.z; + projection += d < 0 ? -d : d; + + var vectorProjection:Number = toBox.x*axis.x + toBox.y*axis.y + toBox.z*axis.z; + // Для оптимизации ось преобразуется в систему треугольника, а не его вершины в мировую систему + t = tri.transform; + var ax:Number = t.a*axis.x + t.e*axis.y + t.i*axis.z; + var ay:Number = t.b*axis.x + t.f*axis.y + t.j*axis.z; + var az:Number = t.c*axis.x + t.g*axis.y + t.k*axis.z; + var max:Number = 0; + if (vectorProjection < 0) { + vectorProjection = -vectorProjection; + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d < max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d < max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d < max) max = d; + max = -max; + } else { + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d > max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d > max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d > max) max = d; + } + + return projection + max - vectorProjection; + } + + /** + * Определяет точки контакта бокса и треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function findFaceContactPoints(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIndex:int, contact:Contact):Boolean { + if (faceAxisIndex == 0) { + // Столкновение с плоскостью треугольника + return getBoxToTriContact(box, tri, toBox, contact); + } else { + // Столкновение с гранью бокса + return getTriToBoxContact(box, tri, toBox, faceAxisIndex, contact); + } + } + + /** + * Определяет точки контакта бокса с плоскостью треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getBoxToTriContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + colNormal.x = triTransform.c; + colNormal.y = triTransform.g; + colNormal.z = triTransform.k; + + var over:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + if (!over) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var incAxisDot:Number = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colNormal.x + axis.y*colNormal.y + axis.z*colNormal.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incAxisDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Обрезка грани + var negativeFace:Boolean = incAxisDot > 0; + + var code:int = 1 << (incFaceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + getFaceVertsByAxis(box.hs, incFaceAxisIdx, negativeFace, points1); + boxTransform.transformVectorsN(points1, points2, 4); + triTransform.transformVectorsInverseN(points2, points1, 4); + var pnum:int = clipByTriangle(tri); + // Среди конечного списка точек определяются лежащие под плоскостью треугольника + var cp:ContactPoint; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points2[i]; + if ((over && v.z < 0) || (!over && v.z > 0)) { + cp = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z + triTransform.d; + cpPos.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z + triTransform.h; + cpPos.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z + triTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = over ? -v.z : v.z; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Оперделяет точки контакта треугольника с гранью бокса. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getTriToBoxContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + faceAxisIdx--; + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + boxTransform.getAxis(faceAxisIdx, colNormal); + var negativeFace:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + + var code:int = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + if (!negativeFace) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + var v:Vector3 = points1[0]; + v.x = tri.v0.x; + v.y = tri.v0.y; + v.z = tri.v0.z; + + v = points1[1]; + v.x = tri.v1.x; + v.y = tri.v1.y; + v.z = tri.v1.z; + + v = points1[2]; + v.x = tri.v2.x; + v.y = tri.v2.y; + v.z = tri.v2.z; + + triTransform.transformVectorsN(points1, points2, 3); + boxTransform.transformVectorsInverseN(points2, points1, 3); + + var pnum:int = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность боксу и добавляем такие точки в список контактов + var penetration:Number; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + v = points1[i]; + penetration = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace); + if (penetration > -epsilon) { + var cp:ContactPoint = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = penetration; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Вычисляет величину проникновения точки в бокс. + * + * @param hs вектор половинных размеров бокса + * @param p точка в системе координат бокса + * @param axisIndex индекс оси + * @param negativeFace если true, проверяется нижняя грань + * @return величина проникновения точки в бокс + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, negativeFace:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (negativeFace) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (negativeFace) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (negativeFace) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, вершины которой заданы в списке points1. Результат сохраняется в этом же списке. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 3; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Обрезает грань прямоугольника сторонами треугольника. Обрезка выполняется в системе координат и в проекции на + * плоскость треугольника. Входные вершины грани в количестве четырёх штук должны находиться в списке points1. + * Конечные вершины сохраняются в списке points2. + * + * @param tri треугольник + * @return количество врешин в конечном списке + */ + private function clipByTriangle(tri:CollisionTriangle):int { + var vnum:int = 4; + vnum = clipByLine(tri.v0, tri.e0, points1, vnum, points2); + if (vnum == 0) return 0; + vnum = clipByLine(tri.v1, tri.e1, points2, vnum, points1); + if (vnum == 0) return 0; + return clipByLine(tri.v2, tri.e2, points1, vnum, points2);; + } + + /** + * Обрезает полигон указанной прямой. + * + * @param linePoint точка на прямой + * @param lineDir единичный направляющий вектор прямой + * @param verticesIn список вершин исходного полигона + * @param vnum количество вершин исходного полигона + * @param verticesOut список, куда будут записаны вершины конечного полигона + * @return количество вершин конечного полигона + */ + private function clipByLine(linePoint:Vector3, lineDir:Vector3, verticesIn:Vector., vnum:int, verticesOut:Vector.):int { + var nx:Number = -lineDir.y; + var ny:Number = lineDir.x; + var offset:Number = linePoint.x*nx + linePoint.y*ny; + var v1:Vector3 = verticesIn[int(vnum - 1)]; + var offset1:Number = v1.x*nx + v1.y*ny; + var t:Number; + var v:Vector3; + var num:int = 0; + for (var i:int = 0; i < vnum; i++) { + var v2:Vector3 = verticesIn[i]; + var offset2:Number = v2.x*nx + v2.y*ny; + if (offset1 < offset) { + // Первая точка ребра во внешней полуплоскости + if (offset2 > offset) { + // Вторая точка ребра во внутренней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } else { + // Первая точка ребра во внутренней полуплоскости. Добавляем её в конечный список. + v = verticesOut[num]; + v.x = v1.x; + v.y = v1.y; + v.z = v1.z; + num++; + if (offset2 < offset) { + // Вторая точка ребра во внешней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } + v1 = v2; + offset1 = offset2; + } + return num; + } + + /** + * + * @param box + * @param tri + * @param toBox + * @param boxAxisIdx + * @param triAxisIdx + * @param contact + */ + private function findEdgesIntersection(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, boxAxisIdx:int, triAxisIdx:int, contact:Contact):Boolean { + // Определение точки и направляющего вектора ребра треугольника + var tmpx1:Number; + var tmpy1:Number; + var tmpz1:Number; + var tmpx2:Number; + var tmpy2:Number; + var tmpz2:Number; + switch (triAxisIdx) { + case 0: + tmpx1 = tri.e0.x; + tmpy1 = tri.e0.y; + tmpz1 = tri.e0.z; + tmpx2 = tri.v0.x; + tmpy2 = tri.v0.y; + tmpz2 = tri.v0.z; + break; + case 1: + tmpx1 = tri.e1.x; + tmpy1 = tri.e1.y; + tmpz1 = tri.e1.z; + tmpx2 = tri.v1.x; + tmpy2 = tri.v1.y; + tmpz2 = tri.v1.z; + break; + case 2: + tmpx1 = tri.e2.x; + tmpy1 = tri.e2.y; + tmpz1 = tri.e2.z; + tmpx2 = tri.v2.x; + tmpy2 = tri.v2.y; + tmpz2 = tri.v2.z; + break; + } + var triTransform:Matrix4 = tri.transform; + axis20.x = triTransform.a*tmpx1 + triTransform.b*tmpy1 + triTransform.c*tmpz1; + axis20.y = triTransform.e*tmpx1 + triTransform.f*tmpy1 + triTransform.g*tmpz1; + axis20.z = triTransform.i*tmpx1 + triTransform.j*tmpy1 + triTransform.k*tmpz1; + var x2:Number = triTransform.a*tmpx2 + triTransform.b*tmpy2 + triTransform.c*tmpz2 + triTransform.d; + var y2:Number = triTransform.e*tmpx2 + triTransform.f*tmpy2 + triTransform.g*tmpz2 + triTransform.h; + var z2:Number = triTransform.i*tmpx2 + triTransform.j*tmpy2 + triTransform.k*tmpz2 + triTransform.l; + + // Определение нормали контакта, точки и направляющего вектора ребра бокса + var boxTransform:Matrix4 = box.transform; + boxTransform.getAxis(boxAxisIdx, axis10); + + // Нормаль контакта + var v:Vector3 = contact.normal; + v.x = axis10.y*axis20.z - axis10.z*axis20.y; + v.y = axis10.z*axis20.x - axis10.x*axis20.z; + v.z = axis10.x*axis20.y - axis10.y*axis20.x; + k = 1/Math.sqrt(v.x*v.x + v.y*v.y + v.z*v.z); + v.x *= k; + v.y *= k; + v.z *= k; + // Разворот нормали в сторону бокса + if (v.x*toBox.x + v.y*toBox.y + v.z*toBox.z < 0) { + v.x = -v.x; + v.y = -v.y; + v.z = -v.z; + } + + var boxHalfLen:Number; + tmpx1 = box.hs.x; + tmpy1 = box.hs.y; + tmpz1 = box.hs.z; + // X + if (boxAxisIdx == 0) { + tmpx1 = 0; + boxHalfLen = box.hs.x; + } else { + if (boxTransform.a*v.x + boxTransform.e*v.y + boxTransform.i*v.z > 0) { + tmpx1 = -tmpx1; + if ((box.excludedFaces & 2) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 1) != 0) { + return false; + } + } + } + // Y + if (boxAxisIdx == 1) { + tmpy1 = 0; + boxHalfLen = box.hs.y; + } else { + if (boxTransform.b*v.x + boxTransform.f*v.y + boxTransform.j*v.z > 0) { + tmpy1 = -tmpy1; + if ((box.excludedFaces & 8) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 4) != 0) { + return false; + } + } + } + // Z + if (boxAxisIdx == 2) { + tmpz1 = 0; + boxHalfLen = box.hs.z; + } else { + if (boxTransform.c*v.x + boxTransform.g*v.y + boxTransform.k*v.z > 0) { + tmpz1 = -tmpz1; + if ((box.excludedFaces & 32) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 16) != 0) { + return false; + } + } + } + var x1:Number = boxTransform.a*tmpx1 + boxTransform.b*tmpy1 + boxTransform.c*tmpz1 + boxTransform.d; + var y1:Number = boxTransform.e*tmpx1 + boxTransform.f*tmpy1 + boxTransform.g*tmpz1 + boxTransform.h; + var z1:Number = boxTransform.i*tmpx1 + boxTransform.j*tmpy1 + boxTransform.k*tmpz1 + boxTransform.l; + + // Находим точку пересечения рёбер, решая систему уравнений + // axis10 - направляющий вектор ребра бокса + // x1, y1, z1 - средняя точка ребра бокса + // axis20 - направляющий вектор ребра треугольника + // x2, y2, z2 - начальная точка ребра треугольника + var k:Number = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + var vx:Number = x2 - x1; + var vy:Number = y2 - y1; + var vz:Number = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + + // Запись данных о контакте + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + v = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + v.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + v.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + v.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = v.x - boxTransform.d; + r.y = v.y - boxTransform.h; + r.z = v.z - boxTransform.l; + r = cp.r2; + r.x = v.x - triTransform.d; + r.y = v.y - triTransform.h; + r.z = v.z - triTransform.l; + return true; + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/colliders/.svn/text-base/SpherePlaneCollider.as.svn-base b/0.0.8.0/src/alternativa/physics/collision/colliders/.svn/text-base/SpherePlaneCollider.as.svn-base new file mode 100644 index 0000000..1ac5d35 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/colliders/.svn/text-base/SpherePlaneCollider.as.svn-base @@ -0,0 +1,68 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.math.Vector3; + + /** + * + */ + public class SpherePlaneCollider implements ICollider { + + private var normal:Vector3 = new Vector3(); + + /** + * + */ + public function SpherePlaneCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var sphere:RigidSphere = body1 as RigidSphere; +// var plane:RigidPlane; +// if (sphere == null) { +// sphere = body2 as RigidSphere; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// +// var dist:Number = sphere.state.pos.dot(normal) - offset; +// if (dist > sphere.r) return false; +// +// collisionInfo.body1 = sphere; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// collisionInfo.pcount = 1; +// +// var cp:ContactPoint = collisionInfo.points[0]; +// cp.penetration = sphere.r - dist; +// cp.pos.copy(normal).reverse().scale(sphere.r).add(sphere.state.pos); +// cp.r1.diff(cp.pos, sphere.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/colliders/.svn/text-base/SphereSphereCollider.as.svn-base b/0.0.8.0/src/alternativa/physics/collision/colliders/.svn/text-base/SphereSphereCollider.as.svn-base new file mode 100644 index 0000000..a90fabb --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/colliders/.svn/text-base/SphereSphereCollider.as.svn-base @@ -0,0 +1,70 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.math.Vector3; + use namespace altphysics; + + public class SphereSphereCollider implements ICollider { + + private var p1:Vector3 = new Vector3(); + private var p2:Vector3 = new Vector3(); + + public function SphereSphereCollider() { + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var s1:CollisionSphere; + var s2:CollisionSphere; + if (prim1.body != null) { + s1 = prim1 as CollisionSphere; + s2 = prim2 as CollisionSphere; + } else { + s1 = prim2 as CollisionSphere; + s2 = prim1 as CollisionSphere; + } + + s1.transform.getAxis(3, p1); + s2.transform.getAxis(3, p2); + var dx:Number = p1.x - p2.x; + var dy:Number = p1.y - p2.y; + var dz:Number = p1.z - p2.z; + var len:Number = dx*dx + dy*dy + dz*dz; + var sum:Number = s1.r + s2.r; + if (len > sum*sum) return false; + len = Math.sqrt(len); + dx /= len; + dy /= len; + dz /= len; + + contact.body1 = s1.body; + contact.body2 = s2.body; + contact.normal.x = dx; + contact.normal.y = dy; + contact.normal.z = dz; + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = sum - len; + cp.pos.x = p1.x - dx*s1.r; + cp.pos.y = p1.y - dy*s1.r; + cp.pos.z = p1.z - dz*s1.r; + cp.r1.vDiff(cp.pos, p1); + cp.r2.vDiff(cp.pos, p2); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/colliders/BoxBoxCollider.as b/0.0.8.0/src/alternativa/physics/collision/colliders/BoxBoxCollider.as new file mode 100644 index 0000000..b003e98 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/colliders/BoxBoxCollider.as @@ -0,0 +1,589 @@ +package alternativa.physics.collision.colliders { + import __AS3__.vec.Vector; + + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.math.Matrix4; + import alternativa.physics.math.Vector3; + + use namespace altphysics; + + /** + * Расчитывает точки контакта двух боксов. Нормаль контакта направляется в сторону бокса с меньшим ID. + */ + public class BoxBoxCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + private var vectorToBox1:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var bestAxisIndex:int; + private var minOverlap:Number; + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + private var tmpPoints:Vector. = new Vector.(8, true); + private var pcount:int; + + /** + * + */ + public function BoxBoxCollider() { + for (var i:int = 0; i < 8; i++) { + tmpPoints[i] = new ContactPoint(); + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + + if (bestAxisIndex < 6) { + // Контакт грань-(грань|ребро|вершина) + if (!findFaceContactPoints(box1, box2, vectorToBox1, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 6; + findEdgesIntersection(box1, box2, vectorToBox1, int(bestAxisIndex/3), bestAxisIndex%3, contact); + } + contact.body1 = box1.body; + contact.body2 = box2.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + // Вектор из центра второго бокса в центр первого + vectorToBox1.x = transform1.d - transform2.d; + vectorToBox1.y = transform1.h - transform2.h; + vectorToBox1.z = transform1.l - transform2.l; + + // Проверка пересечения по основным осям + axis10.x = transform1.a; + axis10.y = transform1.e; + axis10.z = transform1.i; + if (!testMainAxis(box1, box2, axis10, 0, vectorToBox1)) return false; + axis11.x = transform1.b; + axis11.y = transform1.f; + axis11.z = transform1.j; + if (!testMainAxis(box1, box2, axis11, 1, vectorToBox1)) return false; + axis12.x = transform1.c; + axis12.y = transform1.g; + axis12.z = transform1.k; + if (!testMainAxis(box1, box2, axis12, 2, vectorToBox1)) return false; + + axis20.x = transform2.a; + axis20.y = transform2.e; + axis20.z = transform2.i; + if (!testMainAxis(box1, box2, axis20, 3, vectorToBox1)) return false; + axis21.x = transform2.b; + axis21.y = transform2.f; + axis21.z = transform2.j; + if (!testMainAxis(box1, box2, axis21, 4, vectorToBox1)) return false; + axis22.x = transform2.c; + axis22.y = transform2.g; + axis22.z = transform2.k; + if (!testMainAxis(box1, box2, axis22, 5, vectorToBox1)) return false; + + // Проверка производных осей + if (!testDerivedAxis(box1, box2, axis10, axis20, 6, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis21, 7, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis22, 8, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis11, axis20, 9, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis21, 10, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis22, 11, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis12, axis20, 12, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis21, 13, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis22, 14, vectorToBox1)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта грани одного бокса с гранью/ребром/вершиной другого. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param faceAxisIdx индекс оси первого бокса, перпендикулярной грани, с которой произошло столкновение + * @param contactInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var swapNormal:Boolean = false; + if (faceAxisIdx > 2) { + // Столкновение с гранью второго бокса. Для дальнейших расчётов боксы меняются местами, + // но нормаль контакта всё равно должна быть направлена в сторону первоначального box1 + var tmpBox:CollisionBox = box1; + box1 = box2; + box2 = tmpBox; + vectorToBox1.x = -vectorToBox1.x; + vectorToBox1.y = -vectorToBox1.y; + vectorToBox1.z = -vectorToBox1.z; + faceAxisIdx -= 3; + swapNormal = true; + } + + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + var colAxis:Vector3 = contact.normal; + + transform1.getAxis(faceAxisIdx, colAxis); + var negativeFace:Boolean = colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z > 0; + + var code:int = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box1.excludedFaces) != 0) return false; + + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + // Ищем ось второго бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + transform2.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisDot = dot; + incidentAxisIdx = axisIdx; + } + } + // Получаем список вершин грани второго бокса, переводим их в систему координат первого бокса и выполняем обрезку + // по грани первого бокса. Таким образом получается список потенциальных точек контакта. + transform2.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box2.hs, incidentAxisIdx, incidentAxisDot < 0, points1); + + // TODO: Вычислить результирующую матрицу, затем преобразовать векторы за один заход + transform2.transformVectorsN(points1, points2, 4); + transform1.transformVectorsInverseN(points2, points1, 4); + + var pnum:int = clip(box1.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points1[i]; + if ((pen = getPointBoxPenetration(box1.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + var cp:ContactPoint = tmpPoints[pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = transform1.a*v.x + transform1.b*v.y + transform1.c*v.z + transform1.d; + cpPos.y = transform1.e*v.x + transform1.f*v.y + transform1.g*v.z + transform1.h; + cpPos.z = transform1.i*v.x + transform1.j*v.y + transform1.k*v.z + transform1.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + + cp.penetration = pen; + } + } + if (swapNormal) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (pcount > 4) { + reducePoints(); + } + for (i = 0; i < pcount; i++) { + cp = contact.points[i]; + cp.copyFrom(tmpPoints[i]); + } + contact.pcount = pcount; + return true; + } + + /** + * + * @param contactInfo + */ + private function reducePoints():void { + var i:int; + var minIdx:int; + var cp1:ContactPoint; + var cp2:ContactPoint; + while (pcount > 4) { + var minLen:Number = 1e10; + var p1:Vector3 = (tmpPoints[int(pcount - 1)] as ContactPoint).pos; + var p2:Vector3; + for (i = 0; i < pcount; i++) { + p2 = (tmpPoints[i] as ContactPoint).pos; + var dx:Number = p2.x - p1.x; + var dy:Number = p2.y - p1.y; + var dz:Number = p2.z - p1.z; + var len:Number = dx*dx + dy*dy + dz*dz; + if (len < minLen) { + minLen = len; + minIdx = i; + } + p1 = p2; + } + var ii:int = minIdx == 0 ? (pcount - 1) : (minIdx - 1); + cp1 = tmpPoints[ii]; + cp2 = tmpPoints[minIdx]; + p1 = cp1.pos; + p2 = cp2.pos; + p2.x = 0.5*(p1.x + p2.x); + p2.y = 0.5*(p1.y + p2.y); + p2.z = 0.5*(p1.z + p2.z); + cp2.penetration = 0.5*(cp1.penetration + cp2.penetration); + if (minIdx > 0) { + for (i = minIdx; i < pcount; i++) { + tmpPoints[int(i - 1)] = tmpPoints[i]; + } + tmpPoints[int(pcount - 1)] = cp1; + } + pcount--; + } + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clip(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param contactInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):void { + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + transform1.getAxis(axisIdx1, axis10); + transform2.getAxis(axisIdx2, axis20); + + var colAxis:Vector3 = contact.normal; + + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону первого бокса + if (colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + + var tx:Number = box1.hs.x; + var ty:Number = box1.hs.y; + var tz:Number = box1.hs.z; + + var x2:Number = box2.hs.x; + var y2:Number = box2.hs.y; + var z2:Number = box2.hs.z; + // x + if (axisIdx1 == 0) { + tx = 0; + halfLen1 = box1.hs.x; + } else { + if (colAxis.x*transform1.a + colAxis.y*transform1.e + colAxis.z*transform1.i > 0) { + tx = -tx; + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = box2.hs.x; + } else { + if (colAxis.x*transform2.a + colAxis.y*transform2.e + colAxis.z*transform2.i < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + ty = 0; + halfLen1 = box1.hs.y; + } else { + if (colAxis.x*transform1.b + colAxis.y*transform1.f + colAxis.z*transform1.j > 0) { + ty = -ty; + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = box2.hs.y; + } else { + if (colAxis.x*transform2.b + colAxis.y*transform2.f + colAxis.z*transform2.j < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + tz = 0; + halfLen1 = box1.hs.z; + } else { + if (colAxis.x*transform1.c + colAxis.y*transform1.g + colAxis.z*transform1.k > 0) { + tz = -tz; + } + } + if (axisIdx2 == 2) { + z2 = 0; + halfLen2 = box2.hs.z; + } else { + if (colAxis.x*transform2.c + colAxis.y*transform2.g + colAxis.z*transform2.k < 0) { + z2 = -z2; + } + } + // Получаем глобальные координаты средних точек рёбер + var x1:Number = transform1.a*tx + transform1.b*ty + transform1.c*tz + transform1.d; + var y1:Number = transform1.e*tx + transform1.f*ty + transform1.g*tz + transform1.h; + var z1:Number = transform1.i*tx + transform1.j*ty + transform1.k*tz + transform1.l; + tx = x2; + ty = y2; + tz = z2; + x2 = transform2.a*tx + transform2.b*ty + transform2.c*tz + transform2.d; + y2 = transform2.e*tx + transform2.f*ty + transform2.g*tz + transform2.h; + z2 = transform2.i*tx + transform2.j*ty + transform2.k*tz + transform2.l; + + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + tx = x2 - x1; + ty = y2 - y1; + tz = z2 - z1; + var c1:Number = axis10.x*tx + axis10.y*ty + axis10.z*tz; + var c2:Number = axis20.x*tx + axis20.y*ty + axis20.z*tz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cp.pos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cp.pos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение боксов вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box1 + * @param box2 + * @param axis + * @param axisIndex + * @param toBox1 + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box1 + * @param box2 + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox1 + * @return + */ + private function testDerivedAxis(box1:CollisionBox, box2:CollisionBox, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия двух боксов вдоль заданной оси. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param axis ось + * @param vectorToBox1 вектор, соединяющий центр второго бокса с центром первого + * @return величина перекрытия боксов вдоль оси + */ + public function overlapOnAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, vectorToBox1:Vector3):Number { + var m:Matrix4 = box1.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box1.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box1.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box1.hs.z; + if (d < 0) d = -d; + projection += d; + + m = box2.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box2.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box2.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box2.hs.z; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox1.x*axis.x + vectorToBox1.y*axis.y + vectorToBox1.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} + import alternativa.physics.math.Vector3; + + class CollisionPointTmp { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + } diff --git a/0.0.8.0/src/alternativa/physics/collision/colliders/BoxCollider.as b/0.0.8.0/src/alternativa/physics/collision/colliders/BoxCollider.as new file mode 100644 index 0000000..0021854 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/colliders/BoxCollider.as @@ -0,0 +1,427 @@ +package alternativa.physics.collision.colliders { + import __AS3__.vec.Vector; + + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.math.Vector3; + + /** + * + */ + public class BoxCollider implements ICollider { + + /** + * + */ + public function BoxCollider() { + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + return false; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + protected function getFaceVertsByAxis(hs:Vector3, axisIdx:int, negativeFace:Boolean, result:Vector.):void { + var v:Vector3; + switch (axisIdx) { + case 0: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + } + break; + case 1: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + } + break; + case 2: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + } else { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } + break; + } + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/colliders/BoxPlaneCollider.as b/0.0.8.0/src/alternativa/physics/collision/colliders/BoxPlaneCollider.as new file mode 100644 index 0000000..bb1ed0f --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/colliders/BoxPlaneCollider.as @@ -0,0 +1,88 @@ +package alternativa.physics.collision.colliders { + import __AS3__.vec.Vector; + + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.math.Vector3; + + public class BoxPlaneCollider implements ICollider { + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var normal:Vector3 = new Vector3(); + + public function BoxPlaneCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var box:RigidBox = body1 as RigidBox; +// var plane:RigidPlane; +// if (box == null) { +// box = body2 as RigidBox; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные координаты вершин бокса +// var sx:Number = box.halfSize.x; +// var sy:Number = box.halfSize.y; +// var sz:Number = box.halfSize.z; +// (verts1[0] as Vector3).reset(-sx, -sy, -sz); +// (verts1[1] as Vector3).reset(sx, -sy, -sz); +// (verts1[2] as Vector3).reset(sx, sy, -sz); +// (verts1[3] as Vector3).reset(-sx, sy, -sz); +// (verts1[4] as Vector3).reset(-sx, -sy, sz); +// (verts1[5] as Vector3).reset(sx, -sy, sz); +// (verts1[6] as Vector3).reset(sx, sy, sz); +// (verts1[7] as Vector3).reset(-sx, sy, sz); +// +// box.transform.transformVectors(verts1, verts2); +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// // Проверяем наличие столкновений с каждой вершиной +// collisionInfo.pcount = 0; +// for (var i:int = 0; i < 8; i++) { +// // Вершина добавляется в список точек столкновения, если лежит под плоскостью +// var dist:Number = (verts2[i] as Vector3).dot(normal); +// if (dist < offset) { +// var cp:ContactPoint; +// if (collisionInfo.pcount == collisionInfo.points.length) { +// cp = new ContactPoint(); +// collisionInfo.points[collisionInfo.pcount] = cp; +// } else { +// cp = collisionInfo.points[collisionInfo.pcount]; +// } +// cp.pos.copy(verts2[i]); +// cp.r1.diff(cp.pos, box.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); +// cp.penetration = offset - dist; +// collisionInfo.pcount++; +// } +// } +// if (collisionInfo.pcount > 0) { +// collisionInfo.body1 = box; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// return true; +// } + return false; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/colliders/BoxRectCollider.as b/0.0.8.0/src/alternativa/physics/collision/colliders/BoxRectCollider.as new file mode 100644 index 0000000..35f904e --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/colliders/BoxRectCollider.as @@ -0,0 +1,568 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.physics.math.Matrix4; + import alternativa.physics.math.Vector3; + use namespace altphysics; + + /** + * + */ + public class BoxRectCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + + private var vectorToBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxRectCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, rect, vectorToBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + if (!findEdgesIntersection(box, rect, vectorToBox, int(bestAxisIndex/2), bestAxisIndex%2, contact)) { + return false; + } + } + contact.body1 = box.body; + contact.body2 = rect.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + // Вектор из центра прямоугольника в центр бокса + vectorToBox.x = boxTransform.d - rectTransform.d; + vectorToBox.y = boxTransform.h - rectTransform.h; + vectorToBox.z = boxTransform.l - rectTransform.l; + + // Проверка пересечения по нормали прямоугольника + rectTransform.getAxis(2, axis22); + if (!testMainAxis(box, rect, axis22, 0, vectorToBox)) return false; + + // Проверка пересечения по основным осям бокса + boxTransform.getAxis(0, axis10); + if (!testMainAxis(box, rect, axis10, 1, vectorToBox)) return false; + boxTransform.getAxis(1, axis11); + if (!testMainAxis(box, rect, axis11, 2, vectorToBox)) return false; + boxTransform.getAxis(2, axis12); + if (!testMainAxis(box, rect, axis12, 3, vectorToBox)) return false; + + // Получаем направляющие рёбер прямоугольника + rectTransform.getAxis(0, axis20); + rectTransform.getAxis(1, axis21); + + // Проверка производных осей + if (!testDerivedAxis(box, rect, axis10, axis20, 4, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis10, axis21, 5, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis11, axis20, 6, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis11, axis21, 7, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis12, axis20, 8, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis12, axis21, 9, vectorToBox)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта бокса с прямоугольником. + * + * @param box бокс + * @param rect прямоугольник + * @param vectorToBox вектор, направленный из центра прямоугольника в центр бокса + * @param faceAxisIdx индекс оси, идентифицирующей полскость столкновения (грань бокса или полскость прямоугольника) + * @param colInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var pnum:int; + var i:int; + var v:Vector3; + var cp:ContactPoint; + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + var colAxis:Vector3 = contact.normal; + + var negativeFace:Boolean; + var code:int; + + if (faceAxisIdx == 0) { + // Столкновение с плоскостью прямоугольника + + // Проверим положение бокса относительно плоскости прямоугольника + colAxis.x = rectTransform.c; + colAxis.y = rectTransform.g; + colAxis.z = rectTransform.k; + +// var offset:Number = colAxis.x*rectTransform.d + colAxis.y*rectTransform.h + colAxis.z*rectTransform.l; +// if (bbPos.vDot(colAxis) < offset) return false; + + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisIdx = axisIdx; + incidentAxisDot = dot; + } + } + negativeFace = incidentAxisDot > 0; + + code = 1 << (incidentAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + + // Получаем список вершин грани бокса, переводим их в систему координат прямоугольника и выполняем обрезку + // по прямоугольнику. Таким образом получается список потенциальных точек контакта. + boxTransform.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box.hs, incidentAxisIdx, negativeFace, points1); + boxTransform.transformVectorsN(points1, points2, 4); + rectTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByRect(rect.hs); + // Проверяем каждую потенциальную точку на принадлежность нижней полуплоскости прямоугольника и добавляем такие точки в список контактов + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if (v.z < epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = -v.z; + var cpPos:Vector3 = cp.pos; + cpPos.x = rectTransform.a*v.x + rectTransform.b*v.y + rectTransform.c*v.z + rectTransform.d; + cpPos.y = rectTransform.e*v.x + rectTransform.f*v.y + rectTransform.g*v.z + rectTransform.h; + cpPos.z = rectTransform.i*v.x + rectTransform.j*v.y + rectTransform.k*v.z + rectTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } else { + // Столкновение с гранью бокса + faceAxisIdx--; + boxTransform.getAxis(faceAxisIdx, colAxis); + negativeFace = colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z > 0; + + code = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) { + return false; + } + + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (rectTransform.c*colAxis.x + rectTransform.g*colAxis.y + rectTransform.k*colAxis.z < 0) return false; + + getFaceVertsByAxis(rect.hs, 2, false, points1); + rectTransform.transformVectorsN(points1, points2, 4); + boxTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if ((pen = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = pen; + cpPos = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } + return true; + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * + * @param hs + * @return + */ + private function clipByRect(hs:Vector3):int { + var pnum:int = 4; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box первый бокс + * @param rect второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param colInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + boxTransform.getAxis(axisIdx1, axis10); + rectTransform.getAxis(axisIdx2, axis20); + var colAxis:Vector3 = contact.normal; + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону бокса + if (colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + var vx:Number = box.hs.x; + var vy:Number = box.hs.y; + var vz:Number = box.hs.z; + var x2:Number = rect.hs.x; + var y2:Number = rect.hs.y; + var z2:Number = rect.hs.z; + // x + if (axisIdx1 == 0) { + vx = 0; + halfLen1 = box.hs.x; + } else { + if (boxTransform.a*colAxis.x + boxTransform.e*colAxis.y + boxTransform.i*colAxis.z > 0) { + vx = -vx; + if ((box.excludedFaces & 2) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 1) != 0) { + return false; + } + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = rect.hs.x; + } else { + if (rectTransform.a*colAxis.x + rectTransform.e*colAxis.y + rectTransform.i*colAxis.z < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + vy = 0; + halfLen1 = box.hs.y; + } else { + if (boxTransform.b*colAxis.x + boxTransform.f*colAxis.y + boxTransform.j*colAxis.z > 0) { + vy = -vy; + if ((box.excludedFaces & 8) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 4) != 0) { + return false; + } + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = rect.hs.y; + } else { + if (rectTransform.b*colAxis.x + rectTransform.f*colAxis.y + rectTransform.j*colAxis.z < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + vz = 0; + halfLen1 = box.hs.z; + } else { + if (boxTransform.c*colAxis.x + boxTransform.g*colAxis.y + boxTransform.k*colAxis.z > 0) { + vz = -vz; + if ((box.excludedFaces & 32) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 16) != 0) { + return false; + } + } + } + // Получаем глобальные координаты средних точек рёбер + + var x1:Number = boxTransform.a*vx + boxTransform.b*vy + boxTransform.c*vz + boxTransform.d; + var y1:Number = boxTransform.e*vx + boxTransform.f*vy + boxTransform.g*vz + boxTransform.h; + var z1:Number = boxTransform.i*vx + boxTransform.j*vy + boxTransform.k*vz + boxTransform.l; + vx = x2; + vy = y2; + vz = z2; + x2 = rectTransform.a*vx + rectTransform.b*vy + rectTransform.c*vz + rectTransform.d; + y2 = rectTransform.e*vx + rectTransform.f*vy + rectTransform.g*vz + rectTransform.h; + z2 = rectTransform.i*vx + rectTransform.j*vy + rectTransform.k*vz + rectTransform.l; + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + vx = x2 - x1; + vy = y2 - y1; + vz = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cpPos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cpPos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cpPos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var v:Vector3 = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + return true; + } + + /** + * Проверяет пересечение вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box + * @param rect + * @param axis + * @param axisIndex + * @param vectorToBox + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param rect + * @param axis1 + * @param axis2 + * @param axisIndex + * @param vectorToBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, rect:CollisionRect, axis1:Vector3, axis2:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия вдоль заданной оси. + * + * @param box бокс + * @param rect прямоугольник + * @param axis ось + * @param vectorToBox вектор, соединяющий центр прямоугольника с центром бокса + * @return величина перекрытия вдоль оси + */ + public function overlapOnAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, vectorToBox:Vector3):Number { + var m:Matrix4 = box.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box.hs.z; + if (d < 0) d = -d; + projection += d; + + m = rect.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*rect.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*rect.hs.y; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox.x*axis.x + vectorToBox.y*axis.y + vectorToBox.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/colliders/BoxSphereCollider.as b/0.0.8.0/src/alternativa/physics/collision/colliders/BoxSphereCollider.as new file mode 100644 index 0000000..2ba3ce2 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/colliders/BoxSphereCollider.as @@ -0,0 +1,114 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.math.Vector3; + use namespace altphysics; + /** + * + */ + public class BoxSphereCollider implements ICollider { + + private var center:Vector3 = new Vector3(); + private var closestPt:Vector3 = new Vector3(); + + private var bPos:Vector3 = new Vector3(); + private var sPos:Vector3 = new Vector3(); + + /** + * + */ + public function BoxSphereCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + // Зафиксированно столкновение + contact.body1 = sphere.body; + contact.body2 = box.body; + contact.normal.vCopy(closestPt).vTransformBy4(box.transform).vSubtract(sPos).vNormalize().vReverse(); + contact.pcount = 1; + + var cp:ContactPoint = contact.points[0]; + cp.penetration = sphere.r - Math.sqrt(distSqr); + cp.pos.vCopy(contact.normal).vScale(-sphere.r).vAdd(sPos); + cp.r1.vDiff(cp.pos, sPos); + cp.r2.vDiff(cp.pos, bPos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/colliders/BoxTriangleCollider.as b/0.0.8.0/src/alternativa/physics/collision/colliders/BoxTriangleCollider.as new file mode 100644 index 0000000..b22db8e --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/colliders/BoxTriangleCollider.as @@ -0,0 +1,718 @@ +package alternativa.physics.collision.colliders { + import __AS3__.vec.Vector; + + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionTriangle; + import alternativa.physics.math.Matrix4; + import alternativa.physics.math.Vector3; + + /** + * + */ + public class BoxTriangleCollider extends BoxCollider { + + public var epsilon:Number = 0.001; + + private var bestAxisIndex:int; + private var minOverlap:Number; + private var toBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var colNormal:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxTriangleCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, tri, toBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + if (!findEdgesIntersection(box, tri, toBox, bestAxisIndex%3, int(bestAxisIndex/3), contact)) return false; + } + + contact.body1 = box.body; + contact.body2 = tri.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + toBox.x = boxTransform.d - triTransform.d; + toBox.y = boxTransform.h - triTransform.h; + toBox.z = boxTransform.l - triTransform.l; + + minOverlap = 1e308; + + // Сначала проверяется нормаль треугольника + axis.x = triTransform.c; + axis.y = triTransform.g; + axis.z = triTransform.k; + if (!testMainAxis(box, tri, axis, 0, toBox)) return false; + + // Проверка основных осей бокса + axis10.x = boxTransform.a; + axis10.y = boxTransform.e; + axis10.z = boxTransform.i; + if (!testMainAxis(box, tri, axis10, 1, toBox)) return false; + axis11.x = boxTransform.b; + axis11.y = boxTransform.f; + axis11.z = boxTransform.j; + if (!testMainAxis(box, tri, axis11, 2, toBox)) return false; + axis12.x = boxTransform.c; + axis12.y = boxTransform.g; + axis12.z = boxTransform.k; + if (!testMainAxis(box, tri, axis12, 3, toBox)) return false; + + // Проверка производных осей + // TODO: заменить вычисления векторных произведений инлайнами + var v:Vector3 = tri.e0; + axis20.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis20.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis20.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis20, 4, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis20, 5, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis20, 6, toBox)) return false; + + v = tri.e1; + axis21.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis21.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis21.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis21, 7, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis21, 8, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis21, 9, toBox)) return false; + + v = tri.e2; + axis22.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis22.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis22.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis22, 10, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis22, 11, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis22, 12, toBox)) return false; + + return true; + } + + /** + * Тестирует пересечение примитивов вдоль заданной оси. + * + * @param box бокс + * @param tri треугольник + * @param axis ось + * @param axisIndex индекс оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return true, если примитивы имеют перекрытие проекций вдоль указанной оси, иначе false + */ + private function testMainAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, axisIndex:int, toBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param tri + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, tri:CollisionTriangle, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Рассчитывает величину перекрытия проекций бокса и треугольника на заданную ось. + * + * @param box бокс + * @param tri треугольник + * @param axis единичный направляющий вектор оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return величина перекрытия. Положительное значение указывает на наличие перекрытия, нулевое или отрицательное значение указывает на отсутствие перекрытия. + */ + private function overlapOnAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, toBox:Vector3):Number { + var t:Matrix4 = box.transform; + var projection:Number = (t.a*axis.x + t.e*axis.y + t.i*axis.z)*box.hs.x; + if (projection < 0) projection = -projection; + var d:Number = (t.b*axis.x + t.f*axis.y + t.j*axis.z)*box.hs.y; + projection += d < 0 ? -d : d; + d = (t.c*axis.x + t.g*axis.y + t.k*axis.z)*box.hs.z; + projection += d < 0 ? -d : d; + + var vectorProjection:Number = toBox.x*axis.x + toBox.y*axis.y + toBox.z*axis.z; + // Для оптимизации ось преобразуется в систему треугольника, а не его вершины в мировую систему + t = tri.transform; + var ax:Number = t.a*axis.x + t.e*axis.y + t.i*axis.z; + var ay:Number = t.b*axis.x + t.f*axis.y + t.j*axis.z; + var az:Number = t.c*axis.x + t.g*axis.y + t.k*axis.z; + var max:Number = 0; + if (vectorProjection < 0) { + vectorProjection = -vectorProjection; + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d < max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d < max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d < max) max = d; + max = -max; + } else { + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d > max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d > max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d > max) max = d; + } + + return projection + max - vectorProjection; + } + + /** + * Определяет точки контакта бокса и треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function findFaceContactPoints(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIndex:int, contact:Contact):Boolean { + if (faceAxisIndex == 0) { + // Столкновение с плоскостью треугольника + return getBoxToTriContact(box, tri, toBox, contact); + } else { + // Столкновение с гранью бокса + return getTriToBoxContact(box, tri, toBox, faceAxisIndex, contact); + } + } + + /** + * Определяет точки контакта бокса с плоскостью треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getBoxToTriContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + colNormal.x = triTransform.c; + colNormal.y = triTransform.g; + colNormal.z = triTransform.k; + + var over:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + if (!over) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var incAxisDot:Number = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colNormal.x + axis.y*colNormal.y + axis.z*colNormal.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incAxisDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Обрезка грани + var negativeFace:Boolean = incAxisDot > 0; + + var code:int = 1 << (incFaceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + getFaceVertsByAxis(box.hs, incFaceAxisIdx, negativeFace, points1); + boxTransform.transformVectorsN(points1, points2, 4); + triTransform.transformVectorsInverseN(points2, points1, 4); + var pnum:int = clipByTriangle(tri); + // Среди конечного списка точек определяются лежащие под плоскостью треугольника + var cp:ContactPoint; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points2[i]; + if ((over && v.z < 0) || (!over && v.z > 0)) { + cp = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z + triTransform.d; + cpPos.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z + triTransform.h; + cpPos.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z + triTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = over ? -v.z : v.z; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Оперделяет точки контакта треугольника с гранью бокса. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getTriToBoxContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + faceAxisIdx--; + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + boxTransform.getAxis(faceAxisIdx, colNormal); + var negativeFace:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + + var code:int = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + if (!negativeFace) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + var v:Vector3 = points1[0]; + v.x = tri.v0.x; + v.y = tri.v0.y; + v.z = tri.v0.z; + + v = points1[1]; + v.x = tri.v1.x; + v.y = tri.v1.y; + v.z = tri.v1.z; + + v = points1[2]; + v.x = tri.v2.x; + v.y = tri.v2.y; + v.z = tri.v2.z; + + triTransform.transformVectorsN(points1, points2, 3); + boxTransform.transformVectorsInverseN(points2, points1, 3); + + var pnum:int = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность боксу и добавляем такие точки в список контактов + var penetration:Number; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + v = points1[i]; + penetration = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace); + if (penetration > -epsilon) { + var cp:ContactPoint = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = penetration; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Вычисляет величину проникновения точки в бокс. + * + * @param hs вектор половинных размеров бокса + * @param p точка в системе координат бокса + * @param axisIndex индекс оси + * @param negativeFace если true, проверяется нижняя грань + * @return величина проникновения точки в бокс + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, negativeFace:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (negativeFace) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (negativeFace) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (negativeFace) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, вершины которой заданы в списке points1. Результат сохраняется в этом же списке. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 3; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Обрезает грань прямоугольника сторонами треугольника. Обрезка выполняется в системе координат и в проекции на + * плоскость треугольника. Входные вершины грани в количестве четырёх штук должны находиться в списке points1. + * Конечные вершины сохраняются в списке points2. + * + * @param tri треугольник + * @return количество врешин в конечном списке + */ + private function clipByTriangle(tri:CollisionTriangle):int { + var vnum:int = 4; + vnum = clipByLine(tri.v0, tri.e0, points1, vnum, points2); + if (vnum == 0) return 0; + vnum = clipByLine(tri.v1, tri.e1, points2, vnum, points1); + if (vnum == 0) return 0; + return clipByLine(tri.v2, tri.e2, points1, vnum, points2);; + } + + /** + * Обрезает полигон указанной прямой. + * + * @param linePoint точка на прямой + * @param lineDir единичный направляющий вектор прямой + * @param verticesIn список вершин исходного полигона + * @param vnum количество вершин исходного полигона + * @param verticesOut список, куда будут записаны вершины конечного полигона + * @return количество вершин конечного полигона + */ + private function clipByLine(linePoint:Vector3, lineDir:Vector3, verticesIn:Vector., vnum:int, verticesOut:Vector.):int { + var nx:Number = -lineDir.y; + var ny:Number = lineDir.x; + var offset:Number = linePoint.x*nx + linePoint.y*ny; + var v1:Vector3 = verticesIn[int(vnum - 1)]; + var offset1:Number = v1.x*nx + v1.y*ny; + var t:Number; + var v:Vector3; + var num:int = 0; + for (var i:int = 0; i < vnum; i++) { + var v2:Vector3 = verticesIn[i]; + var offset2:Number = v2.x*nx + v2.y*ny; + if (offset1 < offset) { + // Первая точка ребра во внешней полуплоскости + if (offset2 > offset) { + // Вторая точка ребра во внутренней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } else { + // Первая точка ребра во внутренней полуплоскости. Добавляем её в конечный список. + v = verticesOut[num]; + v.x = v1.x; + v.y = v1.y; + v.z = v1.z; + num++; + if (offset2 < offset) { + // Вторая точка ребра во внешней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } + v1 = v2; + offset1 = offset2; + } + return num; + } + + /** + * + * @param box + * @param tri + * @param toBox + * @param boxAxisIdx + * @param triAxisIdx + * @param contact + */ + private function findEdgesIntersection(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, boxAxisIdx:int, triAxisIdx:int, contact:Contact):Boolean { + // Определение точки и направляющего вектора ребра треугольника + var tmpx1:Number; + var tmpy1:Number; + var tmpz1:Number; + var tmpx2:Number; + var tmpy2:Number; + var tmpz2:Number; + switch (triAxisIdx) { + case 0: + tmpx1 = tri.e0.x; + tmpy1 = tri.e0.y; + tmpz1 = tri.e0.z; + tmpx2 = tri.v0.x; + tmpy2 = tri.v0.y; + tmpz2 = tri.v0.z; + break; + case 1: + tmpx1 = tri.e1.x; + tmpy1 = tri.e1.y; + tmpz1 = tri.e1.z; + tmpx2 = tri.v1.x; + tmpy2 = tri.v1.y; + tmpz2 = tri.v1.z; + break; + case 2: + tmpx1 = tri.e2.x; + tmpy1 = tri.e2.y; + tmpz1 = tri.e2.z; + tmpx2 = tri.v2.x; + tmpy2 = tri.v2.y; + tmpz2 = tri.v2.z; + break; + } + var triTransform:Matrix4 = tri.transform; + axis20.x = triTransform.a*tmpx1 + triTransform.b*tmpy1 + triTransform.c*tmpz1; + axis20.y = triTransform.e*tmpx1 + triTransform.f*tmpy1 + triTransform.g*tmpz1; + axis20.z = triTransform.i*tmpx1 + triTransform.j*tmpy1 + triTransform.k*tmpz1; + var x2:Number = triTransform.a*tmpx2 + triTransform.b*tmpy2 + triTransform.c*tmpz2 + triTransform.d; + var y2:Number = triTransform.e*tmpx2 + triTransform.f*tmpy2 + triTransform.g*tmpz2 + triTransform.h; + var z2:Number = triTransform.i*tmpx2 + triTransform.j*tmpy2 + triTransform.k*tmpz2 + triTransform.l; + + // Определение нормали контакта, точки и направляющего вектора ребра бокса + var boxTransform:Matrix4 = box.transform; + boxTransform.getAxis(boxAxisIdx, axis10); + + // Нормаль контакта + var v:Vector3 = contact.normal; + v.x = axis10.y*axis20.z - axis10.z*axis20.y; + v.y = axis10.z*axis20.x - axis10.x*axis20.z; + v.z = axis10.x*axis20.y - axis10.y*axis20.x; + k = 1/Math.sqrt(v.x*v.x + v.y*v.y + v.z*v.z); + v.x *= k; + v.y *= k; + v.z *= k; + // Разворот нормали в сторону бокса + if (v.x*toBox.x + v.y*toBox.y + v.z*toBox.z < 0) { + v.x = -v.x; + v.y = -v.y; + v.z = -v.z; + } + + var boxHalfLen:Number; + tmpx1 = box.hs.x; + tmpy1 = box.hs.y; + tmpz1 = box.hs.z; + // X + if (boxAxisIdx == 0) { + tmpx1 = 0; + boxHalfLen = box.hs.x; + } else { + if (boxTransform.a*v.x + boxTransform.e*v.y + boxTransform.i*v.z > 0) { + tmpx1 = -tmpx1; + if ((box.excludedFaces & 2) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 1) != 0) { + return false; + } + } + } + // Y + if (boxAxisIdx == 1) { + tmpy1 = 0; + boxHalfLen = box.hs.y; + } else { + if (boxTransform.b*v.x + boxTransform.f*v.y + boxTransform.j*v.z > 0) { + tmpy1 = -tmpy1; + if ((box.excludedFaces & 8) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 4) != 0) { + return false; + } + } + } + // Z + if (boxAxisIdx == 2) { + tmpz1 = 0; + boxHalfLen = box.hs.z; + } else { + if (boxTransform.c*v.x + boxTransform.g*v.y + boxTransform.k*v.z > 0) { + tmpz1 = -tmpz1; + if ((box.excludedFaces & 32) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 16) != 0) { + return false; + } + } + } + var x1:Number = boxTransform.a*tmpx1 + boxTransform.b*tmpy1 + boxTransform.c*tmpz1 + boxTransform.d; + var y1:Number = boxTransform.e*tmpx1 + boxTransform.f*tmpy1 + boxTransform.g*tmpz1 + boxTransform.h; + var z1:Number = boxTransform.i*tmpx1 + boxTransform.j*tmpy1 + boxTransform.k*tmpz1 + boxTransform.l; + + // Находим точку пересечения рёбер, решая систему уравнений + // axis10 - направляющий вектор ребра бокса + // x1, y1, z1 - средняя точка ребра бокса + // axis20 - направляющий вектор ребра треугольника + // x2, y2, z2 - начальная точка ребра треугольника + var k:Number = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + var vx:Number = x2 - x1; + var vy:Number = y2 - y1; + var vz:Number = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + + // Запись данных о контакте + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + v = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + v.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + v.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + v.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = v.x - boxTransform.d; + r.y = v.y - boxTransform.h; + r.z = v.z - boxTransform.l; + r = cp.r2; + r.x = v.x - triTransform.d; + r.y = v.y - triTransform.h; + r.z = v.z - triTransform.l; + return true; + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/colliders/SpherePlaneCollider.as b/0.0.8.0/src/alternativa/physics/collision/colliders/SpherePlaneCollider.as new file mode 100644 index 0000000..1ac5d35 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/colliders/SpherePlaneCollider.as @@ -0,0 +1,68 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.math.Vector3; + + /** + * + */ + public class SpherePlaneCollider implements ICollider { + + private var normal:Vector3 = new Vector3(); + + /** + * + */ + public function SpherePlaneCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var sphere:RigidSphere = body1 as RigidSphere; +// var plane:RigidPlane; +// if (sphere == null) { +// sphere = body2 as RigidSphere; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// +// var dist:Number = sphere.state.pos.dot(normal) - offset; +// if (dist > sphere.r) return false; +// +// collisionInfo.body1 = sphere; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// collisionInfo.pcount = 1; +// +// var cp:ContactPoint = collisionInfo.points[0]; +// cp.penetration = sphere.r - dist; +// cp.pos.copy(normal).reverse().scale(sphere.r).add(sphere.state.pos); +// cp.r1.diff(cp.pos, sphere.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/colliders/SphereSphereCollider.as b/0.0.8.0/src/alternativa/physics/collision/colliders/SphereSphereCollider.as new file mode 100644 index 0000000..a90fabb --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/colliders/SphereSphereCollider.as @@ -0,0 +1,70 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.math.Vector3; + use namespace altphysics; + + public class SphereSphereCollider implements ICollider { + + private var p1:Vector3 = new Vector3(); + private var p2:Vector3 = new Vector3(); + + public function SphereSphereCollider() { + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var s1:CollisionSphere; + var s2:CollisionSphere; + if (prim1.body != null) { + s1 = prim1 as CollisionSphere; + s2 = prim2 as CollisionSphere; + } else { + s1 = prim2 as CollisionSphere; + s2 = prim1 as CollisionSphere; + } + + s1.transform.getAxis(3, p1); + s2.transform.getAxis(3, p2); + var dx:Number = p1.x - p2.x; + var dy:Number = p1.y - p2.y; + var dz:Number = p1.z - p2.z; + var len:Number = dx*dx + dy*dy + dz*dz; + var sum:Number = s1.r + s2.r; + if (len > sum*sum) return false; + len = Math.sqrt(len); + dx /= len; + dy /= len; + dz /= len; + + contact.body1 = s1.body; + contact.body2 = s2.body; + contact.normal.x = dx; + contact.normal.y = dy; + contact.normal.z = dz; + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = sum - len; + cp.pos.x = p1.x - dx*s1.r; + cp.pos.y = p1.y - dy*s1.r; + cp.pos.z = p1.z - dz*s1.r; + cp.r1.vDiff(cp.pos, p1); + cp.r2.vDiff(cp.pos, p2); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/primitives/.svn/all-wcprops b/0.0.8.0/src/alternativa/physics/collision/primitives/.svn/all-wcprops new file mode 100644 index 0000000..6b5dff4 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/primitives/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 124 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/collision/primitives +END +CollisionTriangle.as +K 25 +svn:wc:ra_dav:version-url +V 145 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/collision/primitives/CollisionTriangle.as +END +CollisionSphere.as +K 25 +svn:wc:ra_dav:version-url +V 143 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/collision/primitives/CollisionSphere.as +END +CollisionBox.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/collision/primitives/CollisionBox.as +END +CollisionRect.as +K 25 +svn:wc:ra_dav:version-url +V 141 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/collision/primitives/CollisionRect.as +END diff --git a/0.0.8.0/src/alternativa/physics/collision/primitives/.svn/entries b/0.0.8.0/src/alternativa/physics/collision/primitives/.svn/entries new file mode 100644 index 0000000..5ebe62e --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/primitives/.svn/entries @@ -0,0 +1,76 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/collision/primitives +http://svndev.alternativaplatform.com + + + +2009-10-23T05:59:29.567666Z +22444 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +CollisionTriangle.as +file + + + + +2010-10-28T04:32:42.000000Z +f974b5617d3a5068378d72f540fd4290 +2009-10-19T07:17:33.113306Z +22253 +mike + +CollisionSphere.as +file + + + + +2010-10-28T04:32:42.000000Z +759c8a4e451e224d04fff0bc21d559f2 +2009-10-19T07:17:33.113306Z +22253 +mike + +CollisionBox.as +file + + + + +2010-10-28T04:32:42.000000Z +eaf5410bc4fb8c70444ca57e2a9f4c34 +2009-10-23T05:59:29.567666Z +22444 +mike + +CollisionRect.as +file + + + + +2010-10-28T04:32:42.000000Z +2e5629bb7cf350dd18485fe68675d0f0 +2009-10-19T07:17:33.113306Z +22253 +mike + diff --git a/0.0.8.0/src/alternativa/physics/collision/primitives/.svn/format b/0.0.8.0/src/alternativa/physics/collision/primitives/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/primitives/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.8.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base b/0.0.8.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base new file mode 100644 index 0000000..3d2f391 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base @@ -0,0 +1,204 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.math.Matrix4; + import alternativa.physics.math.Vector3; + use namespace altphysics; + + /** + * Ориентированный бокс. + */ + public class CollisionBox extends CollisionPrimitive { + + // Половинные размеры вдоль каждой из осей + public var hs:Vector3 = new Vector3(); + + public var excludedFaces:int; + + /** + * @param hs + * @param collisionGroup + */ + public function CollisionBox(hs:Vector3, collisionGroup:int) { + super(BOX, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + var t:Matrix4 = transform; + + var xx:Number; + var yy:Number; + var zz:Number; + + xx = t.a < 0 ? -t.a : t.a; + yy = t.b < 0 ? -t.b : t.b; + zz = t.c < 0 ? -t.c : t.c; + aabb.maxX = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minX = -aabb.maxX; + + xx = t.e < 0 ? -t.e : t.e; + yy = t.f < 0 ? -t.f : t.f; + zz = t.g < 0 ? -t.g : t.g; + aabb.maxY = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minY = -aabb.maxY; + + xx = t.i < 0 ? -t.i : t.i; + yy = t.j < 0 ? -t.j : t.j; + zz = t.k < 0 ? -t.k : t.k; + aabb.maxZ = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var box:CollisionBox = source as CollisionBox; + if (box == null) return this; + super.copyFrom(box); + hs.vCopy(box.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionBox(hs, collisionGroup); + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + var tMin:Number = -1; + var tMax:Number = 1e308; + var t1:Number; + var t2:Number; + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + // X + if (vx < epsilon && vx > -epsilon) { + if (ox < -hs.x || ox > hs.x) return -1; + } else { + t1 = (-hs.x - ox)/vx; + t2 = (hs.x - ox)/vx; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.x = -1; + normal.y = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.x = 1; + normal.y = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Y + if (vy < epsilon && vy > -epsilon) { + if (oy < -hs.y || oy > hs.y) return -1; + } else { + t1 = (-hs.y - oy)/vy; + t2 = (hs.y - oy)/vy; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.y = -1; + normal.x = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.y = 1; + normal.x = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Z + if (vz < epsilon && vz > -epsilon) { + if (oz < -hs.z || oz > hs.z) return -1; + } else { + t1 = (-hs.z - oz)/vz; + t2 = (hs.z - oz)/vz; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.z = -1; + normal.x = normal.y = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.z = 1; + normal.x = normal.y = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Перевод нормали в мировую систему координат + // Inlined normal.vDeltaTransformBy4(transform); + vx = normal.x; + vy = normal.y; + vz = normal.z; + normal.x = transform.a*vx + transform.b*vy + transform.c*vz; + normal.y = transform.e*vx + transform.f*vy + transform.g*vz; + normal.z = transform.i*vx + transform.j*vy + transform.k*vz; + + return tMin; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionBox hs=" + hs + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base b/0.0.8.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base new file mode 100644 index 0000000..08001ad --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base @@ -0,0 +1,143 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.math.Matrix4; + import alternativa.physics.math.Vector3; + use namespace altphysics; + + /** + * Ориентированный прямоугольник. Задаётся половинами размеров вдоль осей X (ширина) и Y (длина) локальной системы + * координат примитива. Таким образом, прямоугольник лежит в плоскости XY, его стороны параллельны осям этой + * плоскости, а нормаль направлена вдоль локальной оси Z. + * + * Прямоугольник может быть одно- или двусторонним. В случае одностороннего прямоугольника, + * столкновения с внутренней стороны не регистрируются. + */ + public class CollisionRect extends CollisionPrimitive { + // Половинные размеры прямоугольника вдоль осей X и Y. Нормаль направлена вдоль оси Z. + public var hs:Vector3 = new Vector3(); + // Флаг указывает, является примитив одно- или двусторонним + public var twoSided:Boolean = true; + + // Малое значение. Используется для указания фиктивной высоты примитива и в функции определения пересечения с сегментом. + private static const EPSILON:Number = 0.005; + + /** + * Создаёт новый экземпляр примитива. + * + * @param hs половинные размерв прямоугольника вдоль осей X и Y. Значение z игнорируется. + * @param collisionGroup группа примитива + */ + public function CollisionRect(hs:Vector3, collisionGroup:int) { + super(RECT, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * Расчитывает ограничивающий бокс прямоугольника. Для избежания проблем высота примитива принимается равной + * не нулю, а малому значению. + * + * @return + */ + override public function calculateAABB():BoundBox { + // Баунд бокс прямоугольника имеет минимальную высоту, отличную от нуля во избежание проблем с построением kd-дерева + var t:Matrix4 = transform; + var xx:Number = t.a < 0 ? -t.a : t.a; + var yy:Number = t.b < 0 ? -t.b : t.b; + var zz:Number = t.c < 0 ? -t.c : t.c; + aabb.maxX = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minX = -aabb.maxX; + + xx = t.e < 0 ? -t.e : t.e; + yy = t.f < 0 ? -t.f : t.f; + zz = t.g < 0 ? -t.g : t.g; + aabb.maxY = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minY = -aabb.maxY; + + xx = t.i < 0 ? -t.i : t.i; + yy = t.j < 0 ? -t.j : t.j; + zz = t.k < 0 ? -t.k : t.k; + aabb.maxZ = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var rect:CollisionRect = source as CollisionRect; + if (rect == null) return this; + super.copyFrom(rect); + hs.vCopy(rect.hs); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionRect hs=" + hs + "]"; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionRect(hs, collisionGroup); + } + + /** + * @param origin + * @param vector + * @param collisionGroup + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + + // Проверка параллельности сегмента и плоскости примитива + if (vz > -threshold && vz < threshold) return -1; + var t:Number = -oz/vz; + if (t < 0) return -1; + // Проверка вхождения точки пересечения в прямоугольник + ox += vx*t; + oy += vy*t; + oz = 0; + if (ox < (-hs.x - threshold) || ox > (hs.x + threshold) || oy < (-hs.y - threshold) || oy > (hs.y + threshold)) return -1; + + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + return t; + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base b/0.0.8.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base new file mode 100644 index 0000000..887a19a --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base @@ -0,0 +1,81 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.math.Vector3; + use namespace altphysics; + + + /** + * Сфера. + */ + public class CollisionSphere extends CollisionPrimitive { + + // Радиус сферы + public var r:Number = 0; + + /** + * + * @param r + * @param collisionGroup + */ + public function CollisionSphere(r:Number, collisionGroup:int) { + super(SPHERE, collisionGroup); + this.r = r; + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + aabb.maxX = transform.d + r; + aabb.minX = transform.d - r; + + aabb.maxY = transform.h + r; + aabb.minY = transform.h - r; + + aabb.maxZ = transform.l + r; + aabb.minZ = transform.l - r; + + return aabb; + } + + /** + * @param origin + * @param vector + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var px:Number = origin.x - transform.d; + var py:Number = origin.y - transform.h; + var pz:Number = origin.z - transform.l; + var k:Number = vector.x*px + vector.y*py + vector.z*pz; + if (k > 0) return -1; + var a:Number = vector.x*vector.x + vector.y*vector.y + vector.z*vector.z; + var D:Number = k*k - a*(px*px + py*py + pz*pz - r*r); + if (D < 0) return -1; + return -(k + Math.sqrt(D))/a; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var sphere:CollisionSphere = source as CollisionSphere; + if (sphere == null) return this; + super.copyFrom(sphere); + r = sphere.r; + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionSphere(r, collisionGroup); + } + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base b/0.0.8.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base new file mode 100644 index 0000000..9cd6d00 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base @@ -0,0 +1,244 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.math.Vector3; + use namespace altphysics; + + /** + * Примитив треугольник. + */ + public class CollisionTriangle extends CollisionPrimitive { + + public var v0:Vector3 = new Vector3(); + public var v1:Vector3 = new Vector3(); + public var v2:Vector3 = new Vector3(); + + public var e0:Vector3 = new Vector3(); + public var e1:Vector3 = new Vector3(); + public var e2:Vector3 = new Vector3(); + +// public var len0:Number; +// public var len1:Number; +// public var len2:Number; +// + /** + * + * @param v0 + * @param v1 + * @param v2 + * @param collisionGroup + */ + public function CollisionTriangle(v0:Vector3, v1:Vector3, v2:Vector3, collisionGroup:int) { + super(TRIANGLE, collisionGroup); + initVertices(v0, v1, v2); + } + + /** + * Рассчитывает AABB примитива. + * + * @return ссылка на свой AABB + */ + override public function calculateAABB():BoundBox { + var epsilon:Number = 0.005; + var a:Number; + var b:Number; + var eps_c:Number = epsilon*transform.c; + var eps_g:Number = epsilon*transform.g; + var eps_k:Number = epsilon*transform.k; + + // Вершина 0 + // Ось X + a = v0.x*transform.a + v0.y*transform.b; + aabb.minX = aabb.maxX = a + eps_c; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v0.x*transform.e + v0.y*transform.f; + aabb.minY = aabb.maxY = a + eps_g; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v0.x*transform.i + v0.y*transform.j; + aabb.minZ = aabb.maxZ = a + eps_k; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 1 + // Ось X + a = v1.x*transform.a + v1.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v1.x*transform.e + v1.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v1.x*transform.i + v1.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 2 + // Ось X + a = v2.x*transform.a + v2.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v2.x*transform.e + v2.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v2.x*transform.i + v2.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + aabb.minX += transform.d; + aabb.maxX += transform.d; + + aabb.minY += transform.h; + aabb.maxY += transform.h; + + aabb.minZ += transform.l; + aabb.maxZ += transform.l; + + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + // Луч трансформируется в систему координат примитива, затем проверяется пересечение + + var vz:Number = vector.x*transform.c + vector.y*transform.g + vector.z*transform.k; + // Если луч параллелен плоскости птимитива, то пересечения нет + if (vz < epsilon && vz > -epsilon) return -1; + + var tx:Number = origin.x - transform.d; + var ty:Number = origin.y - transform.h; + var tz:Number = origin.z - transform.l; + + var oz:Number = tx*transform.c + ty*transform.g + tz*transform.k; + var t:Number = -oz/vz; + if (t < 0) return -1; + + var ox:Number = tx*transform.a + ty*transform.e + tz*transform.i; + var oy:Number = tx*transform.b + ty*transform.f + tz*transform.j; + + tx = ox + t*(vector.x*transform.a + vector.y*transform.e + vector.z*transform.i); + ty = oy + t*(vector.x*transform.b + vector.y*transform.f + vector.z*transform.j); + tz = oz + t*vz; + + // Проверка вхождения точки в треугольник + if ((e0.x*(ty - v0.y) - e0.y*(tx - v0.x) < 0) || (e1.x*(ty - v1.y) - e1.y*(tx - v1.x) < 0) || (e2.x*(ty - v2.y) - e2.y*(tx - v2.x) < 0)) return -1; + + // Запись нормали + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + + return t; + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + super.copyFrom(source); + var tri:CollisionTriangle = source as CollisionTriangle; + if (tri != null) { + v0.vCopy(tri.v0); + v1.vCopy(tri.v1); + v2.vCopy(tri.v2); + + e0.vCopy(tri.e0); + e1.vCopy(tri.e1); + e2.vCopy(tri.e2); + +// len0 = tri.len0; +// len1 = tri.len1; +// len2 = tri.len2; + } + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionTriangle v0=" + v0 + ", v1=" + v1 + ", v2=" + v2 + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionTriangle(v0, v1, v2, collisionGroup); + } + + /** + * + * @param v0 + * @param v1 + * @param v2 + */ + private function initVertices(v0:Vector3, v1:Vector3, v2:Vector3):void { + this.v0.vCopy(v0); + this.v1.vCopy(v1); + this.v2.vCopy(v2); + + e0.vDiff(v1, v0); +// len0 = e0.vLength(); + e0.vNormalize(); + + e1.vDiff(v2, v1); +// len1 = e1.vLength(); + e1.vNormalize(); + + e2.vDiff(v0, v2); +// len2 = e2.vLength(); + e2.vNormalize(); + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/primitives/CollisionBox.as b/0.0.8.0/src/alternativa/physics/collision/primitives/CollisionBox.as new file mode 100644 index 0000000..3d2f391 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/primitives/CollisionBox.as @@ -0,0 +1,204 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.math.Matrix4; + import alternativa.physics.math.Vector3; + use namespace altphysics; + + /** + * Ориентированный бокс. + */ + public class CollisionBox extends CollisionPrimitive { + + // Половинные размеры вдоль каждой из осей + public var hs:Vector3 = new Vector3(); + + public var excludedFaces:int; + + /** + * @param hs + * @param collisionGroup + */ + public function CollisionBox(hs:Vector3, collisionGroup:int) { + super(BOX, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + var t:Matrix4 = transform; + + var xx:Number; + var yy:Number; + var zz:Number; + + xx = t.a < 0 ? -t.a : t.a; + yy = t.b < 0 ? -t.b : t.b; + zz = t.c < 0 ? -t.c : t.c; + aabb.maxX = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minX = -aabb.maxX; + + xx = t.e < 0 ? -t.e : t.e; + yy = t.f < 0 ? -t.f : t.f; + zz = t.g < 0 ? -t.g : t.g; + aabb.maxY = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minY = -aabb.maxY; + + xx = t.i < 0 ? -t.i : t.i; + yy = t.j < 0 ? -t.j : t.j; + zz = t.k < 0 ? -t.k : t.k; + aabb.maxZ = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var box:CollisionBox = source as CollisionBox; + if (box == null) return this; + super.copyFrom(box); + hs.vCopy(box.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionBox(hs, collisionGroup); + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + var tMin:Number = -1; + var tMax:Number = 1e308; + var t1:Number; + var t2:Number; + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + // X + if (vx < epsilon && vx > -epsilon) { + if (ox < -hs.x || ox > hs.x) return -1; + } else { + t1 = (-hs.x - ox)/vx; + t2 = (hs.x - ox)/vx; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.x = -1; + normal.y = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.x = 1; + normal.y = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Y + if (vy < epsilon && vy > -epsilon) { + if (oy < -hs.y || oy > hs.y) return -1; + } else { + t1 = (-hs.y - oy)/vy; + t2 = (hs.y - oy)/vy; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.y = -1; + normal.x = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.y = 1; + normal.x = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Z + if (vz < epsilon && vz > -epsilon) { + if (oz < -hs.z || oz > hs.z) return -1; + } else { + t1 = (-hs.z - oz)/vz; + t2 = (hs.z - oz)/vz; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.z = -1; + normal.x = normal.y = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.z = 1; + normal.x = normal.y = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Перевод нормали в мировую систему координат + // Inlined normal.vDeltaTransformBy4(transform); + vx = normal.x; + vy = normal.y; + vz = normal.z; + normal.x = transform.a*vx + transform.b*vy + transform.c*vz; + normal.y = transform.e*vx + transform.f*vy + transform.g*vz; + normal.z = transform.i*vx + transform.j*vy + transform.k*vz; + + return tMin; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionBox hs=" + hs + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/primitives/CollisionRect.as b/0.0.8.0/src/alternativa/physics/collision/primitives/CollisionRect.as new file mode 100644 index 0000000..08001ad --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/primitives/CollisionRect.as @@ -0,0 +1,143 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.math.Matrix4; + import alternativa.physics.math.Vector3; + use namespace altphysics; + + /** + * Ориентированный прямоугольник. Задаётся половинами размеров вдоль осей X (ширина) и Y (длина) локальной системы + * координат примитива. Таким образом, прямоугольник лежит в плоскости XY, его стороны параллельны осям этой + * плоскости, а нормаль направлена вдоль локальной оси Z. + * + * Прямоугольник может быть одно- или двусторонним. В случае одностороннего прямоугольника, + * столкновения с внутренней стороны не регистрируются. + */ + public class CollisionRect extends CollisionPrimitive { + // Половинные размеры прямоугольника вдоль осей X и Y. Нормаль направлена вдоль оси Z. + public var hs:Vector3 = new Vector3(); + // Флаг указывает, является примитив одно- или двусторонним + public var twoSided:Boolean = true; + + // Малое значение. Используется для указания фиктивной высоты примитива и в функции определения пересечения с сегментом. + private static const EPSILON:Number = 0.005; + + /** + * Создаёт новый экземпляр примитива. + * + * @param hs половинные размерв прямоугольника вдоль осей X и Y. Значение z игнорируется. + * @param collisionGroup группа примитива + */ + public function CollisionRect(hs:Vector3, collisionGroup:int) { + super(RECT, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * Расчитывает ограничивающий бокс прямоугольника. Для избежания проблем высота примитива принимается равной + * не нулю, а малому значению. + * + * @return + */ + override public function calculateAABB():BoundBox { + // Баунд бокс прямоугольника имеет минимальную высоту, отличную от нуля во избежание проблем с построением kd-дерева + var t:Matrix4 = transform; + var xx:Number = t.a < 0 ? -t.a : t.a; + var yy:Number = t.b < 0 ? -t.b : t.b; + var zz:Number = t.c < 0 ? -t.c : t.c; + aabb.maxX = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minX = -aabb.maxX; + + xx = t.e < 0 ? -t.e : t.e; + yy = t.f < 0 ? -t.f : t.f; + zz = t.g < 0 ? -t.g : t.g; + aabb.maxY = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minY = -aabb.maxY; + + xx = t.i < 0 ? -t.i : t.i; + yy = t.j < 0 ? -t.j : t.j; + zz = t.k < 0 ? -t.k : t.k; + aabb.maxZ = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var rect:CollisionRect = source as CollisionRect; + if (rect == null) return this; + super.copyFrom(rect); + hs.vCopy(rect.hs); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionRect hs=" + hs + "]"; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionRect(hs, collisionGroup); + } + + /** + * @param origin + * @param vector + * @param collisionGroup + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + + // Проверка параллельности сегмента и плоскости примитива + if (vz > -threshold && vz < threshold) return -1; + var t:Number = -oz/vz; + if (t < 0) return -1; + // Проверка вхождения точки пересечения в прямоугольник + ox += vx*t; + oy += vy*t; + oz = 0; + if (ox < (-hs.x - threshold) || ox > (hs.x + threshold) || oy < (-hs.y - threshold) || oy > (hs.y + threshold)) return -1; + + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + return t; + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/primitives/CollisionSphere.as b/0.0.8.0/src/alternativa/physics/collision/primitives/CollisionSphere.as new file mode 100644 index 0000000..887a19a --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/primitives/CollisionSphere.as @@ -0,0 +1,81 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.math.Vector3; + use namespace altphysics; + + + /** + * Сфера. + */ + public class CollisionSphere extends CollisionPrimitive { + + // Радиус сферы + public var r:Number = 0; + + /** + * + * @param r + * @param collisionGroup + */ + public function CollisionSphere(r:Number, collisionGroup:int) { + super(SPHERE, collisionGroup); + this.r = r; + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + aabb.maxX = transform.d + r; + aabb.minX = transform.d - r; + + aabb.maxY = transform.h + r; + aabb.minY = transform.h - r; + + aabb.maxZ = transform.l + r; + aabb.minZ = transform.l - r; + + return aabb; + } + + /** + * @param origin + * @param vector + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var px:Number = origin.x - transform.d; + var py:Number = origin.y - transform.h; + var pz:Number = origin.z - transform.l; + var k:Number = vector.x*px + vector.y*py + vector.z*pz; + if (k > 0) return -1; + var a:Number = vector.x*vector.x + vector.y*vector.y + vector.z*vector.z; + var D:Number = k*k - a*(px*px + py*py + pz*pz - r*r); + if (D < 0) return -1; + return -(k + Math.sqrt(D))/a; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var sphere:CollisionSphere = source as CollisionSphere; + if (sphere == null) return this; + super.copyFrom(sphere); + r = sphere.r; + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionSphere(r, collisionGroup); + } + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/primitives/CollisionTriangle.as b/0.0.8.0/src/alternativa/physics/collision/primitives/CollisionTriangle.as new file mode 100644 index 0000000..9cd6d00 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/primitives/CollisionTriangle.as @@ -0,0 +1,244 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.math.Vector3; + use namespace altphysics; + + /** + * Примитив треугольник. + */ + public class CollisionTriangle extends CollisionPrimitive { + + public var v0:Vector3 = new Vector3(); + public var v1:Vector3 = new Vector3(); + public var v2:Vector3 = new Vector3(); + + public var e0:Vector3 = new Vector3(); + public var e1:Vector3 = new Vector3(); + public var e2:Vector3 = new Vector3(); + +// public var len0:Number; +// public var len1:Number; +// public var len2:Number; +// + /** + * + * @param v0 + * @param v1 + * @param v2 + * @param collisionGroup + */ + public function CollisionTriangle(v0:Vector3, v1:Vector3, v2:Vector3, collisionGroup:int) { + super(TRIANGLE, collisionGroup); + initVertices(v0, v1, v2); + } + + /** + * Рассчитывает AABB примитива. + * + * @return ссылка на свой AABB + */ + override public function calculateAABB():BoundBox { + var epsilon:Number = 0.005; + var a:Number; + var b:Number; + var eps_c:Number = epsilon*transform.c; + var eps_g:Number = epsilon*transform.g; + var eps_k:Number = epsilon*transform.k; + + // Вершина 0 + // Ось X + a = v0.x*transform.a + v0.y*transform.b; + aabb.minX = aabb.maxX = a + eps_c; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v0.x*transform.e + v0.y*transform.f; + aabb.minY = aabb.maxY = a + eps_g; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v0.x*transform.i + v0.y*transform.j; + aabb.minZ = aabb.maxZ = a + eps_k; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 1 + // Ось X + a = v1.x*transform.a + v1.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v1.x*transform.e + v1.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v1.x*transform.i + v1.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 2 + // Ось X + a = v2.x*transform.a + v2.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v2.x*transform.e + v2.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v2.x*transform.i + v2.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + aabb.minX += transform.d; + aabb.maxX += transform.d; + + aabb.minY += transform.h; + aabb.maxY += transform.h; + + aabb.minZ += transform.l; + aabb.maxZ += transform.l; + + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + // Луч трансформируется в систему координат примитива, затем проверяется пересечение + + var vz:Number = vector.x*transform.c + vector.y*transform.g + vector.z*transform.k; + // Если луч параллелен плоскости птимитива, то пересечения нет + if (vz < epsilon && vz > -epsilon) return -1; + + var tx:Number = origin.x - transform.d; + var ty:Number = origin.y - transform.h; + var tz:Number = origin.z - transform.l; + + var oz:Number = tx*transform.c + ty*transform.g + tz*transform.k; + var t:Number = -oz/vz; + if (t < 0) return -1; + + var ox:Number = tx*transform.a + ty*transform.e + tz*transform.i; + var oy:Number = tx*transform.b + ty*transform.f + tz*transform.j; + + tx = ox + t*(vector.x*transform.a + vector.y*transform.e + vector.z*transform.i); + ty = oy + t*(vector.x*transform.b + vector.y*transform.f + vector.z*transform.j); + tz = oz + t*vz; + + // Проверка вхождения точки в треугольник + if ((e0.x*(ty - v0.y) - e0.y*(tx - v0.x) < 0) || (e1.x*(ty - v1.y) - e1.y*(tx - v1.x) < 0) || (e2.x*(ty - v2.y) - e2.y*(tx - v2.x) < 0)) return -1; + + // Запись нормали + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + + return t; + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + super.copyFrom(source); + var tri:CollisionTriangle = source as CollisionTriangle; + if (tri != null) { + v0.vCopy(tri.v0); + v1.vCopy(tri.v1); + v2.vCopy(tri.v2); + + e0.vCopy(tri.e0); + e1.vCopy(tri.e1); + e2.vCopy(tri.e2); + +// len0 = tri.len0; +// len1 = tri.len1; +// len2 = tri.len2; + } + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionTriangle v0=" + v0 + ", v1=" + v1 + ", v2=" + v2 + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionTriangle(v0, v1, v2, collisionGroup); + } + + /** + * + * @param v0 + * @param v1 + * @param v2 + */ + private function initVertices(v0:Vector3, v1:Vector3, v2:Vector3):void { + this.v0.vCopy(v0); + this.v1.vCopy(v1); + this.v2.vCopy(v2); + + e0.vDiff(v1, v0); +// len0 = e0.vLength(); + e0.vNormalize(); + + e1.vDiff(v2, v1); +// len1 = e1.vLength(); + e1.vNormalize(); + + e2.vDiff(v0, v2); +// len2 = e2.vLength(); + e2.vNormalize(); + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/types/.svn/all-wcprops b/0.0.8.0/src/alternativa/physics/collision/types/.svn/all-wcprops new file mode 100644 index 0000000..db76e1d --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/types/.svn/all-wcprops @@ -0,0 +1,23 @@ +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/collision/types +END +BoundBox.as +K 25 +svn:wc:ra_dav:version-url +V 131 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/collision/types/BoundBox.as +END +RayIntersection.as +K 25 +svn:wc:ra_dav:version-url +V 138 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/collision/types/RayIntersection.as +END +Ray.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/collision/types/Ray.as +END diff --git a/0.0.8.0/src/alternativa/physics/collision/types/.svn/entries b/0.0.8.0/src/alternativa/physics/collision/types/.svn/entries new file mode 100644 index 0000000..f76a680 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/types/.svn/entries @@ -0,0 +1,64 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/collision/types +http://svndev.alternativaplatform.com + + + +2009-10-19T07:17:33.113306Z +22253 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +BoundBox.as +file + + + + +2010-10-28T04:32:42.000000Z +d92d4ab3dde698feb674fb9488f59678 +2009-09-09T14:02:48.134744Z +19633 +mike + +RayIntersection.as +file + + + + +2010-10-28T04:32:42.000000Z +4002cb4f3effa05aedf5c38dfb3ade7c +2009-10-19T07:17:33.113306Z +22253 +mike + +Ray.as +file + + + + +2010-10-28T04:32:42.000000Z +77598eacffac4c232aeeaaf83f4b27e5 +2009-10-19T07:17:33.113306Z +22253 +mike + diff --git a/0.0.8.0/src/alternativa/physics/collision/types/.svn/format b/0.0.8.0/src/alternativa/physics/collision/types/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/types/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.8.0/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base b/0.0.8.0/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base new file mode 100644 index 0000000..b4d5278 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base @@ -0,0 +1,75 @@ +package alternativa.physics.collision.types { + + /** + * Ограничивающий параллелепипед. + */ + public class BoundBox { + + public var minX:Number = 1e308; + public var minY:Number = 1e308; + public var minZ:Number = 1e308; + public var maxX:Number = -1e308; + public var maxY:Number = -1e308; + public var maxZ:Number = -1e308; + + public function setSize(minX:Number, minY:Number, minZ:Number, maxX:Number, maxY:Number, maxZ:Number):void { + this.minX = minX; + this.minY = minY; + this.minZ = minZ; + this.maxX = maxX; + this.maxY = maxY; + this.maxZ = maxZ; + } + + public function addBoundBox(boundBox:BoundBox):void { + minX = (boundBox.minX < minX) ? boundBox.minX : minX; + minY = (boundBox.minY < minY) ? boundBox.minY : minY; + minZ = (boundBox.minZ < minZ) ? boundBox.minZ : minZ; + maxX = (boundBox.maxX > maxX) ? boundBox.maxX : maxX; + maxY = (boundBox.maxY > maxY) ? boundBox.maxY : maxY; + maxZ = (boundBox.maxZ > maxZ) ? boundBox.maxZ : maxZ; + } + + public function addPoint(x:Number, y:Number, z:Number):void { + if (x < minX) minX = x; + if (x > maxX) maxX = x; + if (y < minY) minY = y; + if (y > maxY) maxY = y; + if (z < minZ) minZ = z; + if (z > maxZ) maxZ = z; + } + + public function infinity():void { + minX = 1e308; + minY = 1e308; + minZ = 1e308; + maxX = -1e308; + maxY = -1e308; + maxZ = -1e308; + } + + public function intersects(bb:BoundBox, epsilon:Number):Boolean { + return !(minX > bb.maxX + epsilon || maxX < bb.minX - epsilon || minY > bb.maxY + epsilon || maxY < bb.minY - epsilon || minZ > bb.maxZ + epsilon || maxZ < bb.minZ - epsilon); + } + + public function copyFrom(boundBox:BoundBox):void { + minX = boundBox.minX; + minY = boundBox.minY; + minZ = boundBox.minZ; + maxX = boundBox.maxX; + maxY = boundBox.maxY; + maxZ = boundBox.maxZ; + } + + public function clone():BoundBox { + var clone:BoundBox = new BoundBox(); + clone.copyFrom(this); + return clone; + } + + public function toString():String { + return "BoundBox [" + minX + ", " + minY + ", " + minZ + " : " + maxX + ", " + maxY + ", " + maxZ + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base b/0.0.8.0/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base new file mode 100644 index 0000000..01c48e3 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base @@ -0,0 +1,13 @@ +package alternativa.physics.collision.types { + import alternativa.physics.math.Vector3; + + /** + * Структура, описывающая луч и сегмент прямой в пространстве. + */ + public class Ray { + // Начало луча. Начальная точка сегмента на прямой луча. + public var origin:Vector3 = new Vector3(); + // Направляющий вектор луча. Длина вектора задаёт длину сегмента на луче. Конечная точка сегмента вычисляется как origin + dir. + public var dir:Vector3 = new Vector3(); + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base b/0.0.8.0/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base new file mode 100644 index 0000000..cbf3607 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base @@ -0,0 +1,25 @@ +package alternativa.physics.collision.types { + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.math.Vector3; + + /** + * Структура описывает точку пересечения луча с физической геометрией. + */ + public class RayIntersection { + // Физический примитив, с которым пересекается луч + public var primitive:CollisionPrimitive; + // Положенеи точки пересечения + public var pos:Vector3 = new Vector3(); + // Нормаль поверхности физического примитива в точке пересечения + public var normal:Vector3 = new Vector3(); + // Время пересечения (|pos - ray.origin|/|ray.dir|) + public var t:Number = 0; + + public function copy(source:RayIntersection):void { + primitive = source.primitive; + pos.vCopy(source.pos); + normal.vCopy(source.normal); + t = source.t; + } + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/types/BoundBox.as b/0.0.8.0/src/alternativa/physics/collision/types/BoundBox.as new file mode 100644 index 0000000..b4d5278 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/types/BoundBox.as @@ -0,0 +1,75 @@ +package alternativa.physics.collision.types { + + /** + * Ограничивающий параллелепипед. + */ + public class BoundBox { + + public var minX:Number = 1e308; + public var minY:Number = 1e308; + public var minZ:Number = 1e308; + public var maxX:Number = -1e308; + public var maxY:Number = -1e308; + public var maxZ:Number = -1e308; + + public function setSize(minX:Number, minY:Number, minZ:Number, maxX:Number, maxY:Number, maxZ:Number):void { + this.minX = minX; + this.minY = minY; + this.minZ = minZ; + this.maxX = maxX; + this.maxY = maxY; + this.maxZ = maxZ; + } + + public function addBoundBox(boundBox:BoundBox):void { + minX = (boundBox.minX < minX) ? boundBox.minX : minX; + minY = (boundBox.minY < minY) ? boundBox.minY : minY; + minZ = (boundBox.minZ < minZ) ? boundBox.minZ : minZ; + maxX = (boundBox.maxX > maxX) ? boundBox.maxX : maxX; + maxY = (boundBox.maxY > maxY) ? boundBox.maxY : maxY; + maxZ = (boundBox.maxZ > maxZ) ? boundBox.maxZ : maxZ; + } + + public function addPoint(x:Number, y:Number, z:Number):void { + if (x < minX) minX = x; + if (x > maxX) maxX = x; + if (y < minY) minY = y; + if (y > maxY) maxY = y; + if (z < minZ) minZ = z; + if (z > maxZ) maxZ = z; + } + + public function infinity():void { + minX = 1e308; + minY = 1e308; + minZ = 1e308; + maxX = -1e308; + maxY = -1e308; + maxZ = -1e308; + } + + public function intersects(bb:BoundBox, epsilon:Number):Boolean { + return !(minX > bb.maxX + epsilon || maxX < bb.minX - epsilon || minY > bb.maxY + epsilon || maxY < bb.minY - epsilon || minZ > bb.maxZ + epsilon || maxZ < bb.minZ - epsilon); + } + + public function copyFrom(boundBox:BoundBox):void { + minX = boundBox.minX; + minY = boundBox.minY; + minZ = boundBox.minZ; + maxX = boundBox.maxX; + maxY = boundBox.maxY; + maxZ = boundBox.maxZ; + } + + public function clone():BoundBox { + var clone:BoundBox = new BoundBox(); + clone.copyFrom(this); + return clone; + } + + public function toString():String { + return "BoundBox [" + minX + ", " + minY + ", " + minZ + " : " + maxX + ", " + maxY + ", " + maxZ + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/types/Ray.as b/0.0.8.0/src/alternativa/physics/collision/types/Ray.as new file mode 100644 index 0000000..01c48e3 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/types/Ray.as @@ -0,0 +1,13 @@ +package alternativa.physics.collision.types { + import alternativa.physics.math.Vector3; + + /** + * Структура, описывающая луч и сегмент прямой в пространстве. + */ + public class Ray { + // Начало луча. Начальная точка сегмента на прямой луча. + public var origin:Vector3 = new Vector3(); + // Направляющий вектор луча. Длина вектора задаёт длину сегмента на луче. Конечная точка сегмента вычисляется как origin + dir. + public var dir:Vector3 = new Vector3(); + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/collision/types/RayIntersection.as b/0.0.8.0/src/alternativa/physics/collision/types/RayIntersection.as new file mode 100644 index 0000000..cbf3607 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/collision/types/RayIntersection.as @@ -0,0 +1,25 @@ +package alternativa.physics.collision.types { + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.math.Vector3; + + /** + * Структура описывает точку пересечения луча с физической геометрией. + */ + public class RayIntersection { + // Физический примитив, с которым пересекается луч + public var primitive:CollisionPrimitive; + // Положенеи точки пересечения + public var pos:Vector3 = new Vector3(); + // Нормаль поверхности физического примитива в точке пересечения + public var normal:Vector3 = new Vector3(); + // Время пересечения (|pos - ray.origin|/|ray.dir|) + public var t:Number = 0; + + public function copy(source:RayIntersection):void { + primitive = source.primitive; + pos.vCopy(source.pos); + normal.vCopy(source.normal); + t = source.t; + } + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/constraints/.svn/all-wcprops b/0.0.8.0/src/alternativa/physics/constraints/.svn/all-wcprops new file mode 100644 index 0000000..b1c275d --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/constraints/.svn/all-wcprops @@ -0,0 +1,17 @@ +K 25 +svn:wc:ra_dav:version-url +V 115 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/constraints +END +Constraint.as +K 25 +svn:wc:ra_dav:version-url +V 129 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/constraints/Constraint.as +END +MaxDistanceConstraint.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/constraints/MaxDistanceConstraint.as +END diff --git a/0.0.8.0/src/alternativa/physics/constraints/.svn/entries b/0.0.8.0/src/alternativa/physics/constraints/.svn/entries new file mode 100644 index 0000000..cd3d1a4 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/constraints/.svn/entries @@ -0,0 +1,52 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/constraints +http://svndev.alternativaplatform.com + + + +2009-10-19T07:17:33.113306Z +22253 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Constraint.as +file + + + + +2010-10-28T04:32:43.000000Z +2a02ebce2cfc45515549bd41adc8215b +2009-10-19T07:17:33.113306Z +22253 +mike + +MaxDistanceConstraint.as +file + + + + +2010-10-28T04:32:43.000000Z +d6159b2e8bdaa7da4f1b419002c71a09 +2009-10-19T07:17:33.113306Z +22253 +mike + diff --git a/0.0.8.0/src/alternativa/physics/constraints/.svn/format b/0.0.8.0/src/alternativa/physics/constraints/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/constraints/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.8.0/src/alternativa/physics/constraints/.svn/text-base/Constraint.as.svn-base b/0.0.8.0/src/alternativa/physics/constraints/.svn/text-base/Constraint.as.svn-base new file mode 100644 index 0000000..4676d0d --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/constraints/.svn/text-base/Constraint.as.svn-base @@ -0,0 +1,35 @@ +package alternativa.physics.constraints { + + import alternativa.physics.World; + import alternativa.physics.altphysics; + + use namespace altphysics; + + /** + * + */ + public class Constraint { + + altphysics var satisfied:Boolean; + altphysics var world:World; + + /** + * + */ + public function Constraint() { + } + + /** + * @param dt + */ + public function preProcess(dt:Number):void { + } + + /** + * @param dt + */ + public function apply(dt:Number):void { + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base b/0.0.8.0/src/alternativa/physics/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base new file mode 100644 index 0000000..c87e0a3 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base @@ -0,0 +1,183 @@ +package alternativa.physics.constraints { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.math.Matrix3; + import alternativa.physics.math.Vector3; + + use namespace altphysics; + + /** + * + */ + public class MaxDistanceConstraint extends Constraint { + // Первое тело ограничения + altphysics var body1:Body; + // Второе тело ограничения. Может быть null. + altphysics var body2:Body; + // Первая точка крепления в системе координат первого тела + altphysics var r1:Vector3; + // Вторая точка привязки в системе координат второго тела. В случае отсутствия второго тела, координаты задаются в мировой системе. + altphysics var r2:Vector3; + // Максимально допустимое расстояние между точками крепления + altphysics var maxDistance:Number; + // Радиус-вектор первой точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr1:Vector3 = new Vector3(); + // Радиус-вектор второй точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr2:Vector3 = new Vector3(); + // Минимальная скорость сближения для удовлетворения ограничения + private var minClosingVel:Number; + // Изменение относительной скорости на единицу импульса + private var velByUnitImpulseN:Number; + // Направление корректирующего импульса + private var impulseDirection:Vector3 = new Vector3(); + + // Вспомогательные переменные +// private static var _v:Vector3 = new Vector3(); +// private static var _v1:Vector3 = new Vector3(); +// private static var _v2:Vector3 = new Vector3(); + + /** + * @param body1 + * @param body2 + * @param r1 + * @param r2 + * @param maxDistance + */ + public function MaxDistanceConstraint(body1:Body, body2:Body, r1:Vector3, r2:Vector3, maxDistance:Number) { + super(); + this.body1 = body1; + this.body2 = body2; + this.r1 = r1.vClone(); + this.r2 = r2.vClone(); + this.maxDistance = maxDistance; + } + + /** + * @param dt + */ + override public function preProcess(dt:Number):void { + // Вычислим расстояние между точками + var m:Matrix3 = body1.baseMatrix; + wr1.x = m.a*r1.x + m.b*r1.y + m.c*r1.z; + wr1.y = m.e*r1.x + m.f*r1.y + m.g*r1.z; + wr1.z = m.i*r1.x + m.j*r1.y + m.k*r1.z; + if (body2 != null) { + m = body2.baseMatrix; + wr2.x = m.a*r2.x + m.b*r2.y + m.c*r2.z; + wr2.y = m.e*r2.x + m.f*r2.y + m.g*r2.z; + wr2.z = m.i*r2.x + m.j*r2.y + m.k*r2.z; + } else { + wr2.x = r2.x; + wr2.y = r2.y; + wr2.z = r2.z; + } + var p1:Vector3 = body1.state.pos; + impulseDirection.x = wr2.x - wr1.x - p1.x; + impulseDirection.y = wr2.y - wr1.y - p1.y; + impulseDirection.z = wr2.z - wr1.z - p1.z; + if (body2 != null) { + var p2:Vector3 = body2.state.pos; + impulseDirection.x += p2.x; + impulseDirection.y += p2.y; + impulseDirection.z += p2.z; + } + + var len:Number = Math.sqrt(impulseDirection.x*impulseDirection.x + impulseDirection.y*impulseDirection.y + impulseDirection.z*impulseDirection.z); + var delta:Number = len - maxDistance; + if (delta > 0) { + satisfied = false; + if (len < 0.001) { + impulseDirection.x = 1; + } else { + len = 1/len; + impulseDirection.x *= len; + impulseDirection.y *= len; + impulseDirection.z *= len; + } + minClosingVel = delta/(world.penResolutionSteps*dt); + if (minClosingVel > world.maxPenResolutionSpeed) { + minClosingVel = world.maxPenResolutionSpeed; + } + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + var x:Number; + var y:Number; + var z:Number; + var vx:Number; + var vy:Number; + var vz:Number; + velByUnitImpulseN = 0; + if (body1.movable) { +// velByUnitImpulseN += body1.invMass + _v.vCross2(wr1, impulseDirection).vTransformBy3(body1.invInertiaWorld).vCross(wr1).vDot(impulseDirection); + vx = wr1.y*impulseDirection.z - wr1.z*impulseDirection.y; + vy = wr1.z*impulseDirection.x - wr1.x*impulseDirection.z; + vz = wr1.x*impulseDirection.y - wr1.y*impulseDirection.x; + + m = body1.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr1.z - z*wr1.y; + vy = z*wr1.x - x*wr1.z; + vz = x*wr1.y - y*wr1.x; + velByUnitImpulseN += body1.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + if (body2 != null && body2.movable) { +// velByUnitImpulseN += body2.invMass + _v.vCross2(wr2, impulseDirection).vTransformBy3(body2.invInertiaWorld).vCross(wr2).vDot(impulseDirection); + vx = wr2.y*impulseDirection.z - wr2.z*impulseDirection.y; + vy = wr2.z*impulseDirection.x - wr2.x*impulseDirection.z; + vz = wr2.x*impulseDirection.y - wr2.y*impulseDirection.x; + + m = body2.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr2.z - z*wr2.y; + vy = z*wr2.x - x*wr2.z; + vz = x*wr2.y - y*wr2.x; + velByUnitImpulseN += body2.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + } else { + satisfied = true; + } + } + + /** + * @param dt + */ + override public function apply(dt:Number):void { + if (satisfied) return; + // Расчитываем проекцию скорости относительной скорости + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 + var vel:Vector3 = body1.state.velocity; + var rot:Vector3 = body1.state.rotation; +// v1Cross2(state.rotation, wr1); + var vx:Number = vel.x + rot.y*wr1.z - rot.z*wr1.y; + var vy:Number = vel.y + rot.z*wr1.x - rot.x*wr1.z; + var vz:Number = vel.z + rot.x*wr1.y - rot.y*wr1.x; + // V2 = V2_c + w2%r2 + if (body2 != null) { + vel = body2.state.velocity; + rot = body2.state.rotation; +// _v2.vCross2(state.rotation, wr2); + vx -= vel.x + rot.y*wr2.z - rot.z*wr2.y; + vy -= vel.y + rot.z*wr2.x - rot.x*wr2.z; + vz -= vel.z + rot.x*wr2.y - rot.y*wr2.x; + } + var closingVel:Number = vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + if (closingVel > minClosingVel) return; + var impulse:Number = (minClosingVel - closingVel)/velByUnitImpulseN; + if (body1.movable) { + body1.applyRelPosWorldImpulse(wr1, impulseDirection, impulse); + } + if (body2 != null && body2.movable) { + body2.applyRelPosWorldImpulse(wr2, impulseDirection, -impulse); + } + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/constraints/Constraint.as b/0.0.8.0/src/alternativa/physics/constraints/Constraint.as new file mode 100644 index 0000000..4676d0d --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/constraints/Constraint.as @@ -0,0 +1,35 @@ +package alternativa.physics.constraints { + + import alternativa.physics.World; + import alternativa.physics.altphysics; + + use namespace altphysics; + + /** + * + */ + public class Constraint { + + altphysics var satisfied:Boolean; + altphysics var world:World; + + /** + * + */ + public function Constraint() { + } + + /** + * @param dt + */ + public function preProcess(dt:Number):void { + } + + /** + * @param dt + */ + public function apply(dt:Number):void { + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/constraints/MaxDistanceConstraint.as b/0.0.8.0/src/alternativa/physics/constraints/MaxDistanceConstraint.as new file mode 100644 index 0000000..c87e0a3 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/constraints/MaxDistanceConstraint.as @@ -0,0 +1,183 @@ +package alternativa.physics.constraints { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.math.Matrix3; + import alternativa.physics.math.Vector3; + + use namespace altphysics; + + /** + * + */ + public class MaxDistanceConstraint extends Constraint { + // Первое тело ограничения + altphysics var body1:Body; + // Второе тело ограничения. Может быть null. + altphysics var body2:Body; + // Первая точка крепления в системе координат первого тела + altphysics var r1:Vector3; + // Вторая точка привязки в системе координат второго тела. В случае отсутствия второго тела, координаты задаются в мировой системе. + altphysics var r2:Vector3; + // Максимально допустимое расстояние между точками крепления + altphysics var maxDistance:Number; + // Радиус-вектор первой точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr1:Vector3 = new Vector3(); + // Радиус-вектор второй точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr2:Vector3 = new Vector3(); + // Минимальная скорость сближения для удовлетворения ограничения + private var minClosingVel:Number; + // Изменение относительной скорости на единицу импульса + private var velByUnitImpulseN:Number; + // Направление корректирующего импульса + private var impulseDirection:Vector3 = new Vector3(); + + // Вспомогательные переменные +// private static var _v:Vector3 = new Vector3(); +// private static var _v1:Vector3 = new Vector3(); +// private static var _v2:Vector3 = new Vector3(); + + /** + * @param body1 + * @param body2 + * @param r1 + * @param r2 + * @param maxDistance + */ + public function MaxDistanceConstraint(body1:Body, body2:Body, r1:Vector3, r2:Vector3, maxDistance:Number) { + super(); + this.body1 = body1; + this.body2 = body2; + this.r1 = r1.vClone(); + this.r2 = r2.vClone(); + this.maxDistance = maxDistance; + } + + /** + * @param dt + */ + override public function preProcess(dt:Number):void { + // Вычислим расстояние между точками + var m:Matrix3 = body1.baseMatrix; + wr1.x = m.a*r1.x + m.b*r1.y + m.c*r1.z; + wr1.y = m.e*r1.x + m.f*r1.y + m.g*r1.z; + wr1.z = m.i*r1.x + m.j*r1.y + m.k*r1.z; + if (body2 != null) { + m = body2.baseMatrix; + wr2.x = m.a*r2.x + m.b*r2.y + m.c*r2.z; + wr2.y = m.e*r2.x + m.f*r2.y + m.g*r2.z; + wr2.z = m.i*r2.x + m.j*r2.y + m.k*r2.z; + } else { + wr2.x = r2.x; + wr2.y = r2.y; + wr2.z = r2.z; + } + var p1:Vector3 = body1.state.pos; + impulseDirection.x = wr2.x - wr1.x - p1.x; + impulseDirection.y = wr2.y - wr1.y - p1.y; + impulseDirection.z = wr2.z - wr1.z - p1.z; + if (body2 != null) { + var p2:Vector3 = body2.state.pos; + impulseDirection.x += p2.x; + impulseDirection.y += p2.y; + impulseDirection.z += p2.z; + } + + var len:Number = Math.sqrt(impulseDirection.x*impulseDirection.x + impulseDirection.y*impulseDirection.y + impulseDirection.z*impulseDirection.z); + var delta:Number = len - maxDistance; + if (delta > 0) { + satisfied = false; + if (len < 0.001) { + impulseDirection.x = 1; + } else { + len = 1/len; + impulseDirection.x *= len; + impulseDirection.y *= len; + impulseDirection.z *= len; + } + minClosingVel = delta/(world.penResolutionSteps*dt); + if (minClosingVel > world.maxPenResolutionSpeed) { + minClosingVel = world.maxPenResolutionSpeed; + } + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + var x:Number; + var y:Number; + var z:Number; + var vx:Number; + var vy:Number; + var vz:Number; + velByUnitImpulseN = 0; + if (body1.movable) { +// velByUnitImpulseN += body1.invMass + _v.vCross2(wr1, impulseDirection).vTransformBy3(body1.invInertiaWorld).vCross(wr1).vDot(impulseDirection); + vx = wr1.y*impulseDirection.z - wr1.z*impulseDirection.y; + vy = wr1.z*impulseDirection.x - wr1.x*impulseDirection.z; + vz = wr1.x*impulseDirection.y - wr1.y*impulseDirection.x; + + m = body1.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr1.z - z*wr1.y; + vy = z*wr1.x - x*wr1.z; + vz = x*wr1.y - y*wr1.x; + velByUnitImpulseN += body1.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + if (body2 != null && body2.movable) { +// velByUnitImpulseN += body2.invMass + _v.vCross2(wr2, impulseDirection).vTransformBy3(body2.invInertiaWorld).vCross(wr2).vDot(impulseDirection); + vx = wr2.y*impulseDirection.z - wr2.z*impulseDirection.y; + vy = wr2.z*impulseDirection.x - wr2.x*impulseDirection.z; + vz = wr2.x*impulseDirection.y - wr2.y*impulseDirection.x; + + m = body2.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr2.z - z*wr2.y; + vy = z*wr2.x - x*wr2.z; + vz = x*wr2.y - y*wr2.x; + velByUnitImpulseN += body2.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + } else { + satisfied = true; + } + } + + /** + * @param dt + */ + override public function apply(dt:Number):void { + if (satisfied) return; + // Расчитываем проекцию скорости относительной скорости + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 + var vel:Vector3 = body1.state.velocity; + var rot:Vector3 = body1.state.rotation; +// v1Cross2(state.rotation, wr1); + var vx:Number = vel.x + rot.y*wr1.z - rot.z*wr1.y; + var vy:Number = vel.y + rot.z*wr1.x - rot.x*wr1.z; + var vz:Number = vel.z + rot.x*wr1.y - rot.y*wr1.x; + // V2 = V2_c + w2%r2 + if (body2 != null) { + vel = body2.state.velocity; + rot = body2.state.rotation; +// _v2.vCross2(state.rotation, wr2); + vx -= vel.x + rot.y*wr2.z - rot.z*wr2.y; + vy -= vel.y + rot.z*wr2.x - rot.x*wr2.z; + vz -= vel.z + rot.x*wr2.y - rot.y*wr2.x; + } + var closingVel:Number = vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + if (closingVel > minClosingVel) return; + var impulse:Number = (minClosingVel - closingVel)/velByUnitImpulseN; + if (body1.movable) { + body1.applyRelPosWorldImpulse(wr1, impulseDirection, impulse); + } + if (body2 != null && body2.movable) { + body2.applyRelPosWorldImpulse(wr2, impulseDirection, -impulse); + } + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/math/.svn/all-wcprops b/0.0.8.0/src/alternativa/physics/math/.svn/all-wcprops new file mode 100644 index 0000000..25bba2f --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/math/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 108 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/math +END +Quaternion.as +K 25 +svn:wc:ra_dav:version-url +V 122 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/math/Quaternion.as +END +Vector3.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/math/Vector3.as +END +Matrix3.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/math/Matrix3.as +END +Matrix4.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/math/Matrix4.as +END diff --git a/0.0.8.0/src/alternativa/physics/math/.svn/entries b/0.0.8.0/src/alternativa/physics/math/.svn/entries new file mode 100644 index 0000000..a054205 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/math/.svn/entries @@ -0,0 +1,76 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/math +http://svndev.alternativaplatform.com + + + +2009-11-01T12:35:52.270571Z +22955 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Quaternion.as +file + + + + +2010-10-28T04:32:42.000000Z +541207ae104f151fb3370fe92c0f7b67 +2009-11-01T12:35:52.270571Z +22955 +mike + +Vector3.as +file + + + + +2010-10-28T04:32:42.000000Z +72a5b87dac5c32096c716639488b92e2 +2009-10-19T07:17:33.113306Z +22253 +mike + +Matrix3.as +file + + + + +2010-10-28T04:32:42.000000Z +c99517d0c388915e157464d4b31a0b86 +2009-10-19T07:17:33.113306Z +22253 +mike + +Matrix4.as +file + + + + +2010-10-28T04:32:42.000000Z +3f37b8a6a7e9f111a35e6ee1fe63dfbc +2009-10-19T07:17:33.113306Z +22253 +mike + diff --git a/0.0.8.0/src/alternativa/physics/math/.svn/format b/0.0.8.0/src/alternativa/physics/math/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/math/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.8.0/src/alternativa/physics/math/.svn/text-base/Matrix3.as.svn-base b/0.0.8.0/src/alternativa/physics/math/.svn/text-base/Matrix3.as.svn-base new file mode 100644 index 0000000..0b4dcd0 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/math/.svn/text-base/Matrix3.as.svn-base @@ -0,0 +1,391 @@ +package alternativa.physics.math { + import flash.geom.Vector3D; + + + /** + * + */ + public class Matrix3 { + + public static const ZERO:Matrix3 = new Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0); + public static const IDENTITY:Matrix3 = new Matrix3(); + + public var a:Number; + public var b:Number; + public var c:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix3(a:Number = 1, b:Number = 0, c:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1) { + this.a = a; + this.b = b; + this.c = c; + + this.e = e; + this.f = f; + this.g = g; + + this.i = i; + this.j = j; + this.k = k; + } + + /** + * + * @return + */ + public function toIdentity():Matrix3 { + a = f = k = 1; + b = c = e = g = i = j = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + var det:Number = 1/(-cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk); + + a = (ff*kk - gg*jj)*det; + b = (cc*jj - bb*kk)*det; + c = (bb*gg - cc*ff)*det; + e = (gg*ii - ee*kk)*det; + f = (aa*kk - cc*ii)*det; + g = (cc*ee - aa*gg)*det; + i = (ee*jj - ff*ii)*det; + j = (bb*ii - aa*jj)*det; + k = (aa*ff - bb*ee)*det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prependTransposed(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.b + cc*m.c; + b = aa*m.e + bb*m.f + cc*m.g; + c = aa*m.i + bb*m.j + cc*m.k; + e = ee*m.a + ff*m.b + gg*m.c; + f = ee*m.e + ff*m.f + gg*m.g; + g = ee*m.i + ff*m.j + gg*m.k; + i = ii*m.a + jj*m.b + kk*m.c; + j = ii*m.e + jj*m.f + kk*m.g; + k = ii*m.i + jj*m.j + kk*m.k; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix3):Matrix3 { + a += m.a; + b += m.b; + c += m.c; + e += m.e; + f += m.f; + g += m.g; + i += m.i; + j += m.j; + k += m.k; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix3):Matrix3 { + a -= m.a; + b -= m.b; + c -= m.c; + e -= m.e; + f -= m.f; + g -= m.g; + i -= m.i; + j -= m.j; + k -= m.k; + + return this; + } + + /** + * + * @return + */ + public function transpose():Matrix3 { + var tmp:Number = b; + b = e; + e = tmp; + tmp = c; + c = i; + i = tmp; + tmp = g; + g = j; + j = tmp; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector3To3D(vin:Vector3, vout:Vector3D):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * + * @param matrix + */ + public function createSkewSymmetric(v:Vector3):Matrix3 { + a = f = k = 0; + b = -v.z; + c = v.y; + e = v.z; + g = -v.x; + i = -v.y; + j = v.x; + return this; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix3):Matrix3 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @return + */ + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):Matrix3 { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + + return this; + } + + /** + * Метод формирует матрицу поворота на заданный угол относительно заданной оси. + * + * @param axis нормализованный вектор, задающий ось, относительно которой выполняется поворот + * @param angle угол поворота в радианах + */ + public function fromAxisAngle(axis:Vector3, angle:Number):void { + var c1:Number = Math.cos(angle); + var s:Number = Math.sin(angle); + var t:Number = 1 - c1; + var x:Number = axis.x; + var y:Number = axis.y; + var z:Number = axis.z; + + a = t*x*x + c1; + b = t*x*y - z*s; + c = t*x*z + y*s; + + e = t*x*y + z*s; + f = t*y*y + c1; + g = t*y*z - x*s; + + i = t*x*z - y*s; + j = t*y*z + x*s; + k = t*z*z + c1; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix3 { + return new Matrix3(a, b, c, e, f, g, i, j, k); + } + + /** + * + * @return + */ + public function toString():String { + return "[Matrix3 (" + a + ", " + b + ", " + c + "), (" + e + ", " + f + ", " + g + "), (" + i + ", " + j + ", " + k + ")]"; + } + + /** + * @param angles + */ + public function getEulerAngles(angles:Vector3):void { + if (-1 < i && i < 1) { + angles.x = Math.atan2(j, k); + angles.y = -Math.asin(i); + angles.z = Math.atan2(e, a); + } else { + angles.x = 0; + angles.y = (i <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-b, f); + } + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/math/.svn/text-base/Matrix4.as.svn-base b/0.0.8.0/src/alternativa/physics/math/.svn/text-base/Matrix4.as.svn-base new file mode 100644 index 0000000..939d46a --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/math/.svn/text-base/Matrix4.as.svn-base @@ -0,0 +1,461 @@ +package alternativa.physics.math { + import __AS3__.vec.Vector; + + /** + * + */ + public class Matrix4 { + + public static const IDENTITY:Matrix4 = new Matrix4(); + + public var a:Number; + public var b:Number; + public var c:Number; + public var d:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + public var h:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + public var l:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix4(a:Number = 1, b:Number = 0, c:Number = 0, d:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, h:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1, l:Number = 0) { + this.a = a; + this.b = b; + this.c = c; + this.d = d; + + this.e = e; + this.f = f; + this.g = g; + this.h = h; + + this.i = i; + this.j = j; + this.k = k; + this.l = l; + } + + /** + * + * @return + */ + public function toIdentity():Matrix4 { + a = f = k = 1; + b = c = e = g = i = j = d = h = l = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + var det:Number = -cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk; + + a = (-gg*jj + ff*kk)/det; + b = (cc*jj - bb*kk)/det; + c = (-cc*ff + bb*gg)/det; + d = (dd*gg*jj - cc*hh*jj - dd*ff*kk + bb*hh*kk + cc*ff*ll - bb*gg*ll)/det; + e = (gg*ii - ee*kk)/det; + f = (-cc*ii + aa*kk)/det; + g = (cc*ee - aa*gg)/det; + h = (cc*hh*ii - dd*gg*ii + dd*ee*kk - aa*hh*kk - cc*ee*ll + aa*gg*ll)/det; + i = (-ff*ii + ee*jj)/det; + j = (bb*ii - aa*jj)/det; + k = (-bb*ee + aa*ff)/det; + l = (dd*ff*ii - bb*hh*ii - dd*ee*jj + aa*hh*jj + bb*ee*ll - aa*ff*ll)/det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + d = m.a*dd + m.b*hh + m.c*ll + m.d; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + h = m.e*dd + m.f*hh + m.g*ll + m.h; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + l = m.i*dd + m.j*hh + m.k*ll + m.l; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + d = aa*m.d + bb*m.h + cc*m.l + dd; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + h = ee*m.d + ff*m.h + gg*m.l + hh; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + l = ii*m.d + jj*m.h + kk*m.l + ll; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix4):Matrix4 { + a += m.a; + b += m.b; + c += m.c; + d += m.d; + e += m.e; + f += m.f; + g += m.g; + h += m.h; + i += m.i; + j += m.j; + k += m.k; + l += m.l; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix4):Matrix4 { + a -= m.a; + b -= m.b; + c -= m.c; + d -= m.d; + e -= m.e; + f -= m.f; + g -= m.g; + h -= m.h; + i -= m.i; + j -= m.j; + k -= m.k; + l -= m.l; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectors(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + * @param len + */ + public function transformVectorsN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverse(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverseN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(idx:int, axis:Vector3):void { + switch (idx) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + case 3: + axis.x = d; + axis.y = h; + axis.z = l; + return; + } + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix4):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = m.d; + e = m.e; + f = m.f; + g = m.g; + h = m.h; + i = m.i; + j = m.j; + k = m.k; + l = m.l; + + return this; + } + + public function setFromMatrix3(m:Matrix3, offset:Vector3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = offset.x; + e = m.e; + f = m.f; + g = m.g; + h = offset.y; + i = m.i; + j = m.j; + k = m.k; + l = offset.z; + + return this; + } + + public function setOrientationFromMatrix3(m:Matrix3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @param angles + */ + public function getEulerAngles(angles:Vector3):void { + if (-1 < i && i < 1) { + angles.x = Math.atan2(j, k); + angles.y = -Math.asin(i); + angles.z = Math.atan2(e, a); + } else { + angles.x = 0; + angles.y = (i <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-b, f); + } + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix4 { + return new Matrix4(a, b, c, d, e, f, g, h, i, j, k, l); + } + + public function toString():String { + return "[Matrix4 [" + a.toFixed(3) + " " + b.toFixed(3) + " " + c.toFixed(3) + " " + d.toFixed(3) + "] [" + e.toFixed(3) + " " + f.toFixed(3) + " " + g.toFixed(3) + " " + h.toFixed(3) + "] [" + i.toFixed(3) + " " + j.toFixed(3) + " " + k.toFixed(3) + " " + l.toFixed(3) + "]]"; + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/math/.svn/text-base/Quaternion.as.svn-base b/0.0.8.0/src/alternativa/physics/math/.svn/text-base/Quaternion.as.svn-base new file mode 100644 index 0000000..c37ff51 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/math/.svn/text-base/Quaternion.as.svn-base @@ -0,0 +1,417 @@ +package alternativa.physics.math { + import flash.geom.Vector3D; + + + public class Quaternion { + + public var w:Number; + public var x:Number; + public var y:Number; + public var z:Number; + + public static function multiply(q1:Quaternion, q2:Quaternion, result:Quaternion):void { + result.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z; + result.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y; + result.y = q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z; + result.z = q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x; + } + + public static function createFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngle(axis, angle); + return q; + } + + public static function createFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngleComponents(x, y, z, angle); + return q; + } + + private static var _q:Quaternion = new Quaternion(); + + public function Quaternion(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0) { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + } + + public function reset(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0):Quaternion { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + return this; + } + + public function normalize():Quaternion { + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * Умножает на указанный кватернион слева: this * q + * + * @param q множитель + */ + public function prepend(q:Quaternion):Quaternion { + var ww:Number = w*q.w - x*q.x - y*q.y - z*q.z; + var xx:Number = w*q.x + x*q.w + y*q.z - z*q.y; + var yy:Number = w*q.y + y*q.w + z*q.x - x*q.z; + var zz:Number = w*q.z + z*q.w + x*q.y - y*q.x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Умножает на указанный кватернион справа: q * this + * + * @param q множитель + */ + public function append(q:Quaternion):Quaternion { + var ww:Number = q.w*w - q.x*x - q.y*y - q.z*z; + var xx:Number = q.w*x + q.x*w + q.y*z - q.z*y; + var yy:Number = q.w*y + q.y*w + q.z*x - q.x*z; + var zz:Number = q.w*z + q.z*w + q.x*y - q.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * + * @param vector + */ + public function rotateByVector(v:Vector3):Quaternion { + var ww:Number = -v.x*x - v.y*y - v.z*z; + var xx:Number = v.x*w + v.y*z - v.z*y; + var yy:Number = v.y*w + v.z*x - v.x*z; + var zz:Number = v.z*w + v.x*y - v.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Добавляет вращение, приданное вектором угловой скорости за указанное время. + * + * @param v + * @param scale + */ + public function addScaledVector(v:Vector3, scale:Number):Quaternion { + var vx:Number = v.x*scale; + var vy:Number = v.y*scale; + var vz:Number = v.z*scale; + var ww:Number = -x*vx - y*vy - z*vz; + var xx:Number = vx*w + vy*z - vz*y; + var yy:Number = vy*w + vz*x - vx*z; + var zz:Number = vz*w + vx*y - vy*x; + w += 0.5*ww; + x += 0.5*xx; + y += 0.5*yy; + z += 0.5*zz; + // inlined normalize + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param m + * @return + */ + public function toMatrix3(m:Matrix3):Quaternion { + var xx2:Number = 2.0*x*x; + var yy2:Number = 2.0*y*y; + var zz2:Number = 2.0*z*z; + var xy2:Number = 2.0*x*y; + var yz2:Number = 2.0*y*z; + var zx2:Number = 2.0*z*x; + var wx2:Number = 2.0*w*x; + var wy2:Number = 2.0*w*y; + var wz2:Number = 2.0*w*z; + + m.a = 1.0 - yy2 - zz2; + m.b = xy2 - wz2; + m.c = zx2 + wy2; + + m.e = xy2 + wz2; + m.f = 1.0 - xx2 - zz2; + m.g = yz2 - wx2; + + m.i = zx2 - wy2; + m.j = yz2 + wx2; + m.k = 1.0 - xx2 - yy2; + return this; + } + + /** + * + * @return + */ + public function length():Number { + return Math.sqrt(w*w + x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function lengthSqr():Number { + return w*w + x*x + y*y + z*z; + } + + /** + * + * @param axis + * @param angle + * @return + */ + public function setFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z); + x = axis.x*k; + y = axis.y*k; + z = axis.z*k; + return this; + } + + /** + * + * @param x + * @param y + * @param z + * @param angle + */ + public function setFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(x*x + y*y + z*z); + this.x = x*k; + this.y = y*k; + this.z = z*k; + return this; + } + + /** + * + * @param vector + */ + public function toAxisVector(v:Vector3 = null):Vector3 { + if (w < -1 || w > 1) { + normalize(); + } + if (v == null) { + v = new Vector3(); + } + if (w > -1 && w < 1) { + if (w == 0) { + v.x = x; + v.y = y; + v.z = z; + } else { + var angle:Number = 2*Math.acos(w); + var coeff:Number = 1/Math.sqrt(1 - w*w); + v.x = x*coeff*angle; + v.y = y*coeff*angle; + v.z = z*coeff*angle; + } + } else { + v.x = 0; + v.y = 0; + v.z = 0; + } + return v; + } + + /** + * + * @param rotations + */ + public function getEulerAngles(angles:Vector3):Vector3 { + var qi2:Number = 2*x*x; + var qj2:Number = 2*y*y; + var qk2:Number = 2*z*z; + var qij:Number = 2*x*y; + var qjk:Number = 2*y*z; + var qki:Number = 2*z*x; + var qri:Number = 2*w*x; + var qrj:Number = 2*w*y; + var qrk:Number = 2*w*z; + + var aa:Number = 1 - qj2 - qk2; + var bb:Number = qij - qrk; + var ee:Number = qij + qrk; + var ff:Number = 1 - qi2 - qk2; + var ii:Number = qki - qrj; + var jj:Number = qjk + qri; + var kk:Number = 1 - qi2 - qj2; + + if (-1 < ii && ii < 1) { + if (angles == null) { + angles = new Vector3(Math.atan2(jj, kk), -Math.asin(ii), Math.atan2(ee, aa)); + } else { + angles.x = Math.atan2(jj, kk); + angles.y = -Math.asin(ii); + angles.z = Math.atan2(ee, aa); + } + } else { + if (angles == null) { + angles = new Vector3(0, 0.5*((ii <= -1) ? Math.PI : -Math.PI), Math.atan2(-bb, ff)); + } else { + angles.x = 0; + angles.y = (ii <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-bb, ff); + } + } + return angles; + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setFromEulerAnglesXYZ(x:Number, y:Number, z:Number):void { + setFromAxisAngleComponents(1, 0, 0, x); + + _q.setFromAxisAngleComponents(0, 1, 0, y); + append(_q); + normalize(); + + _q.setFromAxisAngleComponents(0, 0, 1, z); + append(_q); + normalize(); + } + + /** + * + */ + public function conjugate():void { + x = -x; + y = -y; + z = -z; + } + + /** + * Выполняет линейную интерполяцию. + * + * @param q1 начало отрезка + * @param q2 конец отрезка + * @param t время, обычно задаётся в интервале [0, 1] + */ + public function nlerp(q1:Quaternion, q2:Quaternion, t:Number):Quaternion { + var d:Number = 1 - t; + w = q1.w*d + q2.w*t; + x = q1.x*d + q2.x*t; + y = q1.y*d + q2.y*t; + z = q1.z*d + q2.z*t; + // inlined normalize + d = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param q + * @return + */ + public function subtract(q:Quaternion):Quaternion { + w -= q.w; + x -= q.x; + y -= q.y; + z -= q.z; + return this; + } + + /** + * + * @param q1 + * @param q2 + * @return + */ + public function diff(q1:Quaternion, q2:Quaternion):Quaternion { + w = q2.w - q1.w; + x = q2.x - q1.x; + y = q2.y - q1.y; + z = q2.z - q1.z; + return this; + } + + /** + * + * @param q + * @return + */ + public function copy(q:Quaternion):Quaternion { + w = q.w; + x = q.x; + y = q.y; + z = q.z; + return this; + } + + /** + * @param reslult + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + result.w = w; + return result; + } + + /** + * + * @return + */ + public function clone():Quaternion { + return new Quaternion(w, x, y, z); + } + + /** + * + * @return + */ + public function toString():String { + return "[" + w + ", " + x + ", " + y + ", " + z + "]"; + } + } +} diff --git a/0.0.8.0/src/alternativa/physics/math/.svn/text-base/Vector3.as.svn-base b/0.0.8.0/src/alternativa/physics/math/.svn/text-base/Vector3.as.svn-base new file mode 100644 index 0000000..232e499 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/math/.svn/text-base/Vector3.as.svn-base @@ -0,0 +1,335 @@ +package alternativa.physics.math { + import flash.geom.Vector3D; + + + /** + * + */ + public class Vector3 extends Vector3D { + + public static const ZERO:Vector3 = new Vector3(0, 0, 0); + public static const X_AXIS:Vector3 = new Vector3(1, 0, 0); + public static const Y_AXIS:Vector3 = new Vector3(0, 1, 0); + public static const Z_AXIS:Vector3 = new Vector3(0, 0, 1); + + /** + * + * @param x + * @param y + * @param z + */ + public function Vector3(x:Number = 0, y:Number = 0, z:Number = 0) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * + * @return + */ + public function vLength():Number { + return Math.sqrt(x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function vLengthSqr():Number { + return x*x + y*y + z*z; + } + + /** + * + * @param length + * @return + */ + public function vSetLength(length:Number):Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = length; + } else { + var k:Number = length/Math.sqrt(x*x + y*y + z*z); + x *= k; + y *= k; + z *= k; + } + return this; + } + + /** + * + * @return + */ + public function vNormalize():Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = 1; + } else { + d = Math.sqrt(d); + x /= d; + y /= d; + z /= d; + } + return this; + } + + /** + * Сложение координат. + * + * @param v точка, координаты которой прибавляются к собственным + */ + public function vAdd(v:Vector3):Vector3 { + x += v.x; + y += v.y; + z += v.z; + return this; + } + + /** + * + * @param k + * @param v + * @return + */ + public function vAddScaled(k:Number, v:Vector3):Vector3 { + x += k*v.x; + y += k*v.y; + z += k*v.z; + return this; + } + + /** + * Вычитание координат. + * + * @param v точка, координаты которой вычитаются из собственных + */ + public function vSubtract(v:Vector3):Vector3 { + x -= v.x; + y -= v.y; + z -= v.z; + return this; + } + + /** + * Вычисляет сумму векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + * @return this + */ + public function vSum(a:Vector3, b:Vector3):Vector3 { + x = a.x + b.x; + y = a.y + b.y; + z = a.z + b.z; + return this; + } + + /** + * Вычисление разности векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + */ + public function vDiff(a:Vector3, b:Vector3):Vector3 { + x = a.x - b.x; + y = a.y - b.y; + z = a.z - b.z; + return this; + } + + /** + * Умножение на скаляр. + * + * @param k число, на которое умножаются координаты + */ + public function vScale(k:Number):Vector3 { + x *= k; + y *= k; + z *= k; + return this; + } + + /** + * Инвертирование вектора. + */ + public function vReverse():Vector3 { + x = -x; + y = -y; + z = -z; + return this; + } + + /** + * + * @param v + * @return + */ + public function vDot(v:Vector3):Number { + return x*v.x + y*v.y + z*v.z; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross(v:Vector3):Vector3 { + var xx:Number = y*v.z - z*v.y; + var yy:Number = z*v.x - x*v.z; + var zz:Number = x*v.y - y*v.x; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross2(a:Vector3, b:Vector3):Vector3 { + x = a.y*b.z - a.z*b.y; + y = a.z*b.x - a.x*b.z; + z = a.x*b.y - a.y*b.x; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy3(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * + * @param matrix + */ + public function vTransformBy3Tr(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz + m.d; + y = m.e*xx + m.f*yy + m.g*zz + m.h; + z = m.i*xx + m.j*yy + m.k*zz + m.l; + return this; + } + + /** + * + * @param m + * @return + */ + public function vTransformInverseBy4(m:Matrix4):Vector3 { + var xx:Number = x - m.d; + var yy:Number = y - m.h; + var zz:Number = z - m.l; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * + * @param m + */ + public function vDeltaTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * Установка координат. + */ + public function vReset(x:Number = 0, y:Number = 0, z:Number = 0):Vector3 { + this.x = x; + this.y = y; + this.z = z; + return this; + } + + /** + * Копирование координат точки. + * + * @param v точка, координаты которой копируются + */ + public function vCopy(v:Vector3):Vector3 { + x = v.x; + y = v.y; + z = v.z; + return this; + } + + /** + * Клонирование точки. + * + * @return клонированная точка + */ + public function vClone():Vector3 { + return new Vector3(x, y, z); + } + + /** + * Копирует координаты вектора в экземпляр класса Vector3D. + * + * @param result экземпляр, в который копируются координаты + * @return переданный параметр + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + return result; + } + + /** + * @param source + * @return + */ + public function copyFromVector3D(source:Vector3D):Vector3 { + x = source.x; + y = source.y; + z = source.z; + return this; + } + + override public function toString():String { + return "Vector3(" + x + "," + y + ", " + z + ")"; + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/math/Matrix3.as b/0.0.8.0/src/alternativa/physics/math/Matrix3.as new file mode 100644 index 0000000..0b4dcd0 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/math/Matrix3.as @@ -0,0 +1,391 @@ +package alternativa.physics.math { + import flash.geom.Vector3D; + + + /** + * + */ + public class Matrix3 { + + public static const ZERO:Matrix3 = new Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0); + public static const IDENTITY:Matrix3 = new Matrix3(); + + public var a:Number; + public var b:Number; + public var c:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix3(a:Number = 1, b:Number = 0, c:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1) { + this.a = a; + this.b = b; + this.c = c; + + this.e = e; + this.f = f; + this.g = g; + + this.i = i; + this.j = j; + this.k = k; + } + + /** + * + * @return + */ + public function toIdentity():Matrix3 { + a = f = k = 1; + b = c = e = g = i = j = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + var det:Number = 1/(-cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk); + + a = (ff*kk - gg*jj)*det; + b = (cc*jj - bb*kk)*det; + c = (bb*gg - cc*ff)*det; + e = (gg*ii - ee*kk)*det; + f = (aa*kk - cc*ii)*det; + g = (cc*ee - aa*gg)*det; + i = (ee*jj - ff*ii)*det; + j = (bb*ii - aa*jj)*det; + k = (aa*ff - bb*ee)*det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prependTransposed(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.b + cc*m.c; + b = aa*m.e + bb*m.f + cc*m.g; + c = aa*m.i + bb*m.j + cc*m.k; + e = ee*m.a + ff*m.b + gg*m.c; + f = ee*m.e + ff*m.f + gg*m.g; + g = ee*m.i + ff*m.j + gg*m.k; + i = ii*m.a + jj*m.b + kk*m.c; + j = ii*m.e + jj*m.f + kk*m.g; + k = ii*m.i + jj*m.j + kk*m.k; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix3):Matrix3 { + a += m.a; + b += m.b; + c += m.c; + e += m.e; + f += m.f; + g += m.g; + i += m.i; + j += m.j; + k += m.k; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix3):Matrix3 { + a -= m.a; + b -= m.b; + c -= m.c; + e -= m.e; + f -= m.f; + g -= m.g; + i -= m.i; + j -= m.j; + k -= m.k; + + return this; + } + + /** + * + * @return + */ + public function transpose():Matrix3 { + var tmp:Number = b; + b = e; + e = tmp; + tmp = c; + c = i; + i = tmp; + tmp = g; + g = j; + j = tmp; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector3To3D(vin:Vector3, vout:Vector3D):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * + * @param matrix + */ + public function createSkewSymmetric(v:Vector3):Matrix3 { + a = f = k = 0; + b = -v.z; + c = v.y; + e = v.z; + g = -v.x; + i = -v.y; + j = v.x; + return this; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix3):Matrix3 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @return + */ + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):Matrix3 { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + + return this; + } + + /** + * Метод формирует матрицу поворота на заданный угол относительно заданной оси. + * + * @param axis нормализованный вектор, задающий ось, относительно которой выполняется поворот + * @param angle угол поворота в радианах + */ + public function fromAxisAngle(axis:Vector3, angle:Number):void { + var c1:Number = Math.cos(angle); + var s:Number = Math.sin(angle); + var t:Number = 1 - c1; + var x:Number = axis.x; + var y:Number = axis.y; + var z:Number = axis.z; + + a = t*x*x + c1; + b = t*x*y - z*s; + c = t*x*z + y*s; + + e = t*x*y + z*s; + f = t*y*y + c1; + g = t*y*z - x*s; + + i = t*x*z - y*s; + j = t*y*z + x*s; + k = t*z*z + c1; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix3 { + return new Matrix3(a, b, c, e, f, g, i, j, k); + } + + /** + * + * @return + */ + public function toString():String { + return "[Matrix3 (" + a + ", " + b + ", " + c + "), (" + e + ", " + f + ", " + g + "), (" + i + ", " + j + ", " + k + ")]"; + } + + /** + * @param angles + */ + public function getEulerAngles(angles:Vector3):void { + if (-1 < i && i < 1) { + angles.x = Math.atan2(j, k); + angles.y = -Math.asin(i); + angles.z = Math.atan2(e, a); + } else { + angles.x = 0; + angles.y = (i <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-b, f); + } + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/math/Matrix4.as b/0.0.8.0/src/alternativa/physics/math/Matrix4.as new file mode 100644 index 0000000..939d46a --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/math/Matrix4.as @@ -0,0 +1,461 @@ +package alternativa.physics.math { + import __AS3__.vec.Vector; + + /** + * + */ + public class Matrix4 { + + public static const IDENTITY:Matrix4 = new Matrix4(); + + public var a:Number; + public var b:Number; + public var c:Number; + public var d:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + public var h:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + public var l:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix4(a:Number = 1, b:Number = 0, c:Number = 0, d:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, h:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1, l:Number = 0) { + this.a = a; + this.b = b; + this.c = c; + this.d = d; + + this.e = e; + this.f = f; + this.g = g; + this.h = h; + + this.i = i; + this.j = j; + this.k = k; + this.l = l; + } + + /** + * + * @return + */ + public function toIdentity():Matrix4 { + a = f = k = 1; + b = c = e = g = i = j = d = h = l = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + var det:Number = -cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk; + + a = (-gg*jj + ff*kk)/det; + b = (cc*jj - bb*kk)/det; + c = (-cc*ff + bb*gg)/det; + d = (dd*gg*jj - cc*hh*jj - dd*ff*kk + bb*hh*kk + cc*ff*ll - bb*gg*ll)/det; + e = (gg*ii - ee*kk)/det; + f = (-cc*ii + aa*kk)/det; + g = (cc*ee - aa*gg)/det; + h = (cc*hh*ii - dd*gg*ii + dd*ee*kk - aa*hh*kk - cc*ee*ll + aa*gg*ll)/det; + i = (-ff*ii + ee*jj)/det; + j = (bb*ii - aa*jj)/det; + k = (-bb*ee + aa*ff)/det; + l = (dd*ff*ii - bb*hh*ii - dd*ee*jj + aa*hh*jj + bb*ee*ll - aa*ff*ll)/det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + d = m.a*dd + m.b*hh + m.c*ll + m.d; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + h = m.e*dd + m.f*hh + m.g*ll + m.h; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + l = m.i*dd + m.j*hh + m.k*ll + m.l; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + d = aa*m.d + bb*m.h + cc*m.l + dd; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + h = ee*m.d + ff*m.h + gg*m.l + hh; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + l = ii*m.d + jj*m.h + kk*m.l + ll; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix4):Matrix4 { + a += m.a; + b += m.b; + c += m.c; + d += m.d; + e += m.e; + f += m.f; + g += m.g; + h += m.h; + i += m.i; + j += m.j; + k += m.k; + l += m.l; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix4):Matrix4 { + a -= m.a; + b -= m.b; + c -= m.c; + d -= m.d; + e -= m.e; + f -= m.f; + g -= m.g; + h -= m.h; + i -= m.i; + j -= m.j; + k -= m.k; + l -= m.l; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectors(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + * @param len + */ + public function transformVectorsN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverse(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverseN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(idx:int, axis:Vector3):void { + switch (idx) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + case 3: + axis.x = d; + axis.y = h; + axis.z = l; + return; + } + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix4):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = m.d; + e = m.e; + f = m.f; + g = m.g; + h = m.h; + i = m.i; + j = m.j; + k = m.k; + l = m.l; + + return this; + } + + public function setFromMatrix3(m:Matrix3, offset:Vector3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = offset.x; + e = m.e; + f = m.f; + g = m.g; + h = offset.y; + i = m.i; + j = m.j; + k = m.k; + l = offset.z; + + return this; + } + + public function setOrientationFromMatrix3(m:Matrix3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @param angles + */ + public function getEulerAngles(angles:Vector3):void { + if (-1 < i && i < 1) { + angles.x = Math.atan2(j, k); + angles.y = -Math.asin(i); + angles.z = Math.atan2(e, a); + } else { + angles.x = 0; + angles.y = (i <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-b, f); + } + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix4 { + return new Matrix4(a, b, c, d, e, f, g, h, i, j, k, l); + } + + public function toString():String { + return "[Matrix4 [" + a.toFixed(3) + " " + b.toFixed(3) + " " + c.toFixed(3) + " " + d.toFixed(3) + "] [" + e.toFixed(3) + " " + f.toFixed(3) + " " + g.toFixed(3) + " " + h.toFixed(3) + "] [" + i.toFixed(3) + " " + j.toFixed(3) + " " + k.toFixed(3) + " " + l.toFixed(3) + "]]"; + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/math/Quaternion.as b/0.0.8.0/src/alternativa/physics/math/Quaternion.as new file mode 100644 index 0000000..c37ff51 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/math/Quaternion.as @@ -0,0 +1,417 @@ +package alternativa.physics.math { + import flash.geom.Vector3D; + + + public class Quaternion { + + public var w:Number; + public var x:Number; + public var y:Number; + public var z:Number; + + public static function multiply(q1:Quaternion, q2:Quaternion, result:Quaternion):void { + result.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z; + result.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y; + result.y = q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z; + result.z = q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x; + } + + public static function createFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngle(axis, angle); + return q; + } + + public static function createFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngleComponents(x, y, z, angle); + return q; + } + + private static var _q:Quaternion = new Quaternion(); + + public function Quaternion(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0) { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + } + + public function reset(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0):Quaternion { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + return this; + } + + public function normalize():Quaternion { + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * Умножает на указанный кватернион слева: this * q + * + * @param q множитель + */ + public function prepend(q:Quaternion):Quaternion { + var ww:Number = w*q.w - x*q.x - y*q.y - z*q.z; + var xx:Number = w*q.x + x*q.w + y*q.z - z*q.y; + var yy:Number = w*q.y + y*q.w + z*q.x - x*q.z; + var zz:Number = w*q.z + z*q.w + x*q.y - y*q.x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Умножает на указанный кватернион справа: q * this + * + * @param q множитель + */ + public function append(q:Quaternion):Quaternion { + var ww:Number = q.w*w - q.x*x - q.y*y - q.z*z; + var xx:Number = q.w*x + q.x*w + q.y*z - q.z*y; + var yy:Number = q.w*y + q.y*w + q.z*x - q.x*z; + var zz:Number = q.w*z + q.z*w + q.x*y - q.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * + * @param vector + */ + public function rotateByVector(v:Vector3):Quaternion { + var ww:Number = -v.x*x - v.y*y - v.z*z; + var xx:Number = v.x*w + v.y*z - v.z*y; + var yy:Number = v.y*w + v.z*x - v.x*z; + var zz:Number = v.z*w + v.x*y - v.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Добавляет вращение, приданное вектором угловой скорости за указанное время. + * + * @param v + * @param scale + */ + public function addScaledVector(v:Vector3, scale:Number):Quaternion { + var vx:Number = v.x*scale; + var vy:Number = v.y*scale; + var vz:Number = v.z*scale; + var ww:Number = -x*vx - y*vy - z*vz; + var xx:Number = vx*w + vy*z - vz*y; + var yy:Number = vy*w + vz*x - vx*z; + var zz:Number = vz*w + vx*y - vy*x; + w += 0.5*ww; + x += 0.5*xx; + y += 0.5*yy; + z += 0.5*zz; + // inlined normalize + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param m + * @return + */ + public function toMatrix3(m:Matrix3):Quaternion { + var xx2:Number = 2.0*x*x; + var yy2:Number = 2.0*y*y; + var zz2:Number = 2.0*z*z; + var xy2:Number = 2.0*x*y; + var yz2:Number = 2.0*y*z; + var zx2:Number = 2.0*z*x; + var wx2:Number = 2.0*w*x; + var wy2:Number = 2.0*w*y; + var wz2:Number = 2.0*w*z; + + m.a = 1.0 - yy2 - zz2; + m.b = xy2 - wz2; + m.c = zx2 + wy2; + + m.e = xy2 + wz2; + m.f = 1.0 - xx2 - zz2; + m.g = yz2 - wx2; + + m.i = zx2 - wy2; + m.j = yz2 + wx2; + m.k = 1.0 - xx2 - yy2; + return this; + } + + /** + * + * @return + */ + public function length():Number { + return Math.sqrt(w*w + x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function lengthSqr():Number { + return w*w + x*x + y*y + z*z; + } + + /** + * + * @param axis + * @param angle + * @return + */ + public function setFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z); + x = axis.x*k; + y = axis.y*k; + z = axis.z*k; + return this; + } + + /** + * + * @param x + * @param y + * @param z + * @param angle + */ + public function setFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(x*x + y*y + z*z); + this.x = x*k; + this.y = y*k; + this.z = z*k; + return this; + } + + /** + * + * @param vector + */ + public function toAxisVector(v:Vector3 = null):Vector3 { + if (w < -1 || w > 1) { + normalize(); + } + if (v == null) { + v = new Vector3(); + } + if (w > -1 && w < 1) { + if (w == 0) { + v.x = x; + v.y = y; + v.z = z; + } else { + var angle:Number = 2*Math.acos(w); + var coeff:Number = 1/Math.sqrt(1 - w*w); + v.x = x*coeff*angle; + v.y = y*coeff*angle; + v.z = z*coeff*angle; + } + } else { + v.x = 0; + v.y = 0; + v.z = 0; + } + return v; + } + + /** + * + * @param rotations + */ + public function getEulerAngles(angles:Vector3):Vector3 { + var qi2:Number = 2*x*x; + var qj2:Number = 2*y*y; + var qk2:Number = 2*z*z; + var qij:Number = 2*x*y; + var qjk:Number = 2*y*z; + var qki:Number = 2*z*x; + var qri:Number = 2*w*x; + var qrj:Number = 2*w*y; + var qrk:Number = 2*w*z; + + var aa:Number = 1 - qj2 - qk2; + var bb:Number = qij - qrk; + var ee:Number = qij + qrk; + var ff:Number = 1 - qi2 - qk2; + var ii:Number = qki - qrj; + var jj:Number = qjk + qri; + var kk:Number = 1 - qi2 - qj2; + + if (-1 < ii && ii < 1) { + if (angles == null) { + angles = new Vector3(Math.atan2(jj, kk), -Math.asin(ii), Math.atan2(ee, aa)); + } else { + angles.x = Math.atan2(jj, kk); + angles.y = -Math.asin(ii); + angles.z = Math.atan2(ee, aa); + } + } else { + if (angles == null) { + angles = new Vector3(0, 0.5*((ii <= -1) ? Math.PI : -Math.PI), Math.atan2(-bb, ff)); + } else { + angles.x = 0; + angles.y = (ii <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-bb, ff); + } + } + return angles; + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setFromEulerAnglesXYZ(x:Number, y:Number, z:Number):void { + setFromAxisAngleComponents(1, 0, 0, x); + + _q.setFromAxisAngleComponents(0, 1, 0, y); + append(_q); + normalize(); + + _q.setFromAxisAngleComponents(0, 0, 1, z); + append(_q); + normalize(); + } + + /** + * + */ + public function conjugate():void { + x = -x; + y = -y; + z = -z; + } + + /** + * Выполняет линейную интерполяцию. + * + * @param q1 начало отрезка + * @param q2 конец отрезка + * @param t время, обычно задаётся в интервале [0, 1] + */ + public function nlerp(q1:Quaternion, q2:Quaternion, t:Number):Quaternion { + var d:Number = 1 - t; + w = q1.w*d + q2.w*t; + x = q1.x*d + q2.x*t; + y = q1.y*d + q2.y*t; + z = q1.z*d + q2.z*t; + // inlined normalize + d = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param q + * @return + */ + public function subtract(q:Quaternion):Quaternion { + w -= q.w; + x -= q.x; + y -= q.y; + z -= q.z; + return this; + } + + /** + * + * @param q1 + * @param q2 + * @return + */ + public function diff(q1:Quaternion, q2:Quaternion):Quaternion { + w = q2.w - q1.w; + x = q2.x - q1.x; + y = q2.y - q1.y; + z = q2.z - q1.z; + return this; + } + + /** + * + * @param q + * @return + */ + public function copy(q:Quaternion):Quaternion { + w = q.w; + x = q.x; + y = q.y; + z = q.z; + return this; + } + + /** + * @param reslult + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + result.w = w; + return result; + } + + /** + * + * @return + */ + public function clone():Quaternion { + return new Quaternion(w, x, y, z); + } + + /** + * + * @return + */ + public function toString():String { + return "[" + w + ", " + x + ", " + y + ", " + z + "]"; + } + } +} diff --git a/0.0.8.0/src/alternativa/physics/math/Vector3.as b/0.0.8.0/src/alternativa/physics/math/Vector3.as new file mode 100644 index 0000000..232e499 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/math/Vector3.as @@ -0,0 +1,335 @@ +package alternativa.physics.math { + import flash.geom.Vector3D; + + + /** + * + */ + public class Vector3 extends Vector3D { + + public static const ZERO:Vector3 = new Vector3(0, 0, 0); + public static const X_AXIS:Vector3 = new Vector3(1, 0, 0); + public static const Y_AXIS:Vector3 = new Vector3(0, 1, 0); + public static const Z_AXIS:Vector3 = new Vector3(0, 0, 1); + + /** + * + * @param x + * @param y + * @param z + */ + public function Vector3(x:Number = 0, y:Number = 0, z:Number = 0) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * + * @return + */ + public function vLength():Number { + return Math.sqrt(x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function vLengthSqr():Number { + return x*x + y*y + z*z; + } + + /** + * + * @param length + * @return + */ + public function vSetLength(length:Number):Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = length; + } else { + var k:Number = length/Math.sqrt(x*x + y*y + z*z); + x *= k; + y *= k; + z *= k; + } + return this; + } + + /** + * + * @return + */ + public function vNormalize():Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = 1; + } else { + d = Math.sqrt(d); + x /= d; + y /= d; + z /= d; + } + return this; + } + + /** + * Сложение координат. + * + * @param v точка, координаты которой прибавляются к собственным + */ + public function vAdd(v:Vector3):Vector3 { + x += v.x; + y += v.y; + z += v.z; + return this; + } + + /** + * + * @param k + * @param v + * @return + */ + public function vAddScaled(k:Number, v:Vector3):Vector3 { + x += k*v.x; + y += k*v.y; + z += k*v.z; + return this; + } + + /** + * Вычитание координат. + * + * @param v точка, координаты которой вычитаются из собственных + */ + public function vSubtract(v:Vector3):Vector3 { + x -= v.x; + y -= v.y; + z -= v.z; + return this; + } + + /** + * Вычисляет сумму векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + * @return this + */ + public function vSum(a:Vector3, b:Vector3):Vector3 { + x = a.x + b.x; + y = a.y + b.y; + z = a.z + b.z; + return this; + } + + /** + * Вычисление разности векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + */ + public function vDiff(a:Vector3, b:Vector3):Vector3 { + x = a.x - b.x; + y = a.y - b.y; + z = a.z - b.z; + return this; + } + + /** + * Умножение на скаляр. + * + * @param k число, на которое умножаются координаты + */ + public function vScale(k:Number):Vector3 { + x *= k; + y *= k; + z *= k; + return this; + } + + /** + * Инвертирование вектора. + */ + public function vReverse():Vector3 { + x = -x; + y = -y; + z = -z; + return this; + } + + /** + * + * @param v + * @return + */ + public function vDot(v:Vector3):Number { + return x*v.x + y*v.y + z*v.z; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross(v:Vector3):Vector3 { + var xx:Number = y*v.z - z*v.y; + var yy:Number = z*v.x - x*v.z; + var zz:Number = x*v.y - y*v.x; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross2(a:Vector3, b:Vector3):Vector3 { + x = a.y*b.z - a.z*b.y; + y = a.z*b.x - a.x*b.z; + z = a.x*b.y - a.y*b.x; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy3(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * + * @param matrix + */ + public function vTransformBy3Tr(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz + m.d; + y = m.e*xx + m.f*yy + m.g*zz + m.h; + z = m.i*xx + m.j*yy + m.k*zz + m.l; + return this; + } + + /** + * + * @param m + * @return + */ + public function vTransformInverseBy4(m:Matrix4):Vector3 { + var xx:Number = x - m.d; + var yy:Number = y - m.h; + var zz:Number = z - m.l; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * + * @param m + */ + public function vDeltaTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * Установка координат. + */ + public function vReset(x:Number = 0, y:Number = 0, z:Number = 0):Vector3 { + this.x = x; + this.y = y; + this.z = z; + return this; + } + + /** + * Копирование координат точки. + * + * @param v точка, координаты которой копируются + */ + public function vCopy(v:Vector3):Vector3 { + x = v.x; + y = v.y; + z = v.z; + return this; + } + + /** + * Клонирование точки. + * + * @return клонированная точка + */ + public function vClone():Vector3 { + return new Vector3(x, y, z); + } + + /** + * Копирует координаты вектора в экземпляр класса Vector3D. + * + * @param result экземпляр, в который копируются координаты + * @return переданный параметр + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + return result; + } + + /** + * @param source + * @return + */ + public function copyFromVector3D(source:Vector3D):Vector3 { + x = source.x; + y = source.y; + z = source.z; + return this; + } + + override public function toString():String { + return "Vector3(" + x + "," + y + ", " + z + ")"; + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/primitives/.svn/all-wcprops b/0.0.8.0/src/alternativa/physics/primitives/.svn/all-wcprops new file mode 100644 index 0000000..6efea92 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/primitives/.svn/all-wcprops @@ -0,0 +1,35 @@ +K 25 +svn:wc:ra_dav:version-url +V 114 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/primitives +END +RigidPlane.as +K 25 +svn:wc:ra_dav:version-url +V 128 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/primitives/RigidPlane.as +END +RigidSphere.as +K 25 +svn:wc:ra_dav:version-url +V 129 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/primitives/RigidSphere.as +END +RigidBox.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/primitives/RigidBox.as +END +RigidCylinder.as +K 25 +svn:wc:ra_dav:version-url +V 131 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/primitives/RigidCylinder.as +END +RigidRect.as +K 25 +svn:wc:ra_dav:version-url +V 127 +/!svn/ver/23340/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/primitives/RigidRect.as +END diff --git a/0.0.8.0/src/alternativa/physics/primitives/.svn/entries b/0.0.8.0/src/alternativa/physics/primitives/.svn/entries new file mode 100644 index 0000000..1812b52 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/primitives/.svn/entries @@ -0,0 +1,88 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.8.0/src/alternativa/physics/primitives +http://svndev.alternativaplatform.com + + + +2009-10-19T07:17:33.113306Z +22253 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +RigidPlane.as +file + + + + +2010-10-28T04:32:43.000000Z +3a8402814cf99a68d018a90077f62746 +2009-10-19T07:17:33.113306Z +22253 +mike + +RigidSphere.as +file + + + + +2010-10-28T04:32:43.000000Z +1a898ad68fc203a75a157cbf21f7dc92 +2009-10-19T07:17:33.113306Z +22253 +mike + +RigidBox.as +file + + + + +2010-10-28T04:32:43.000000Z +ae534a73f34403a99de94e649d45e238 +2009-10-19T07:17:33.113306Z +22253 +mike + +RigidCylinder.as +file + + + + +2010-10-28T04:32:43.000000Z +86388b2dc5c35fd45cc1b0bcbd09c517 +2009-10-19T07:17:33.113306Z +22253 +mike + +RigidRect.as +file + + + + +2010-10-28T04:32:43.000000Z +bdc2ecbdfebbd0417a19519c338ef54c +2009-10-19T07:17:33.113306Z +22253 +mike + diff --git a/0.0.8.0/src/alternativa/physics/primitives/.svn/format b/0.0.8.0/src/alternativa/physics/primitives/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/primitives/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.8.0/src/alternativa/physics/primitives/.svn/text-base/RigidBox.as.svn-base b/0.0.8.0/src/alternativa/physics/primitives/.svn/text-base/RigidBox.as.svn-base new file mode 100644 index 0000000..bd59d52 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/primitives/.svn/text-base/RigidBox.as.svn-base @@ -0,0 +1,56 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.math.Matrix3; + import alternativa.physics.math.Vector3; + + use namespace altphysics; + + /** + * + */ + public class RigidBox extends Body { + + /** + * + * @param halfSize + * @param mass + */ + public function RigidBox(halfSize:Vector3, mass:Number) { + super(0, Matrix3.ZERO); + addCollisionPrimitive(new CollisionBox(halfSize, 1)); + setParams(halfSize, mass); + } + + /** + * @param halfSize + * @param mass + */ + public function setParams(halfSize:Vector3, mass:Number):void { + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + invInertia.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + invInertia.a = 3*invMass/(yy + zz); + invInertia.f = 3*invMass/(zz + xx); + invInertia.k = 3*invMass/(xx + yy); + } + var prim:CollisionBox = CollisionBox(collisionPrimitives.head.primitive); + prim.hs.vCopy(halfSize); + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/primitives/.svn/text-base/RigidCylinder.as.svn-base b/0.0.8.0/src/alternativa/physics/primitives/.svn/text-base/RigidCylinder.as.svn-base new file mode 100644 index 0000000..a3ef3cd --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/primitives/.svn/text-base/RigidCylinder.as.svn-base @@ -0,0 +1,32 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.math.Matrix3; + + use namespace altphysics; + + public class RigidCylinder extends Body { + + altphysics var r:Number; + altphysics var h:Number; + + public function RigidCylinder(mass:Number, radius:Number, height:Number) { + super(0, Matrix3.ZERO); + setParams(mass, radius, height); + } + + public function setParams(mass:Number, radius:Number, height:Number):void { + r = radius; + h = height; + invInertiaWorld.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + invInertia.a = invInertia.f = mass*(h*h/12 + r*r/4); + invInertia.k = 0.5*mass*r*r; + invInertia.invert(); + } + } + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/primitives/.svn/text-base/RigidPlane.as.svn-base b/0.0.8.0/src/alternativa/physics/primitives/.svn/text-base/RigidPlane.as.svn-base new file mode 100644 index 0000000..3df7672 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/primitives/.svn/text-base/RigidPlane.as.svn-base @@ -0,0 +1,23 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.math.Matrix3; + import alternativa.physics.math.Vector3; + + use namespace altphysics; + + public class RigidPlane extends Body { + + public var normal:Vector3 = new Vector3(); + public var offset:Number; + + public function RigidPlane() { + super(0, Matrix3.ZERO); + this.normal.vCopy(Vector3.Z_AXIS); + this.offset = 0; + movable = false; + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/primitives/.svn/text-base/RigidRect.as.svn-base b/0.0.8.0/src/alternativa/physics/primitives/.svn/text-base/RigidRect.as.svn-base new file mode 100644 index 0000000..5219666 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/primitives/.svn/text-base/RigidRect.as.svn-base @@ -0,0 +1,20 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.physics.math.Matrix3; + import alternativa.physics.math.Vector3; + + use namespace altphysics; + + public class RigidRect extends Body { + + public function RigidRect(halfWidth:Number, halfLength:Number) { + super(0, Matrix3.ZERO); + movable = false; + addCollisionPrimitive(new CollisionRect(new Vector3(halfWidth, halfLength, 0), 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/primitives/.svn/text-base/RigidSphere.as.svn-base b/0.0.8.0/src/alternativa/physics/primitives/.svn/text-base/RigidSphere.as.svn-base new file mode 100644 index 0000000..63931e4 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/primitives/.svn/text-base/RigidSphere.as.svn-base @@ -0,0 +1,29 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.math.Matrix3; + + use namespace altphysics; + + public class RigidSphere extends Body { + + public function RigidSphere(radius:Number, mass:Number) { + var m:Matrix3; + if (mass == Infinity) { + mass = 0; + m = Matrix3.ZERO; + } else { + mass = 1/mass; + var r2:Number = radius*radius; + m = new Matrix3(); + m.a = m.f = m.k = 2.5*mass/r2; + } + super(mass, m); + if (mass == 0) movable = false; + addCollisionPrimitive(new CollisionSphere(radius, 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/primitives/RigidBox.as b/0.0.8.0/src/alternativa/physics/primitives/RigidBox.as new file mode 100644 index 0000000..bd59d52 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/primitives/RigidBox.as @@ -0,0 +1,56 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.math.Matrix3; + import alternativa.physics.math.Vector3; + + use namespace altphysics; + + /** + * + */ + public class RigidBox extends Body { + + /** + * + * @param halfSize + * @param mass + */ + public function RigidBox(halfSize:Vector3, mass:Number) { + super(0, Matrix3.ZERO); + addCollisionPrimitive(new CollisionBox(halfSize, 1)); + setParams(halfSize, mass); + } + + /** + * @param halfSize + * @param mass + */ + public function setParams(halfSize:Vector3, mass:Number):void { + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + invInertia.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + invInertia.a = 3*invMass/(yy + zz); + invInertia.f = 3*invMass/(zz + xx); + invInertia.k = 3*invMass/(xx + yy); + } + var prim:CollisionBox = CollisionBox(collisionPrimitives.head.primitive); + prim.hs.vCopy(halfSize); + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/primitives/RigidCylinder.as b/0.0.8.0/src/alternativa/physics/primitives/RigidCylinder.as new file mode 100644 index 0000000..a3ef3cd --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/primitives/RigidCylinder.as @@ -0,0 +1,32 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.math.Matrix3; + + use namespace altphysics; + + public class RigidCylinder extends Body { + + altphysics var r:Number; + altphysics var h:Number; + + public function RigidCylinder(mass:Number, radius:Number, height:Number) { + super(0, Matrix3.ZERO); + setParams(mass, radius, height); + } + + public function setParams(mass:Number, radius:Number, height:Number):void { + r = radius; + h = height; + invInertiaWorld.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + invInertia.a = invInertia.f = mass*(h*h/12 + r*r/4); + invInertia.k = 0.5*mass*r*r; + invInertia.invert(); + } + } + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/primitives/RigidPlane.as b/0.0.8.0/src/alternativa/physics/primitives/RigidPlane.as new file mode 100644 index 0000000..3df7672 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/primitives/RigidPlane.as @@ -0,0 +1,23 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.math.Matrix3; + import alternativa.physics.math.Vector3; + + use namespace altphysics; + + public class RigidPlane extends Body { + + public var normal:Vector3 = new Vector3(); + public var offset:Number; + + public function RigidPlane() { + super(0, Matrix3.ZERO); + this.normal.vCopy(Vector3.Z_AXIS); + this.offset = 0; + movable = false; + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/primitives/RigidRect.as b/0.0.8.0/src/alternativa/physics/primitives/RigidRect.as new file mode 100644 index 0000000..5219666 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/primitives/RigidRect.as @@ -0,0 +1,20 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.physics.math.Matrix3; + import alternativa.physics.math.Vector3; + + use namespace altphysics; + + public class RigidRect extends Body { + + public function RigidRect(halfWidth:Number, halfLength:Number) { + super(0, Matrix3.ZERO); + movable = false; + addCollisionPrimitive(new CollisionRect(new Vector3(halfWidth, halfLength, 0), 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.8.0/src/alternativa/physics/primitives/RigidSphere.as b/0.0.8.0/src/alternativa/physics/primitives/RigidSphere.as new file mode 100644 index 0000000..63931e4 --- /dev/null +++ b/0.0.8.0/src/alternativa/physics/primitives/RigidSphere.as @@ -0,0 +1,29 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.math.Matrix3; + + use namespace altphysics; + + public class RigidSphere extends Body { + + public function RigidSphere(radius:Number, mass:Number) { + var m:Matrix3; + if (mass == Infinity) { + mass = 0; + m = Matrix3.ZERO; + } else { + mass = 1/mass; + var r2:Number = radius*radius; + m = new Matrix3(); + m.a = m.f = m.k = 2.5*mass/r2; + } + super(mass, m); + if (mass == 0) movable = false; + addCollisionPrimitive(new CollisionSphere(radius, 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/.actionScriptProperties b/0.0.9.0/.actionScriptProperties new file mode 100644 index 0000000..f73adc8 --- /dev/null +++ b/0.0.9.0/.actionScriptProperties @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/0.0.9.0/.flexLibProperties b/0.0.9.0/.flexLibProperties new file mode 100644 index 0000000..0b65396 --- /dev/null +++ b/0.0.9.0/.flexLibProperties @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/0.0.9.0/.project b/0.0.9.0/.project new file mode 100644 index 0000000..1f1425f --- /dev/null +++ b/0.0.9.0/.project @@ -0,0 +1,19 @@ + + + AlternativaPhysics + + + + + + com.adobe.flexbuilder.project.flexbuilder + + + + + + org.maven.ide.eclipse.maven2Nature + com.adobe.flexbuilder.project.flexlibnature + com.adobe.flexbuilder.project.actionscriptnature + + diff --git a/0.0.9.0/.settings/.svn/all-wcprops b/0.0.9.0/.settings/.svn/all-wcprops new file mode 100644 index 0000000..50a1812 --- /dev/null +++ b/0.0.9.0/.settings/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 89 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/.settings +END +org.eclipse.core.resources.prefs +K 25 +svn:wc:ra_dav:version-url +V 122 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/.settings/org.eclipse.core.resources.prefs +END diff --git a/0.0.9.0/.settings/.svn/entries b/0.0.9.0/.settings/.svn/entries new file mode 100644 index 0000000..4df32df --- /dev/null +++ b/0.0.9.0/.settings/.svn/entries @@ -0,0 +1,40 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/.settings +http://svndev.alternativaplatform.com + + + +2009-04-01T12:25:29.638016Z +10301 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +org.eclipse.core.resources.prefs +file + + + + +2010-10-28T04:32:43.000000Z +4f70b476f3e5075e399505021df2f89b +2009-04-01T12:25:29.638016Z +10301 +mike + diff --git a/0.0.9.0/.settings/.svn/format b/0.0.9.0/.settings/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.0/.settings/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.0/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base b/0.0.9.0/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base new file mode 100644 index 0000000..b3d0d49 --- /dev/null +++ b/0.0.9.0/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base @@ -0,0 +1,3 @@ +#Fri Mar 20 00:19:33 YEKT 2009 +eclipse.preferences.version=1 +encoding/=UTF-8 diff --git a/0.0.9.0/.settings/org.eclipse.core.resources.prefs b/0.0.9.0/.settings/org.eclipse.core.resources.prefs new file mode 100644 index 0000000..b3d0d49 --- /dev/null +++ b/0.0.9.0/.settings/org.eclipse.core.resources.prefs @@ -0,0 +1,3 @@ +#Fri Mar 20 00:19:33 YEKT 2009 +eclipse.preferences.version=1 +encoding/=UTF-8 diff --git a/0.0.9.0/.svn/all-wcprops b/0.0.9.0/.svn/all-wcprops new file mode 100644 index 0000000..11fa6f4 --- /dev/null +++ b/0.0.9.0/.svn/all-wcprops @@ -0,0 +1,35 @@ +K 25 +svn:wc:ra_dav:version-url +V 79 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0 +END +.flexLibProperties +K 25 +svn:wc:ra_dav:version-url +V 98 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/.flexLibProperties +END +.project +K 25 +svn:wc:ra_dav:version-url +V 88 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/.project +END +pom.xml +K 25 +svn:wc:ra_dav:version-url +V 87 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/pom.xml +END +.actionScriptProperties +K 25 +svn:wc:ra_dav:version-url +V 103 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/.actionScriptProperties +END +AlternativaPhysics.iml +K 25 +svn:wc:ra_dav:version-url +V 102 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/AlternativaPhysics.iml +END diff --git a/0.0.9.0/.svn/dir-prop-base b/0.0.9.0/.svn/dir-prop-base new file mode 100644 index 0000000..c31ca28 --- /dev/null +++ b/0.0.9.0/.svn/dir-prop-base @@ -0,0 +1,7 @@ +K 13 +svn:mergeinfo +V 278 +/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized:19806-19860 +/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.7.0.LinkedList:20360-22217,22226-22252 +/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D:13892-14098 +END diff --git a/0.0.9.0/.svn/entries b/0.0.9.0/.svn/entries new file mode 100644 index 0000000..4cc9ae5 --- /dev/null +++ b/0.0.9.0/.svn/entries @@ -0,0 +1,97 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0 +http://svndev.alternativaplatform.com + + + +2010-02-19T13:52:11.808507Z +28758 +mike +has-props + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +META-INF +dir + +.flexLibProperties +file + + + + +2010-10-28T04:32:43.000000Z +b170dccb732dcb12462ac421cdca9723 +2010-02-12T12:34:25.192477Z +28424 +mike + +.project +file + + + + +2010-10-28T04:32:43.000000Z +1e90cc68b52b93173b2b5de88eb5c3a3 +2009-04-20T17:15:55.260110Z +11541 +mike + +src +dir + +pom.xml +file + + + + +2010-10-28T04:32:43.000000Z +71f691b351b5d17f63f651cdabd32d16 +2010-02-19T13:52:11.808507Z +28758 +mike + +.actionScriptProperties +file + + + + +2010-10-28T04:32:43.000000Z +b8656f882fff2baedd57a92db86fcf8c +2010-01-14T07:05:07.823716Z +26358 +mike + +AlternativaPhysics.iml +file + + + + +2010-10-28T04:32:43.000000Z +5162d434bd39d50f59a8057c1fa2c961 +2010-02-19T13:52:11.808507Z +28758 +mike + +.settings +dir + diff --git a/0.0.9.0/.svn/format b/0.0.9.0/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.0/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.0/.svn/text-base/.actionScriptProperties.svn-base b/0.0.9.0/.svn/text-base/.actionScriptProperties.svn-base new file mode 100644 index 0000000..f73adc8 --- /dev/null +++ b/0.0.9.0/.svn/text-base/.actionScriptProperties.svn-base @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/0.0.9.0/.svn/text-base/.flexLibProperties.svn-base b/0.0.9.0/.svn/text-base/.flexLibProperties.svn-base new file mode 100644 index 0000000..0b65396 --- /dev/null +++ b/0.0.9.0/.svn/text-base/.flexLibProperties.svn-base @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/0.0.9.0/.svn/text-base/.project.svn-base b/0.0.9.0/.svn/text-base/.project.svn-base new file mode 100644 index 0000000..1f1425f --- /dev/null +++ b/0.0.9.0/.svn/text-base/.project.svn-base @@ -0,0 +1,19 @@ + + + AlternativaPhysics + + + + + + com.adobe.flexbuilder.project.flexbuilder + + + + + + org.maven.ide.eclipse.maven2Nature + com.adobe.flexbuilder.project.flexlibnature + com.adobe.flexbuilder.project.actionscriptnature + + diff --git a/0.0.9.0/.svn/text-base/AlternativaPhysics.iml.svn-base b/0.0.9.0/.svn/text-base/AlternativaPhysics.iml.svn-base new file mode 100644 index 0000000..7783af3 --- /dev/null +++ b/0.0.9.0/.svn/text-base/AlternativaPhysics.iml.svn-base @@ -0,0 +1,65 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/0.0.9.0/.svn/text-base/pom.xml.svn-base b/0.0.9.0/.svn/text-base/pom.xml.svn-base new file mode 100644 index 0000000..3255930 --- /dev/null +++ b/0.0.9.0/.svn/text-base/pom.xml.svn-base @@ -0,0 +1,17 @@ + + 4.0.0 + platform.clients.fp10.libraries + AlternativaPhysics + swc + 0.0.9.0 + + platform.tools.maven + FlashBasePom + 2.0.0.0 + + + scm:svn:http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0 + + + + \ No newline at end of file diff --git a/0.0.9.0/AlternativaPhysics.iml b/0.0.9.0/AlternativaPhysics.iml new file mode 100644 index 0000000..7783af3 --- /dev/null +++ b/0.0.9.0/AlternativaPhysics.iml @@ -0,0 +1,65 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/0.0.9.0/META-INF/.svn/all-wcprops b/0.0.9.0/META-INF/.svn/all-wcprops new file mode 100644 index 0000000..ae2a9d0 --- /dev/null +++ b/0.0.9.0/META-INF/.svn/all-wcprops @@ -0,0 +1,17 @@ +K 25 +svn:wc:ra_dav:version-url +V 88 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/META-INF +END +security.sspj +K 25 +svn:wc:ra_dav:version-url +V 102 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/META-INF/security.sspj +END +MANIFEST.MF +K 25 +svn:wc:ra_dav:version-url +V 100 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/META-INF/MANIFEST.MF +END diff --git a/0.0.9.0/META-INF/.svn/entries b/0.0.9.0/META-INF/.svn/entries new file mode 100644 index 0000000..eef3e0d --- /dev/null +++ b/0.0.9.0/META-INF/.svn/entries @@ -0,0 +1,52 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/META-INF +http://svndev.alternativaplatform.com + + + +2009-12-25T05:53:34.807593Z +25872 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +security.sspj +file + + + + +2010-10-28T04:32:43.000000Z +4684eb6f77fe6999c75cd467c18cf5d6 +2009-12-25T05:53:34.807593Z +25872 +mike + +MANIFEST.MF +file + + + + +2010-10-28T04:32:43.000000Z +7b64dceb50a6905850ff00a0bfbe77eb +2009-04-20T17:15:55.260110Z +11541 +mike + diff --git a/0.0.9.0/META-INF/.svn/format b/0.0.9.0/META-INF/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.0/META-INF/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.0/META-INF/.svn/text-base/MANIFEST.MF.svn-base b/0.0.9.0/META-INF/.svn/text-base/MANIFEST.MF.svn-base new file mode 100644 index 0000000..5066a55 --- /dev/null +++ b/0.0.9.0/META-INF/.svn/text-base/MANIFEST.MF.svn-base @@ -0,0 +1 @@ +Bundle-Name: platform.clients.fp10.libraries.AlternativaPhysics diff --git a/0.0.9.0/META-INF/.svn/text-base/security.sspj.svn-base b/0.0.9.0/META-INF/.svn/text-base/security.sspj.svn-base new file mode 100644 index 0000000..e463c72 --- /dev/null +++ b/0.0.9.0/META-INF/.svn/text-base/security.sspj.svn-base @@ -0,0 +1,11923 @@ + + + + + true + + + true + + + true + + + 3.4 + + + true + + + true + + + true + + + + + + + + + + + true + + + FFFFFF + + + D:\work\eclipse\ws_projects\TanksTestingTool\lib\AlternativaPhysics.swc + + + AlternativaPhysics.swc + + + 1 + + + 24.0 + + + Not Present + + + 500 X 375 + + + + + + + true + + + + + + + + + + + + + + + + append + + + true + + + 10 + + + Public Function + + + + + + + getEulerAngles + + + true + + + 10 + + + Public Function + + + + + + + invert + + + true + + + 10 + + + Public Function + + + + + + + prepend + + + true + + + 10 + + + Public Function + + + + + + + setRotationMatrix + + + true + + + 10 + + + Public Function + + + + + + + subtract + + + true + + + 10 + + + Public Function + + + + + + + toIdentity + + + true + + + 10 + + + Public Function + + + + + + + transformVector + + + true + + + 10 + + + Public Function + + + + + + + transformVector3To3D + + + true + + + 10 + + + Public Function + + + + + + + transpose + + + true + + + 10 + + + Public Function + + + + + + + a + + + true + + + 10 + + + Public Variable + + + + + + + b + + + true + + + 10 + + + Public Variable + + + + + + + c + + + true + + + 10 + + + Public Variable + + + + + + + clone + + + true + + + 10 + + + Public Function + + + + + + + e + + + true + + + 10 + + + Public Variable + + + + + + + f + + + true + + + 10 + + + Public Variable + + + + + + + g + + + true + + + 10 + + + Public Variable + + + + + + + add + + + true + + + 10 + + + Public Function + + + + + + + i + + + true + + + 10 + + + Public Variable + + + + + + + j + + + true + + + 10 + + + Public Variable + + + + + + + k + + + true + + + 10 + + + Public Variable + + + + + + + copy + + + true + + + 10 + + + Public Function + + + + + + + createSkewSymmetric + + + true + + + 10 + + + Public Function + + + + + + + fromAxisAngle + + + true + + + 10 + + + Public Function + + + + + + + prependTransposed + + + true + + + 10 + + + Public Function + + + + + + + toString + + + true + + + 10 + + + Public Function + + + + + + + transformVectorInverse + + + true + + + 10 + + + Public Function + + + + + + + IDENTITY + + + true + + + 10 + + + Public Variable + + + + + + + ZERO + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + Matrix3 + + + true + + + 10 + + + Class + + + + + + + + + + + append + + + true + + + 10 + + + Public Function + + + + + + + deltaTransformVectorInverse + + + true + + + 10 + + + Public Function + + + + + + + getAxis + + + true + + + 10 + + + Public Function + + + + + + + invert + + + true + + + 10 + + + Public Function + + + + + + + prepend + + + true + + + 10 + + + Public Function + + + + + + + setFromMatrix3 + + + true + + + 10 + + + Public Function + + + + + + + subtract + + + true + + + 10 + + + Public Function + + + + + + + toIdentity + + + true + + + 10 + + + Public Function + + + + + + + transformVector + + + true + + + 10 + + + Public Function + + + + + + + transformVectors + + + true + + + 10 + + + Public Function + + + + + + + transformVectorsInverse + + + true + + + 10 + + + Public Function + + + + + + + transformVectorsInverseN + + + true + + + 10 + + + Public Function + + + + + + + transformVectorsN + + + true + + + 10 + + + Public Function + + + + + + + a + + + true + + + 10 + + + Public Variable + + + + + + + b + + + true + + + 10 + + + Public Variable + + + + + + + c + + + true + + + 10 + + + Public Variable + + + + + + + d + + + true + + + 10 + + + Public Variable + + + + + + + e + + + true + + + 10 + + + Public Variable + + + + + + + f + + + true + + + 10 + + + Public Variable + + + + + + + g + + + true + + + 10 + + + Public Variable + + + + + + + h + + + true + + + 10 + + + Public Variable + + + + + + + i + + + true + + + 10 + + + Public Variable + + + + + + + j + + + true + + + 10 + + + Public Variable + + + + + + + k + + + true + + + 10 + + + Public Variable + + + + + + + l + + + true + + + 10 + + + Public Variable + + + + + + + add + + + true + + + 10 + + + Public Function + + + + + + + clone + + + true + + + 10 + + + Public Function + + + + + + + copy + + + true + + + 10 + + + Public Function + + + + + + + deltaTransformVector + + + true + + + 10 + + + Public Function + + + + + + + getEulerAngles + + + true + + + 10 + + + Public Function + + + + + + + setOrientationFromMatrix3 + + + true + + + 10 + + + Public Function + + + + + + + toString + + + true + + + 10 + + + Public Function + + + + + + + transformVectorInverse + + + true + + + 10 + + + Public Function + + + + + + + IDENTITY + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + Matrix4 + + + true + + + 10 + + + Class + + + + + + + + + + + append + + + true + + + 10 + + + Public Function + + + + + + + clone + + + true + + + 10 + + + Public Function + + + + + + + conjugate + + + true + + + 10 + + + Public Function + + + + + + + diff + + + true + + + 10 + + + Public Function + + + + + + + getEulerAngles + + + true + + + 10 + + + Public Function + + + + + + + length + + + true + + + 10 + + + Public Function + + + + + + + lengthSqr + + + true + + + 10 + + + Public Function + + + + + + + nlerp + + + true + + + 10 + + + Public Function + + + + + + + normalize + + + true + + + 10 + + + Public Function + + + + + + + prepend + + + true + + + 10 + + + Public Function + + + + + + + reset + + + true + + + 10 + + + Public Function + + + + + + + rotateByVector + + + true + + + 10 + + + Public Function + + + + + + + setFromAxisAngle + + + true + + + 10 + + + Public Function + + + + + + + setFromAxisAngleComponents + + + true + + + 10 + + + Public Function + + + + + + + setFromEulerAnglesXYZ + + + true + + + 10 + + + Public Function + + + + + + + slerp + + + true + + + 10 + + + Public Function + + + + + + + subtract + + + true + + + 10 + + + Public Function + + + + + + + toAxisVector + + + true + + + 10 + + + Public Function + + + + + + + toMatrix3 + + + true + + + 10 + + + Public Function + + + + + + + toString + + + true + + + 10 + + + Public Function + + + + + + + toVector3D + + + true + + + 10 + + + Public Function + + + + + + + w + + + true + + + 10 + + + Public Variable + + + + + + + x + + + true + + + 10 + + + Public Variable + + + + + + + y + + + true + + + 10 + + + Public Variable + + + + + + + z + + + true + + + 10 + + + Public Variable + + + + + + + addScaledVector + + + true + + + 10 + + + Public Function + + + + + + + copy + + + true + + + 10 + + + Public Function + + + + + + + createFromAxisAngle + + + true + + + 10 + + + Public Function + + + + + + + createFromAxisAngleComponents + + + true + + + 10 + + + Public Function + + + + + + + multiply + + + true + + + 10 + + + Public Function + + + + + + + _q + + + + + + true + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + Quaternion + + + true + + + 10 + + + Class + + + + + + + + + + + copyFromVector3D + + + true + + + 10 + + + Public Function + + + + + + + toString + + + true + + + 10 + + + Public Function + + + + + + + toVector3D + + + true + + + 10 + + + Public Function + + + + + + + vAdd + + + true + + + 10 + + + Public Function + + + + + + + vAddScaled + + + true + + + 10 + + + Public Function + + + + + + + vClone + + + true + + + 10 + + + Public Function + + + + + + + vCopy + + + true + + + 10 + + + Public Function + + + + + + + vCross + + + true + + + 10 + + + Public Function + + + + + + + vCross2 + + + true + + + 10 + + + Public Function + + + + + + + vDeltaTransformBy4 + + + true + + + 10 + + + Public Function + + + + + + + vDiff + + + true + + + 10 + + + Public Function + + + + + + + vDot + + + true + + + 10 + + + Public Function + + + + + + + vLength + + + true + + + 10 + + + Public Function + + + + + + + vLengthSqr + + + true + + + 10 + + + Public Function + + + + + + + vNormalize + + + true + + + 10 + + + Public Function + + + + + + + vReset + + + true + + + 10 + + + Public Function + + + + + + + vReverse + + + true + + + 10 + + + Public Function + + + + + + + vScale + + + true + + + 10 + + + Public Function + + + + + + + vSetLength + + + true + + + 10 + + + Public Function + + + + + + + vSubtract + + + true + + + 10 + + + Public Function + + + + + + + vSum + + + true + + + 10 + + + Public Function + + + + + + + vTransformBy3 + + + true + + + 10 + + + Public Function + + + + + + + vTransformBy3Tr + + + true + + + 10 + + + Public Function + + + + + + + vTransformBy4 + + + true + + + 10 + + + Public Function + + + + + + + vTransformInverseBy4 + + + true + + + 10 + + + Public Function + + + + + + + X_AXIS + + + true + + + 10 + + + Public Variable + + + + + + + Y_AXIS + + + true + + + 10 + + + Public Variable + + + + + + + Z_AXIS + + + true + + + 10 + + + Public Variable + + + + + + + ZERO + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + Vector3 + + + true + + + 10 + + + Class + + + + + + + alternativa.math + + + 10 + + + Package + + + + + + + + + + + + + + + interpolate + + + true + + + 10 + + + Public Function + + + + + + + setRotation + + + true + + + 10 + + + Public Function + + + + + + + canFreeze + + + true + + + 10 + + + Public Variable + + + + + + + invInertiaWorld + + + true + + + 10 + + + Public Variable + + + + + + + invMass + + + true + + + 10 + + + Public Variable + + + + + + + setRotationXYZ + + + true + + + 10 + + + Public Function + + + + + + + invInertia + + + true + + + 10 + + + Public Variable + + + + + + + name + + + true + + + 10 + + + Public Variable + + + + + + + state + + + true + + + 10 + + + Public Variable + + + + + + + world + + + true + + + 10 + + + Public Variable + + + + + + + addTorque + + + true + + + 10 + + + Public Function + + + + + + + true + + + saveState + + + + + + Public Function + + + + + + + setOrientation + + + true + + + 10 + + + Public Function + + + + + + + id + + + true + + + 10 + + + Public Variable + + + + + + + applyRelPosWorldImpulse + + + true + + + 10 + + + Public Function + + + + + + + true + + + integratePosition + + + + + + Public Function + + + + + + + frozen + + + true + + + 10 + + + Public Variable + + + + + + + prevState + + + true + + + 10 + + + Public Variable + + + + + + + true + + + calcAccelerations + + + + + + Public Function + + + + + + + true + + + restoreState + + + + + + Public Function + + + + + + + material + + + true + + + 10 + + + Public Variable + + + + + + + true + + + clearAccumulators + + + + + + Public Function + + + + + + + setPosition + + + true + + + 10 + + + Public Function + + + + + + + contactsNum + + + true + + + 10 + + + Public Variable + + + + + + + forceAccum + + + true + + + 10 + + + Public Variable + + + + + + + addWorldForceAtLocalPoint + + + true + + + 10 + + + Public Function + + + + + + + beforePhysicsStep + + + true + + + 10 + + + Public Function + + + + + + + true + + + integrateVelocity + + + + + + Public Function + + + + + + + baseMatrix + + + true + + + 10 + + + Public Variable + + + + + + + collisionPrimitives + + + true + + + 10 + + + Public Variable + + + + + + + addLocalForce + + + true + + + 10 + + + Public Function + + + + + + + addWorldForceXYZ + + + true + + + 10 + + + Public Function + + + + + + + calcDerivedData + + + true + + + 10 + + + Public Function + + + + + + + setVelocityXYZ + + + true + + + 10 + + + Public Function + + + + + + + angleAccel + + + true + + + 10 + + + Public Variable + + + + + + + contacts + + + true + + + 10 + + + Public Variable + + + + + + + addCollisionPrimitive + + + true + + + 10 + + + Public Function + + + + + + + addForce + + + true + + + 10 + + + Public Function + + + + + + + setVelocity + + + true + + + 10 + + + Public Function + + + + + + + aabb + + + true + + + 10 + + + Public Variable + + + + + + + accel + + + true + + + 10 + + + Public Variable + + + + + + + freezeCounter + + + true + + + 10 + + + Public Variable + + + + + + + movable + + + true + + + 10 + + + Public Variable + + + + + + + postCollisionPredicate + + + true + + + 10 + + + Public Variable + + + + + + + torqueAccum + + + true + + + 10 + + + Public Variable + + + + + + + addWorldForce + + + true + + + 10 + + + Public Function + + + + + + + addWorldForceScaled + + + true + + + 10 + + + Public Function + + + + + + + MAX_CONTACTS + + + true + + + 10 + + + Public Variable + + + + + + + addForceXYZ + + + true + + + 10 + + + Public Function + + + + + + + removeCollisionPrimitive + + + true + + + 10 + + + Public Function + + + + + + + setPositionXYZ + + + true + + + 10 + + + Public Function + + + + + + + linDamping + + + true + + + 10 + + + Public Variable + + + + + + + _f + + + + + + true + + + Private Variable + + + + + + + _r + + + + + + true + + + Private Variable + + + + + + + rotDamping + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + Body + + + true + + + 10 + + + Class + + + + + + + + + + + head + + + true + + + 10 + + + Public Variable + + + + + + + size + + + true + + + 10 + + + Public Variable + + + + + + + remove + + + true + + + 10 + + + Public Function + + + + + + + tail + + + true + + + 10 + + + Public Variable + + + + + + + append + + + true + + + 10 + + + Public Function + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + findItem + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + BodyList + + + true + + + 10 + + + Class + + + + + + + + + + + body + + + true + + + 10 + + + Public Variable + + + + + + + next + + + true + + + 10 + + + Public Variable + + + + + + + prev + + + true + + + 10 + + + Public Variable + + + + + + + clearPool + + + true + + + 10 + + + Public Function + + + + + + + dispose + + + true + + + 10 + + + Public Function + + + + + + + poolTop + + + + + + true + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + create + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + BodyListItem + + + true + + + 10 + + + Class + + + + + + + + + + + friction + + + true + + + 10 + + + Public Variable + + + + + + + restitution + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + BodyMaterial + + + true + + + 10 + + + Class + + + + + + + + + + + pos + + + true + + + 10 + + + Public Variable + + + + + + + copy + + + true + + + 10 + + + Public Function + + + + + + + orientation + + + true + + + 10 + + + Public Variable + + + + + + + rotation + + + true + + + 10 + + + Public Variable + + + + + + + velocity + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + BodyState + + + true + + + 10 + + + Class + + + + + + + + + + + head + + + true + + + 10 + + + Public Variable + + + + + + + size + + + true + + + 10 + + + Public Variable + + + + + + + remove + + + true + + + 10 + + + Public Function + + + + + + + tail + + + true + + + 10 + + + Public Variable + + + + + + + append + + + true + + + 10 + + + Public Function + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + clear + + + true + + + 10 + + + Public Function + + + + + + + findItem + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + CollisionPrimitiveList + + + true + + + 10 + + + Class + + + + + + + + + + + next + + + true + + + 10 + + + Public Variable + + + + + + + prev + + + true + + + 10 + + + Public Variable + + + + + + + primitive + + + true + + + 10 + + + Public Variable + + + + + + + clearPool + + + true + + + 10 + + + Public Function + + + + + + + dispose + + + true + + + 10 + + + Public Function + + + + + + + poolTop + + + + + + true + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + create + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + CollisionPrimitiveListItem + + + true + + + 10 + + + Class + + + + + + + + + + + body1 + + + true + + + 10 + + + Public Variable + + + + + + + body2 + + + true + + + 10 + + + Public Variable + + + + + + + friction + + + true + + + 10 + + + Public Variable + + + + + + + MAX_POINTS + + + + + + true + + + Private Variable + + + + + + + index + + + true + + + 10 + + + Public Variable + + + + + + + maxPenetration + + + true + + + 10 + + + Public Variable + + + + + + + next + + + true + + + 10 + + + Public Variable + + + + + + + true + + + normal + + + true + + + 10 + + + Public Variable + + + + + + + true + + + pcount + + + true + + + 10 + + + Public Variable + + + + + + + points + + + true + + + 10 + + + Public Variable + + + + + + + restitution + + + true + + + 10 + + + Public Variable + + + + + + + satisfied + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + Contact + + + true + + + 10 + + + Class + + + + + + + + + + + accumImpulseN + + + true + + + 10 + + + Public Variable + + + + + + + angularInertia2 + + + true + + + 10 + + + Public Variable + + + + + + + feature1 + + + true + + + 10 + + + Public Variable + + + + + + + feature2 + + + true + + + 10 + + + Public Variable + + + + + + + minSepVel + + + true + + + 10 + + + Public Variable + + + + + + + normalVel + + + true + + + 10 + + + Public Variable + + + + + + + pos + + + true + + + 10 + + + Public Variable + + + + + + + r1 + + + true + + + 10 + + + Public Variable + + + + + + + r2 + + + true + + + 10 + + + Public Variable + + + + + + + copyFrom + + + true + + + 10 + + + Public Function + + + + + + + angularInertia1 + + + true + + + 10 + + + Public Variable + + + + + + + penetration + + + true + + + 10 + + + Public Variable + + + + + + + satisfied + + + true + + + 10 + + + Public Variable + + + + + + + true + + + velByUnitImpulseN + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + ContactPoint + + + true + + + 10 + + + Class + + + + + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + getBoxInvInertia + + + true + + + 10 + + + Public Function + + + + + + + getCylinderInvInertia + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + PhysicsUtils + + + true + + + 10 + + + Class + + + + + + + + + + + removeConstraint + + + true + + + 10 + + + Public Function + + + + + + + _gravityMagnitude + + + true + + + 10 + + + Public Variable + + + + + + + allowedPenetration + + + true + + + 10 + + + Public Variable + + + + + + + collisionIterations + + + true + + + 10 + + + Public Variable + + + + + + + penResolutionSteps + + + true + + + 10 + + + Public Variable + + + + + + + timeStamp + + + true + + + 10 + + + Public Variable + + + + + + + detectCollisions + + + + + + true + + + Private Function + + + + + + + intergateVelocities + + + + + + true + + + Private Function + + + + + + + resolveInterpenetration + + + + + + true + + + Private Function + + + + + + + _gravity + + + true + + + 10 + + + Public Variable + + + + + + + postPhysics + + + + + + true + + + Private Function + + + + + + + resolveContact + + + + + + true + + + Private Function + + + + + + + separateContactPoint + + + + + + true + + + Private Function + + + + + + + angSpeedFreezeLimit + + + true + + + 10 + + + Public Variable + + + + + + + collisionDetector + + + true + + + 10 + + + Public Variable + + + + + + + _v1 + + + + + + true + + + Private Variable + + + + + + + _v2 + + + + + + true + + + Private Variable + + + + + + + maxPenResolutionSpeed + + + true + + + 10 + + + Public Variable + + + + + + + staticSeparationIterations + + + true + + + 10 + + + Public Variable + + + + + + + usePrediction + + + true + + + 10 + + + Public Variable + + + + + + + gravity + + + true + + + 10 + + + Public Function + + + + + + + applyForces + + + + + + true + + + Private Function + + + + + + + true + + + constraints + + + + + + Public Variable + + + + + + + staticSeparationSteps + + + true + + + 10 + + + Public Variable + + + + + + + integratePositions + + + + + + true + + + Private Function + + + + + + + removeBody + + + true + + + 10 + + + Public Function + + + + + + + runPhysics + + + true + + + 10 + + + Public Function + + + + + + + preProcessContacts + + + + + + true + + + Private Function + + + + + + + bodies + + + true + + + 10 + + + Public Variable + + + + + + + true + + + contacts + + + + + + Public Variable + + + + + + + freezeSteps + + + true + + + 10 + + + Public Variable + + + + + + + linSpeedFreezeLimit + + + true + + + 10 + + + Public Variable + + + + + + + maxAngleMove + + + true + + + 10 + + + Public Variable + + + + + + + time + + + true + + + 10 + + + Public Variable + + + + + + + gravity + + + true + + + 10 + + + Public Function + + + + + + + borderContact + + + + + + true + + + Private Variable + + + + + + + addConstraint + + + true + + + 10 + + + Public Function + + + + + + + processContacts + + + + + + true + + + Private Function + + + + + + + addBody + + + true + + + 10 + + + Public Function + + + + + + + calcSepVelocity + + + + + + true + + + Private Function + + + + + + + performStaticSeparation + + + + + + true + + + Private Function + + + + + + + resolveContactPoint + + + + + + true + + + Private Function + + + + + + + true + + + constraintsNum + + + + + + Public Variable + + + + + + + true + + + MAX_CONTACTS + + + + + + Public Variable + + + + + + + _r + + + + + + true + + + Private Variable + + + + + + + _t + + + + + + true + + + Private Variable + + + + + + + _v + + + + + + true + + + Private Variable + + + + + + + contactIterations + + + true + + + 10 + + + Public Variable + + + + + + + useStaticSeparation + + + true + + + 10 + + + Public Variable + + + + + + + lastBodyId + + + + + + true + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + World + + + true + + + 10 + + + Class + + + + + + + alternativa.physics + + + 10 + + + Package + + + + + + + + + + + + + + + true + + + axis + + + true + + + 10 + + + Public Variable + + + + + + + boundBox + + + true + + + 10 + + + Public Variable + + + + + + + coord + + + true + + + 10 + + + Public Variable + + + + + + + indices + + + true + + + 10 + + + Public Variable + + + + + + + negativeNode + + + true + + + 10 + + + Public Variable + + + + + + + parent + + + true + + + 10 + + + Public Variable + + + + + + + positiveNode + + + true + + + 10 + + + Public Variable + + + + + + + splitIndices + + + true + + + 10 + + + Public Variable + + + + + + + splitTree + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + CollisionKdNode + + + true + + + 10 + + + Class + + + + + + + + + + + minPrimitivesPerNode + + + true + + + 10 + + + Public Variable + + + + + + + splitNode + + + + + + true + + + Private Function + + + + + + + rootNode + + + true + + + 10 + + + Public Variable + + + + + + + splitCost + + + + + + true + + + Private Variable + + + + + + + staticBoundBoxes + + + true + + + 10 + + + Public Variable + + + + + + + splitCoord + + + + + + true + + + Private Variable + + + + + + + staticChildren + + + true + + + 10 + + + Public Variable + + + + + + + threshold + + + true + + + 10 + + + Public Variable + + + + + + + traceTree + + + true + + + 10 + + + Public Function + + + + + + + numStaticChildren + + + true + + + 10 + + + Public Variable + + + + + + + checkNodeAxis + + + + + + true + + + Private Function + + + + + + + _bb + + + + + + true + + + Private Variable + + + + + + + _nodeBB + + + + + + true + + + Private Variable + + + + + + + nodeBoundBoxThreshold + + + + + + true + + + Private Variable + + + + + + + splitAxis + + + + + + true + + + Private Variable + + + + + + + createTree + + + true + + + 10 + + + Public Function + + + + + + + traceNode + + + + + + true + + + Private Function + + + + + + + splitCoordsX + + + + + + true + + + Private Variable + + + + + + + splitCoordsY + + + + + + true + + + Private Variable + + + + + + + splitCoordsZ + + + + + + true + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + CollisionKdTree + + + true + + + 10 + + + Class + + + + + + + + + + + minPrimitivesPerNode + + + true + + + 10 + + + Public Variable + + + + + + + rootNode + + + true + + + 10 + + + Public Variable + + + + + + + splitCoord + + + + + + true + + + Private Variable + + + + + + + splitCost + + + + + + true + + + Private Variable + + + + + + + parentNode + + + true + + + 10 + + + Public Variable + + + + + + + parentTree + + + true + + + 10 + + + Public Variable + + + + + + + threshold + + + true + + + 10 + + + Public Variable + + + + + + + checkNodeAxis + + + + + + true + + + Private Function + + + + + + + createTree + + + true + + + 10 + + + Public Function + + + + + + + _bb + + + + + + true + + + Private Variable + + + + + + + nodeBoundBoxThreshold + + + + + + true + + + Private Variable + + + + + + + splitAxis + + + + + + true + + + Private Variable + + + + + + + splitNode + + + + + + true + + + Private Function + + + + + + + _nodeBB + + + + + + true + + + Private Variable + + + + + + + splitCoordsX + + + + + + true + + + Private Variable + + + + + + + splitCoordsY + + + + + + true + + + Private Variable + + + + + + + splitCoordsZ + + + + + + true + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + CollisionKdTree2D + + + true + + + 10 + + + Class + + + + + + + + + + + aabb + + + true + + + 10 + + + Public Variable + + + + + + + body + + + true + + + 10 + + + Public Variable + + + + + + + localTransform + + + true + + + 10 + + + Public Variable + + + + + + + postCollisionPredicate + + + true + + + 10 + + + Public Variable + + + + + + + calculateAABB + + + true + + + 10 + + + Public Function + + + + + + + setBody + + + true + + + 10 + + + Public Function + + + + + + + collisionGroup + + + true + + + 10 + + + Public Variable + + + + + + + transform + + + true + + + 10 + + + Public Variable + + + + + + + true + + + createPrimitive + + + + + + true + + + 10 + + + Private Function + + + + + + + clone + + + true + + + 10 + + + Public Function + + + + + + + BOX + + + true + + + 10 + + + Public Variable + + + + + + + PLANE + + + true + + + 10 + + + Public Variable + + + + + + + SPHERE + + + true + + + 10 + + + Public Variable + + + + + + + TRIANGLE + + + true + + + 10 + + + Public Variable + + + + + + + type + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + copyFrom + + + true + + + 10 + + + Public Function + + + + + + + getRayIntersection + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + toString + + + true + + + 10 + + + Public Function + + + + + + + RECT + + + true + + + 10 + + + Public Variable + + + + + + + CollisionPrimitive + + + true + + + 10 + + + Class + + + + + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + true + + + considerBodies + + + + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + IBodyCollisionPredicate + + + true + + + 10 + + + Class + + + + + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + getContact + + + + + + 10 + + + Public Function + + + + + + + haveCollision + + + + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + ICollider + + + true + + + 10 + + + Class + + + + + + + true + + + + + + + true + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + true + + + getAllContacts + + + + + + Public Function + + + + + + + true + + + getContact + + + + + + 10 + + + Public Function + + + + + + + true + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + true + + + intersectRay + + + + + + Public Function + + + + + + + true + + + testCollision + + + + + + Public Function + + + + + + + ICollisionDetector + + + true + + + 10 + + + Class + + + + + + + true + + + + + + + true + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + true + + + considerCollision + + + + + + Public Function + + + + + + + true + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + ICollisionPredicate + + + true + + + 10 + + + Class + + + + + + + true + + + + + + + true + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + true + + + considerBody + + + + + + Public Function + + + + + + + true + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + IRayCollisionPredicate + + + true + + + 10 + + + Class + + + + + + + + + + + addPrimitive + + + true + + + 10 + + + Public Function + + + + + + + removePrimitive + + + true + + + 10 + + + Public Function + + + + + + + testCollision + + + true + + + 10 + + + Public Function + + + + + + + true + + + dynamicPrimitives + + + + + + Public Variable + + + + + + + init + + + true + + + 10 + + + Public Function + + + + + + + intersectRayWithStatic + + + true + + + 10 + + + Public Function + + + + + + + intersectRayWithDynamic + + + + + + true + + + Private Function + + + + + + + intersectRay + + + true + + + 10 + + + Public Function + + + + + + + testBodyPrimitiveCollision + + + true + + + 10 + + + Public Function + + + + + + + getRayNodeIntersection + + + + + + true + + + Private Function + + + + + + + _dynamicIntersection + + + + + + true + + + Private Variable + + + + + + + getContact + + + true + + + 10 + + + Public Function + + + + + + + true + + + threshold + + + + + + Public Variable + + + + + + + _time + + + + + + true + + + Private Variable + + + + + + + getAllContacts + + + true + + + 10 + + + Public Function + + + + + + + _n + + + + + + true + + + Private Variable + + + + + + + _o + + + + + + true + + + Private Variable + + + + + + + addCollider + + + + + + true + + + Private Function + + + + + + + getPrimitiveNodeCollisions + + + + + + true + + + Private Function + + + + + + + true + + + tree + + + + + + Public Variable + + + + + + + getRayBoundBoxIntersection + + + + + + true + + + Private Function + + + + + + + testRayAgainstNode + + + + + + true + + + Private Function + + + + + + + true + + + dynamicPrimitivesNum + + + + + + Public Variable + + + + + + + _rayAABB + + + + + + true + + + Private Variable + + + + + + + colliders + + + + + + true + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + KdTreeCollisionDetector + + + true + + + 10 + + + Class + + + + + + + alternativa.physics.collision + + + 10 + + + Package + + + + + + + + + + + + + + + axis10 + + + + + + true + + + Private Variable + + + + + + + axis11 + + + + + + true + + + Private Variable + + + + + + + findEdgesIntersection + + + + + + true + + + Private Function + + + + + + + axis12 + + + + + + true + + + Private Variable + + + + + + + true + + + vectorToBox1 + + + + + + Private Variable + + + + + + + findFaceContactPoints + + + + + + true + + + Private Function + + + + + + + true + + + overlapOnAxis + + + true + + + 10 + + + Public Function + + + + + + + clip + + + + + + true + + + Private Function + + + + + + + minOverlap + + + + + + true + + + Private Variable + + + + + + + haveCollision + + + true + + + 10 + + + Public Function + + + + + + + true + + + axis + + + + + + Private Variable + + + + + + + axis20 + + + + + + true + + + Private Variable + + + + + + + axis21 + + + + + + true + + + Private Variable + + + + + + + axis22 + + + + + + true + + + Private Variable + + + + + + + true + + + epsilon + + + + + + Private Variable + + + + + + + true + + + pcount + + + + + + Private Variable + + + + + + + tmpPoints + + + + + + true + + + Private Variable + + + + + + + testMainAxis + + + + + + true + + + Private Function + + + + + + + bestAxisIndex + + + + + + true + + + Private Variable + + + + + + + points1 + + + + + + true + + + Private Variable + + + + + + + points2 + + + + + + true + + + Private Variable + + + + + + + reducePoints + + + + + + true + + + Private Function + + + + + + + getContact + + + true + + + 10 + + + Public Function + + + + + + + getPointBoxPenetration + + + + + + true + + + Private Function + + + + + + + testDerivedAxis + + + + + + true + + + Private Function + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + BoxBoxCollider + + + true + + + 10 + + + Class + + + + + + + + + + + getContact + + + true + + + 10 + + + Public Function + + + + + + + clipHighX + + + + + + true + + + Private Function + + + + + + + clipHighZ + + + + + + true + + + Private Function + + + + + + + clipLowX + + + + + + true + + + Private Function + + + + + + + clipLowY + + + + + + true + + + Private Function + + + + + + + getFaceVertsByAxis + + + + + + true + + + Private Function + + + + + + + haveCollision + + + true + + + 10 + + + Public Function + + + + + + + clipHighY + + + + + + true + + + Private Function + + + + + + + clipLowZ + + + + + + true + + + Private Function + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + BoxCollider + + + true + + + 10 + + + Class + + + + + + + + + + + true + + + normal + + + + + + Private Variable + + + + + + + verts1 + + + + + + true + + + Private Variable + + + + + + + verts2 + + + + + + true + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + getContact + + + true + + + 10 + + + Public Function + + + + + + + haveCollision + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + BoxPlaneCollider + + + true + + + 10 + + + Class + + + + + + + + + + + true + + + axis + + + + + + Private Variable + + + + + + + axis10 + + + + + + true + + + Private Variable + + + + + + + axis11 + + + + + + true + + + Private Variable + + + + + + + findEdgesIntersection + + + + + + true + + + Private Function + + + + + + + axis12 + + + + + + true + + + Private Variable + + + + + + + clipByBox + + + + + + true + + + Private Function + + + + + + + true + + + vectorToBox + + + + + + Private Variable + + + + + + + true + + + overlapOnAxis + + + true + + + 10 + + + Public Function + + + + + + + clipByRect + + + + + + true + + + Private Function + + + + + + + minOverlap + + + + + + true + + + Private Variable + + + + + + + haveCollision + + + true + + + 10 + + + Public Function + + + + + + + axis20 + + + + + + true + + + Private Variable + + + + + + + axis21 + + + + + + true + + + Private Variable + + + + + + + findFaceContactPoints + + + + + + true + + + Private Function + + + + + + + axis22 + + + + + + true + + + Private Variable + + + + + + + true + + + epsilon + + + + + + Private Variable + + + + + + + testMainAxis + + + + + + true + + + Private Function + + + + + + + bestAxisIndex + + + + + + true + + + Private Variable + + + + + + + points1 + + + + + + true + + + Private Variable + + + + + + + points2 + + + + + + true + + + Private Variable + + + + + + + getContact + + + true + + + 10 + + + Public Function + + + + + + + getPointBoxPenetration + + + + + + true + + + Private Function + + + + + + + testDerivedAxis + + + + + + true + + + Private Function + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + BoxRectCollider + + + true + + + 10 + + + Class + + + + + + + + + + + sPos + + + + + + true + + + Private Variable + + + + + + + getContact + + + true + + + 10 + + + Public Function + + + + + + + haveCollision + + + true + + + 10 + + + Public Function + + + + + + + bPos + + + + + + true + + + Private Variable + + + + + + + center + + + + + + true + + + Private Variable + + + + + + + closestPt + + + + + + true + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + BoxSphereCollider + + + true + + + 10 + + + Class + + + + + + + + + + + axis11 + + + + + + true + + + Private Variable + + + + + + + axis12 + + + + + + true + + + Private Variable + + + + + + + true + + + vector + + + + + + Private Variable + + + + + + + findFaceContactPoints + + + + + + true + + + Private Function + + + + + + + getBoxToTriContact + + + + + + true + + + Private Function + + + + + + + axis10 + + + + + + true + + + Private Variable + + + + + + + colNormal + + + + + + true + + + Private Variable + + + + + + + true + + + toBox + + + + + + Private Variable + + + + + + + clipByBox + + + + + + true + + + Private Function + + + + + + + minOverlap + + + + + + true + + + Private Variable + + + + + + + true + + + overlapOnAxis + + + + + + Private Function + + + + + + + haveCollision + + + true + + + 10 + + + Public Function + + + + + + + clipByLine + + + + + + true + + + Private Function + + + + + + + getTriToBoxContact + + + + + + true + + + Private Function + + + + + + + true + + + axis + + + + + + Private Variable + + + + + + + axis20 + + + + + + true + + + Private Variable + + + + + + + axis21 + + + + + + true + + + Private Variable + + + + + + + axis22 + + + + + + true + + + Private Variable + + + + + + + point2 + + + + + + true + + + Private Variable + + + + + + + true + + + epsilon + + + true + + + 10 + + + Public Variable + + + + + + + testMainAxis + + + + + + true + + + Private Function + + + + + + + bestAxisIndex + + + + + + true + + + Private Variable + + + + + + + point1 + + + + + + true + + + Private Variable + + + + + + + points1 + + + + + + true + + + Private Variable + + + + + + + clipByTriangle + + + + + + true + + + Private Function + + + + + + + points2 + + + + + + true + + + Private Variable + + + + + + + getContact + + + true + + + 10 + + + Public Function + + + + + + + findEdgesIntersection + + + + + + true + + + Private Function + + + + + + + getPointBoxPenetration + + + + + + true + + + Private Function + + + + + + + testDerivedAxis + + + + + + true + + + Private Function + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + BoxTriangleCollider + + + true + + + 10 + + + Class + + + + + + + + + + + getContact + + + true + + + 10 + + + Public Function + + + + + + + true + + + normal + + + + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + haveCollision + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + SpherePlaneCollider + + + true + + + 10 + + + Class + + + + + + + + + + + p1 + + + + + + true + + + Private Variable + + + + + + + haveCollision + + + true + + + 10 + + + Public Function + + + + + + + p2 + + + + + + true + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + getContact + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + SphereSphereCollider + + + true + + + 10 + + + Class + + + + + + + alternativa.physics.collision.colliders + + + 10 + + + Package + + + + + + + + + + + + + + + true + + + createPrimitive + + + + + + true + + + 10 + + + Private Function + + + + + + + calculateAABB + + + true + + + 10 + + + Public Function + + + + + + + getRayIntersection + + + true + + + 10 + + + Public Function + + + + + + + excludedFaces + + + true + + + 10 + + + Public Variable + + + + + + + hs + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + copyFrom + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + toString + + + true + + + 10 + + + Public Function + + + + + + + CollisionBox + + + true + + + 10 + + + Class + + + + + + + + + + + twoSided + + + true + + + 10 + + + Public Variable + + + + + + + calculateAABB + + + true + + + 10 + + + Public Function + + + + + + + getRayIntersection + + + true + + + 10 + + + Public Function + + + + + + + true + + + createPrimitive + + + + + + true + + + 10 + + + Private Function + + + + + + + hs + + + true + + + 10 + + + Public Variable + + + + + + + copyFrom + + + true + + + 10 + + + Public Function + + + + + + + toString + + + true + + + 10 + + + Public Function + + + + + + + EPSILON + + + + + + true + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + CollisionRect + + + true + + + 10 + + + Class + + + + + + + + + + + r + + + true + + + 10 + + + Public Variable + + + + + + + calculateAABB + + + true + + + 10 + + + Public Function + + + + + + + true + + + createPrimitive + + + + + + true + + + 10 + + + Private Function + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + copyFrom + + + true + + + 10 + + + Public Function + + + + + + + getRayIntersection + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + CollisionSphere + + + true + + + 10 + + + Class + + + + + + + + + + + v0 + + + true + + + 10 + + + Public Variable + + + + + + + calculateAABB + + + true + + + 10 + + + Public Function + + + + + + + getRayIntersection + + + true + + + 10 + + + Public Function + + + + + + + initVertices + + + + + + true + + + Private Function + + + + + + + e0 + + + true + + + 10 + + + Public Variable + + + + + + + e1 + + + true + + + 10 + + + Public Variable + + + + + + + e2 + + + true + + + 10 + + + Public Variable + + + + + + + copyFrom + + + true + + + 10 + + + Public Function + + + + + + + true + + + createPrimitive + + + + + + true + + + 10 + + + Private Function + + + + + + + toString + + + true + + + 10 + + + Public Function + + + + + + + v1 + + + true + + + 10 + + + Public Variable + + + + + + + v2 + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + CollisionTriangle + + + true + + + 10 + + + Class + + + + + + + alternativa.physics.collision.primitives + + + 10 + + + Package + + + + + + + + + + + + + + + clone + + + true + + + 10 + + + Public Function + + + + + + + setSize + + + true + + + 10 + + + Public Function + + + + + + + maxX + + + true + + + 10 + + + Public Variable + + + + + + + maxY + + + true + + + 10 + + + Public Variable + + + + + + + maxZ + + + true + + + 10 + + + Public Variable + + + + + + + minX + + + true + + + 10 + + + Public Variable + + + + + + + minY + + + true + + + 10 + + + Public Variable + + + + + + + minZ + + + true + + + 10 + + + Public Variable + + + + + + + addBoundBox + + + true + + + 10 + + + Public Function + + + + + + + addPoint + + + true + + + 10 + + + Public Function + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + copyFrom + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + infinity + + + true + + + 10 + + + Public Function + + + + + + + intersects + + + true + + + 10 + + + Public Function + + + + + + + toString + + + true + + + 10 + + + Public Function + + + + + + + BoundBox + + + true + + + 10 + + + Class + + + + + + + + + + + dir + + + true + + + 10 + + + Public Variable + + + + + + + origin + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + Ray + + + true + + + 10 + + + Class + + + + + + + + + + + true + + + normal + + + true + + + 10 + + + Public Variable + + + + + + + pos + + + true + + + 10 + + + Public Variable + + + + + + + primitive + + + true + + + 10 + + + Public Variable + + + + + + + t + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + copy + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + RayIntersection + + + true + + + 10 + + + Class + + + + + + + alternativa.physics.collision.types + + + 10 + + + Package + + + + + + + + + + + true + + + + + + + true + + + satisfied + + + + + + Public Variable + + + + + + + true + + + world + + + + + + Public Variable + + + + + + + true + + + apply + + + true + + + 10 + + + Public Function + + + + + + + true + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + true + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + true + + + preProcess + + + true + + + 10 + + + Public Function + + + + + + + Constraint + + + true + + + 10 + + + Class + + + + + + + + + + + preProcess + + + true + + + 10 + + + Public Function + + + + + + + impulseDirection + + + + + + true + + + Private Variable + + + + + + + true + + + body1 + + + + + + Public Variable + + + + + + + true + + + body2 + + + + + + Public Variable + + + + + + + true + + + velByUnitImpulseN + + + + + + Private Variable + + + + + + + apply + + + true + + + 10 + + + Public Function + + + + + + + true + + + maxDistance + + + + + + Public Variable + + + + + + + true + + + r2 + + + + + + Public Variable + + + + + + + true + + + wr1 + + + + + + Public Variable + + + + + + + true + + + wr2 + + + + + + Public Variable + + + + + + + minClosingVel + + + + + + true + + + Private Variable + + + + + + + true + + + r1 + + + + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + MaxDistanceConstraint + + + true + + + 10 + + + Class + + + + + + + alternativa.physics.constraints + + + 10 + + + Package + + + + + + + + + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + setParams + + + true + + + 10 + + + Public Function + + + + + + + RigidBox + + + true + + + 10 + + + Class + + + + + + + true + + + + + + + true + + + h + + + + + + Public Variable + + + + + + + true + + + r + + + + + + Public Variable + + + + + + + true + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + true + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + true + + + setParams + + + true + + + 10 + + + Public Function + + + + + + + RigidCylinder + + + true + + + 10 + + + Class + + + + + + + + + + + true + + + normal + + + true + + + 10 + + + Public Variable + + + + + + + offset + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + RigidPlane + + + true + + + 10 + + + Class + + + + + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + RigidRect + + + true + + + 10 + + + Class + + + + + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + RigidSphere + + + true + + + 10 + + + Class + + + + + + + alternativa.physics.primitives + + + 10 + + + Package + + + + + + + FFFFFF + + + true + + + D:\work\eclipse\ws_projects\TanksTestingTool\lib\AlternativaPhysics.swc + + + AlternativaPhysics.swc + + + 1 + + + 24.0 + + + Not Present + + + + + + + Time Line + + + + + + + 500 X 375 + + + 157458 + + + 10 + + + + + + + 157458 + + + 10 + + + + + + + diff --git a/0.0.9.0/META-INF/MANIFEST.MF b/0.0.9.0/META-INF/MANIFEST.MF new file mode 100644 index 0000000..5066a55 --- /dev/null +++ b/0.0.9.0/META-INF/MANIFEST.MF @@ -0,0 +1 @@ +Bundle-Name: platform.clients.fp10.libraries.AlternativaPhysics diff --git a/0.0.9.0/META-INF/security.sspj b/0.0.9.0/META-INF/security.sspj new file mode 100644 index 0000000..e463c72 --- /dev/null +++ b/0.0.9.0/META-INF/security.sspj @@ -0,0 +1,11923 @@ + + + + + true + + + true + + + true + + + 3.4 + + + true + + + true + + + true + + + + + + + + + + + true + + + FFFFFF + + + D:\work\eclipse\ws_projects\TanksTestingTool\lib\AlternativaPhysics.swc + + + AlternativaPhysics.swc + + + 1 + + + 24.0 + + + Not Present + + + 500 X 375 + + + + + + + true + + + + + + + + + + + + + + + + append + + + true + + + 10 + + + Public Function + + + + + + + getEulerAngles + + + true + + + 10 + + + Public Function + + + + + + + invert + + + true + + + 10 + + + Public Function + + + + + + + prepend + + + true + + + 10 + + + Public Function + + + + + + + setRotationMatrix + + + true + + + 10 + + + Public Function + + + + + + + subtract + + + true + + + 10 + + + Public Function + + + + + + + toIdentity + + + true + + + 10 + + + Public Function + + + + + + + transformVector + + + true + + + 10 + + + Public Function + + + + + + + transformVector3To3D + + + true + + + 10 + + + Public Function + + + + + + + transpose + + + true + + + 10 + + + Public Function + + + + + + + a + + + true + + + 10 + + + Public Variable + + + + + + + b + + + true + + + 10 + + + Public Variable + + + + + + + c + + + true + + + 10 + + + Public Variable + + + + + + + clone + + + true + + + 10 + + + Public Function + + + + + + + e + + + true + + + 10 + + + Public Variable + + + + + + + f + + + true + + + 10 + + + Public Variable + + + + + + + g + + + true + + + 10 + + + Public Variable + + + + + + + add + + + true + + + 10 + + + Public Function + + + + + + + i + + + true + + + 10 + + + Public Variable + + + + + + + j + + + true + + + 10 + + + Public Variable + + + + + + + k + + + true + + + 10 + + + Public Variable + + + + + + + copy + + + true + + + 10 + + + Public Function + + + + + + + createSkewSymmetric + + + true + + + 10 + + + Public Function + + + + + + + fromAxisAngle + + + true + + + 10 + + + Public Function + + + + + + + prependTransposed + + + true + + + 10 + + + Public Function + + + + + + + toString + + + true + + + 10 + + + Public Function + + + + + + + transformVectorInverse + + + true + + + 10 + + + Public Function + + + + + + + IDENTITY + + + true + + + 10 + + + Public Variable + + + + + + + ZERO + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + Matrix3 + + + true + + + 10 + + + Class + + + + + + + + + + + append + + + true + + + 10 + + + Public Function + + + + + + + deltaTransformVectorInverse + + + true + + + 10 + + + Public Function + + + + + + + getAxis + + + true + + + 10 + + + Public Function + + + + + + + invert + + + true + + + 10 + + + Public Function + + + + + + + prepend + + + true + + + 10 + + + Public Function + + + + + + + setFromMatrix3 + + + true + + + 10 + + + Public Function + + + + + + + subtract + + + true + + + 10 + + + Public Function + + + + + + + toIdentity + + + true + + + 10 + + + Public Function + + + + + + + transformVector + + + true + + + 10 + + + Public Function + + + + + + + transformVectors + + + true + + + 10 + + + Public Function + + + + + + + transformVectorsInverse + + + true + + + 10 + + + Public Function + + + + + + + transformVectorsInverseN + + + true + + + 10 + + + Public Function + + + + + + + transformVectorsN + + + true + + + 10 + + + Public Function + + + + + + + a + + + true + + + 10 + + + Public Variable + + + + + + + b + + + true + + + 10 + + + Public Variable + + + + + + + c + + + true + + + 10 + + + Public Variable + + + + + + + d + + + true + + + 10 + + + Public Variable + + + + + + + e + + + true + + + 10 + + + Public Variable + + + + + + + f + + + true + + + 10 + + + Public Variable + + + + + + + g + + + true + + + 10 + + + Public Variable + + + + + + + h + + + true + + + 10 + + + Public Variable + + + + + + + i + + + true + + + 10 + + + Public Variable + + + + + + + j + + + true + + + 10 + + + Public Variable + + + + + + + k + + + true + + + 10 + + + Public Variable + + + + + + + l + + + true + + + 10 + + + Public Variable + + + + + + + add + + + true + + + 10 + + + Public Function + + + + + + + clone + + + true + + + 10 + + + Public Function + + + + + + + copy + + + true + + + 10 + + + Public Function + + + + + + + deltaTransformVector + + + true + + + 10 + + + Public Function + + + + + + + getEulerAngles + + + true + + + 10 + + + Public Function + + + + + + + setOrientationFromMatrix3 + + + true + + + 10 + + + Public Function + + + + + + + toString + + + true + + + 10 + + + Public Function + + + + + + + transformVectorInverse + + + true + + + 10 + + + Public Function + + + + + + + IDENTITY + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + Matrix4 + + + true + + + 10 + + + Class + + + + + + + + + + + append + + + true + + + 10 + + + Public Function + + + + + + + clone + + + true + + + 10 + + + Public Function + + + + + + + conjugate + + + true + + + 10 + + + Public Function + + + + + + + diff + + + true + + + 10 + + + Public Function + + + + + + + getEulerAngles + + + true + + + 10 + + + Public Function + + + + + + + length + + + true + + + 10 + + + Public Function + + + + + + + lengthSqr + + + true + + + 10 + + + Public Function + + + + + + + nlerp + + + true + + + 10 + + + Public Function + + + + + + + normalize + + + true + + + 10 + + + Public Function + + + + + + + prepend + + + true + + + 10 + + + Public Function + + + + + + + reset + + + true + + + 10 + + + Public Function + + + + + + + rotateByVector + + + true + + + 10 + + + Public Function + + + + + + + setFromAxisAngle + + + true + + + 10 + + + Public Function + + + + + + + setFromAxisAngleComponents + + + true + + + 10 + + + Public Function + + + + + + + setFromEulerAnglesXYZ + + + true + + + 10 + + + Public Function + + + + + + + slerp + + + true + + + 10 + + + Public Function + + + + + + + subtract + + + true + + + 10 + + + Public Function + + + + + + + toAxisVector + + + true + + + 10 + + + Public Function + + + + + + + toMatrix3 + + + true + + + 10 + + + Public Function + + + + + + + toString + + + true + + + 10 + + + Public Function + + + + + + + toVector3D + + + true + + + 10 + + + Public Function + + + + + + + w + + + true + + + 10 + + + Public Variable + + + + + + + x + + + true + + + 10 + + + Public Variable + + + + + + + y + + + true + + + 10 + + + Public Variable + + + + + + + z + + + true + + + 10 + + + Public Variable + + + + + + + addScaledVector + + + true + + + 10 + + + Public Function + + + + + + + copy + + + true + + + 10 + + + Public Function + + + + + + + createFromAxisAngle + + + true + + + 10 + + + Public Function + + + + + + + createFromAxisAngleComponents + + + true + + + 10 + + + Public Function + + + + + + + multiply + + + true + + + 10 + + + Public Function + + + + + + + _q + + + + + + true + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + Quaternion + + + true + + + 10 + + + Class + + + + + + + + + + + copyFromVector3D + + + true + + + 10 + + + Public Function + + + + + + + toString + + + true + + + 10 + + + Public Function + + + + + + + toVector3D + + + true + + + 10 + + + Public Function + + + + + + + vAdd + + + true + + + 10 + + + Public Function + + + + + + + vAddScaled + + + true + + + 10 + + + Public Function + + + + + + + vClone + + + true + + + 10 + + + Public Function + + + + + + + vCopy + + + true + + + 10 + + + Public Function + + + + + + + vCross + + + true + + + 10 + + + Public Function + + + + + + + vCross2 + + + true + + + 10 + + + Public Function + + + + + + + vDeltaTransformBy4 + + + true + + + 10 + + + Public Function + + + + + + + vDiff + + + true + + + 10 + + + Public Function + + + + + + + vDot + + + true + + + 10 + + + Public Function + + + + + + + vLength + + + true + + + 10 + + + Public Function + + + + + + + vLengthSqr + + + true + + + 10 + + + Public Function + + + + + + + vNormalize + + + true + + + 10 + + + Public Function + + + + + + + vReset + + + true + + + 10 + + + Public Function + + + + + + + vReverse + + + true + + + 10 + + + Public Function + + + + + + + vScale + + + true + + + 10 + + + Public Function + + + + + + + vSetLength + + + true + + + 10 + + + Public Function + + + + + + + vSubtract + + + true + + + 10 + + + Public Function + + + + + + + vSum + + + true + + + 10 + + + Public Function + + + + + + + vTransformBy3 + + + true + + + 10 + + + Public Function + + + + + + + vTransformBy3Tr + + + true + + + 10 + + + Public Function + + + + + + + vTransformBy4 + + + true + + + 10 + + + Public Function + + + + + + + vTransformInverseBy4 + + + true + + + 10 + + + Public Function + + + + + + + X_AXIS + + + true + + + 10 + + + Public Variable + + + + + + + Y_AXIS + + + true + + + 10 + + + Public Variable + + + + + + + Z_AXIS + + + true + + + 10 + + + Public Variable + + + + + + + ZERO + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + Vector3 + + + true + + + 10 + + + Class + + + + + + + alternativa.math + + + 10 + + + Package + + + + + + + + + + + + + + + interpolate + + + true + + + 10 + + + Public Function + + + + + + + setRotation + + + true + + + 10 + + + Public Function + + + + + + + canFreeze + + + true + + + 10 + + + Public Variable + + + + + + + invInertiaWorld + + + true + + + 10 + + + Public Variable + + + + + + + invMass + + + true + + + 10 + + + Public Variable + + + + + + + setRotationXYZ + + + true + + + 10 + + + Public Function + + + + + + + invInertia + + + true + + + 10 + + + Public Variable + + + + + + + name + + + true + + + 10 + + + Public Variable + + + + + + + state + + + true + + + 10 + + + Public Variable + + + + + + + world + + + true + + + 10 + + + Public Variable + + + + + + + addTorque + + + true + + + 10 + + + Public Function + + + + + + + true + + + saveState + + + + + + Public Function + + + + + + + setOrientation + + + true + + + 10 + + + Public Function + + + + + + + id + + + true + + + 10 + + + Public Variable + + + + + + + applyRelPosWorldImpulse + + + true + + + 10 + + + Public Function + + + + + + + true + + + integratePosition + + + + + + Public Function + + + + + + + frozen + + + true + + + 10 + + + Public Variable + + + + + + + prevState + + + true + + + 10 + + + Public Variable + + + + + + + true + + + calcAccelerations + + + + + + Public Function + + + + + + + true + + + restoreState + + + + + + Public Function + + + + + + + material + + + true + + + 10 + + + Public Variable + + + + + + + true + + + clearAccumulators + + + + + + Public Function + + + + + + + setPosition + + + true + + + 10 + + + Public Function + + + + + + + contactsNum + + + true + + + 10 + + + Public Variable + + + + + + + forceAccum + + + true + + + 10 + + + Public Variable + + + + + + + addWorldForceAtLocalPoint + + + true + + + 10 + + + Public Function + + + + + + + beforePhysicsStep + + + true + + + 10 + + + Public Function + + + + + + + true + + + integrateVelocity + + + + + + Public Function + + + + + + + baseMatrix + + + true + + + 10 + + + Public Variable + + + + + + + collisionPrimitives + + + true + + + 10 + + + Public Variable + + + + + + + addLocalForce + + + true + + + 10 + + + Public Function + + + + + + + addWorldForceXYZ + + + true + + + 10 + + + Public Function + + + + + + + calcDerivedData + + + true + + + 10 + + + Public Function + + + + + + + setVelocityXYZ + + + true + + + 10 + + + Public Function + + + + + + + angleAccel + + + true + + + 10 + + + Public Variable + + + + + + + contacts + + + true + + + 10 + + + Public Variable + + + + + + + addCollisionPrimitive + + + true + + + 10 + + + Public Function + + + + + + + addForce + + + true + + + 10 + + + Public Function + + + + + + + setVelocity + + + true + + + 10 + + + Public Function + + + + + + + aabb + + + true + + + 10 + + + Public Variable + + + + + + + accel + + + true + + + 10 + + + Public Variable + + + + + + + freezeCounter + + + true + + + 10 + + + Public Variable + + + + + + + movable + + + true + + + 10 + + + Public Variable + + + + + + + postCollisionPredicate + + + true + + + 10 + + + Public Variable + + + + + + + torqueAccum + + + true + + + 10 + + + Public Variable + + + + + + + addWorldForce + + + true + + + 10 + + + Public Function + + + + + + + addWorldForceScaled + + + true + + + 10 + + + Public Function + + + + + + + MAX_CONTACTS + + + true + + + 10 + + + Public Variable + + + + + + + addForceXYZ + + + true + + + 10 + + + Public Function + + + + + + + removeCollisionPrimitive + + + true + + + 10 + + + Public Function + + + + + + + setPositionXYZ + + + true + + + 10 + + + Public Function + + + + + + + linDamping + + + true + + + 10 + + + Public Variable + + + + + + + _f + + + + + + true + + + Private Variable + + + + + + + _r + + + + + + true + + + Private Variable + + + + + + + rotDamping + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + Body + + + true + + + 10 + + + Class + + + + + + + + + + + head + + + true + + + 10 + + + Public Variable + + + + + + + size + + + true + + + 10 + + + Public Variable + + + + + + + remove + + + true + + + 10 + + + Public Function + + + + + + + tail + + + true + + + 10 + + + Public Variable + + + + + + + append + + + true + + + 10 + + + Public Function + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + findItem + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + BodyList + + + true + + + 10 + + + Class + + + + + + + + + + + body + + + true + + + 10 + + + Public Variable + + + + + + + next + + + true + + + 10 + + + Public Variable + + + + + + + prev + + + true + + + 10 + + + Public Variable + + + + + + + clearPool + + + true + + + 10 + + + Public Function + + + + + + + dispose + + + true + + + 10 + + + Public Function + + + + + + + poolTop + + + + + + true + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + create + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + BodyListItem + + + true + + + 10 + + + Class + + + + + + + + + + + friction + + + true + + + 10 + + + Public Variable + + + + + + + restitution + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + BodyMaterial + + + true + + + 10 + + + Class + + + + + + + + + + + pos + + + true + + + 10 + + + Public Variable + + + + + + + copy + + + true + + + 10 + + + Public Function + + + + + + + orientation + + + true + + + 10 + + + Public Variable + + + + + + + rotation + + + true + + + 10 + + + Public Variable + + + + + + + velocity + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + BodyState + + + true + + + 10 + + + Class + + + + + + + + + + + head + + + true + + + 10 + + + Public Variable + + + + + + + size + + + true + + + 10 + + + Public Variable + + + + + + + remove + + + true + + + 10 + + + Public Function + + + + + + + tail + + + true + + + 10 + + + Public Variable + + + + + + + append + + + true + + + 10 + + + Public Function + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + clear + + + true + + + 10 + + + Public Function + + + + + + + findItem + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + CollisionPrimitiveList + + + true + + + 10 + + + Class + + + + + + + + + + + next + + + true + + + 10 + + + Public Variable + + + + + + + prev + + + true + + + 10 + + + Public Variable + + + + + + + primitive + + + true + + + 10 + + + Public Variable + + + + + + + clearPool + + + true + + + 10 + + + Public Function + + + + + + + dispose + + + true + + + 10 + + + Public Function + + + + + + + poolTop + + + + + + true + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + create + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + CollisionPrimitiveListItem + + + true + + + 10 + + + Class + + + + + + + + + + + body1 + + + true + + + 10 + + + Public Variable + + + + + + + body2 + + + true + + + 10 + + + Public Variable + + + + + + + friction + + + true + + + 10 + + + Public Variable + + + + + + + MAX_POINTS + + + + + + true + + + Private Variable + + + + + + + index + + + true + + + 10 + + + Public Variable + + + + + + + maxPenetration + + + true + + + 10 + + + Public Variable + + + + + + + next + + + true + + + 10 + + + Public Variable + + + + + + + true + + + normal + + + true + + + 10 + + + Public Variable + + + + + + + true + + + pcount + + + true + + + 10 + + + Public Variable + + + + + + + points + + + true + + + 10 + + + Public Variable + + + + + + + restitution + + + true + + + 10 + + + Public Variable + + + + + + + satisfied + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + Contact + + + true + + + 10 + + + Class + + + + + + + + + + + accumImpulseN + + + true + + + 10 + + + Public Variable + + + + + + + angularInertia2 + + + true + + + 10 + + + Public Variable + + + + + + + feature1 + + + true + + + 10 + + + Public Variable + + + + + + + feature2 + + + true + + + 10 + + + Public Variable + + + + + + + minSepVel + + + true + + + 10 + + + Public Variable + + + + + + + normalVel + + + true + + + 10 + + + Public Variable + + + + + + + pos + + + true + + + 10 + + + Public Variable + + + + + + + r1 + + + true + + + 10 + + + Public Variable + + + + + + + r2 + + + true + + + 10 + + + Public Variable + + + + + + + copyFrom + + + true + + + 10 + + + Public Function + + + + + + + angularInertia1 + + + true + + + 10 + + + Public Variable + + + + + + + penetration + + + true + + + 10 + + + Public Variable + + + + + + + satisfied + + + true + + + 10 + + + Public Variable + + + + + + + true + + + velByUnitImpulseN + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + ContactPoint + + + true + + + 10 + + + Class + + + + + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + getBoxInvInertia + + + true + + + 10 + + + Public Function + + + + + + + getCylinderInvInertia + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + PhysicsUtils + + + true + + + 10 + + + Class + + + + + + + + + + + removeConstraint + + + true + + + 10 + + + Public Function + + + + + + + _gravityMagnitude + + + true + + + 10 + + + Public Variable + + + + + + + allowedPenetration + + + true + + + 10 + + + Public Variable + + + + + + + collisionIterations + + + true + + + 10 + + + Public Variable + + + + + + + penResolutionSteps + + + true + + + 10 + + + Public Variable + + + + + + + timeStamp + + + true + + + 10 + + + Public Variable + + + + + + + detectCollisions + + + + + + true + + + Private Function + + + + + + + intergateVelocities + + + + + + true + + + Private Function + + + + + + + resolveInterpenetration + + + + + + true + + + Private Function + + + + + + + _gravity + + + true + + + 10 + + + Public Variable + + + + + + + postPhysics + + + + + + true + + + Private Function + + + + + + + resolveContact + + + + + + true + + + Private Function + + + + + + + separateContactPoint + + + + + + true + + + Private Function + + + + + + + angSpeedFreezeLimit + + + true + + + 10 + + + Public Variable + + + + + + + collisionDetector + + + true + + + 10 + + + Public Variable + + + + + + + _v1 + + + + + + true + + + Private Variable + + + + + + + _v2 + + + + + + true + + + Private Variable + + + + + + + maxPenResolutionSpeed + + + true + + + 10 + + + Public Variable + + + + + + + staticSeparationIterations + + + true + + + 10 + + + Public Variable + + + + + + + usePrediction + + + true + + + 10 + + + Public Variable + + + + + + + gravity + + + true + + + 10 + + + Public Function + + + + + + + applyForces + + + + + + true + + + Private Function + + + + + + + true + + + constraints + + + + + + Public Variable + + + + + + + staticSeparationSteps + + + true + + + 10 + + + Public Variable + + + + + + + integratePositions + + + + + + true + + + Private Function + + + + + + + removeBody + + + true + + + 10 + + + Public Function + + + + + + + runPhysics + + + true + + + 10 + + + Public Function + + + + + + + preProcessContacts + + + + + + true + + + Private Function + + + + + + + bodies + + + true + + + 10 + + + Public Variable + + + + + + + true + + + contacts + + + + + + Public Variable + + + + + + + freezeSteps + + + true + + + 10 + + + Public Variable + + + + + + + linSpeedFreezeLimit + + + true + + + 10 + + + Public Variable + + + + + + + maxAngleMove + + + true + + + 10 + + + Public Variable + + + + + + + time + + + true + + + 10 + + + Public Variable + + + + + + + gravity + + + true + + + 10 + + + Public Function + + + + + + + borderContact + + + + + + true + + + Private Variable + + + + + + + addConstraint + + + true + + + 10 + + + Public Function + + + + + + + processContacts + + + + + + true + + + Private Function + + + + + + + addBody + + + true + + + 10 + + + Public Function + + + + + + + calcSepVelocity + + + + + + true + + + Private Function + + + + + + + performStaticSeparation + + + + + + true + + + Private Function + + + + + + + resolveContactPoint + + + + + + true + + + Private Function + + + + + + + true + + + constraintsNum + + + + + + Public Variable + + + + + + + true + + + MAX_CONTACTS + + + + + + Public Variable + + + + + + + _r + + + + + + true + + + Private Variable + + + + + + + _t + + + + + + true + + + Private Variable + + + + + + + _v + + + + + + true + + + Private Variable + + + + + + + contactIterations + + + true + + + 10 + + + Public Variable + + + + + + + useStaticSeparation + + + true + + + 10 + + + Public Variable + + + + + + + lastBodyId + + + + + + true + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + World + + + true + + + 10 + + + Class + + + + + + + alternativa.physics + + + 10 + + + Package + + + + + + + + + + + + + + + true + + + axis + + + true + + + 10 + + + Public Variable + + + + + + + boundBox + + + true + + + 10 + + + Public Variable + + + + + + + coord + + + true + + + 10 + + + Public Variable + + + + + + + indices + + + true + + + 10 + + + Public Variable + + + + + + + negativeNode + + + true + + + 10 + + + Public Variable + + + + + + + parent + + + true + + + 10 + + + Public Variable + + + + + + + positiveNode + + + true + + + 10 + + + Public Variable + + + + + + + splitIndices + + + true + + + 10 + + + Public Variable + + + + + + + splitTree + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + CollisionKdNode + + + true + + + 10 + + + Class + + + + + + + + + + + minPrimitivesPerNode + + + true + + + 10 + + + Public Variable + + + + + + + splitNode + + + + + + true + + + Private Function + + + + + + + rootNode + + + true + + + 10 + + + Public Variable + + + + + + + splitCost + + + + + + true + + + Private Variable + + + + + + + staticBoundBoxes + + + true + + + 10 + + + Public Variable + + + + + + + splitCoord + + + + + + true + + + Private Variable + + + + + + + staticChildren + + + true + + + 10 + + + Public Variable + + + + + + + threshold + + + true + + + 10 + + + Public Variable + + + + + + + traceTree + + + true + + + 10 + + + Public Function + + + + + + + numStaticChildren + + + true + + + 10 + + + Public Variable + + + + + + + checkNodeAxis + + + + + + true + + + Private Function + + + + + + + _bb + + + + + + true + + + Private Variable + + + + + + + _nodeBB + + + + + + true + + + Private Variable + + + + + + + nodeBoundBoxThreshold + + + + + + true + + + Private Variable + + + + + + + splitAxis + + + + + + true + + + Private Variable + + + + + + + createTree + + + true + + + 10 + + + Public Function + + + + + + + traceNode + + + + + + true + + + Private Function + + + + + + + splitCoordsX + + + + + + true + + + Private Variable + + + + + + + splitCoordsY + + + + + + true + + + Private Variable + + + + + + + splitCoordsZ + + + + + + true + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + CollisionKdTree + + + true + + + 10 + + + Class + + + + + + + + + + + minPrimitivesPerNode + + + true + + + 10 + + + Public Variable + + + + + + + rootNode + + + true + + + 10 + + + Public Variable + + + + + + + splitCoord + + + + + + true + + + Private Variable + + + + + + + splitCost + + + + + + true + + + Private Variable + + + + + + + parentNode + + + true + + + 10 + + + Public Variable + + + + + + + parentTree + + + true + + + 10 + + + Public Variable + + + + + + + threshold + + + true + + + 10 + + + Public Variable + + + + + + + checkNodeAxis + + + + + + true + + + Private Function + + + + + + + createTree + + + true + + + 10 + + + Public Function + + + + + + + _bb + + + + + + true + + + Private Variable + + + + + + + nodeBoundBoxThreshold + + + + + + true + + + Private Variable + + + + + + + splitAxis + + + + + + true + + + Private Variable + + + + + + + splitNode + + + + + + true + + + Private Function + + + + + + + _nodeBB + + + + + + true + + + Private Variable + + + + + + + splitCoordsX + + + + + + true + + + Private Variable + + + + + + + splitCoordsY + + + + + + true + + + Private Variable + + + + + + + splitCoordsZ + + + + + + true + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + CollisionKdTree2D + + + true + + + 10 + + + Class + + + + + + + + + + + aabb + + + true + + + 10 + + + Public Variable + + + + + + + body + + + true + + + 10 + + + Public Variable + + + + + + + localTransform + + + true + + + 10 + + + Public Variable + + + + + + + postCollisionPredicate + + + true + + + 10 + + + Public Variable + + + + + + + calculateAABB + + + true + + + 10 + + + Public Function + + + + + + + setBody + + + true + + + 10 + + + Public Function + + + + + + + collisionGroup + + + true + + + 10 + + + Public Variable + + + + + + + transform + + + true + + + 10 + + + Public Variable + + + + + + + true + + + createPrimitive + + + + + + true + + + 10 + + + Private Function + + + + + + + clone + + + true + + + 10 + + + Public Function + + + + + + + BOX + + + true + + + 10 + + + Public Variable + + + + + + + PLANE + + + true + + + 10 + + + Public Variable + + + + + + + SPHERE + + + true + + + 10 + + + Public Variable + + + + + + + TRIANGLE + + + true + + + 10 + + + Public Variable + + + + + + + type + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + copyFrom + + + true + + + 10 + + + Public Function + + + + + + + getRayIntersection + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + toString + + + true + + + 10 + + + Public Function + + + + + + + RECT + + + true + + + 10 + + + Public Variable + + + + + + + CollisionPrimitive + + + true + + + 10 + + + Class + + + + + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + true + + + considerBodies + + + + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + IBodyCollisionPredicate + + + true + + + 10 + + + Class + + + + + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + getContact + + + + + + 10 + + + Public Function + + + + + + + haveCollision + + + + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + ICollider + + + true + + + 10 + + + Class + + + + + + + true + + + + + + + true + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + true + + + getAllContacts + + + + + + Public Function + + + + + + + true + + + getContact + + + + + + 10 + + + Public Function + + + + + + + true + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + true + + + intersectRay + + + + + + Public Function + + + + + + + true + + + testCollision + + + + + + Public Function + + + + + + + ICollisionDetector + + + true + + + 10 + + + Class + + + + + + + true + + + + + + + true + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + true + + + considerCollision + + + + + + Public Function + + + + + + + true + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + ICollisionPredicate + + + true + + + 10 + + + Class + + + + + + + true + + + + + + + true + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + true + + + considerBody + + + + + + Public Function + + + + + + + true + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + IRayCollisionPredicate + + + true + + + 10 + + + Class + + + + + + + + + + + addPrimitive + + + true + + + 10 + + + Public Function + + + + + + + removePrimitive + + + true + + + 10 + + + Public Function + + + + + + + testCollision + + + true + + + 10 + + + Public Function + + + + + + + true + + + dynamicPrimitives + + + + + + Public Variable + + + + + + + init + + + true + + + 10 + + + Public Function + + + + + + + intersectRayWithStatic + + + true + + + 10 + + + Public Function + + + + + + + intersectRayWithDynamic + + + + + + true + + + Private Function + + + + + + + intersectRay + + + true + + + 10 + + + Public Function + + + + + + + testBodyPrimitiveCollision + + + true + + + 10 + + + Public Function + + + + + + + getRayNodeIntersection + + + + + + true + + + Private Function + + + + + + + _dynamicIntersection + + + + + + true + + + Private Variable + + + + + + + getContact + + + true + + + 10 + + + Public Function + + + + + + + true + + + threshold + + + + + + Public Variable + + + + + + + _time + + + + + + true + + + Private Variable + + + + + + + getAllContacts + + + true + + + 10 + + + Public Function + + + + + + + _n + + + + + + true + + + Private Variable + + + + + + + _o + + + + + + true + + + Private Variable + + + + + + + addCollider + + + + + + true + + + Private Function + + + + + + + getPrimitiveNodeCollisions + + + + + + true + + + Private Function + + + + + + + true + + + tree + + + + + + Public Variable + + + + + + + getRayBoundBoxIntersection + + + + + + true + + + Private Function + + + + + + + testRayAgainstNode + + + + + + true + + + Private Function + + + + + + + true + + + dynamicPrimitivesNum + + + + + + Public Variable + + + + + + + _rayAABB + + + + + + true + + + Private Variable + + + + + + + colliders + + + + + + true + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + KdTreeCollisionDetector + + + true + + + 10 + + + Class + + + + + + + alternativa.physics.collision + + + 10 + + + Package + + + + + + + + + + + + + + + axis10 + + + + + + true + + + Private Variable + + + + + + + axis11 + + + + + + true + + + Private Variable + + + + + + + findEdgesIntersection + + + + + + true + + + Private Function + + + + + + + axis12 + + + + + + true + + + Private Variable + + + + + + + true + + + vectorToBox1 + + + + + + Private Variable + + + + + + + findFaceContactPoints + + + + + + true + + + Private Function + + + + + + + true + + + overlapOnAxis + + + true + + + 10 + + + Public Function + + + + + + + clip + + + + + + true + + + Private Function + + + + + + + minOverlap + + + + + + true + + + Private Variable + + + + + + + haveCollision + + + true + + + 10 + + + Public Function + + + + + + + true + + + axis + + + + + + Private Variable + + + + + + + axis20 + + + + + + true + + + Private Variable + + + + + + + axis21 + + + + + + true + + + Private Variable + + + + + + + axis22 + + + + + + true + + + Private Variable + + + + + + + true + + + epsilon + + + + + + Private Variable + + + + + + + true + + + pcount + + + + + + Private Variable + + + + + + + tmpPoints + + + + + + true + + + Private Variable + + + + + + + testMainAxis + + + + + + true + + + Private Function + + + + + + + bestAxisIndex + + + + + + true + + + Private Variable + + + + + + + points1 + + + + + + true + + + Private Variable + + + + + + + points2 + + + + + + true + + + Private Variable + + + + + + + reducePoints + + + + + + true + + + Private Function + + + + + + + getContact + + + true + + + 10 + + + Public Function + + + + + + + getPointBoxPenetration + + + + + + true + + + Private Function + + + + + + + testDerivedAxis + + + + + + true + + + Private Function + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + BoxBoxCollider + + + true + + + 10 + + + Class + + + + + + + + + + + getContact + + + true + + + 10 + + + Public Function + + + + + + + clipHighX + + + + + + true + + + Private Function + + + + + + + clipHighZ + + + + + + true + + + Private Function + + + + + + + clipLowX + + + + + + true + + + Private Function + + + + + + + clipLowY + + + + + + true + + + Private Function + + + + + + + getFaceVertsByAxis + + + + + + true + + + Private Function + + + + + + + haveCollision + + + true + + + 10 + + + Public Function + + + + + + + clipHighY + + + + + + true + + + Private Function + + + + + + + clipLowZ + + + + + + true + + + Private Function + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + BoxCollider + + + true + + + 10 + + + Class + + + + + + + + + + + true + + + normal + + + + + + Private Variable + + + + + + + verts1 + + + + + + true + + + Private Variable + + + + + + + verts2 + + + + + + true + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + getContact + + + true + + + 10 + + + Public Function + + + + + + + haveCollision + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + BoxPlaneCollider + + + true + + + 10 + + + Class + + + + + + + + + + + true + + + axis + + + + + + Private Variable + + + + + + + axis10 + + + + + + true + + + Private Variable + + + + + + + axis11 + + + + + + true + + + Private Variable + + + + + + + findEdgesIntersection + + + + + + true + + + Private Function + + + + + + + axis12 + + + + + + true + + + Private Variable + + + + + + + clipByBox + + + + + + true + + + Private Function + + + + + + + true + + + vectorToBox + + + + + + Private Variable + + + + + + + true + + + overlapOnAxis + + + true + + + 10 + + + Public Function + + + + + + + clipByRect + + + + + + true + + + Private Function + + + + + + + minOverlap + + + + + + true + + + Private Variable + + + + + + + haveCollision + + + true + + + 10 + + + Public Function + + + + + + + axis20 + + + + + + true + + + Private Variable + + + + + + + axis21 + + + + + + true + + + Private Variable + + + + + + + findFaceContactPoints + + + + + + true + + + Private Function + + + + + + + axis22 + + + + + + true + + + Private Variable + + + + + + + true + + + epsilon + + + + + + Private Variable + + + + + + + testMainAxis + + + + + + true + + + Private Function + + + + + + + bestAxisIndex + + + + + + true + + + Private Variable + + + + + + + points1 + + + + + + true + + + Private Variable + + + + + + + points2 + + + + + + true + + + Private Variable + + + + + + + getContact + + + true + + + 10 + + + Public Function + + + + + + + getPointBoxPenetration + + + + + + true + + + Private Function + + + + + + + testDerivedAxis + + + + + + true + + + Private Function + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + BoxRectCollider + + + true + + + 10 + + + Class + + + + + + + + + + + sPos + + + + + + true + + + Private Variable + + + + + + + getContact + + + true + + + 10 + + + Public Function + + + + + + + haveCollision + + + true + + + 10 + + + Public Function + + + + + + + bPos + + + + + + true + + + Private Variable + + + + + + + center + + + + + + true + + + Private Variable + + + + + + + closestPt + + + + + + true + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + BoxSphereCollider + + + true + + + 10 + + + Class + + + + + + + + + + + axis11 + + + + + + true + + + Private Variable + + + + + + + axis12 + + + + + + true + + + Private Variable + + + + + + + true + + + vector + + + + + + Private Variable + + + + + + + findFaceContactPoints + + + + + + true + + + Private Function + + + + + + + getBoxToTriContact + + + + + + true + + + Private Function + + + + + + + axis10 + + + + + + true + + + Private Variable + + + + + + + colNormal + + + + + + true + + + Private Variable + + + + + + + true + + + toBox + + + + + + Private Variable + + + + + + + clipByBox + + + + + + true + + + Private Function + + + + + + + minOverlap + + + + + + true + + + Private Variable + + + + + + + true + + + overlapOnAxis + + + + + + Private Function + + + + + + + haveCollision + + + true + + + 10 + + + Public Function + + + + + + + clipByLine + + + + + + true + + + Private Function + + + + + + + getTriToBoxContact + + + + + + true + + + Private Function + + + + + + + true + + + axis + + + + + + Private Variable + + + + + + + axis20 + + + + + + true + + + Private Variable + + + + + + + axis21 + + + + + + true + + + Private Variable + + + + + + + axis22 + + + + + + true + + + Private Variable + + + + + + + point2 + + + + + + true + + + Private Variable + + + + + + + true + + + epsilon + + + true + + + 10 + + + Public Variable + + + + + + + testMainAxis + + + + + + true + + + Private Function + + + + + + + bestAxisIndex + + + + + + true + + + Private Variable + + + + + + + point1 + + + + + + true + + + Private Variable + + + + + + + points1 + + + + + + true + + + Private Variable + + + + + + + clipByTriangle + + + + + + true + + + Private Function + + + + + + + points2 + + + + + + true + + + Private Variable + + + + + + + getContact + + + true + + + 10 + + + Public Function + + + + + + + findEdgesIntersection + + + + + + true + + + Private Function + + + + + + + getPointBoxPenetration + + + + + + true + + + Private Function + + + + + + + testDerivedAxis + + + + + + true + + + Private Function + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + BoxTriangleCollider + + + true + + + 10 + + + Class + + + + + + + + + + + getContact + + + true + + + 10 + + + Public Function + + + + + + + true + + + normal + + + + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + haveCollision + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + SpherePlaneCollider + + + true + + + 10 + + + Class + + + + + + + + + + + p1 + + + + + + true + + + Private Variable + + + + + + + haveCollision + + + true + + + 10 + + + Public Function + + + + + + + p2 + + + + + + true + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + getContact + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + SphereSphereCollider + + + true + + + 10 + + + Class + + + + + + + alternativa.physics.collision.colliders + + + 10 + + + Package + + + + + + + + + + + + + + + true + + + createPrimitive + + + + + + true + + + 10 + + + Private Function + + + + + + + calculateAABB + + + true + + + 10 + + + Public Function + + + + + + + getRayIntersection + + + true + + + 10 + + + Public Function + + + + + + + excludedFaces + + + true + + + 10 + + + Public Variable + + + + + + + hs + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + copyFrom + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + toString + + + true + + + 10 + + + Public Function + + + + + + + CollisionBox + + + true + + + 10 + + + Class + + + + + + + + + + + twoSided + + + true + + + 10 + + + Public Variable + + + + + + + calculateAABB + + + true + + + 10 + + + Public Function + + + + + + + getRayIntersection + + + true + + + 10 + + + Public Function + + + + + + + true + + + createPrimitive + + + + + + true + + + 10 + + + Private Function + + + + + + + hs + + + true + + + 10 + + + Public Variable + + + + + + + copyFrom + + + true + + + 10 + + + Public Function + + + + + + + toString + + + true + + + 10 + + + Public Function + + + + + + + EPSILON + + + + + + true + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + CollisionRect + + + true + + + 10 + + + Class + + + + + + + + + + + r + + + true + + + 10 + + + Public Variable + + + + + + + calculateAABB + + + true + + + 10 + + + Public Function + + + + + + + true + + + createPrimitive + + + + + + true + + + 10 + + + Private Function + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + copyFrom + + + true + + + 10 + + + Public Function + + + + + + + getRayIntersection + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + CollisionSphere + + + true + + + 10 + + + Class + + + + + + + + + + + v0 + + + true + + + 10 + + + Public Variable + + + + + + + calculateAABB + + + true + + + 10 + + + Public Function + + + + + + + getRayIntersection + + + true + + + 10 + + + Public Function + + + + + + + initVertices + + + + + + true + + + Private Function + + + + + + + e0 + + + true + + + 10 + + + Public Variable + + + + + + + e1 + + + true + + + 10 + + + Public Variable + + + + + + + e2 + + + true + + + 10 + + + Public Variable + + + + + + + copyFrom + + + true + + + 10 + + + Public Function + + + + + + + true + + + createPrimitive + + + + + + true + + + 10 + + + Private Function + + + + + + + toString + + + true + + + 10 + + + Public Function + + + + + + + v1 + + + true + + + 10 + + + Public Variable + + + + + + + v2 + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + CollisionTriangle + + + true + + + 10 + + + Class + + + + + + + alternativa.physics.collision.primitives + + + 10 + + + Package + + + + + + + + + + + + + + + clone + + + true + + + 10 + + + Public Function + + + + + + + setSize + + + true + + + 10 + + + Public Function + + + + + + + maxX + + + true + + + 10 + + + Public Variable + + + + + + + maxY + + + true + + + 10 + + + Public Variable + + + + + + + maxZ + + + true + + + 10 + + + Public Variable + + + + + + + minX + + + true + + + 10 + + + Public Variable + + + + + + + minY + + + true + + + 10 + + + Public Variable + + + + + + + minZ + + + true + + + 10 + + + Public Variable + + + + + + + addBoundBox + + + true + + + 10 + + + Public Function + + + + + + + addPoint + + + true + + + 10 + + + Public Function + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + copyFrom + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + infinity + + + true + + + 10 + + + Public Function + + + + + + + intersects + + + true + + + 10 + + + Public Function + + + + + + + toString + + + true + + + 10 + + + Public Function + + + + + + + BoundBox + + + true + + + 10 + + + Class + + + + + + + + + + + dir + + + true + + + 10 + + + Public Variable + + + + + + + origin + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + Ray + + + true + + + 10 + + + Class + + + + + + + + + + + true + + + normal + + + true + + + 10 + + + Public Variable + + + + + + + pos + + + true + + + 10 + + + Public Variable + + + + + + + primitive + + + true + + + 10 + + + Public Variable + + + + + + + t + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + copy + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + RayIntersection + + + true + + + 10 + + + Class + + + + + + + alternativa.physics.collision.types + + + 10 + + + Package + + + + + + + + + + + true + + + + + + + true + + + satisfied + + + + + + Public Variable + + + + + + + true + + + world + + + + + + Public Variable + + + + + + + true + + + apply + + + true + + + 10 + + + Public Function + + + + + + + true + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + true + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + true + + + preProcess + + + true + + + 10 + + + Public Function + + + + + + + Constraint + + + true + + + 10 + + + Class + + + + + + + + + + + preProcess + + + true + + + 10 + + + Public Function + + + + + + + impulseDirection + + + + + + true + + + Private Variable + + + + + + + true + + + body1 + + + + + + Public Variable + + + + + + + true + + + body2 + + + + + + Public Variable + + + + + + + true + + + velByUnitImpulseN + + + + + + Private Variable + + + + + + + apply + + + true + + + 10 + + + Public Function + + + + + + + true + + + maxDistance + + + + + + Public Variable + + + + + + + true + + + r2 + + + + + + Public Variable + + + + + + + true + + + wr1 + + + + + + Public Variable + + + + + + + true + + + wr2 + + + + + + Public Variable + + + + + + + minClosingVel + + + + + + true + + + Private Variable + + + + + + + true + + + r1 + + + + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + MaxDistanceConstraint + + + true + + + 10 + + + Class + + + + + + + alternativa.physics.constraints + + + 10 + + + Package + + + + + + + + + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + setParams + + + true + + + 10 + + + Public Function + + + + + + + RigidBox + + + true + + + 10 + + + Class + + + + + + + true + + + + + + + true + + + h + + + + + + Public Variable + + + + + + + true + + + r + + + + + + Public Variable + + + + + + + true + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + true + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + true + + + setParams + + + true + + + 10 + + + Public Function + + + + + + + RigidCylinder + + + true + + + 10 + + + Class + + + + + + + + + + + true + + + normal + + + true + + + 10 + + + Public Variable + + + + + + + offset + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + RigidPlane + + + true + + + 10 + + + Class + + + + + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + RigidRect + + + true + + + 10 + + + Class + + + + + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + RigidSphere + + + true + + + 10 + + + Class + + + + + + + alternativa.physics.primitives + + + 10 + + + Package + + + + + + + FFFFFF + + + true + + + D:\work\eclipse\ws_projects\TanksTestingTool\lib\AlternativaPhysics.swc + + + AlternativaPhysics.swc + + + 1 + + + 24.0 + + + Not Present + + + + + + + Time Line + + + + + + + 500 X 375 + + + 157458 + + + 10 + + + + + + + 157458 + + + 10 + + + + + + + diff --git a/0.0.9.0/pom.xml b/0.0.9.0/pom.xml new file mode 100644 index 0000000..3255930 --- /dev/null +++ b/0.0.9.0/pom.xml @@ -0,0 +1,17 @@ + + 4.0.0 + platform.clients.fp10.libraries + AlternativaPhysics + swc + 0.0.9.0 + + platform.tools.maven + FlashBasePom + 2.0.0.0 + + + scm:svn:http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0 + + + + \ No newline at end of file diff --git a/0.0.9.0/src/.svn/all-wcprops b/0.0.9.0/src/.svn/all-wcprops new file mode 100644 index 0000000..6cc71a2 --- /dev/null +++ b/0.0.9.0/src/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 83 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src +END diff --git a/0.0.9.0/src/.svn/entries b/0.0.9.0/src/.svn/entries new file mode 100644 index 0000000..ab4639f --- /dev/null +++ b/0.0.9.0/src/.svn/entries @@ -0,0 +1,31 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src +http://svndev.alternativaplatform.com + + + +2010-02-19T13:09:00.412754Z +28731 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +alternativa +dir + diff --git a/0.0.9.0/src/.svn/format b/0.0.9.0/src/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.0/src/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.0/src/alternativa/.svn/all-wcprops b/0.0.9.0/src/alternativa/.svn/all-wcprops new file mode 100644 index 0000000..b83e2ea --- /dev/null +++ b/0.0.9.0/src/alternativa/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 95 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa +END diff --git a/0.0.9.0/src/alternativa/.svn/entries b/0.0.9.0/src/alternativa/.svn/entries new file mode 100644 index 0000000..3eb687a --- /dev/null +++ b/0.0.9.0/src/alternativa/.svn/entries @@ -0,0 +1,34 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa +http://svndev.alternativaplatform.com + + + +2010-02-19T13:09:00.412754Z +28731 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +physics +dir + +math +dir + diff --git a/0.0.9.0/src/alternativa/.svn/format b/0.0.9.0/src/alternativa/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.0/src/alternativa/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.0/src/alternativa/math/.svn/all-wcprops b/0.0.9.0/src/alternativa/math/.svn/all-wcprops new file mode 100644 index 0000000..7dee4b5 --- /dev/null +++ b/0.0.9.0/src/alternativa/math/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 100 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/math +END +Quaternion.as +K 25 +svn:wc:ra_dav:version-url +V 114 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/math/Quaternion.as +END +Vector3.as +K 25 +svn:wc:ra_dav:version-url +V 111 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/math/Vector3.as +END +Matrix3.as +K 25 +svn:wc:ra_dav:version-url +V 111 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/math/Matrix3.as +END +Matrix4.as +K 25 +svn:wc:ra_dav:version-url +V 111 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/math/Matrix4.as +END diff --git a/0.0.9.0/src/alternativa/math/.svn/entries b/0.0.9.0/src/alternativa/math/.svn/entries new file mode 100644 index 0000000..7c888ac --- /dev/null +++ b/0.0.9.0/src/alternativa/math/.svn/entries @@ -0,0 +1,76 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/math +http://svndev.alternativaplatform.com + + + +2010-02-12T12:34:25.192477Z +28424 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Quaternion.as +file + + + + +2010-10-28T04:32:43.000000Z +7332c9620420bd10062d913decfb9501 +2010-02-12T12:34:25.192477Z +28424 +mike + +Vector3.as +file + + + + +2010-10-28T04:32:43.000000Z +a1571b527786783cef1a9f56db3f2af6 +2010-02-12T12:34:25.192477Z +28424 +mike + +Matrix3.as +file + + + + +2010-10-28T04:32:43.000000Z +0054bd91fe694474a28c8ec17697292b +2010-02-12T12:34:25.192477Z +28424 +mike + +Matrix4.as +file + + + + +2010-10-28T04:32:43.000000Z +47b31d4350499ebe77a5973a938e4b44 +2010-02-12T12:34:25.192477Z +28424 +mike + diff --git a/0.0.9.0/src/alternativa/math/.svn/format b/0.0.9.0/src/alternativa/math/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.0/src/alternativa/math/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.0/src/alternativa/math/.svn/text-base/Matrix3.as.svn-base b/0.0.9.0/src/alternativa/math/.svn/text-base/Matrix3.as.svn-base new file mode 100644 index 0000000..244cc98 --- /dev/null +++ b/0.0.9.0/src/alternativa/math/.svn/text-base/Matrix3.as.svn-base @@ -0,0 +1,390 @@ +package alternativa.math { + import flash.geom.Vector3D; + + /** + * + */ + public class Matrix3 { + + public static const ZERO:Matrix3 = new Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0); + public static const IDENTITY:Matrix3 = new Matrix3(); + + public var a:Number; + public var b:Number; + public var c:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix3(a:Number = 1, b:Number = 0, c:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1) { + this.a = a; + this.b = b; + this.c = c; + + this.e = e; + this.f = f; + this.g = g; + + this.i = i; + this.j = j; + this.k = k; + } + + /** + * + * @return + */ + public function toIdentity():Matrix3 { + a = f = k = 1; + b = c = e = g = i = j = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + var det:Number = 1/(-cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk); + + a = (ff*kk - gg*jj)*det; + b = (cc*jj - bb*kk)*det; + c = (bb*gg - cc*ff)*det; + e = (gg*ii - ee*kk)*det; + f = (aa*kk - cc*ii)*det; + g = (cc*ee - aa*gg)*det; + i = (ee*jj - ff*ii)*det; + j = (bb*ii - aa*jj)*det; + k = (aa*ff - bb*ee)*det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prependTransposed(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.b + cc*m.c; + b = aa*m.e + bb*m.f + cc*m.g; + c = aa*m.i + bb*m.j + cc*m.k; + e = ee*m.a + ff*m.b + gg*m.c; + f = ee*m.e + ff*m.f + gg*m.g; + g = ee*m.i + ff*m.j + gg*m.k; + i = ii*m.a + jj*m.b + kk*m.c; + j = ii*m.e + jj*m.f + kk*m.g; + k = ii*m.i + jj*m.j + kk*m.k; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix3):Matrix3 { + a += m.a; + b += m.b; + c += m.c; + e += m.e; + f += m.f; + g += m.g; + i += m.i; + j += m.j; + k += m.k; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix3):Matrix3 { + a -= m.a; + b -= m.b; + c -= m.c; + e -= m.e; + f -= m.f; + g -= m.g; + i -= m.i; + j -= m.j; + k -= m.k; + + return this; + } + + /** + * + * @return + */ + public function transpose():Matrix3 { + var tmp:Number = b; + b = e; + e = tmp; + tmp = c; + c = i; + i = tmp; + tmp = g; + g = j; + j = tmp; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector3To3D(vin:Vector3, vout:Vector3D):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * + * @param matrix + */ + public function createSkewSymmetric(v:Vector3):Matrix3 { + a = f = k = 0; + b = -v.z; + c = v.y; + e = v.z; + g = -v.x; + i = -v.y; + j = v.x; + return this; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix3):Matrix3 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @return + */ + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):Matrix3 { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + + return this; + } + + /** + * Метод формирует матрицу поворота на заданный угол относительно заданной оси. + * + * @param axis нормализованный вектор, задающий ось, относительно которой выполняется поворот + * @param angle угол поворота в радианах + */ + public function fromAxisAngle(axis:Vector3, angle:Number):void { + var c1:Number = Math.cos(angle); + var s:Number = Math.sin(angle); + var t:Number = 1 - c1; + var x:Number = axis.x; + var y:Number = axis.y; + var z:Number = axis.z; + + a = t*x*x + c1; + b = t*x*y - z*s; + c = t*x*z + y*s; + + e = t*x*y + z*s; + f = t*y*y + c1; + g = t*y*z - x*s; + + i = t*x*z - y*s; + j = t*y*z + x*s; + k = t*z*z + c1; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix3 { + return new Matrix3(a, b, c, e, f, g, i, j, k); + } + + /** + * + * @return + */ + public function toString():String { + return "[Matrix3 (" + a + ", " + b + ", " + c + "), (" + e + ", " + f + ", " + g + "), (" + i + ", " + j + ", " + k + ")]"; + } + + /** + * @param angles + */ + public function getEulerAngles(angles:Vector3):void { + if (-1 < i && i < 1) { + angles.x = Math.atan2(j, k); + angles.y = -Math.asin(i); + angles.z = Math.atan2(e, a); + } else { + angles.x = 0; + angles.y = (i <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-b, f); + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/math/.svn/text-base/Matrix4.as.svn-base b/0.0.9.0/src/alternativa/math/.svn/text-base/Matrix4.as.svn-base new file mode 100644 index 0000000..d06eb5f --- /dev/null +++ b/0.0.9.0/src/alternativa/math/.svn/text-base/Matrix4.as.svn-base @@ -0,0 +1,475 @@ +package alternativa.math { + + /** + * + */ + public class Matrix4 { + + public static const IDENTITY:Matrix4 = new Matrix4(); + + public var a:Number; + public var b:Number; + public var c:Number; + public var d:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + public var h:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + public var l:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix4(a:Number = 1, b:Number = 0, c:Number = 0, d:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, h:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1, l:Number = 0) { + this.a = a; + this.b = b; + this.c = c; + this.d = d; + + this.e = e; + this.f = f; + this.g = g; + this.h = h; + + this.i = i; + this.j = j; + this.k = k; + this.l = l; + } + + /** + * + * @return + */ + public function toIdentity():Matrix4 { + a = f = k = 1; + b = c = e = g = i = j = d = h = l = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + var det:Number = -cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk; + + a = (-gg*jj + ff*kk)/det; + b = (cc*jj - bb*kk)/det; + c = (-cc*ff + bb*gg)/det; + d = (dd*gg*jj - cc*hh*jj - dd*ff*kk + bb*hh*kk + cc*ff*ll - bb*gg*ll)/det; + e = (gg*ii - ee*kk)/det; + f = (-cc*ii + aa*kk)/det; + g = (cc*ee - aa*gg)/det; + h = (cc*hh*ii - dd*gg*ii + dd*ee*kk - aa*hh*kk - cc*ee*ll + aa*gg*ll)/det; + i = (-ff*ii + ee*jj)/det; + j = (bb*ii - aa*jj)/det; + k = (-bb*ee + aa*ff)/det; + l = (dd*ff*ii - bb*hh*ii - dd*ee*jj + aa*hh*jj + bb*ee*ll - aa*ff*ll)/det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + d = m.a*dd + m.b*hh + m.c*ll + m.d; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + h = m.e*dd + m.f*hh + m.g*ll + m.h; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + l = m.i*dd + m.j*hh + m.k*ll + m.l; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + d = aa*m.d + bb*m.h + cc*m.l + dd; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + h = ee*m.d + ff*m.h + gg*m.l + hh; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + l = ii*m.d + jj*m.h + kk*m.l + ll; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix4):Matrix4 { + a += m.a; + b += m.b; + c += m.c; + d += m.d; + e += m.e; + f += m.f; + g += m.g; + h += m.h; + i += m.i; + j += m.j; + k += m.k; + l += m.l; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix4):Matrix4 { + a -= m.a; + b -= m.b; + c -= m.c; + d -= m.d; + e -= m.e; + f -= m.f; + g -= m.g; + h -= m.h; + i -= m.i; + j -= m.j; + k -= m.k; + l -= m.l; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectors(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + * @param len + */ + public function transformVectorsN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverse(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverseN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(idx:int, axis:Vector3):void { + switch (idx) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + case 3: + axis.x = d; + axis.y = h; + axis.z = l; + return; + } + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix4):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = m.d; + e = m.e; + f = m.f; + g = m.g; + h = m.h; + i = m.i; + j = m.j; + k = m.k; + l = m.l; + + return this; + } + + public function setFromMatrix3(m:Matrix3, offset:Vector3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = offset.x; + e = m.e; + f = m.f; + g = m.g; + h = offset.y; + i = m.i; + j = m.j; + k = m.k; + l = offset.z; + + return this; + } + + public function setOrientationFromMatrix3(m:Matrix3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @param angles + */ + public function getEulerAngles(angles:Vector3):void { + if (-1 < i && i < 1) { + angles.x = Math.atan2(j, k); + angles.y = -Math.asin(i); + angles.z = Math.atan2(e, a); + } else { + angles.x = 0; + angles.y = (i <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-b, f); + } + } + + /** + * Устанавливает координаты матрицы. + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + d = pos.x; + h = pos.y; + l = pos.z; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix4 { + return new Matrix4(a, b, c, d, e, f, g, h, i, j, k, l); + } + + /** + * + * @return + */ + public function toString():String { + return "[Matrix4 [" + a.toFixed(3) + " " + b.toFixed(3) + " " + c.toFixed(3) + " " + d.toFixed(3) + "] [" + e.toFixed(3) + " " + f.toFixed(3) + " " + g.toFixed(3) + " " + h.toFixed(3) + "] [" + i.toFixed(3) + " " + j.toFixed(3) + " " + k.toFixed(3) + " " + l.toFixed(3) + "]]"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/math/.svn/text-base/Quaternion.as.svn-base b/0.0.9.0/src/alternativa/math/.svn/text-base/Quaternion.as.svn-base new file mode 100644 index 0000000..ddc85f5 --- /dev/null +++ b/0.0.9.0/src/alternativa/math/.svn/text-base/Quaternion.as.svn-base @@ -0,0 +1,458 @@ +package alternativa.math { + import flash.geom.Vector3D; + + public class Quaternion { + + public var w:Number; + public var x:Number; + public var y:Number; + public var z:Number; + + public static function multiply(q1:Quaternion, q2:Quaternion, result:Quaternion):void { + result.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z; + result.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y; + result.y = q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z; + result.z = q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x; + } + + public static function createFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngle(axis, angle); + return q; + } + + public static function createFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngleComponents(x, y, z, angle); + return q; + } + + private static var _q:Quaternion = new Quaternion(); + + public function Quaternion(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0) { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + } + + public function reset(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0):Quaternion { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + return this; + } + + public function normalize():Quaternion { + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * Умножает на указанный кватернион слева: this * q + * + * @param q множитель + */ + public function prepend(q:Quaternion):Quaternion { + var ww:Number = w*q.w - x*q.x - y*q.y - z*q.z; + var xx:Number = w*q.x + x*q.w + y*q.z - z*q.y; + var yy:Number = w*q.y + y*q.w + z*q.x - x*q.z; + var zz:Number = w*q.z + z*q.w + x*q.y - y*q.x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Умножает на указанный кватернион справа: q * this + * + * @param q множитель + */ + public function append(q:Quaternion):Quaternion { + var ww:Number = q.w*w - q.x*x - q.y*y - q.z*z; + var xx:Number = q.w*x + q.x*w + q.y*z - q.z*y; + var yy:Number = q.w*y + q.y*w + q.z*x - q.x*z; + var zz:Number = q.w*z + q.z*w + q.x*y - q.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * + * @param vector + */ + public function rotateByVector(v:Vector3):Quaternion { + var ww:Number = -v.x*x - v.y*y - v.z*z; + var xx:Number = v.x*w + v.y*z - v.z*y; + var yy:Number = v.y*w + v.z*x - v.x*z; + var zz:Number = v.z*w + v.x*y - v.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Добавляет вращение, приданное вектором угловой скорости за указанное время. + * + * @param v + * @param scale + */ + public function addScaledVector(v:Vector3, scale:Number):Quaternion { + var vx:Number = v.x*scale; + var vy:Number = v.y*scale; + var vz:Number = v.z*scale; + var ww:Number = -x*vx - y*vy - z*vz; + var xx:Number = vx*w + vy*z - vz*y; + var yy:Number = vy*w + vz*x - vx*z; + var zz:Number = vz*w + vx*y - vy*x; + w += 0.5*ww; + x += 0.5*xx; + y += 0.5*yy; + z += 0.5*zz; + // inlined normalize + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param m + * @return + */ + public function toMatrix3(m:Matrix3):Quaternion { + var xx2:Number = 2.0*x*x; + var yy2:Number = 2.0*y*y; + var zz2:Number = 2.0*z*z; + var xy2:Number = 2.0*x*y; + var yz2:Number = 2.0*y*z; + var zx2:Number = 2.0*z*x; + var wx2:Number = 2.0*w*x; + var wy2:Number = 2.0*w*y; + var wz2:Number = 2.0*w*z; + + m.a = 1.0 - yy2 - zz2; + m.b = xy2 - wz2; + m.c = zx2 + wy2; + + m.e = xy2 + wz2; + m.f = 1.0 - xx2 - zz2; + m.g = yz2 - wx2; + + m.i = zx2 - wy2; + m.j = yz2 + wx2; + m.k = 1.0 - xx2 - yy2; + return this; + } + + /** + * + * @return + */ + public function length():Number { + return Math.sqrt(w*w + x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function lengthSqr():Number { + return w*w + x*x + y*y + z*z; + } + + /** + * + * @param axis + * @param angle + * @return + */ + public function setFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z); + x = axis.x*k; + y = axis.y*k; + z = axis.z*k; + return this; + } + + /** + * + * @param x + * @param y + * @param z + * @param angle + */ + public function setFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(x*x + y*y + z*z); + this.x = x*k; + this.y = y*k; + this.z = z*k; + return this; + } + + /** + * + * @param vector + */ + public function toAxisVector(v:Vector3 = null):Vector3 { + if (w < -1 || w > 1) { + normalize(); + } + if (v == null) { + v = new Vector3(); + } + if (w > -1 && w < 1) { + if (w == 0) { + v.x = x; + v.y = y; + v.z = z; + } else { + var angle:Number = 2*Math.acos(w); + var coeff:Number = 1/Math.sqrt(1 - w*w); + v.x = x*coeff*angle; + v.y = y*coeff*angle; + v.z = z*coeff*angle; + } + } else { + v.x = 0; + v.y = 0; + v.z = 0; + } + return v; + } + + /** + * + * @param rotations + */ + public function getEulerAngles(angles:Vector3):Vector3 { + var qi2:Number = 2*x*x; + var qj2:Number = 2*y*y; + var qk2:Number = 2*z*z; + var qij:Number = 2*x*y; + var qjk:Number = 2*y*z; + var qki:Number = 2*z*x; + var qri:Number = 2*w*x; + var qrj:Number = 2*w*y; + var qrk:Number = 2*w*z; + + var aa:Number = 1 - qj2 - qk2; + var bb:Number = qij - qrk; + var ee:Number = qij + qrk; + var ff:Number = 1 - qi2 - qk2; + var ii:Number = qki - qrj; + var jj:Number = qjk + qri; + var kk:Number = 1 - qi2 - qj2; + + if (-1 < ii && ii < 1) { + if (angles == null) { + angles = new Vector3(Math.atan2(jj, kk), -Math.asin(ii), Math.atan2(ee, aa)); + } else { + angles.x = Math.atan2(jj, kk); + angles.y = -Math.asin(ii); + angles.z = Math.atan2(ee, aa); + } + } else { + if (angles == null) { + angles = new Vector3(0, 0.5*((ii <= -1) ? Math.PI : -Math.PI), Math.atan2(-bb, ff)); + } else { + angles.x = 0; + angles.y = (ii <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-bb, ff); + } + } + return angles; + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setFromEulerAnglesXYZ(x:Number, y:Number, z:Number):void { + setFromAxisAngleComponents(1, 0, 0, x); + + _q.setFromAxisAngleComponents(0, 1, 0, y); + append(_q); + normalize(); + + _q.setFromAxisAngleComponents(0, 0, 1, z); + append(_q); + normalize(); + } + + /** + * + */ + public function conjugate():void { + x = -x; + y = -y; + z = -z; + } + + /** + * Выполняет линейную интерполяцию. + * + * @param q1 начало отрезка + * @param q2 конец отрезка + * @param t время, обычно задаётся в интервале [0, 1] + */ + public function nlerp(q1:Quaternion, q2:Quaternion, t:Number):Quaternion { + var d:Number = 1 - t; + w = q1.w*d + q2.w*t; + x = q1.x*d + q2.x*t; + y = q1.y*d + q2.y*t; + z = q1.z*d + q2.z*t; + // inlined normalize + d = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param q + * @return + */ + public function subtract(q:Quaternion):Quaternion { + w -= q.w; + x -= q.x; + y -= q.y; + z -= q.z; + return this; + } + + /** + * + * @param q1 + * @param q2 + * @return + */ + public function diff(q1:Quaternion, q2:Quaternion):Quaternion { + w = q2.w - q1.w; + x = q2.x - q1.x; + y = q2.y - q1.y; + z = q2.z - q1.z; + return this; + } + + /** + * + * @param q + * @return + */ + public function copy(q:Quaternion):Quaternion { + w = q.w; + x = q.x; + y = q.y; + z = q.z; + return this; + } + + /** + * @param reslult + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + result.w = w; + return result; + } + + /** + * + * @return + */ + public function clone():Quaternion { + return new Quaternion(w, x, y, z); + } + + /** + * + * @return + */ + public function toString():String { + return "[" + w + ", " + x + ", " + y + ", " + z + "]"; + } + + /** + * Выполняет сферическую интерполяцию между двумя заданными кватернионами по наименьшему расстоянию. + * + * @param a первый кватерион + * @param b второй кватернион + * @param t параметр интерполяции, обычно принадлежит отрезку [0, 1] + * @return this + */ + public function slerp(a:Quaternion, b:Quaternion, t:Number):Quaternion { + var flip:Number = 1; + // Так как одна и та же ориентация представляется двумя значениями q и -q, нужно сменить знак одного из кватернионов + // если скалярное произведение отрицательно. Иначе будет получено интерполированное значение по наибольшему расстоянию. + var cosine:Number = a.w*b.w + a.x*b.x + a.y*b.y + a.z*b.z; + if (cosine < 0) { + cosine = -cosine; + flip = -1; + } + + if ((1 - cosine) < 0.001) { + // Вблизи нуля используется линейная интерполяция + var k1:Number = 1 - t; + var k2:Number = t*flip; + w = a.w*k1 + b.w*k2; + x = a.x*k1 + b.x*k2; + y = a.y*k1 + b.y*k2; + z = a.z*k1 + b.z*k2; + normalize(); + } else { + var theta:Number = Math.acos(cosine); + var sine:Number = Math.sin(theta); + var beta:Number = Math.sin((1 - t)*theta)/sine; + var alpha:Number = Math.sin(t*theta)/sine*flip; + w = a.w*beta + b.w*alpha; + x = a.x*beta + b.x*alpha; + y = a.y*beta + b.y*alpha; + z = a.z*beta + b.z*alpha; + } + + return this; + } + + } +} diff --git a/0.0.9.0/src/alternativa/math/.svn/text-base/Vector3.as.svn-base b/0.0.9.0/src/alternativa/math/.svn/text-base/Vector3.as.svn-base new file mode 100644 index 0000000..89010a8 --- /dev/null +++ b/0.0.9.0/src/alternativa/math/.svn/text-base/Vector3.as.svn-base @@ -0,0 +1,334 @@ +package alternativa.math { + import flash.geom.Vector3D; + + /** + * + */ + public class Vector3 extends Vector3D { + + public static const ZERO:Vector3 = new Vector3(0, 0, 0); + public static const X_AXIS:Vector3 = new Vector3(1, 0, 0); + public static const Y_AXIS:Vector3 = new Vector3(0, 1, 0); + public static const Z_AXIS:Vector3 = new Vector3(0, 0, 1); + + /** + * + * @param x + * @param y + * @param z + */ + public function Vector3(x:Number = 0, y:Number = 0, z:Number = 0) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * + * @return + */ + public function vLength():Number { + return Math.sqrt(x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function vLengthSqr():Number { + return x*x + y*y + z*z; + } + + /** + * + * @param length + * @return + */ + public function vSetLength(length:Number):Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = length; + } else { + var k:Number = length/Math.sqrt(x*x + y*y + z*z); + x *= k; + y *= k; + z *= k; + } + return this; + } + + /** + * + * @return + */ + public function vNormalize():Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = 1; + } else { + d = Math.sqrt(d); + x /= d; + y /= d; + z /= d; + } + return this; + } + + /** + * Сложение координат. + * + * @param v точка, координаты которой прибавляются к собственным + */ + public function vAdd(v:Vector3):Vector3 { + x += v.x; + y += v.y; + z += v.z; + return this; + } + + /** + * + * @param k + * @param v + * @return + */ + public function vAddScaled(k:Number, v:Vector3):Vector3 { + x += k*v.x; + y += k*v.y; + z += k*v.z; + return this; + } + + /** + * Вычитание координат. + * + * @param v точка, координаты которой вычитаются из собственных + */ + public function vSubtract(v:Vector3):Vector3 { + x -= v.x; + y -= v.y; + z -= v.z; + return this; + } + + /** + * Вычисляет сумму векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + * @return this + */ + public function vSum(a:Vector3, b:Vector3):Vector3 { + x = a.x + b.x; + y = a.y + b.y; + z = a.z + b.z; + return this; + } + + /** + * Вычисление разности векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + */ + public function vDiff(a:Vector3, b:Vector3):Vector3 { + x = a.x - b.x; + y = a.y - b.y; + z = a.z - b.z; + return this; + } + + /** + * Умножение на скаляр. + * + * @param k число, на которое умножаются координаты + */ + public function vScale(k:Number):Vector3 { + x *= k; + y *= k; + z *= k; + return this; + } + + /** + * Инвертирование вектора. + */ + public function vReverse():Vector3 { + x = -x; + y = -y; + z = -z; + return this; + } + + /** + * + * @param v + * @return + */ + public function vDot(v:Vector3):Number { + return x*v.x + y*v.y + z*v.z; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross(v:Vector3):Vector3 { + var xx:Number = y*v.z - z*v.y; + var yy:Number = z*v.x - x*v.z; + var zz:Number = x*v.y - y*v.x; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross2(a:Vector3, b:Vector3):Vector3 { + x = a.y*b.z - a.z*b.y; + y = a.z*b.x - a.x*b.z; + z = a.x*b.y - a.y*b.x; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy3(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * + * @param matrix + */ + public function vTransformBy3Tr(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz + m.d; + y = m.e*xx + m.f*yy + m.g*zz + m.h; + z = m.i*xx + m.j*yy + m.k*zz + m.l; + return this; + } + + /** + * + * @param m + * @return + */ + public function vTransformInverseBy4(m:Matrix4):Vector3 { + var xx:Number = x - m.d; + var yy:Number = y - m.h; + var zz:Number = z - m.l; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * + * @param m + */ + public function vDeltaTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * Установка координат. + */ + public function vReset(x:Number = 0, y:Number = 0, z:Number = 0):Vector3 { + this.x = x; + this.y = y; + this.z = z; + return this; + } + + /** + * Копирование координат точки. + * + * @param v точка, координаты которой копируются + */ + public function vCopy(v:Vector3):Vector3 { + x = v.x; + y = v.y; + z = v.z; + return this; + } + + /** + * Клонирование точки. + * + * @return клонированная точка + */ + public function vClone():Vector3 { + return new Vector3(x, y, z); + } + + /** + * Копирует координаты вектора в экземпляр класса Vector3D. + * + * @param result экземпляр, в который копируются координаты + * @return переданный параметр + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + return result; + } + + /** + * @param source + * @return + */ + public function copyFromVector3D(source:Vector3D):Vector3 { + x = source.x; + y = source.y; + z = source.z; + return this; + } + + override public function toString():String { + return "Vector3(" + x + "," + y + ", " + z + ")"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/math/Matrix3.as b/0.0.9.0/src/alternativa/math/Matrix3.as new file mode 100644 index 0000000..244cc98 --- /dev/null +++ b/0.0.9.0/src/alternativa/math/Matrix3.as @@ -0,0 +1,390 @@ +package alternativa.math { + import flash.geom.Vector3D; + + /** + * + */ + public class Matrix3 { + + public static const ZERO:Matrix3 = new Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0); + public static const IDENTITY:Matrix3 = new Matrix3(); + + public var a:Number; + public var b:Number; + public var c:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix3(a:Number = 1, b:Number = 0, c:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1) { + this.a = a; + this.b = b; + this.c = c; + + this.e = e; + this.f = f; + this.g = g; + + this.i = i; + this.j = j; + this.k = k; + } + + /** + * + * @return + */ + public function toIdentity():Matrix3 { + a = f = k = 1; + b = c = e = g = i = j = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + var det:Number = 1/(-cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk); + + a = (ff*kk - gg*jj)*det; + b = (cc*jj - bb*kk)*det; + c = (bb*gg - cc*ff)*det; + e = (gg*ii - ee*kk)*det; + f = (aa*kk - cc*ii)*det; + g = (cc*ee - aa*gg)*det; + i = (ee*jj - ff*ii)*det; + j = (bb*ii - aa*jj)*det; + k = (aa*ff - bb*ee)*det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prependTransposed(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.b + cc*m.c; + b = aa*m.e + bb*m.f + cc*m.g; + c = aa*m.i + bb*m.j + cc*m.k; + e = ee*m.a + ff*m.b + gg*m.c; + f = ee*m.e + ff*m.f + gg*m.g; + g = ee*m.i + ff*m.j + gg*m.k; + i = ii*m.a + jj*m.b + kk*m.c; + j = ii*m.e + jj*m.f + kk*m.g; + k = ii*m.i + jj*m.j + kk*m.k; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix3):Matrix3 { + a += m.a; + b += m.b; + c += m.c; + e += m.e; + f += m.f; + g += m.g; + i += m.i; + j += m.j; + k += m.k; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix3):Matrix3 { + a -= m.a; + b -= m.b; + c -= m.c; + e -= m.e; + f -= m.f; + g -= m.g; + i -= m.i; + j -= m.j; + k -= m.k; + + return this; + } + + /** + * + * @return + */ + public function transpose():Matrix3 { + var tmp:Number = b; + b = e; + e = tmp; + tmp = c; + c = i; + i = tmp; + tmp = g; + g = j; + j = tmp; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector3To3D(vin:Vector3, vout:Vector3D):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * + * @param matrix + */ + public function createSkewSymmetric(v:Vector3):Matrix3 { + a = f = k = 0; + b = -v.z; + c = v.y; + e = v.z; + g = -v.x; + i = -v.y; + j = v.x; + return this; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix3):Matrix3 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @return + */ + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):Matrix3 { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + + return this; + } + + /** + * Метод формирует матрицу поворота на заданный угол относительно заданной оси. + * + * @param axis нормализованный вектор, задающий ось, относительно которой выполняется поворот + * @param angle угол поворота в радианах + */ + public function fromAxisAngle(axis:Vector3, angle:Number):void { + var c1:Number = Math.cos(angle); + var s:Number = Math.sin(angle); + var t:Number = 1 - c1; + var x:Number = axis.x; + var y:Number = axis.y; + var z:Number = axis.z; + + a = t*x*x + c1; + b = t*x*y - z*s; + c = t*x*z + y*s; + + e = t*x*y + z*s; + f = t*y*y + c1; + g = t*y*z - x*s; + + i = t*x*z - y*s; + j = t*y*z + x*s; + k = t*z*z + c1; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix3 { + return new Matrix3(a, b, c, e, f, g, i, j, k); + } + + /** + * + * @return + */ + public function toString():String { + return "[Matrix3 (" + a + ", " + b + ", " + c + "), (" + e + ", " + f + ", " + g + "), (" + i + ", " + j + ", " + k + ")]"; + } + + /** + * @param angles + */ + public function getEulerAngles(angles:Vector3):void { + if (-1 < i && i < 1) { + angles.x = Math.atan2(j, k); + angles.y = -Math.asin(i); + angles.z = Math.atan2(e, a); + } else { + angles.x = 0; + angles.y = (i <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-b, f); + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/math/Matrix4.as b/0.0.9.0/src/alternativa/math/Matrix4.as new file mode 100644 index 0000000..d06eb5f --- /dev/null +++ b/0.0.9.0/src/alternativa/math/Matrix4.as @@ -0,0 +1,475 @@ +package alternativa.math { + + /** + * + */ + public class Matrix4 { + + public static const IDENTITY:Matrix4 = new Matrix4(); + + public var a:Number; + public var b:Number; + public var c:Number; + public var d:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + public var h:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + public var l:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix4(a:Number = 1, b:Number = 0, c:Number = 0, d:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, h:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1, l:Number = 0) { + this.a = a; + this.b = b; + this.c = c; + this.d = d; + + this.e = e; + this.f = f; + this.g = g; + this.h = h; + + this.i = i; + this.j = j; + this.k = k; + this.l = l; + } + + /** + * + * @return + */ + public function toIdentity():Matrix4 { + a = f = k = 1; + b = c = e = g = i = j = d = h = l = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + var det:Number = -cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk; + + a = (-gg*jj + ff*kk)/det; + b = (cc*jj - bb*kk)/det; + c = (-cc*ff + bb*gg)/det; + d = (dd*gg*jj - cc*hh*jj - dd*ff*kk + bb*hh*kk + cc*ff*ll - bb*gg*ll)/det; + e = (gg*ii - ee*kk)/det; + f = (-cc*ii + aa*kk)/det; + g = (cc*ee - aa*gg)/det; + h = (cc*hh*ii - dd*gg*ii + dd*ee*kk - aa*hh*kk - cc*ee*ll + aa*gg*ll)/det; + i = (-ff*ii + ee*jj)/det; + j = (bb*ii - aa*jj)/det; + k = (-bb*ee + aa*ff)/det; + l = (dd*ff*ii - bb*hh*ii - dd*ee*jj + aa*hh*jj + bb*ee*ll - aa*ff*ll)/det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + d = m.a*dd + m.b*hh + m.c*ll + m.d; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + h = m.e*dd + m.f*hh + m.g*ll + m.h; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + l = m.i*dd + m.j*hh + m.k*ll + m.l; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + d = aa*m.d + bb*m.h + cc*m.l + dd; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + h = ee*m.d + ff*m.h + gg*m.l + hh; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + l = ii*m.d + jj*m.h + kk*m.l + ll; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix4):Matrix4 { + a += m.a; + b += m.b; + c += m.c; + d += m.d; + e += m.e; + f += m.f; + g += m.g; + h += m.h; + i += m.i; + j += m.j; + k += m.k; + l += m.l; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix4):Matrix4 { + a -= m.a; + b -= m.b; + c -= m.c; + d -= m.d; + e -= m.e; + f -= m.f; + g -= m.g; + h -= m.h; + i -= m.i; + j -= m.j; + k -= m.k; + l -= m.l; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectors(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + * @param len + */ + public function transformVectorsN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverse(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverseN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(idx:int, axis:Vector3):void { + switch (idx) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + case 3: + axis.x = d; + axis.y = h; + axis.z = l; + return; + } + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix4):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = m.d; + e = m.e; + f = m.f; + g = m.g; + h = m.h; + i = m.i; + j = m.j; + k = m.k; + l = m.l; + + return this; + } + + public function setFromMatrix3(m:Matrix3, offset:Vector3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = offset.x; + e = m.e; + f = m.f; + g = m.g; + h = offset.y; + i = m.i; + j = m.j; + k = m.k; + l = offset.z; + + return this; + } + + public function setOrientationFromMatrix3(m:Matrix3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @param angles + */ + public function getEulerAngles(angles:Vector3):void { + if (-1 < i && i < 1) { + angles.x = Math.atan2(j, k); + angles.y = -Math.asin(i); + angles.z = Math.atan2(e, a); + } else { + angles.x = 0; + angles.y = (i <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-b, f); + } + } + + /** + * Устанавливает координаты матрицы. + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + d = pos.x; + h = pos.y; + l = pos.z; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix4 { + return new Matrix4(a, b, c, d, e, f, g, h, i, j, k, l); + } + + /** + * + * @return + */ + public function toString():String { + return "[Matrix4 [" + a.toFixed(3) + " " + b.toFixed(3) + " " + c.toFixed(3) + " " + d.toFixed(3) + "] [" + e.toFixed(3) + " " + f.toFixed(3) + " " + g.toFixed(3) + " " + h.toFixed(3) + "] [" + i.toFixed(3) + " " + j.toFixed(3) + " " + k.toFixed(3) + " " + l.toFixed(3) + "]]"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/math/Quaternion.as b/0.0.9.0/src/alternativa/math/Quaternion.as new file mode 100644 index 0000000..ddc85f5 --- /dev/null +++ b/0.0.9.0/src/alternativa/math/Quaternion.as @@ -0,0 +1,458 @@ +package alternativa.math { + import flash.geom.Vector3D; + + public class Quaternion { + + public var w:Number; + public var x:Number; + public var y:Number; + public var z:Number; + + public static function multiply(q1:Quaternion, q2:Quaternion, result:Quaternion):void { + result.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z; + result.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y; + result.y = q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z; + result.z = q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x; + } + + public static function createFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngle(axis, angle); + return q; + } + + public static function createFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngleComponents(x, y, z, angle); + return q; + } + + private static var _q:Quaternion = new Quaternion(); + + public function Quaternion(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0) { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + } + + public function reset(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0):Quaternion { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + return this; + } + + public function normalize():Quaternion { + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * Умножает на указанный кватернион слева: this * q + * + * @param q множитель + */ + public function prepend(q:Quaternion):Quaternion { + var ww:Number = w*q.w - x*q.x - y*q.y - z*q.z; + var xx:Number = w*q.x + x*q.w + y*q.z - z*q.y; + var yy:Number = w*q.y + y*q.w + z*q.x - x*q.z; + var zz:Number = w*q.z + z*q.w + x*q.y - y*q.x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Умножает на указанный кватернион справа: q * this + * + * @param q множитель + */ + public function append(q:Quaternion):Quaternion { + var ww:Number = q.w*w - q.x*x - q.y*y - q.z*z; + var xx:Number = q.w*x + q.x*w + q.y*z - q.z*y; + var yy:Number = q.w*y + q.y*w + q.z*x - q.x*z; + var zz:Number = q.w*z + q.z*w + q.x*y - q.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * + * @param vector + */ + public function rotateByVector(v:Vector3):Quaternion { + var ww:Number = -v.x*x - v.y*y - v.z*z; + var xx:Number = v.x*w + v.y*z - v.z*y; + var yy:Number = v.y*w + v.z*x - v.x*z; + var zz:Number = v.z*w + v.x*y - v.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Добавляет вращение, приданное вектором угловой скорости за указанное время. + * + * @param v + * @param scale + */ + public function addScaledVector(v:Vector3, scale:Number):Quaternion { + var vx:Number = v.x*scale; + var vy:Number = v.y*scale; + var vz:Number = v.z*scale; + var ww:Number = -x*vx - y*vy - z*vz; + var xx:Number = vx*w + vy*z - vz*y; + var yy:Number = vy*w + vz*x - vx*z; + var zz:Number = vz*w + vx*y - vy*x; + w += 0.5*ww; + x += 0.5*xx; + y += 0.5*yy; + z += 0.5*zz; + // inlined normalize + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param m + * @return + */ + public function toMatrix3(m:Matrix3):Quaternion { + var xx2:Number = 2.0*x*x; + var yy2:Number = 2.0*y*y; + var zz2:Number = 2.0*z*z; + var xy2:Number = 2.0*x*y; + var yz2:Number = 2.0*y*z; + var zx2:Number = 2.0*z*x; + var wx2:Number = 2.0*w*x; + var wy2:Number = 2.0*w*y; + var wz2:Number = 2.0*w*z; + + m.a = 1.0 - yy2 - zz2; + m.b = xy2 - wz2; + m.c = zx2 + wy2; + + m.e = xy2 + wz2; + m.f = 1.0 - xx2 - zz2; + m.g = yz2 - wx2; + + m.i = zx2 - wy2; + m.j = yz2 + wx2; + m.k = 1.0 - xx2 - yy2; + return this; + } + + /** + * + * @return + */ + public function length():Number { + return Math.sqrt(w*w + x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function lengthSqr():Number { + return w*w + x*x + y*y + z*z; + } + + /** + * + * @param axis + * @param angle + * @return + */ + public function setFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z); + x = axis.x*k; + y = axis.y*k; + z = axis.z*k; + return this; + } + + /** + * + * @param x + * @param y + * @param z + * @param angle + */ + public function setFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(x*x + y*y + z*z); + this.x = x*k; + this.y = y*k; + this.z = z*k; + return this; + } + + /** + * + * @param vector + */ + public function toAxisVector(v:Vector3 = null):Vector3 { + if (w < -1 || w > 1) { + normalize(); + } + if (v == null) { + v = new Vector3(); + } + if (w > -1 && w < 1) { + if (w == 0) { + v.x = x; + v.y = y; + v.z = z; + } else { + var angle:Number = 2*Math.acos(w); + var coeff:Number = 1/Math.sqrt(1 - w*w); + v.x = x*coeff*angle; + v.y = y*coeff*angle; + v.z = z*coeff*angle; + } + } else { + v.x = 0; + v.y = 0; + v.z = 0; + } + return v; + } + + /** + * + * @param rotations + */ + public function getEulerAngles(angles:Vector3):Vector3 { + var qi2:Number = 2*x*x; + var qj2:Number = 2*y*y; + var qk2:Number = 2*z*z; + var qij:Number = 2*x*y; + var qjk:Number = 2*y*z; + var qki:Number = 2*z*x; + var qri:Number = 2*w*x; + var qrj:Number = 2*w*y; + var qrk:Number = 2*w*z; + + var aa:Number = 1 - qj2 - qk2; + var bb:Number = qij - qrk; + var ee:Number = qij + qrk; + var ff:Number = 1 - qi2 - qk2; + var ii:Number = qki - qrj; + var jj:Number = qjk + qri; + var kk:Number = 1 - qi2 - qj2; + + if (-1 < ii && ii < 1) { + if (angles == null) { + angles = new Vector3(Math.atan2(jj, kk), -Math.asin(ii), Math.atan2(ee, aa)); + } else { + angles.x = Math.atan2(jj, kk); + angles.y = -Math.asin(ii); + angles.z = Math.atan2(ee, aa); + } + } else { + if (angles == null) { + angles = new Vector3(0, 0.5*((ii <= -1) ? Math.PI : -Math.PI), Math.atan2(-bb, ff)); + } else { + angles.x = 0; + angles.y = (ii <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-bb, ff); + } + } + return angles; + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setFromEulerAnglesXYZ(x:Number, y:Number, z:Number):void { + setFromAxisAngleComponents(1, 0, 0, x); + + _q.setFromAxisAngleComponents(0, 1, 0, y); + append(_q); + normalize(); + + _q.setFromAxisAngleComponents(0, 0, 1, z); + append(_q); + normalize(); + } + + /** + * + */ + public function conjugate():void { + x = -x; + y = -y; + z = -z; + } + + /** + * Выполняет линейную интерполяцию. + * + * @param q1 начало отрезка + * @param q2 конец отрезка + * @param t время, обычно задаётся в интервале [0, 1] + */ + public function nlerp(q1:Quaternion, q2:Quaternion, t:Number):Quaternion { + var d:Number = 1 - t; + w = q1.w*d + q2.w*t; + x = q1.x*d + q2.x*t; + y = q1.y*d + q2.y*t; + z = q1.z*d + q2.z*t; + // inlined normalize + d = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param q + * @return + */ + public function subtract(q:Quaternion):Quaternion { + w -= q.w; + x -= q.x; + y -= q.y; + z -= q.z; + return this; + } + + /** + * + * @param q1 + * @param q2 + * @return + */ + public function diff(q1:Quaternion, q2:Quaternion):Quaternion { + w = q2.w - q1.w; + x = q2.x - q1.x; + y = q2.y - q1.y; + z = q2.z - q1.z; + return this; + } + + /** + * + * @param q + * @return + */ + public function copy(q:Quaternion):Quaternion { + w = q.w; + x = q.x; + y = q.y; + z = q.z; + return this; + } + + /** + * @param reslult + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + result.w = w; + return result; + } + + /** + * + * @return + */ + public function clone():Quaternion { + return new Quaternion(w, x, y, z); + } + + /** + * + * @return + */ + public function toString():String { + return "[" + w + ", " + x + ", " + y + ", " + z + "]"; + } + + /** + * Выполняет сферическую интерполяцию между двумя заданными кватернионами по наименьшему расстоянию. + * + * @param a первый кватерион + * @param b второй кватернион + * @param t параметр интерполяции, обычно принадлежит отрезку [0, 1] + * @return this + */ + public function slerp(a:Quaternion, b:Quaternion, t:Number):Quaternion { + var flip:Number = 1; + // Так как одна и та же ориентация представляется двумя значениями q и -q, нужно сменить знак одного из кватернионов + // если скалярное произведение отрицательно. Иначе будет получено интерполированное значение по наибольшему расстоянию. + var cosine:Number = a.w*b.w + a.x*b.x + a.y*b.y + a.z*b.z; + if (cosine < 0) { + cosine = -cosine; + flip = -1; + } + + if ((1 - cosine) < 0.001) { + // Вблизи нуля используется линейная интерполяция + var k1:Number = 1 - t; + var k2:Number = t*flip; + w = a.w*k1 + b.w*k2; + x = a.x*k1 + b.x*k2; + y = a.y*k1 + b.y*k2; + z = a.z*k1 + b.z*k2; + normalize(); + } else { + var theta:Number = Math.acos(cosine); + var sine:Number = Math.sin(theta); + var beta:Number = Math.sin((1 - t)*theta)/sine; + var alpha:Number = Math.sin(t*theta)/sine*flip; + w = a.w*beta + b.w*alpha; + x = a.x*beta + b.x*alpha; + y = a.y*beta + b.y*alpha; + z = a.z*beta + b.z*alpha; + } + + return this; + } + + } +} diff --git a/0.0.9.0/src/alternativa/math/Vector3.as b/0.0.9.0/src/alternativa/math/Vector3.as new file mode 100644 index 0000000..89010a8 --- /dev/null +++ b/0.0.9.0/src/alternativa/math/Vector3.as @@ -0,0 +1,334 @@ +package alternativa.math { + import flash.geom.Vector3D; + + /** + * + */ + public class Vector3 extends Vector3D { + + public static const ZERO:Vector3 = new Vector3(0, 0, 0); + public static const X_AXIS:Vector3 = new Vector3(1, 0, 0); + public static const Y_AXIS:Vector3 = new Vector3(0, 1, 0); + public static const Z_AXIS:Vector3 = new Vector3(0, 0, 1); + + /** + * + * @param x + * @param y + * @param z + */ + public function Vector3(x:Number = 0, y:Number = 0, z:Number = 0) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * + * @return + */ + public function vLength():Number { + return Math.sqrt(x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function vLengthSqr():Number { + return x*x + y*y + z*z; + } + + /** + * + * @param length + * @return + */ + public function vSetLength(length:Number):Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = length; + } else { + var k:Number = length/Math.sqrt(x*x + y*y + z*z); + x *= k; + y *= k; + z *= k; + } + return this; + } + + /** + * + * @return + */ + public function vNormalize():Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = 1; + } else { + d = Math.sqrt(d); + x /= d; + y /= d; + z /= d; + } + return this; + } + + /** + * Сложение координат. + * + * @param v точка, координаты которой прибавляются к собственным + */ + public function vAdd(v:Vector3):Vector3 { + x += v.x; + y += v.y; + z += v.z; + return this; + } + + /** + * + * @param k + * @param v + * @return + */ + public function vAddScaled(k:Number, v:Vector3):Vector3 { + x += k*v.x; + y += k*v.y; + z += k*v.z; + return this; + } + + /** + * Вычитание координат. + * + * @param v точка, координаты которой вычитаются из собственных + */ + public function vSubtract(v:Vector3):Vector3 { + x -= v.x; + y -= v.y; + z -= v.z; + return this; + } + + /** + * Вычисляет сумму векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + * @return this + */ + public function vSum(a:Vector3, b:Vector3):Vector3 { + x = a.x + b.x; + y = a.y + b.y; + z = a.z + b.z; + return this; + } + + /** + * Вычисление разности векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + */ + public function vDiff(a:Vector3, b:Vector3):Vector3 { + x = a.x - b.x; + y = a.y - b.y; + z = a.z - b.z; + return this; + } + + /** + * Умножение на скаляр. + * + * @param k число, на которое умножаются координаты + */ + public function vScale(k:Number):Vector3 { + x *= k; + y *= k; + z *= k; + return this; + } + + /** + * Инвертирование вектора. + */ + public function vReverse():Vector3 { + x = -x; + y = -y; + z = -z; + return this; + } + + /** + * + * @param v + * @return + */ + public function vDot(v:Vector3):Number { + return x*v.x + y*v.y + z*v.z; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross(v:Vector3):Vector3 { + var xx:Number = y*v.z - z*v.y; + var yy:Number = z*v.x - x*v.z; + var zz:Number = x*v.y - y*v.x; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross2(a:Vector3, b:Vector3):Vector3 { + x = a.y*b.z - a.z*b.y; + y = a.z*b.x - a.x*b.z; + z = a.x*b.y - a.y*b.x; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy3(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * + * @param matrix + */ + public function vTransformBy3Tr(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz + m.d; + y = m.e*xx + m.f*yy + m.g*zz + m.h; + z = m.i*xx + m.j*yy + m.k*zz + m.l; + return this; + } + + /** + * + * @param m + * @return + */ + public function vTransformInverseBy4(m:Matrix4):Vector3 { + var xx:Number = x - m.d; + var yy:Number = y - m.h; + var zz:Number = z - m.l; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * + * @param m + */ + public function vDeltaTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * Установка координат. + */ + public function vReset(x:Number = 0, y:Number = 0, z:Number = 0):Vector3 { + this.x = x; + this.y = y; + this.z = z; + return this; + } + + /** + * Копирование координат точки. + * + * @param v точка, координаты которой копируются + */ + public function vCopy(v:Vector3):Vector3 { + x = v.x; + y = v.y; + z = v.z; + return this; + } + + /** + * Клонирование точки. + * + * @return клонированная точка + */ + public function vClone():Vector3 { + return new Vector3(x, y, z); + } + + /** + * Копирует координаты вектора в экземпляр класса Vector3D. + * + * @param result экземпляр, в который копируются координаты + * @return переданный параметр + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + return result; + } + + /** + * @param source + * @return + */ + public function copyFromVector3D(source:Vector3D):Vector3 { + x = source.x; + y = source.y; + z = source.z; + return this; + } + + override public function toString():String { + return "Vector3(" + x + "," + y + ", " + z + ")"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/.svn/all-wcprops b/0.0.9.0/src/alternativa/physics/.svn/all-wcprops new file mode 100644 index 0000000..0c8a2e2 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/.svn/all-wcprops @@ -0,0 +1,77 @@ +K 25 +svn:wc:ra_dav:version-url +V 103 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics +END +Body.as +K 25 +svn:wc:ra_dav:version-url +V 111 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/Body.as +END +BodyState.as +K 25 +svn:wc:ra_dav:version-url +V 116 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/BodyState.as +END +CollisionPrimitiveListItem.as +K 25 +svn:wc:ra_dav:version-url +V 133 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/CollisionPrimitiveListItem.as +END +CollisionPrimitiveList.as +K 25 +svn:wc:ra_dav:version-url +V 129 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/CollisionPrimitiveList.as +END +PhysicsScene.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/PhysicsScene.as +END +PhysicsUtils.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/PhysicsUtils.as +END +altphysics.as +K 25 +svn:wc:ra_dav:version-url +V 117 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/altphysics.as +END +ContactPoint.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/ContactPoint.as +END +BodyListItem.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/BodyListItem.as +END +BodyList.as +K 25 +svn:wc:ra_dav:version-url +V 115 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/BodyList.as +END +Contact.as +K 25 +svn:wc:ra_dav:version-url +V 114 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/Contact.as +END +BodyMaterial.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/BodyMaterial.as +END diff --git a/0.0.9.0/src/alternativa/physics/.svn/entries b/0.0.9.0/src/alternativa/physics/.svn/entries new file mode 100644 index 0000000..93bcbb0 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/.svn/entries @@ -0,0 +1,181 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics +http://svndev.alternativaplatform.com + + + +2010-02-19T13:09:00.412754Z +28731 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +CollisionPrimitiveList.as +file + + + + +2010-10-28T04:32:43.000000Z +5752b1f4909c5146720b58fbfffed468 +2009-11-01T12:35:52.270571Z +22955 +mike + +altphysics.as +file + + + + +2010-10-28T04:32:43.000000Z +04fbe40a27502dbbd0dba02a76b2df50 +2009-04-20T20:09:14.715403Z +11579 +mike + +collision +dir + +primitives +dir + +BodyList.as +file + + + + +2010-10-28T04:32:43.000000Z +08a90332660bd0273728006d1402dd25 +2009-11-01T12:35:52.270571Z +22955 +mike + +Contact.as +file + + + + +2010-10-28T04:32:43.000000Z +eb89f4f37bcf2d21eadc733f244fcf64 +2010-02-12T12:34:25.192477Z +28424 +mike + +Body.as +file + + + + +2010-10-28T04:32:43.000000Z +355d0fb70dbb4946d4090a7c94c0d58a +2010-02-12T12:34:25.192477Z +28424 +mike + +BodyState.as +file + + + + +2010-10-28T04:32:43.000000Z +5be68820e6f6185d62b3928c39dde285 +2009-11-24T08:44:15.941480Z +23758 +mike + +CollisionPrimitiveListItem.as +file + + + + +2010-10-28T04:32:43.000000Z +cec5dbfefef812d87481478455f367b1 +2009-10-19T07:17:33.113306Z +22253 +mike + +PhysicsScene.as +file + + + + +2010-10-28T04:32:43.000000Z +96c74784bb71c06141cc469eec57ccb8 +2010-02-19T13:09:00.412754Z +28731 +mike + +PhysicsUtils.as +file + + + + +2010-10-28T04:32:43.000000Z +dce0a94f8a861291a2a8942ed0ba54a5 +2009-11-24T08:44:15.941480Z +23758 +mike + +constraints +dir + +ContactPoint.as +file + + + + +2010-10-28T04:32:43.000000Z +14f23db94810328dedd7da2992812ced +2009-11-24T08:44:15.941480Z +23758 +mike + +BodyListItem.as +file + + + + +2010-10-28T04:32:43.000000Z +2fc2b598310b24aef3f8b90e14a8ba4f +2009-10-19T07:17:33.113306Z +22253 +mike + +BodyMaterial.as +file + + + + +2010-10-28T04:32:43.000000Z +9b700692ebb8017882f3e8cec095e1a9 +2009-10-19T07:17:33.113306Z +22253 +mike + diff --git a/0.0.9.0/src/alternativa/physics/.svn/format b/0.0.9.0/src/alternativa/physics/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.0/src/alternativa/physics/.svn/text-base/Body.as.svn-base b/0.0.9.0/src/alternativa/physics/.svn/text-base/Body.as.svn-base new file mode 100644 index 0000000..cd22c5b --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/.svn/text-base/Body.as.svn-base @@ -0,0 +1,405 @@ +package alternativa.physics { + + import alternativa.math.Matrix3; + import alternativa.math.Matrix4; + import alternativa.math.Quaternion; + import alternativa.math.Vector3; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.IBodyCollisionPredicate; + import alternativa.physics.collision.types.BoundBox; + + use namespace altphysics; + + /** + * + */ + public class Body { + + public static var linDamping:Number = 0.997; + public static var rotDamping:Number = 0.997; + + // Идентификатор тела, уникальный в пределах мира + public var id:int; + // Имя тела + public var name:String; + // Мир, в котором находится тело + public var world:PhysicsScene; + // Флаг подвижности тела + public var movable:Boolean = true; + // Флаг указывает, может ли тело быть заморожено + public var canFreeze:Boolean = false; + + public var freezeCounter:int; + public var frozen:Boolean = false; + // Ограничивающий бокс тела + public var aabb:BoundBox = new BoundBox(); + // Предикат, через который тело получает сообщения о столкновениях + public var postCollisionPredicate:IBodyCollisionPredicate; + + // Текущее состояние тела + public var state:BodyState = new BodyState(); + // Предыдущее состояние тела + public var prevState:BodyState = new BodyState(); + // Линейное ускорение тела на текущем шаге симуляции + public var accel:Vector3 = new Vector3(); + // Угловое ускорение тела на текущем шаге симуляции + public var angleAccel:Vector3 = new Vector3(); + // Физический материал тела + public var material:BodyMaterial = new BodyMaterial(); + // Обратная масса тела + public var invMass:Number = 1; + // Обратная матрица тензора инерции в локальных координатах + public var invInertia:Matrix3 = new Matrix3(); + // Обратная матрица тензора инерции в мировых координатах + public var invInertiaWorld:Matrix3 = new Matrix3(); + // Базисная матрица тела в мировых координатах + public var baseMatrix:Matrix3 = new Matrix3(); + + public const MAX_CONTACTS:int = 20; + public var contacts:Vector. = new Vector.(MAX_CONTACTS); + public var contactsNum:int; + + public var collisionPrimitives:CollisionPrimitiveList; + + // Аккумулятор сил + public var forceAccum:Vector3 = new Vector3(); + // Аккумулятор моментов + public var torqueAccum:Vector3 = new Vector3(); + + // Внутренние переменные для избежания создания экземпляров + private static var _r:Vector3 = new Vector3(); + private static var _f:Vector3 = new Vector3(); + + /** + * + * @param invMass + * @param invInertia + */ + public function Body(invMass:Number, invInertia:Matrix3) { + this.invMass = invMass; + this.invInertia.copy(invInertia); + } + + /** + * @param primitive + * @param localTransform + */ + public function addCollisionPrimitive(primitive:CollisionPrimitive, localTransform:Matrix4 = null):void { + if (primitive == null) { + throw new ArgumentError("Primitive cannot be null"); + } + if (collisionPrimitives == null) { + collisionPrimitives = new CollisionPrimitiveList(); + } + collisionPrimitives.append(primitive); + primitive.setBody(this, localTransform); + } + + /** + * + * @param primitive + */ + public function removeCollisionPrimitive(primitive:CollisionPrimitive):void { + if (collisionPrimitives == null) return; + primitive.setBody(null); + collisionPrimitives.remove(primitive); + if (collisionPrimitives.size == 0) { + collisionPrimitives = null; + } + } + + /** + * @param t + * @param result + */ + public function interpolate(t:Number, pos:Vector3, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.w = prevState.orientation.w*t1 + state.orientation.w*t; + orientation.x = prevState.orientation.x*t1 + state.orientation.x*t; + orientation.y = prevState.orientation.y*t1 + state.orientation.y*t; + orientation.z = prevState.orientation.z*t1 + state.orientation.z*t; + } + + /** + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + state.pos.vCopy(pos); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setPositionXYZ(x:Number, y:Number, z:Number):void { + state.pos.vReset(x, y, z); + } + + /** + * + * @param vel + */ + public function setVelocity(vel:Vector3):void { + state.velocity.vCopy(vel); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setVelocityXYZ(x:Number, y:Number, z:Number):void { + state.velocity.vReset(x, y, z); + } + + /** + * + * @param rot + */ + public function setRotation(rot:Vector3):void { + state.rotation.vCopy(rot); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setRotationXYZ(x:Number, y:Number, z:Number):void { + state.rotation.vReset(x, y, z); + } + + /** + * + * @param q + */ + public function setOrientation(q:Quaternion):void { + state.orientation.copy(q); + } + + /** + * @param r + * @param dir + * @param magnitude + */ + public function applyRelPosWorldImpulse(r:Vector3, dir:Vector3, magnitude:Number):void { + var d:Number = magnitude*invMass; + // Линейная часть + state.velocity.x += d*dir.x; + state.velocity.y += d*dir.y; + state.velocity.z += d*dir.z; + + // Вращательная часть + var x:Number = (r.y*dir.z - r.z*dir.y)*magnitude; + var y:Number = (r.z*dir.x - r.x*dir.z)*magnitude; + var z:Number = (r.x*dir.y - r.y*dir.x)*magnitude; + + state.rotation.x += invInertiaWorld.a*x + invInertiaWorld.b*y + invInertiaWorld.c*z; + state.rotation.y += invInertiaWorld.e*x + invInertiaWorld.f*y + invInertiaWorld.g*z; + state.rotation.z += invInertiaWorld.i*x + invInertiaWorld.j*y + invInertiaWorld.k*z; + } + + /** + * @param f + */ + public function addForce(f:Vector3):void { + forceAccum.vAdd(f); + } + + /** + * + * @param fx + * @param fy + * @param fz + */ + public function addForceXYZ(fx:Number, fy:Number, fz:Number):void { + forceAccum.x += fx; + forceAccum.y += fy; + forceAccum.z += fz; + } + + /** + * @param pos + * @param f + */ + public function addWorldForceXYZ(px:Number, py:Number, pz:Number, fx:Number, fy:Number, fz:Number):void { + forceAccum.x += fx; + forceAccum.y += fy; + forceAccum.z += fz; + + var pos:Vector3 = state.pos; + var rx:Number = px - pos.x; + var ry:Number = py - pos.y; + var rz:Number = pz - pos.z; + +// var x:Number = ry*fz - rz*fy; +// var y:Number = rz*fx - rx*fz; +// var z:Number = rx*fy - ry*fx; + + torqueAccum.x += ry*fz - rz*fy; + torqueAccum.y += rz*fx - rx*fz; + torqueAccum.z += rx*fy - ry*fx; + } + + /** + * @param pos + * @param f + */ + public function addWorldForce(pos:Vector3, force:Vector3):void { + forceAccum.vAdd(force); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(force)); + } + + /** + * @param pos + * @param f + */ + public function addWorldForceScaled(pos:Vector3, force:Vector3, scale:Number):void { + _f.x = scale*force.x; + _f.y = scale*force.y; + _f.z = scale*force.z; + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(_f)); + } + + /** + * @param pos + * @param f + */ + public function addLocalForce(pos:Vector3, force:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(pos, _r); + // Трансформируем вектор силы в мировую систему + baseMatrix.transformVector(force, _f); + // Добавляем силу и момент + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vCross(_f)); + } + + /** + * + * @param localPos + * @param worldForce + */ + public function addWorldForceAtLocalPoint(localPos:Vector3, worldForce:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(localPos, _r); + // Добавляем силу и момент + forceAccum.vAdd(worldForce); + torqueAccum.vAdd(_r.vCross(worldForce)); + } + + /** + * @param dt + */ + public function beforePhysicsStep(dt:Number):void { + } + + /** + * @param t + */ + public function addTorque(t:Vector3):void { + torqueAccum.vAdd(t); + } + + /** + * + */ + altphysics function clearAccumulators():void { + forceAccum.x = forceAccum.y = forceAccum.z = 0; + torqueAccum.x = torqueAccum.y = torqueAccum.z = 0; + } + + /** + * + */ + altphysics function calcAccelerations():void { + accel.x = forceAccum.x*invMass; + accel.y = forceAccum.y*invMass; + accel.z = forceAccum.z*invMass; + angleAccel.x = invInertiaWorld.a*torqueAccum.x + invInertiaWorld.b*torqueAccum.y + invInertiaWorld.c*torqueAccum.z; + angleAccel.y = invInertiaWorld.e*torqueAccum.x + invInertiaWorld.f*torqueAccum.y + invInertiaWorld.g*torqueAccum.z; + angleAccel.z = invInertiaWorld.i*torqueAccum.x + invInertiaWorld.j*torqueAccum.y + invInertiaWorld.k*torqueAccum.z; + } + + /** + * Вычисляет производные данные. + */ + public function calcDerivedData():void { + // Вычисление базисной матрицы и обратного тензора инерции в мировых координатах + state.orientation.toMatrix3(baseMatrix); + invInertiaWorld.copy(invInertia).append(baseMatrix).prependTransposed(baseMatrix); + if (collisionPrimitives != null) { + aabb.infinity(); + var item:CollisionPrimitiveListItem = collisionPrimitives.head; + while (item != null) { + var primitive:CollisionPrimitive = item.primitive; + primitive.transform.setFromMatrix3(baseMatrix, state.pos); + if (primitive.localTransform != null) { + primitive.transform.prepend(primitive.localTransform); + } + primitive.calculateAABB(); + aabb.addBoundBox(primitive.aabb); + item = item.next; + } + } + } + + /** + * + */ + altphysics function saveState():void { + prevState.copy(state); + } + + /** + * + */ + altphysics function restoreState():void { + state.copy(prevState); + } + + /** + * @param dt + */ + altphysics function integrateVelocity(dt:Number):void { + // v = v + a*t + state.velocity.x += accel.x*dt; + state.velocity.y += accel.y*dt; + state.velocity.z += accel.z*dt; + // rot = rot + eps*t + state.rotation.x += angleAccel.x*dt; + state.rotation.y += angleAccel.y*dt; + state.rotation.z += angleAccel.z*dt; + + state.velocity.x *= linDamping; + state.velocity.y *= linDamping; + state.velocity.z *= linDamping; + + state.rotation.x *= rotDamping; + state.rotation.y *= rotDamping; + state.rotation.z *= rotDamping; + } + + /** + * + */ + altphysics function integratePosition(dt:Number):void { + // pos = pos + v*t + state.pos.x += state.velocity.x*dt; + state.pos.y += state.velocity.y*dt; + state.pos.z += state.velocity.z*dt; + // q = q + 0.5*rot*q + state.orientation.addScaledVector(state.rotation, dt); + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/.svn/text-base/BodyList.as.svn-base b/0.0.9.0/src/alternativa/physics/.svn/text-base/BodyList.as.svn-base new file mode 100644 index 0000000..aa8f79f --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/.svn/text-base/BodyList.as.svn-base @@ -0,0 +1,76 @@ +package alternativa.physics { + + /** + * + */ + public class BodyList { + + public var head:BodyListItem; + public var tail:BodyListItem; + public var size:int; + + /** + * + */ + public function BodyList() { + } + + /** + * + * @param body + */ + public function append(body:Body):void { + var item:BodyListItem = BodyListItem.create(body); + if (head == null) { + head = tail = item; + } else { + tail.next = item; + item.prev = tail; + tail = item; + } + size++; + } + + /** + * + * @param body + */ + public function remove(body:Body):Boolean { + var item:BodyListItem = findItem(body); + if (item == null) return false; + if (item == head) { + if (size == 1) { + head = tail = null; + } else { + head = item.next; + head.prev = null; + } + } else { + if (item == tail) { + tail = item.prev; + tail.next = null; + } else { + item.prev.next = item.next; + item.next.prev = item.prev; + } + } + item.dispose(); + size--; + return true; + } + + /** + * + * @param body + * @return + */ + public function findItem(body:Body):BodyListItem { + var item:BodyListItem = head; + while (item != null && item.body != body) { + item = item.next; + } + return item; + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/.svn/text-base/BodyListItem.as.svn-base b/0.0.9.0/src/alternativa/physics/.svn/text-base/BodyListItem.as.svn-base new file mode 100644 index 0000000..382e11c --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/.svn/text-base/BodyListItem.as.svn-base @@ -0,0 +1,74 @@ +package alternativa.physics { + + /** + * + */ + public class BodyListItem { + + // Верхний элемент хранилища + private static var poolTop:BodyListItem; + + /** + * Создаёт новый элемент списка. + * + * @param primitive примитив, содержащийся в элементе + * @return новый элемент списка + */ + public static function create(body:Body):BodyListItem { + var item:BodyListItem; + if (poolTop == null) { + item = new BodyListItem(body); + } else { + item = poolTop; + poolTop = item.next; + item.next = null; + item.body = body; + } + return item; + } + + /** + * Очищает хранилище. + */ + public static function clearPool():void { + var item:BodyListItem = poolTop; + while (item != null) { + poolTop = item.next; + item.next = null; + item = poolTop; + } + } + + /** + * + */ + public var body:Body; + /** + * + */ + public var next:BodyListItem; + /** + * + */ + public var prev:BodyListItem; + + /** + * + * @param body + */ + public function BodyListItem(body:Body) { + this.body = body; + } + + /** + * + */ + public function dispose():void { + body = null; + prev = null; + next = poolTop; + poolTop = this; + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/.svn/text-base/BodyMaterial.as.svn-base b/0.0.9.0/src/alternativa/physics/.svn/text-base/BodyMaterial.as.svn-base new file mode 100644 index 0000000..7a7c47d --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/.svn/text-base/BodyMaterial.as.svn-base @@ -0,0 +1,11 @@ +package alternativa.physics { + + public class BodyMaterial { + + public var restitution:Number = 0; + public var friction:Number = 0.3; +// public var dynamicFriction:Number = 0.2; +// public var dynamicFriction:Number = 0.2; + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/.svn/text-base/BodyState.as.svn-base b/0.0.9.0/src/alternativa/physics/.svn/text-base/BodyState.as.svn-base new file mode 100644 index 0000000..3987388 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/.svn/text-base/BodyState.as.svn-base @@ -0,0 +1,39 @@ +package alternativa.physics { + import alternativa.math.Quaternion; + import alternativa.math.Vector3; + + /** + * Класс описывает состояние твёрдого тела. + */ + public class BodyState { + /** + * Положение тела. + */ + public var pos:Vector3 = new Vector3(); + /** + * Ориентация тела. + */ + public var orientation:Quaternion = new Quaternion(); + /** + * Скорость тела. + */ + public var velocity:Vector3 = new Vector3(); + /** + * Угловая скорость тела. + */ + public var rotation:Vector3 = new Vector3(); + + /** + * Копирует значение указанного объекта. + * + * @param state + */ + public function copy(state:BodyState):void { + pos.vCopy(state.pos); + orientation.copy(state.orientation); + velocity.vCopy(state.velocity); + rotation.vCopy(state.rotation); + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/.svn/text-base/CollisionPrimitiveList.as.svn-base b/0.0.9.0/src/alternativa/physics/.svn/text-base/CollisionPrimitiveList.as.svn-base new file mode 100644 index 0000000..5d5304f --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/.svn/text-base/CollisionPrimitiveList.as.svn-base @@ -0,0 +1,89 @@ +package alternativa.physics { + import alternativa.physics.collision.CollisionPrimitive; + + /** + * + */ + public class CollisionPrimitiveList { + + public var head:CollisionPrimitiveListItem; + public var tail:CollisionPrimitiveListItem; + public var size:int; + + /** + * + */ + public function CollisionPrimitiveList() { + } + + /** + * + * @param primitive + */ + public function append(primitive:CollisionPrimitive):void { + var item:CollisionPrimitiveListItem = CollisionPrimitiveListItem.create(primitive); + if (head == null) { + head = tail = item; + } else { + tail.next = item; + item.prev = tail; + tail = item; + } + size++; + } + + /** + * + * @param primitve + */ + public function remove(primitve:CollisionPrimitive):void { + var item:CollisionPrimitiveListItem = findItem(primitve); + if (item == null) return; + if (item == head) { + if (size == 1) { + head = tail = null; + } else { + head = item.next; + head.prev = null; + } + } else { + if (item == tail) { + tail = tail.prev; + tail.next = null; + } else { + item.prev.next = item.next; + item.next.prev = item.prev; + } + } + item.dispose(); + size--; + } + + /** + * + * @param primitive + * @return + */ + public function findItem(primitive:CollisionPrimitive):CollisionPrimitiveListItem { + var item:CollisionPrimitiveListItem = head; + while (item != null && item.primitive != primitive) { + item = item.next; + } + return item; + } + + /** + * + */ + public function clear():void { + while (head != null) { + var item:CollisionPrimitiveListItem = head; + head = head.next; + item.dispose(); + } + tail = null; + size = 0; + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/.svn/text-base/CollisionPrimitiveListItem.as.svn-base b/0.0.9.0/src/alternativa/physics/.svn/text-base/CollisionPrimitiveListItem.as.svn-base new file mode 100644 index 0000000..d204532 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/.svn/text-base/CollisionPrimitiveListItem.as.svn-base @@ -0,0 +1,76 @@ +package alternativa.physics { + import alternativa.physics.collision.CollisionPrimitive; + + /** + * Элемент списка примитивов. + */ + public class CollisionPrimitiveListItem { + + // Верхний элемент хранилища + private static var poolTop:CollisionPrimitiveListItem; + + /** + * Создаёт новый элемент списка. + * + * @param primitive примитив, содержащийся в элементе + * @return новый элемент списка + */ + public static function create(primitive:CollisionPrimitive):CollisionPrimitiveListItem { + var item:CollisionPrimitiveListItem; + if (poolTop == null) { + item = new CollisionPrimitiveListItem(primitive); + } else { + item = poolTop; + item.primitive = primitive; + poolTop = item.next; + item.next = null; + } + return item; + } + + /** + * Очищает хранилище. + */ + public static function clearPool():void { + var curr:CollisionPrimitiveListItem = poolTop; + while (curr != null) { + poolTop = curr.next; + curr.next = null; + curr = poolTop; + } + } + + /** + * Примитив, хранящийся в элементе списка. + */ + public var primitive:CollisionPrimitive; + /** + * Ссылка не следующий элемент списка. + */ + public var next:CollisionPrimitiveListItem; + /** + * Ссылка не предыдущий элемент списка. + */ + public var prev:CollisionPrimitiveListItem; + + /** + * Создаёт новый экземпляр. + * + * @param primitive примитив, хранящийся в элементе списка + */ + public function CollisionPrimitiveListItem(primitive:CollisionPrimitive) { + this.primitive = primitive; + } + + /** + * Очищает внутренние ссылки и помещает элемент в хранилище для дальнейшего использования. + */ + public function dispose():void { + primitive = null; + prev = null; + next = poolTop; + poolTop = this; + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/.svn/text-base/Contact.as.svn-base b/0.0.9.0/src/alternativa/physics/.svn/text-base/Contact.as.svn-base new file mode 100644 index 0000000..3f010ec --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/.svn/text-base/Contact.as.svn-base @@ -0,0 +1,69 @@ +package alternativa.physics { + + + import alternativa.math.Vector3; + + /** + * Описывает контакт одного или двух тел. + */ + public class Contact { + /** + * Первое тело контакта, не может быть равно null. + */ + public var body1:Body; + /** + * Второе тело контакта, может быть равно null. + */ + public var body2:Body; + /** + * Предрассчитанный взаимный коэффициент отскока. + */ + public var restitution:Number; + /** + * Предрассчитанный взаимный коэффициент трения. + */ + public var friction:Number; + /** + * Нормаль контакта. Направлена к первому телу. + */ + public var normal:Vector3 = new Vector3(); + /** + * Список точек контакта. + */ + public var points:Vector. = new Vector.(MAX_POINTS, true); + /** + * Количество точек контакта. + */ + public var pcount:int; + /** + * Максимальная глубина пересечения. + */ + public var maxPenetration:Number = 0; + /** + * Флаг показывает, разрешён контакт или нет. + */ + public var satisfied:Boolean; + /** + * Следующий контакт в списке. + */ + public var next:Contact; + /** + * Индекс контакта. Первый контакт в списке имеет индекс 0. + */ + public var index:int; + + // Максимальное количество точек контакта + private const MAX_POINTS:int = 8; + + /** + * + */ + public function Contact(index:int) { + this.index = index; + for (var i:int = 0; i < MAX_POINTS; i++) { + points[i] = new ContactPoint(); + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/.svn/text-base/ContactPoint.as.svn-base b/0.0.9.0/src/alternativa/physics/.svn/text-base/ContactPoint.as.svn-base new file mode 100644 index 0000000..395d2dc --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/.svn/text-base/ContactPoint.as.svn-base @@ -0,0 +1,55 @@ +package alternativa.physics { + import alternativa.math.Vector3; + + /** + * + */ + public class ContactPoint { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + + // Величины, расчитываемые перед началом фазы решения контактов + + // Требуемая проекция конечной скорости на нормаль для упругого контакта + public var normalVel:Number; + // Минимальная скорость разделения неупругого контакта + public var minSepVel:Number; + // Изменение проекции скорости на единицу нормального импульса + public var velByUnitImpulseN:Number; + + public var angularInertia1:Number; + public var angularInertia2:Number; + + // Радиус-вектор точки контакта относительно центра первого тела + public var r1:Vector3 = new Vector3(); + // Радиус-вектор точки контакта относительно центра второго тела + public var r2:Vector3 = new Vector3(); + + // Величины, накапливаемые во время фазы решения контактов + + // Накопленный импульс, применяемый для получения требуемой относительной скорости в точке контакта + public var accumImpulseN:Number; + // Накопленный импульс, применяемый для разделения тел в точке контакта. Не создаёт момента. +// public var accumSepImpulse:Number; + + public var satisfied:Boolean; + + /** + * + * @param cp + */ + public function copyFrom(cp:ContactPoint):void { + pos.vCopy(cp.pos); + penetration = cp.penetration; + feature1 = cp.feature1; + feature2 = cp.feature2; + r1.vCopy(cp.r1); + r2.vCopy(cp.r2); + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/.svn/text-base/PhysicsScene.as.svn-base b/0.0.9.0/src/alternativa/physics/.svn/text-base/PhysicsScene.as.svn-base new file mode 100644 index 0000000..abdc762 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/.svn/text-base/PhysicsScene.as.svn-base @@ -0,0 +1,743 @@ +package alternativa.physics { + + + import alternativa.physics.collision.ICollisionDetector; + import alternativa.physics.collision.KdTreeCollisionDetector; + import alternativa.physics.constraints.Constraint; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * Класс реализует физическую симуляцию поведения твёрдых тел. + */ + public class PhysicsScene { + + private static var lastBodyId:int; + + // Максимальное количество контактов + public const MAX_CONTACTS:int = 1000; + + ////////////////////////////////////////////////////////////////// + // Настроечные параметры симуляции. Могут быть изменены в любой + // момент времени без нарушения корректной работы симуляции. + ////////////////////////////////////////////////////////////////// + // Количество шагов, за которое пересекающиеся тела должны разделиться + public var penResolutionSteps:int = 10; + // Величина допустимой глубины пересечения + public var allowedPenetration:Number = 0.1; + // Максимальная скорость, добавляемая с целью разделения тел + public var maxPenResolutionSpeed:Number = 0.5; + // Количество итераций для обработки упругих контактов + public var collisionIterations:int = 5; + // Количество итераций для обработки неупругих контактов + public var contactIterations:int = 5; + // Флаг использования предсказания состояний + public var usePrediction:Boolean = false; + + public var freezeSteps:int = 10; + public var linSpeedFreezeLimit:Number = 1; + public var angSpeedFreezeLimit:Number = 0.01; + + // Переменные для процедуры разделения тел путём непосредственного их перемещенеия. + // !!!!!!!!!!!!!!!!!!!!!!!! + // !!! Экспериментально !!! + // !!!!!!!!!!!!!!!!!!!!!!!! + public var staticSeparationIterations:int = 10; + public var staticSeparationSteps:int = 10; + public var maxAngleMove:Number = 10; + public var useStaticSeparation:Boolean = false; + + // Вектор гравитации + public var _gravity:Vector3 = new Vector3(0, 0, -9.8); + // Модуль вектора гравитации + public var _gravityMagnitude:Number = 9.8; + + // Использующийся детектор столкновений + public var collisionDetector:ICollisionDetector; + + // Список тел, участвующих в симуляции + public var bodies:BodyList = new BodyList(); + + // Список контактов на текущем шаге симуляции + altphysics var contacts:Contact; + // Количество контактов на текущем шаге симуляции +// altphysics var contactsNum:int; + // Список ограничений + altphysics var constraints:Vector. = new Vector.(); + // Количество ограничений + altphysics var constraintsNum:int; + // Временная метка. Число прошедших шагов с начала симуляции. + public var timeStamp:int; + // Время с начала симуляции, мс + public var time:int; + // Первый неиспользованный контакт на текущем шаге симуляции + private var borderContact:Contact; + + // Временные переменные для избежания создания экземпляров + private var _r:Vector3 = new Vector3(); + private var _t:Vector3 = new Vector3(); + private var _v:Vector3 = new Vector3(); + private var _v1:Vector3 = new Vector3(); + private var _v2:Vector3 = new Vector3(); + + /** + * + */ + public function PhysicsScene() { + contacts = new Contact(0); + var contact:Contact = contacts; + for (var i:int = 1; i < MAX_CONTACTS; i++) { + contact.next = new Contact(i); + contact = contact.next; + } + collisionDetector = new KdTreeCollisionDetector(); + } + + /** + * Вектор гравитации. + */ + public function get gravity():Vector3 { + return _gravity.vClone(); + } + + /** + * @private + */ + public function set gravity(value:Vector3):void { + _gravity.vCopy(value); + _gravityMagnitude = _gravity.vLength(); + } + + /** + * Добавляет тело в симуляцию. + * + * @param body + */ + public function addBody(body:Body):void { + body.id = lastBodyId++; + body.world = this; + bodies.append(body); + } + + /** + * Удаляет тело из симуляции. + * @param body + * @return + */ + public function removeBody(body:Body):void { + if (bodies.remove(body)) { + body.world = null; + } + } + + /** + * Добавляет ограничение. + * @param c + */ + public function addConstraint(c:Constraint):void { + constraints[constraintsNum++] = c; + c.world = this; + } + + /** + * Удаляет ограничение. + * @param c + */ + public function removeConstraint(c:Constraint):Boolean { + var idx:int = constraints.indexOf(c); + if (idx < 0) return false; + constraints.splice(idx, 1) + constraintsNum--; + c.world = null; + return true; + } + + /** + * Применяет к телам действующте на них силы. Аккумуляторы сил и моментов тел очищаются после завершения шага + * симуляции, поэтому на момент вызова метода могут уже содержать некоторые значения. + * + * @param dt промежуток времени, в течении которого действуют силы + */ + private function applyForces(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + body.beforePhysicsStep(dt); + body.calcAccelerations(); + // Ускорение свободного падения применяется только к подвижным телам во избежание некорректного изменения + // фиктивной скорости неподвижных тел. + if (body.movable && !body.frozen) { + body.accel.x += _gravity.x; + body.accel.y += _gravity.y; + body.accel.z += _gravity.z; + } + item = item.next; + } + } + + /** + * Определяет все столкновения на текущем шаге симуляции и заполняет список получившихся контактов. + * + * @param dt длительность шага симуляции + */ + private function detectCollisions(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + if (!body.frozen) { + body.contactsNum = 0; + body.saveState(); + // При включённом режиме предсказания состояние тел интегрируется на один шаг вперёд + if (usePrediction) { + body.integrateVelocity(dt); + body.integratePosition(dt); + } + body.calcDerivedData(); + } + item = item.next; + } + + borderContact = collisionDetector.getAllContacts(contacts); + + // Расчёт относительных векторов точки контакта вынесен сюда из-за необходимости учитывать + // положение тел в предсказанном состоянии + var contact:Contact = contacts; + while (contact != borderContact) { + var b1:Body = contact.body1; + var b2:Body = contact.body2; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + var bPos:Vector3 = b1.state.pos; + cp.r1.x = cp.pos.x - bPos.x; + cp.r1.y = cp.pos.y - bPos.y; + cp.r1.z = cp.pos.z - bPos.z; + if (b2 != null) { + bPos = b2.state.pos; + cp.r2.x = cp.pos.x - bPos.x; + cp.r2.y = cp.pos.y - bPos.y; + cp.r2.z = cp.pos.z - bPos.z; + } + } + contact = contact.next; + } + + // Восстановление состояния тел + if (usePrediction) { + item = bodies.head; + while (item != null) { + body = item.body; + if (!body.frozen) { + body.restoreState(); + body.calcDerivedData(); + } + item = item.next; + } + } + } + + /** + * Подготваливает полученные из детектора столкновений контакты, расчитывая значения, не меняющиеся + * в ходе шага симуляции. + */ + private function preProcessContacts(dt:Number):void { + var contact:Contact = contacts; + while (contact != borderContact) { + var b1:Body = contact.body1; + var b2:Body = contact.body2; + // Столкнувшиеся тела размораживаются + if (b1.frozen) { + b1.frozen = false; + b1.freezeCounter = 0; + } + if (b2 != null && b2.frozen) { + b2.frozen = false; + b2.freezeCounter = 0; + } + contact.restitution = b1.material.restitution; + if (b2 != null && b2.material.restitution < contact.restitution) contact.restitution = b2.material.restitution; + contact.friction = b1.material.friction; + if (b2 != null && b2.material.friction < contact.friction) contact.friction = b2.material.friction; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.accumImpulseN = 0; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + cp.velByUnitImpulseN = 0; + if (b1.movable) { + cp.angularInertia1 = _v.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1).vDot(contact.normal); + cp.velByUnitImpulseN += b1.invMass + cp.angularInertia1; + } + if (b2 != null && b2.movable) { + cp.angularInertia2 = _v.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2).vDot(contact.normal); + cp.velByUnitImpulseN += b2.invMass + cp.angularInertia2; + } + // Расчёт требуемой конечной скорости для упругого контакта + calcSepVelocity(b1, b2, cp, _v); + cp.normalVel = _v.vDot(contact.normal); + if (cp.normalVel < 0) cp.normalVel = - contact.restitution*cp.normalVel; + // Скорость разделения неупругого контакта + cp.minSepVel = cp.penetration > allowedPenetration ? (cp.penetration - allowedPenetration)/(penResolutionSteps*dt) : 0; + if (cp.minSepVel > maxPenResolutionSpeed) cp.minSepVel = maxPenResolutionSpeed; + } + contact = contact.next; + } + for (var i:int = 0; i < constraintsNum; i++) { + var constraint:Constraint = constraints[i]; + constraint.preProcess(dt); + } + } + + /** + * + * @param dt + * @param forceInelastic + */ + private function processContacts(dt:Number, forceInelastic:Boolean):void { + var iterNum:int = forceInelastic ? contactIterations : collisionIterations; + var i:int; + var forwardLoop:Boolean = false; + for (var iter:int = 0; iter < iterNum; iter++) { + forwardLoop = !forwardLoop; + var contact:Contact = contacts; + while (contact != borderContact) { + resolveContact(contact, forceInelastic, forwardLoop); + contact = contact.next; + } + // Ограничения + for (i = 0; i < constraintsNum; i++) { + var constraint:Constraint = constraints[i]; + constraint.apply(dt); + } + } + + // Разделение контактов путём непосредственного изменения координат и ориентации. Экспериментально. +// if (forceInelastic && useStaticSeparation) performStaticSeparation(); + } + + /** + * + */ + private function resolveContact(contactInfo:Contact, forceInelastic:Boolean, forwardLoop:Boolean):void { + var b1:Body = contactInfo.body1; + var b2:Body = contactInfo.body2; + var normal:Vector3 = contactInfo.normal; + var i:int; + if (forwardLoop) { + for (i = 0; i < contactInfo.pcount; i++) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } else { + for (i = contactInfo.pcount - 1; i >= 0; i--) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } + } + + /** + * + * @param idx + * @param b1 + * @param b2 + * @param colInfo + * @param normal + * @param forceInelastic + */ + private function resolveContactPoint(idx:int, b1:Body, b2:Body, contact:Contact, normal:Vector3, forceInelastic:Boolean):void { + var cp:ContactPoint = contact.points[idx]; + if (!forceInelastic) { + cp.satisfied = true; + } + + var newVel:Number = 0; + calcSepVelocity(b1, b2, cp, _v); + var cnormal:Vector3 = contact.normal; + var sepVel:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + if (forceInelastic) { + var minSpeVel:Number = useStaticSeparation ? 0 : cp.minSepVel; + if (sepVel < minSpeVel) { + cp.satisfied = false; + } else if (cp.satisfied) return; + newVel = minSpeVel; + } else { + newVel = cp.normalVel; + } + var deltaVel:Number = newVel - sepVel; + var impulse:Number = deltaVel/cp.velByUnitImpulseN; + var accumImpulse:Number = cp.accumImpulseN + impulse; + if (accumImpulse < 0) { + accumImpulse = 0; + } + var deltaImpulse:Number = accumImpulse - cp.accumImpulseN; + cp.accumImpulseN = accumImpulse; + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, normal, deltaImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, normal, -deltaImpulse); + } + + // Учёт силы трения + calcSepVelocity(b1, b2, cp, _v); + // Расчитываем изменение касательной скорости на единицу касательного импульса + var tanSpeedByUnitImpulse:Number = 0; + +// _v.vAddScaled(-_v.vDot(contact.normal), contact.normal); + var dot:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + _v.x -= dot*cnormal.x; + _v.y -= dot*cnormal.y; + _v.z -= dot*cnormal.z; + + var tanSpeed:Number = _v.vLength(); + if (tanSpeed < 0.001) return; + +// _t.vCopy(_v).vNormalize().vReverse(); + _t.x = -_v.x; + _t.y = -_v.y; + _t.z = -_v.z; + _t.vNormalize(); + + var r:Vector3; + var m:Matrix3; + var xx:Number; + var yy:Number; + var zz:Number; + // dV = b.invMass + ((invI * (r % t)) % r) * t + if (b1.movable) { +// _v.vCross2(cp.r1, _t).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1); + r = cp.r1; + m = b1.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b1.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + if (b2 != null && b2.movable) { +// _v.vCross2(cp.r2, _t).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2); + + r = cp.r2; + m = b2.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b2.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + + var tanImpulse:Number = tanSpeed/tanSpeedByUnitImpulse; + var max:Number = contact.friction*cp.accumImpulseN; + if (max < 0) { + if (tanImpulse < max) tanImpulse = max; + } else { + if (tanImpulse > max) tanImpulse = max; + } + + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, _t, tanImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, _t, -tanImpulse); + } + } + + /** + * + * @param cp + * @param normal + * @return + */ + private function calcSepVelocity(body1:Body, body2:Body, cp:ContactPoint, result:Vector3):void { + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 +// result.vCopy(body1.state.velocity).vAdd(_v1.vCross2(body1.state.rotation, cp.r1)); + var rot:Vector3 = body1.state.rotation; + var v:Vector3 = cp.r1; + var x:Number = rot.y*v.z - rot.z*v.y; + var y:Number = rot.z*v.x - rot.x*v.z; + var z:Number = rot.x*v.y - rot.y*v.x; + v = body1.state.velocity; + result.x = v.x + x; + result.y = v.y + y; + result.z = v.z + z; + // V2 = V2_c + w2%r2 + if (body2 != null) { +// result.vSubtract(body2.state.velocity).vSubtract(_v2.vCross2(body2.state.rotation, cp.r2)); + rot = body2.state.rotation; + v = cp.r2; + x = rot.y*v.z - rot.z*v.y; + y = rot.z*v.x - rot.x*v.z; + z = rot.x*v.y - rot.y*v.x; + v = body2.state.velocity; + result.x -= v.x + x; + result.y -= v.y + y; + result.z -= v.z + z; + } + } + + /** + * + * @param dt + */ + private function intergateVelocities(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + item.body.integrateVelocity(dt); + item = item.next; + } + } + + /** + * + * @param dt + */ + private function integratePositions(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + if (body.movable && !body.frozen) { + body.integratePosition(dt); + } + item = item.next; + } + } + + /** + * + */ + private function performStaticSeparation():void { + var iterNum:int = staticSeparationIterations; +// iterNum = 100; + + // 1. В начале каждой итерации для всех контактов сбрасывается флаг satisfied с одновременным поиском наихудшего контакта, т.е. имеющего наибольшую величину пересечения тел. + // 2. Если найденный контакт имеет величину пересечения меньше предельно допустимой процедура прерывается, т.к. разделение контактов не требуется. + // 3. Если процедура продолжается, то выполняется разделение найденного контакта и установка его флага satisfied в true, после чего запускается внутренний цикл, состоящий + // из contactsNum - 1 итераций. На каждой итерации ищется наихудший контакт среди оставшихся (satisfied == false). Если такой контакт найден, то выполняется его разделение + // и переход к следующей итерации внутреннего цикла, иначе внутренний цикл прерывается и выполняется переход к следующей итерации внешнего цикла (шаг 1.). +// for (var iter:int = 0; iter < iterNum; iter++) { +// // Ищем контакт с максимальной величиной пересечения тел, одновременно сбрасывая флаги satisfied в false +// var worstContact:Contact = contacts[0]; +// var i:int; +// for (i = 1; i < contactsNum; i++) { +// var contact:Contact = contacts[i]; +// contact.satisfied = false; +// if (contact.maxPenetration > worstContact.maxPenetration) worstContact = contact; +// } +// if (worstContact.maxPenetration <= allowedPenetration) return; +// +// resolveInterpenetration(worstContact); +// // Внутренний цикл по оставшимся контактам +// for (i = 1; i < contactsNum; i++) { +// worstContact = getWorstContact(); +// if (worstContact == null) break; +// resolveInterpenetration(worstContact); +// } +// } + } + + /** + * + */ +// private function getWorstContact():Contact { +// var maxPen:Number = 0; +// var worst:Contact = null; +// for (var i:int = 0; i < contactsNum; i++) { +// var c:Contact = contacts[i]; +// if (!c.satisfied && c.maxPenetration > maxPen) { +// worst = c; +// maxPen = c.maxPenetration; +// } +// } +// return maxPen > allowedPenetration ? worst : null; +// } + + /** + * Разделяет указанный контакт, выполняя staticSeparationSteps итераций по списку точек контакта. + * 1. В начале каждой итерации ищется точка с наибольшим пересечением, одновременно сбрасываются флаги satisfied у точек. + * 2. Для найденной точки выполняется процедура разделения. + * 3. После выполняется pcount - 1 итерация по списку точек, каждый раз ищется наихудшая среди имеющих satisfied == false, для которой выполняется процедура разделения. + * Если наихудшая точка не найдена, то выполнется переход к следующей итерации внешнего цикла (шаг 1). + * + * В конце процедуры разделения обновляются значения пересечений для остальных точек контакта, а также для всех прочих контактов, относящихся к телам текущего. + * + * @param contact контакт для разделения + */ + private function resolveInterpenetration(contact:Contact):void { + contact.satisfied = true; + + for (var step:int = 0; step < staticSeparationSteps; step++) { + var worstCp:ContactPoint = contact.points[0]; + var cp:ContactPoint; + var i:int; + for (i = 1; i < contact.pcount; i++) { + cp = contact.points[i]; + cp.satisfied = false; + if (cp.penetration > worstCp.penetration) worstCp = cp; + } + if (worstCp.penetration <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + // Разделяем оставшиеся точки + var maxPen:Number = 0; + for (i = 1; i < contact.pcount; i++) { + // Поиск наихудшей точки + for (var j:int = 0; j < contact.pcount; j++) { + cp = contact.points[j]; + if (cp.satisfied) continue; + if (cp.penetration > maxPen) { + maxPen = cp.penetration; + worstCp = cp; + } + } + if (maxPen <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + } + } + } + + /** + * + * @param cp + * @param contact + */ + private function separateContactPoint(cp:ContactPoint, contact:Contact):void { +// cp.satisfied = true; +// +// var b1:Body = contact.body1; +// var b2:Body = contact.body2; +// var totalMove:Number = cp.penetration - allowedPenetration; +// var moveCoeff:Number = totalMove/cp.velByUnitImpulseN; +// var linMove1:Number; +// var angleMove1:Number; +// if (b1.movable) { +// linMove1 = b1.invMass*moveCoeff; +// angleMove1 = cp.angularInertia1*moveCoeff; +// if (angleMove1 > maxAngleMove) { +// linMove1 += angleMove1 - maxAngleMove; +// angleMove1 -= maxAngleMove; +// } +// b1.state.pos.vAddScaled(linMove1, contact.normal); +// _v1.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vScale(angleMove1); +// b1.state.orientation.addScaledVector(_v1, 1); +// } +// var linMove2:Number; +// var angleMove2:Number; +// if (b2 != null && b2.movable) { +// linMove2 = b2.invMass*moveCoeff; +// angleMove2 = cp.angularInertia2*moveCoeff; +// if (angleMove2 > maxAngleMove) { +// linMove2 += angleMove2 - maxAngleMove; +// angleMove2 -= maxAngleMove; +// } +// b2.state.pos.vAddScaled(-linMove2, contact.normal); +// _v2.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vScale(angleMove2); +// _v2.vReverse(); +// b2.state.orientation.addScaledVector(_v2, 1); +// } +// cp.penetration = allowedPenetration; +// // Обновляем пересечения в других точках +// var i:int; +// for (i = 0; i < contact.pcount; i++) { +// var cp1:ContactPoint = contact.points[i]; +// if (cp1 == cp) continue; +// var angularMove:Number; +// if (b1.movable) { +// angularMove = _v.vCross2(_v1, cp1.r1).vDot(contact.normal); +// cp1.penetration -= linMove1 + angularMove; +// } +// if (b2 != null && b2.movable) { +// angularMove = _v.vCross2(_v2, cp1.r2).vDot(contact.normal); +// cp1.penetration -= linMove2 - angularMove; +// } +// // Обновление максимального значения для контакта +// if (cp1.penetration > contact.maxPenetration) contact.maxPenetration = cp1.penetration; +// } +// // Обновляем пересечения для других контактов +// var c:Contact; +// var j:int; +// if (b1.movable) { +// for (i = 0; i < b1.contactsNum; i++) { +// c = b1.contacts[i]; +// if (c == contact) continue; +// for (j = 0; j < c.pcount; j++) { +// cp1 = c.points[j]; +// if (b1 == c.body1) cp1.penetration -= linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r1).vDot(c.normal); +// else cp1.penetration += linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r2).vDot(c.normal); +// if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; +// } +// } +// } +// if (b2 != null && b2.movable) { +// for (i = 0; i < b2.contactsNum; i++) { +// c = b2.contacts[i]; +// if (c == contact) continue; +// for (j = 0; j < c.pcount; j++) { +// cp1 = c.points[j]; +// if (b2 == c.body1) cp1.penetration -= linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r1).vDot(c.normal); +// else cp1.penetration += linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r2).vDot(c.normal); +// if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; +// } +// } +// } + } + + /** + * + */ + private function postPhysics():void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + body.clearAccumulators(); + body.calcDerivedData(); + if (body.canFreeze) { + if (body.state.velocity.vLength() < linSpeedFreezeLimit && body.state.rotation.vLength() < angSpeedFreezeLimit) { + if (!body.frozen) { + body.freezeCounter++; + if (body.freezeCounter >= freezeSteps) body.frozen = true; + } + } else { + body.freezeCounter = 0; + body.frozen = false; + } + } + item = item.next; + } + } + + /** + * + * @param delta + */ + public function update(delta:int):void { + timeStamp++; + time += delta; + var dt:Number = 0.001*delta; + applyForces(dt); + detectCollisions(dt); + preProcessContacts(dt); + processContacts(dt, false); + intergateVelocities(dt); + processContacts(dt, true); + integratePositions(dt); + postPhysics(); + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/.svn/text-base/PhysicsUtils.as.svn-base b/0.0.9.0/src/alternativa/physics/.svn/text-base/PhysicsUtils.as.svn-base new file mode 100644 index 0000000..5a2e46f --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/.svn/text-base/PhysicsUtils.as.svn-base @@ -0,0 +1,49 @@ +package alternativa.physics { + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + public class PhysicsUtils { + + /** + * @param mass + * @param halfSize + * @param result + */ + public static function getBoxInvInertia(mass:Number, halfSize:Vector3, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + result.a = 3/(mass*(yy + zz)); + result.f = 3/(mass*(zz + xx)); + result.k = 3/(mass*(xx + yy)); + } + + /** + * @param mass + * @param r + * @param h + * @param result + */ + public static function getCylinderInvInertia(mass:Number, r:Number, h:Number, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + + result.a = result.f = 1/(mass*(h*h/12 + r*r/4)); + result.k = 2/(mass*r*r); + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base b/0.0.9.0/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base new file mode 100644 index 0000000..2715307 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base @@ -0,0 +1,3 @@ +package alternativa.physics { + public namespace altphysics = "http://alternativaplatform.com/en/altphysics"; +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/Body.as b/0.0.9.0/src/alternativa/physics/Body.as new file mode 100644 index 0000000..cd22c5b --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/Body.as @@ -0,0 +1,405 @@ +package alternativa.physics { + + import alternativa.math.Matrix3; + import alternativa.math.Matrix4; + import alternativa.math.Quaternion; + import alternativa.math.Vector3; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.IBodyCollisionPredicate; + import alternativa.physics.collision.types.BoundBox; + + use namespace altphysics; + + /** + * + */ + public class Body { + + public static var linDamping:Number = 0.997; + public static var rotDamping:Number = 0.997; + + // Идентификатор тела, уникальный в пределах мира + public var id:int; + // Имя тела + public var name:String; + // Мир, в котором находится тело + public var world:PhysicsScene; + // Флаг подвижности тела + public var movable:Boolean = true; + // Флаг указывает, может ли тело быть заморожено + public var canFreeze:Boolean = false; + + public var freezeCounter:int; + public var frozen:Boolean = false; + // Ограничивающий бокс тела + public var aabb:BoundBox = new BoundBox(); + // Предикат, через который тело получает сообщения о столкновениях + public var postCollisionPredicate:IBodyCollisionPredicate; + + // Текущее состояние тела + public var state:BodyState = new BodyState(); + // Предыдущее состояние тела + public var prevState:BodyState = new BodyState(); + // Линейное ускорение тела на текущем шаге симуляции + public var accel:Vector3 = new Vector3(); + // Угловое ускорение тела на текущем шаге симуляции + public var angleAccel:Vector3 = new Vector3(); + // Физический материал тела + public var material:BodyMaterial = new BodyMaterial(); + // Обратная масса тела + public var invMass:Number = 1; + // Обратная матрица тензора инерции в локальных координатах + public var invInertia:Matrix3 = new Matrix3(); + // Обратная матрица тензора инерции в мировых координатах + public var invInertiaWorld:Matrix3 = new Matrix3(); + // Базисная матрица тела в мировых координатах + public var baseMatrix:Matrix3 = new Matrix3(); + + public const MAX_CONTACTS:int = 20; + public var contacts:Vector. = new Vector.(MAX_CONTACTS); + public var contactsNum:int; + + public var collisionPrimitives:CollisionPrimitiveList; + + // Аккумулятор сил + public var forceAccum:Vector3 = new Vector3(); + // Аккумулятор моментов + public var torqueAccum:Vector3 = new Vector3(); + + // Внутренние переменные для избежания создания экземпляров + private static var _r:Vector3 = new Vector3(); + private static var _f:Vector3 = new Vector3(); + + /** + * + * @param invMass + * @param invInertia + */ + public function Body(invMass:Number, invInertia:Matrix3) { + this.invMass = invMass; + this.invInertia.copy(invInertia); + } + + /** + * @param primitive + * @param localTransform + */ + public function addCollisionPrimitive(primitive:CollisionPrimitive, localTransform:Matrix4 = null):void { + if (primitive == null) { + throw new ArgumentError("Primitive cannot be null"); + } + if (collisionPrimitives == null) { + collisionPrimitives = new CollisionPrimitiveList(); + } + collisionPrimitives.append(primitive); + primitive.setBody(this, localTransform); + } + + /** + * + * @param primitive + */ + public function removeCollisionPrimitive(primitive:CollisionPrimitive):void { + if (collisionPrimitives == null) return; + primitive.setBody(null); + collisionPrimitives.remove(primitive); + if (collisionPrimitives.size == 0) { + collisionPrimitives = null; + } + } + + /** + * @param t + * @param result + */ + public function interpolate(t:Number, pos:Vector3, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.w = prevState.orientation.w*t1 + state.orientation.w*t; + orientation.x = prevState.orientation.x*t1 + state.orientation.x*t; + orientation.y = prevState.orientation.y*t1 + state.orientation.y*t; + orientation.z = prevState.orientation.z*t1 + state.orientation.z*t; + } + + /** + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + state.pos.vCopy(pos); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setPositionXYZ(x:Number, y:Number, z:Number):void { + state.pos.vReset(x, y, z); + } + + /** + * + * @param vel + */ + public function setVelocity(vel:Vector3):void { + state.velocity.vCopy(vel); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setVelocityXYZ(x:Number, y:Number, z:Number):void { + state.velocity.vReset(x, y, z); + } + + /** + * + * @param rot + */ + public function setRotation(rot:Vector3):void { + state.rotation.vCopy(rot); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setRotationXYZ(x:Number, y:Number, z:Number):void { + state.rotation.vReset(x, y, z); + } + + /** + * + * @param q + */ + public function setOrientation(q:Quaternion):void { + state.orientation.copy(q); + } + + /** + * @param r + * @param dir + * @param magnitude + */ + public function applyRelPosWorldImpulse(r:Vector3, dir:Vector3, magnitude:Number):void { + var d:Number = magnitude*invMass; + // Линейная часть + state.velocity.x += d*dir.x; + state.velocity.y += d*dir.y; + state.velocity.z += d*dir.z; + + // Вращательная часть + var x:Number = (r.y*dir.z - r.z*dir.y)*magnitude; + var y:Number = (r.z*dir.x - r.x*dir.z)*magnitude; + var z:Number = (r.x*dir.y - r.y*dir.x)*magnitude; + + state.rotation.x += invInertiaWorld.a*x + invInertiaWorld.b*y + invInertiaWorld.c*z; + state.rotation.y += invInertiaWorld.e*x + invInertiaWorld.f*y + invInertiaWorld.g*z; + state.rotation.z += invInertiaWorld.i*x + invInertiaWorld.j*y + invInertiaWorld.k*z; + } + + /** + * @param f + */ + public function addForce(f:Vector3):void { + forceAccum.vAdd(f); + } + + /** + * + * @param fx + * @param fy + * @param fz + */ + public function addForceXYZ(fx:Number, fy:Number, fz:Number):void { + forceAccum.x += fx; + forceAccum.y += fy; + forceAccum.z += fz; + } + + /** + * @param pos + * @param f + */ + public function addWorldForceXYZ(px:Number, py:Number, pz:Number, fx:Number, fy:Number, fz:Number):void { + forceAccum.x += fx; + forceAccum.y += fy; + forceAccum.z += fz; + + var pos:Vector3 = state.pos; + var rx:Number = px - pos.x; + var ry:Number = py - pos.y; + var rz:Number = pz - pos.z; + +// var x:Number = ry*fz - rz*fy; +// var y:Number = rz*fx - rx*fz; +// var z:Number = rx*fy - ry*fx; + + torqueAccum.x += ry*fz - rz*fy; + torqueAccum.y += rz*fx - rx*fz; + torqueAccum.z += rx*fy - ry*fx; + } + + /** + * @param pos + * @param f + */ + public function addWorldForce(pos:Vector3, force:Vector3):void { + forceAccum.vAdd(force); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(force)); + } + + /** + * @param pos + * @param f + */ + public function addWorldForceScaled(pos:Vector3, force:Vector3, scale:Number):void { + _f.x = scale*force.x; + _f.y = scale*force.y; + _f.z = scale*force.z; + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(_f)); + } + + /** + * @param pos + * @param f + */ + public function addLocalForce(pos:Vector3, force:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(pos, _r); + // Трансформируем вектор силы в мировую систему + baseMatrix.transformVector(force, _f); + // Добавляем силу и момент + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vCross(_f)); + } + + /** + * + * @param localPos + * @param worldForce + */ + public function addWorldForceAtLocalPoint(localPos:Vector3, worldForce:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(localPos, _r); + // Добавляем силу и момент + forceAccum.vAdd(worldForce); + torqueAccum.vAdd(_r.vCross(worldForce)); + } + + /** + * @param dt + */ + public function beforePhysicsStep(dt:Number):void { + } + + /** + * @param t + */ + public function addTorque(t:Vector3):void { + torqueAccum.vAdd(t); + } + + /** + * + */ + altphysics function clearAccumulators():void { + forceAccum.x = forceAccum.y = forceAccum.z = 0; + torqueAccum.x = torqueAccum.y = torqueAccum.z = 0; + } + + /** + * + */ + altphysics function calcAccelerations():void { + accel.x = forceAccum.x*invMass; + accel.y = forceAccum.y*invMass; + accel.z = forceAccum.z*invMass; + angleAccel.x = invInertiaWorld.a*torqueAccum.x + invInertiaWorld.b*torqueAccum.y + invInertiaWorld.c*torqueAccum.z; + angleAccel.y = invInertiaWorld.e*torqueAccum.x + invInertiaWorld.f*torqueAccum.y + invInertiaWorld.g*torqueAccum.z; + angleAccel.z = invInertiaWorld.i*torqueAccum.x + invInertiaWorld.j*torqueAccum.y + invInertiaWorld.k*torqueAccum.z; + } + + /** + * Вычисляет производные данные. + */ + public function calcDerivedData():void { + // Вычисление базисной матрицы и обратного тензора инерции в мировых координатах + state.orientation.toMatrix3(baseMatrix); + invInertiaWorld.copy(invInertia).append(baseMatrix).prependTransposed(baseMatrix); + if (collisionPrimitives != null) { + aabb.infinity(); + var item:CollisionPrimitiveListItem = collisionPrimitives.head; + while (item != null) { + var primitive:CollisionPrimitive = item.primitive; + primitive.transform.setFromMatrix3(baseMatrix, state.pos); + if (primitive.localTransform != null) { + primitive.transform.prepend(primitive.localTransform); + } + primitive.calculateAABB(); + aabb.addBoundBox(primitive.aabb); + item = item.next; + } + } + } + + /** + * + */ + altphysics function saveState():void { + prevState.copy(state); + } + + /** + * + */ + altphysics function restoreState():void { + state.copy(prevState); + } + + /** + * @param dt + */ + altphysics function integrateVelocity(dt:Number):void { + // v = v + a*t + state.velocity.x += accel.x*dt; + state.velocity.y += accel.y*dt; + state.velocity.z += accel.z*dt; + // rot = rot + eps*t + state.rotation.x += angleAccel.x*dt; + state.rotation.y += angleAccel.y*dt; + state.rotation.z += angleAccel.z*dt; + + state.velocity.x *= linDamping; + state.velocity.y *= linDamping; + state.velocity.z *= linDamping; + + state.rotation.x *= rotDamping; + state.rotation.y *= rotDamping; + state.rotation.z *= rotDamping; + } + + /** + * + */ + altphysics function integratePosition(dt:Number):void { + // pos = pos + v*t + state.pos.x += state.velocity.x*dt; + state.pos.y += state.velocity.y*dt; + state.pos.z += state.velocity.z*dt; + // q = q + 0.5*rot*q + state.orientation.addScaledVector(state.rotation, dt); + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/BodyList.as b/0.0.9.0/src/alternativa/physics/BodyList.as new file mode 100644 index 0000000..aa8f79f --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/BodyList.as @@ -0,0 +1,76 @@ +package alternativa.physics { + + /** + * + */ + public class BodyList { + + public var head:BodyListItem; + public var tail:BodyListItem; + public var size:int; + + /** + * + */ + public function BodyList() { + } + + /** + * + * @param body + */ + public function append(body:Body):void { + var item:BodyListItem = BodyListItem.create(body); + if (head == null) { + head = tail = item; + } else { + tail.next = item; + item.prev = tail; + tail = item; + } + size++; + } + + /** + * + * @param body + */ + public function remove(body:Body):Boolean { + var item:BodyListItem = findItem(body); + if (item == null) return false; + if (item == head) { + if (size == 1) { + head = tail = null; + } else { + head = item.next; + head.prev = null; + } + } else { + if (item == tail) { + tail = item.prev; + tail.next = null; + } else { + item.prev.next = item.next; + item.next.prev = item.prev; + } + } + item.dispose(); + size--; + return true; + } + + /** + * + * @param body + * @return + */ + public function findItem(body:Body):BodyListItem { + var item:BodyListItem = head; + while (item != null && item.body != body) { + item = item.next; + } + return item; + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/BodyListItem.as b/0.0.9.0/src/alternativa/physics/BodyListItem.as new file mode 100644 index 0000000..382e11c --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/BodyListItem.as @@ -0,0 +1,74 @@ +package alternativa.physics { + + /** + * + */ + public class BodyListItem { + + // Верхний элемент хранилища + private static var poolTop:BodyListItem; + + /** + * Создаёт новый элемент списка. + * + * @param primitive примитив, содержащийся в элементе + * @return новый элемент списка + */ + public static function create(body:Body):BodyListItem { + var item:BodyListItem; + if (poolTop == null) { + item = new BodyListItem(body); + } else { + item = poolTop; + poolTop = item.next; + item.next = null; + item.body = body; + } + return item; + } + + /** + * Очищает хранилище. + */ + public static function clearPool():void { + var item:BodyListItem = poolTop; + while (item != null) { + poolTop = item.next; + item.next = null; + item = poolTop; + } + } + + /** + * + */ + public var body:Body; + /** + * + */ + public var next:BodyListItem; + /** + * + */ + public var prev:BodyListItem; + + /** + * + * @param body + */ + public function BodyListItem(body:Body) { + this.body = body; + } + + /** + * + */ + public function dispose():void { + body = null; + prev = null; + next = poolTop; + poolTop = this; + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/BodyMaterial.as b/0.0.9.0/src/alternativa/physics/BodyMaterial.as new file mode 100644 index 0000000..7a7c47d --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/BodyMaterial.as @@ -0,0 +1,11 @@ +package alternativa.physics { + + public class BodyMaterial { + + public var restitution:Number = 0; + public var friction:Number = 0.3; +// public var dynamicFriction:Number = 0.2; +// public var dynamicFriction:Number = 0.2; + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/BodyState.as b/0.0.9.0/src/alternativa/physics/BodyState.as new file mode 100644 index 0000000..3987388 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/BodyState.as @@ -0,0 +1,39 @@ +package alternativa.physics { + import alternativa.math.Quaternion; + import alternativa.math.Vector3; + + /** + * Класс описывает состояние твёрдого тела. + */ + public class BodyState { + /** + * Положение тела. + */ + public var pos:Vector3 = new Vector3(); + /** + * Ориентация тела. + */ + public var orientation:Quaternion = new Quaternion(); + /** + * Скорость тела. + */ + public var velocity:Vector3 = new Vector3(); + /** + * Угловая скорость тела. + */ + public var rotation:Vector3 = new Vector3(); + + /** + * Копирует значение указанного объекта. + * + * @param state + */ + public function copy(state:BodyState):void { + pos.vCopy(state.pos); + orientation.copy(state.orientation); + velocity.vCopy(state.velocity); + rotation.vCopy(state.rotation); + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/CollisionPrimitiveList.as b/0.0.9.0/src/alternativa/physics/CollisionPrimitiveList.as new file mode 100644 index 0000000..5d5304f --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/CollisionPrimitiveList.as @@ -0,0 +1,89 @@ +package alternativa.physics { + import alternativa.physics.collision.CollisionPrimitive; + + /** + * + */ + public class CollisionPrimitiveList { + + public var head:CollisionPrimitiveListItem; + public var tail:CollisionPrimitiveListItem; + public var size:int; + + /** + * + */ + public function CollisionPrimitiveList() { + } + + /** + * + * @param primitive + */ + public function append(primitive:CollisionPrimitive):void { + var item:CollisionPrimitiveListItem = CollisionPrimitiveListItem.create(primitive); + if (head == null) { + head = tail = item; + } else { + tail.next = item; + item.prev = tail; + tail = item; + } + size++; + } + + /** + * + * @param primitve + */ + public function remove(primitve:CollisionPrimitive):void { + var item:CollisionPrimitiveListItem = findItem(primitve); + if (item == null) return; + if (item == head) { + if (size == 1) { + head = tail = null; + } else { + head = item.next; + head.prev = null; + } + } else { + if (item == tail) { + tail = tail.prev; + tail.next = null; + } else { + item.prev.next = item.next; + item.next.prev = item.prev; + } + } + item.dispose(); + size--; + } + + /** + * + * @param primitive + * @return + */ + public function findItem(primitive:CollisionPrimitive):CollisionPrimitiveListItem { + var item:CollisionPrimitiveListItem = head; + while (item != null && item.primitive != primitive) { + item = item.next; + } + return item; + } + + /** + * + */ + public function clear():void { + while (head != null) { + var item:CollisionPrimitiveListItem = head; + head = head.next; + item.dispose(); + } + tail = null; + size = 0; + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/CollisionPrimitiveListItem.as b/0.0.9.0/src/alternativa/physics/CollisionPrimitiveListItem.as new file mode 100644 index 0000000..d204532 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/CollisionPrimitiveListItem.as @@ -0,0 +1,76 @@ +package alternativa.physics { + import alternativa.physics.collision.CollisionPrimitive; + + /** + * Элемент списка примитивов. + */ + public class CollisionPrimitiveListItem { + + // Верхний элемент хранилища + private static var poolTop:CollisionPrimitiveListItem; + + /** + * Создаёт новый элемент списка. + * + * @param primitive примитив, содержащийся в элементе + * @return новый элемент списка + */ + public static function create(primitive:CollisionPrimitive):CollisionPrimitiveListItem { + var item:CollisionPrimitiveListItem; + if (poolTop == null) { + item = new CollisionPrimitiveListItem(primitive); + } else { + item = poolTop; + item.primitive = primitive; + poolTop = item.next; + item.next = null; + } + return item; + } + + /** + * Очищает хранилище. + */ + public static function clearPool():void { + var curr:CollisionPrimitiveListItem = poolTop; + while (curr != null) { + poolTop = curr.next; + curr.next = null; + curr = poolTop; + } + } + + /** + * Примитив, хранящийся в элементе списка. + */ + public var primitive:CollisionPrimitive; + /** + * Ссылка не следующий элемент списка. + */ + public var next:CollisionPrimitiveListItem; + /** + * Ссылка не предыдущий элемент списка. + */ + public var prev:CollisionPrimitiveListItem; + + /** + * Создаёт новый экземпляр. + * + * @param primitive примитив, хранящийся в элементе списка + */ + public function CollisionPrimitiveListItem(primitive:CollisionPrimitive) { + this.primitive = primitive; + } + + /** + * Очищает внутренние ссылки и помещает элемент в хранилище для дальнейшего использования. + */ + public function dispose():void { + primitive = null; + prev = null; + next = poolTop; + poolTop = this; + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/Contact.as b/0.0.9.0/src/alternativa/physics/Contact.as new file mode 100644 index 0000000..3f010ec --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/Contact.as @@ -0,0 +1,69 @@ +package alternativa.physics { + + + import alternativa.math.Vector3; + + /** + * Описывает контакт одного или двух тел. + */ + public class Contact { + /** + * Первое тело контакта, не может быть равно null. + */ + public var body1:Body; + /** + * Второе тело контакта, может быть равно null. + */ + public var body2:Body; + /** + * Предрассчитанный взаимный коэффициент отскока. + */ + public var restitution:Number; + /** + * Предрассчитанный взаимный коэффициент трения. + */ + public var friction:Number; + /** + * Нормаль контакта. Направлена к первому телу. + */ + public var normal:Vector3 = new Vector3(); + /** + * Список точек контакта. + */ + public var points:Vector. = new Vector.(MAX_POINTS, true); + /** + * Количество точек контакта. + */ + public var pcount:int; + /** + * Максимальная глубина пересечения. + */ + public var maxPenetration:Number = 0; + /** + * Флаг показывает, разрешён контакт или нет. + */ + public var satisfied:Boolean; + /** + * Следующий контакт в списке. + */ + public var next:Contact; + /** + * Индекс контакта. Первый контакт в списке имеет индекс 0. + */ + public var index:int; + + // Максимальное количество точек контакта + private const MAX_POINTS:int = 8; + + /** + * + */ + public function Contact(index:int) { + this.index = index; + for (var i:int = 0; i < MAX_POINTS; i++) { + points[i] = new ContactPoint(); + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/ContactPoint.as b/0.0.9.0/src/alternativa/physics/ContactPoint.as new file mode 100644 index 0000000..395d2dc --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/ContactPoint.as @@ -0,0 +1,55 @@ +package alternativa.physics { + import alternativa.math.Vector3; + + /** + * + */ + public class ContactPoint { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + + // Величины, расчитываемые перед началом фазы решения контактов + + // Требуемая проекция конечной скорости на нормаль для упругого контакта + public var normalVel:Number; + // Минимальная скорость разделения неупругого контакта + public var minSepVel:Number; + // Изменение проекции скорости на единицу нормального импульса + public var velByUnitImpulseN:Number; + + public var angularInertia1:Number; + public var angularInertia2:Number; + + // Радиус-вектор точки контакта относительно центра первого тела + public var r1:Vector3 = new Vector3(); + // Радиус-вектор точки контакта относительно центра второго тела + public var r2:Vector3 = new Vector3(); + + // Величины, накапливаемые во время фазы решения контактов + + // Накопленный импульс, применяемый для получения требуемой относительной скорости в точке контакта + public var accumImpulseN:Number; + // Накопленный импульс, применяемый для разделения тел в точке контакта. Не создаёт момента. +// public var accumSepImpulse:Number; + + public var satisfied:Boolean; + + /** + * + * @param cp + */ + public function copyFrom(cp:ContactPoint):void { + pos.vCopy(cp.pos); + penetration = cp.penetration; + feature1 = cp.feature1; + feature2 = cp.feature2; + r1.vCopy(cp.r1); + r2.vCopy(cp.r2); + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/PhysicsScene.as b/0.0.9.0/src/alternativa/physics/PhysicsScene.as new file mode 100644 index 0000000..abdc762 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/PhysicsScene.as @@ -0,0 +1,743 @@ +package alternativa.physics { + + + import alternativa.physics.collision.ICollisionDetector; + import alternativa.physics.collision.KdTreeCollisionDetector; + import alternativa.physics.constraints.Constraint; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * Класс реализует физическую симуляцию поведения твёрдых тел. + */ + public class PhysicsScene { + + private static var lastBodyId:int; + + // Максимальное количество контактов + public const MAX_CONTACTS:int = 1000; + + ////////////////////////////////////////////////////////////////// + // Настроечные параметры симуляции. Могут быть изменены в любой + // момент времени без нарушения корректной работы симуляции. + ////////////////////////////////////////////////////////////////// + // Количество шагов, за которое пересекающиеся тела должны разделиться + public var penResolutionSteps:int = 10; + // Величина допустимой глубины пересечения + public var allowedPenetration:Number = 0.1; + // Максимальная скорость, добавляемая с целью разделения тел + public var maxPenResolutionSpeed:Number = 0.5; + // Количество итераций для обработки упругих контактов + public var collisionIterations:int = 5; + // Количество итераций для обработки неупругих контактов + public var contactIterations:int = 5; + // Флаг использования предсказания состояний + public var usePrediction:Boolean = false; + + public var freezeSteps:int = 10; + public var linSpeedFreezeLimit:Number = 1; + public var angSpeedFreezeLimit:Number = 0.01; + + // Переменные для процедуры разделения тел путём непосредственного их перемещенеия. + // !!!!!!!!!!!!!!!!!!!!!!!! + // !!! Экспериментально !!! + // !!!!!!!!!!!!!!!!!!!!!!!! + public var staticSeparationIterations:int = 10; + public var staticSeparationSteps:int = 10; + public var maxAngleMove:Number = 10; + public var useStaticSeparation:Boolean = false; + + // Вектор гравитации + public var _gravity:Vector3 = new Vector3(0, 0, -9.8); + // Модуль вектора гравитации + public var _gravityMagnitude:Number = 9.8; + + // Использующийся детектор столкновений + public var collisionDetector:ICollisionDetector; + + // Список тел, участвующих в симуляции + public var bodies:BodyList = new BodyList(); + + // Список контактов на текущем шаге симуляции + altphysics var contacts:Contact; + // Количество контактов на текущем шаге симуляции +// altphysics var contactsNum:int; + // Список ограничений + altphysics var constraints:Vector. = new Vector.(); + // Количество ограничений + altphysics var constraintsNum:int; + // Временная метка. Число прошедших шагов с начала симуляции. + public var timeStamp:int; + // Время с начала симуляции, мс + public var time:int; + // Первый неиспользованный контакт на текущем шаге симуляции + private var borderContact:Contact; + + // Временные переменные для избежания создания экземпляров + private var _r:Vector3 = new Vector3(); + private var _t:Vector3 = new Vector3(); + private var _v:Vector3 = new Vector3(); + private var _v1:Vector3 = new Vector3(); + private var _v2:Vector3 = new Vector3(); + + /** + * + */ + public function PhysicsScene() { + contacts = new Contact(0); + var contact:Contact = contacts; + for (var i:int = 1; i < MAX_CONTACTS; i++) { + contact.next = new Contact(i); + contact = contact.next; + } + collisionDetector = new KdTreeCollisionDetector(); + } + + /** + * Вектор гравитации. + */ + public function get gravity():Vector3 { + return _gravity.vClone(); + } + + /** + * @private + */ + public function set gravity(value:Vector3):void { + _gravity.vCopy(value); + _gravityMagnitude = _gravity.vLength(); + } + + /** + * Добавляет тело в симуляцию. + * + * @param body + */ + public function addBody(body:Body):void { + body.id = lastBodyId++; + body.world = this; + bodies.append(body); + } + + /** + * Удаляет тело из симуляции. + * @param body + * @return + */ + public function removeBody(body:Body):void { + if (bodies.remove(body)) { + body.world = null; + } + } + + /** + * Добавляет ограничение. + * @param c + */ + public function addConstraint(c:Constraint):void { + constraints[constraintsNum++] = c; + c.world = this; + } + + /** + * Удаляет ограничение. + * @param c + */ + public function removeConstraint(c:Constraint):Boolean { + var idx:int = constraints.indexOf(c); + if (idx < 0) return false; + constraints.splice(idx, 1) + constraintsNum--; + c.world = null; + return true; + } + + /** + * Применяет к телам действующте на них силы. Аккумуляторы сил и моментов тел очищаются после завершения шага + * симуляции, поэтому на момент вызова метода могут уже содержать некоторые значения. + * + * @param dt промежуток времени, в течении которого действуют силы + */ + private function applyForces(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + body.beforePhysicsStep(dt); + body.calcAccelerations(); + // Ускорение свободного падения применяется только к подвижным телам во избежание некорректного изменения + // фиктивной скорости неподвижных тел. + if (body.movable && !body.frozen) { + body.accel.x += _gravity.x; + body.accel.y += _gravity.y; + body.accel.z += _gravity.z; + } + item = item.next; + } + } + + /** + * Определяет все столкновения на текущем шаге симуляции и заполняет список получившихся контактов. + * + * @param dt длительность шага симуляции + */ + private function detectCollisions(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + if (!body.frozen) { + body.contactsNum = 0; + body.saveState(); + // При включённом режиме предсказания состояние тел интегрируется на один шаг вперёд + if (usePrediction) { + body.integrateVelocity(dt); + body.integratePosition(dt); + } + body.calcDerivedData(); + } + item = item.next; + } + + borderContact = collisionDetector.getAllContacts(contacts); + + // Расчёт относительных векторов точки контакта вынесен сюда из-за необходимости учитывать + // положение тел в предсказанном состоянии + var contact:Contact = contacts; + while (contact != borderContact) { + var b1:Body = contact.body1; + var b2:Body = contact.body2; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + var bPos:Vector3 = b1.state.pos; + cp.r1.x = cp.pos.x - bPos.x; + cp.r1.y = cp.pos.y - bPos.y; + cp.r1.z = cp.pos.z - bPos.z; + if (b2 != null) { + bPos = b2.state.pos; + cp.r2.x = cp.pos.x - bPos.x; + cp.r2.y = cp.pos.y - bPos.y; + cp.r2.z = cp.pos.z - bPos.z; + } + } + contact = contact.next; + } + + // Восстановление состояния тел + if (usePrediction) { + item = bodies.head; + while (item != null) { + body = item.body; + if (!body.frozen) { + body.restoreState(); + body.calcDerivedData(); + } + item = item.next; + } + } + } + + /** + * Подготваливает полученные из детектора столкновений контакты, расчитывая значения, не меняющиеся + * в ходе шага симуляции. + */ + private function preProcessContacts(dt:Number):void { + var contact:Contact = contacts; + while (contact != borderContact) { + var b1:Body = contact.body1; + var b2:Body = contact.body2; + // Столкнувшиеся тела размораживаются + if (b1.frozen) { + b1.frozen = false; + b1.freezeCounter = 0; + } + if (b2 != null && b2.frozen) { + b2.frozen = false; + b2.freezeCounter = 0; + } + contact.restitution = b1.material.restitution; + if (b2 != null && b2.material.restitution < contact.restitution) contact.restitution = b2.material.restitution; + contact.friction = b1.material.friction; + if (b2 != null && b2.material.friction < contact.friction) contact.friction = b2.material.friction; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.accumImpulseN = 0; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + cp.velByUnitImpulseN = 0; + if (b1.movable) { + cp.angularInertia1 = _v.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1).vDot(contact.normal); + cp.velByUnitImpulseN += b1.invMass + cp.angularInertia1; + } + if (b2 != null && b2.movable) { + cp.angularInertia2 = _v.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2).vDot(contact.normal); + cp.velByUnitImpulseN += b2.invMass + cp.angularInertia2; + } + // Расчёт требуемой конечной скорости для упругого контакта + calcSepVelocity(b1, b2, cp, _v); + cp.normalVel = _v.vDot(contact.normal); + if (cp.normalVel < 0) cp.normalVel = - contact.restitution*cp.normalVel; + // Скорость разделения неупругого контакта + cp.minSepVel = cp.penetration > allowedPenetration ? (cp.penetration - allowedPenetration)/(penResolutionSteps*dt) : 0; + if (cp.minSepVel > maxPenResolutionSpeed) cp.minSepVel = maxPenResolutionSpeed; + } + contact = contact.next; + } + for (var i:int = 0; i < constraintsNum; i++) { + var constraint:Constraint = constraints[i]; + constraint.preProcess(dt); + } + } + + /** + * + * @param dt + * @param forceInelastic + */ + private function processContacts(dt:Number, forceInelastic:Boolean):void { + var iterNum:int = forceInelastic ? contactIterations : collisionIterations; + var i:int; + var forwardLoop:Boolean = false; + for (var iter:int = 0; iter < iterNum; iter++) { + forwardLoop = !forwardLoop; + var contact:Contact = contacts; + while (contact != borderContact) { + resolveContact(contact, forceInelastic, forwardLoop); + contact = contact.next; + } + // Ограничения + for (i = 0; i < constraintsNum; i++) { + var constraint:Constraint = constraints[i]; + constraint.apply(dt); + } + } + + // Разделение контактов путём непосредственного изменения координат и ориентации. Экспериментально. +// if (forceInelastic && useStaticSeparation) performStaticSeparation(); + } + + /** + * + */ + private function resolveContact(contactInfo:Contact, forceInelastic:Boolean, forwardLoop:Boolean):void { + var b1:Body = contactInfo.body1; + var b2:Body = contactInfo.body2; + var normal:Vector3 = contactInfo.normal; + var i:int; + if (forwardLoop) { + for (i = 0; i < contactInfo.pcount; i++) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } else { + for (i = contactInfo.pcount - 1; i >= 0; i--) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } + } + + /** + * + * @param idx + * @param b1 + * @param b2 + * @param colInfo + * @param normal + * @param forceInelastic + */ + private function resolveContactPoint(idx:int, b1:Body, b2:Body, contact:Contact, normal:Vector3, forceInelastic:Boolean):void { + var cp:ContactPoint = contact.points[idx]; + if (!forceInelastic) { + cp.satisfied = true; + } + + var newVel:Number = 0; + calcSepVelocity(b1, b2, cp, _v); + var cnormal:Vector3 = contact.normal; + var sepVel:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + if (forceInelastic) { + var minSpeVel:Number = useStaticSeparation ? 0 : cp.minSepVel; + if (sepVel < minSpeVel) { + cp.satisfied = false; + } else if (cp.satisfied) return; + newVel = minSpeVel; + } else { + newVel = cp.normalVel; + } + var deltaVel:Number = newVel - sepVel; + var impulse:Number = deltaVel/cp.velByUnitImpulseN; + var accumImpulse:Number = cp.accumImpulseN + impulse; + if (accumImpulse < 0) { + accumImpulse = 0; + } + var deltaImpulse:Number = accumImpulse - cp.accumImpulseN; + cp.accumImpulseN = accumImpulse; + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, normal, deltaImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, normal, -deltaImpulse); + } + + // Учёт силы трения + calcSepVelocity(b1, b2, cp, _v); + // Расчитываем изменение касательной скорости на единицу касательного импульса + var tanSpeedByUnitImpulse:Number = 0; + +// _v.vAddScaled(-_v.vDot(contact.normal), contact.normal); + var dot:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + _v.x -= dot*cnormal.x; + _v.y -= dot*cnormal.y; + _v.z -= dot*cnormal.z; + + var tanSpeed:Number = _v.vLength(); + if (tanSpeed < 0.001) return; + +// _t.vCopy(_v).vNormalize().vReverse(); + _t.x = -_v.x; + _t.y = -_v.y; + _t.z = -_v.z; + _t.vNormalize(); + + var r:Vector3; + var m:Matrix3; + var xx:Number; + var yy:Number; + var zz:Number; + // dV = b.invMass + ((invI * (r % t)) % r) * t + if (b1.movable) { +// _v.vCross2(cp.r1, _t).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1); + r = cp.r1; + m = b1.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b1.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + if (b2 != null && b2.movable) { +// _v.vCross2(cp.r2, _t).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2); + + r = cp.r2; + m = b2.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b2.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + + var tanImpulse:Number = tanSpeed/tanSpeedByUnitImpulse; + var max:Number = contact.friction*cp.accumImpulseN; + if (max < 0) { + if (tanImpulse < max) tanImpulse = max; + } else { + if (tanImpulse > max) tanImpulse = max; + } + + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, _t, tanImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, _t, -tanImpulse); + } + } + + /** + * + * @param cp + * @param normal + * @return + */ + private function calcSepVelocity(body1:Body, body2:Body, cp:ContactPoint, result:Vector3):void { + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 +// result.vCopy(body1.state.velocity).vAdd(_v1.vCross2(body1.state.rotation, cp.r1)); + var rot:Vector3 = body1.state.rotation; + var v:Vector3 = cp.r1; + var x:Number = rot.y*v.z - rot.z*v.y; + var y:Number = rot.z*v.x - rot.x*v.z; + var z:Number = rot.x*v.y - rot.y*v.x; + v = body1.state.velocity; + result.x = v.x + x; + result.y = v.y + y; + result.z = v.z + z; + // V2 = V2_c + w2%r2 + if (body2 != null) { +// result.vSubtract(body2.state.velocity).vSubtract(_v2.vCross2(body2.state.rotation, cp.r2)); + rot = body2.state.rotation; + v = cp.r2; + x = rot.y*v.z - rot.z*v.y; + y = rot.z*v.x - rot.x*v.z; + z = rot.x*v.y - rot.y*v.x; + v = body2.state.velocity; + result.x -= v.x + x; + result.y -= v.y + y; + result.z -= v.z + z; + } + } + + /** + * + * @param dt + */ + private function intergateVelocities(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + item.body.integrateVelocity(dt); + item = item.next; + } + } + + /** + * + * @param dt + */ + private function integratePositions(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + if (body.movable && !body.frozen) { + body.integratePosition(dt); + } + item = item.next; + } + } + + /** + * + */ + private function performStaticSeparation():void { + var iterNum:int = staticSeparationIterations; +// iterNum = 100; + + // 1. В начале каждой итерации для всех контактов сбрасывается флаг satisfied с одновременным поиском наихудшего контакта, т.е. имеющего наибольшую величину пересечения тел. + // 2. Если найденный контакт имеет величину пересечения меньше предельно допустимой процедура прерывается, т.к. разделение контактов не требуется. + // 3. Если процедура продолжается, то выполняется разделение найденного контакта и установка его флага satisfied в true, после чего запускается внутренний цикл, состоящий + // из contactsNum - 1 итераций. На каждой итерации ищется наихудший контакт среди оставшихся (satisfied == false). Если такой контакт найден, то выполняется его разделение + // и переход к следующей итерации внутреннего цикла, иначе внутренний цикл прерывается и выполняется переход к следующей итерации внешнего цикла (шаг 1.). +// for (var iter:int = 0; iter < iterNum; iter++) { +// // Ищем контакт с максимальной величиной пересечения тел, одновременно сбрасывая флаги satisfied в false +// var worstContact:Contact = contacts[0]; +// var i:int; +// for (i = 1; i < contactsNum; i++) { +// var contact:Contact = contacts[i]; +// contact.satisfied = false; +// if (contact.maxPenetration > worstContact.maxPenetration) worstContact = contact; +// } +// if (worstContact.maxPenetration <= allowedPenetration) return; +// +// resolveInterpenetration(worstContact); +// // Внутренний цикл по оставшимся контактам +// for (i = 1; i < contactsNum; i++) { +// worstContact = getWorstContact(); +// if (worstContact == null) break; +// resolveInterpenetration(worstContact); +// } +// } + } + + /** + * + */ +// private function getWorstContact():Contact { +// var maxPen:Number = 0; +// var worst:Contact = null; +// for (var i:int = 0; i < contactsNum; i++) { +// var c:Contact = contacts[i]; +// if (!c.satisfied && c.maxPenetration > maxPen) { +// worst = c; +// maxPen = c.maxPenetration; +// } +// } +// return maxPen > allowedPenetration ? worst : null; +// } + + /** + * Разделяет указанный контакт, выполняя staticSeparationSteps итераций по списку точек контакта. + * 1. В начале каждой итерации ищется точка с наибольшим пересечением, одновременно сбрасываются флаги satisfied у точек. + * 2. Для найденной точки выполняется процедура разделения. + * 3. После выполняется pcount - 1 итерация по списку точек, каждый раз ищется наихудшая среди имеющих satisfied == false, для которой выполняется процедура разделения. + * Если наихудшая точка не найдена, то выполнется переход к следующей итерации внешнего цикла (шаг 1). + * + * В конце процедуры разделения обновляются значения пересечений для остальных точек контакта, а также для всех прочих контактов, относящихся к телам текущего. + * + * @param contact контакт для разделения + */ + private function resolveInterpenetration(contact:Contact):void { + contact.satisfied = true; + + for (var step:int = 0; step < staticSeparationSteps; step++) { + var worstCp:ContactPoint = contact.points[0]; + var cp:ContactPoint; + var i:int; + for (i = 1; i < contact.pcount; i++) { + cp = contact.points[i]; + cp.satisfied = false; + if (cp.penetration > worstCp.penetration) worstCp = cp; + } + if (worstCp.penetration <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + // Разделяем оставшиеся точки + var maxPen:Number = 0; + for (i = 1; i < contact.pcount; i++) { + // Поиск наихудшей точки + for (var j:int = 0; j < contact.pcount; j++) { + cp = contact.points[j]; + if (cp.satisfied) continue; + if (cp.penetration > maxPen) { + maxPen = cp.penetration; + worstCp = cp; + } + } + if (maxPen <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + } + } + } + + /** + * + * @param cp + * @param contact + */ + private function separateContactPoint(cp:ContactPoint, contact:Contact):void { +// cp.satisfied = true; +// +// var b1:Body = contact.body1; +// var b2:Body = contact.body2; +// var totalMove:Number = cp.penetration - allowedPenetration; +// var moveCoeff:Number = totalMove/cp.velByUnitImpulseN; +// var linMove1:Number; +// var angleMove1:Number; +// if (b1.movable) { +// linMove1 = b1.invMass*moveCoeff; +// angleMove1 = cp.angularInertia1*moveCoeff; +// if (angleMove1 > maxAngleMove) { +// linMove1 += angleMove1 - maxAngleMove; +// angleMove1 -= maxAngleMove; +// } +// b1.state.pos.vAddScaled(linMove1, contact.normal); +// _v1.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vScale(angleMove1); +// b1.state.orientation.addScaledVector(_v1, 1); +// } +// var linMove2:Number; +// var angleMove2:Number; +// if (b2 != null && b2.movable) { +// linMove2 = b2.invMass*moveCoeff; +// angleMove2 = cp.angularInertia2*moveCoeff; +// if (angleMove2 > maxAngleMove) { +// linMove2 += angleMove2 - maxAngleMove; +// angleMove2 -= maxAngleMove; +// } +// b2.state.pos.vAddScaled(-linMove2, contact.normal); +// _v2.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vScale(angleMove2); +// _v2.vReverse(); +// b2.state.orientation.addScaledVector(_v2, 1); +// } +// cp.penetration = allowedPenetration; +// // Обновляем пересечения в других точках +// var i:int; +// for (i = 0; i < contact.pcount; i++) { +// var cp1:ContactPoint = contact.points[i]; +// if (cp1 == cp) continue; +// var angularMove:Number; +// if (b1.movable) { +// angularMove = _v.vCross2(_v1, cp1.r1).vDot(contact.normal); +// cp1.penetration -= linMove1 + angularMove; +// } +// if (b2 != null && b2.movable) { +// angularMove = _v.vCross2(_v2, cp1.r2).vDot(contact.normal); +// cp1.penetration -= linMove2 - angularMove; +// } +// // Обновление максимального значения для контакта +// if (cp1.penetration > contact.maxPenetration) contact.maxPenetration = cp1.penetration; +// } +// // Обновляем пересечения для других контактов +// var c:Contact; +// var j:int; +// if (b1.movable) { +// for (i = 0; i < b1.contactsNum; i++) { +// c = b1.contacts[i]; +// if (c == contact) continue; +// for (j = 0; j < c.pcount; j++) { +// cp1 = c.points[j]; +// if (b1 == c.body1) cp1.penetration -= linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r1).vDot(c.normal); +// else cp1.penetration += linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r2).vDot(c.normal); +// if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; +// } +// } +// } +// if (b2 != null && b2.movable) { +// for (i = 0; i < b2.contactsNum; i++) { +// c = b2.contacts[i]; +// if (c == contact) continue; +// for (j = 0; j < c.pcount; j++) { +// cp1 = c.points[j]; +// if (b2 == c.body1) cp1.penetration -= linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r1).vDot(c.normal); +// else cp1.penetration += linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r2).vDot(c.normal); +// if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; +// } +// } +// } + } + + /** + * + */ + private function postPhysics():void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + body.clearAccumulators(); + body.calcDerivedData(); + if (body.canFreeze) { + if (body.state.velocity.vLength() < linSpeedFreezeLimit && body.state.rotation.vLength() < angSpeedFreezeLimit) { + if (!body.frozen) { + body.freezeCounter++; + if (body.freezeCounter >= freezeSteps) body.frozen = true; + } + } else { + body.freezeCounter = 0; + body.frozen = false; + } + } + item = item.next; + } + } + + /** + * + * @param delta + */ + public function update(delta:int):void { + timeStamp++; + time += delta; + var dt:Number = 0.001*delta; + applyForces(dt); + detectCollisions(dt); + preProcessContacts(dt); + processContacts(dt, false); + intergateVelocities(dt); + processContacts(dt, true); + integratePositions(dt); + postPhysics(); + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/PhysicsUtils.as b/0.0.9.0/src/alternativa/physics/PhysicsUtils.as new file mode 100644 index 0000000..5a2e46f --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/PhysicsUtils.as @@ -0,0 +1,49 @@ +package alternativa.physics { + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + public class PhysicsUtils { + + /** + * @param mass + * @param halfSize + * @param result + */ + public static function getBoxInvInertia(mass:Number, halfSize:Vector3, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + result.a = 3/(mass*(yy + zz)); + result.f = 3/(mass*(zz + xx)); + result.k = 3/(mass*(xx + yy)); + } + + /** + * @param mass + * @param r + * @param h + * @param result + */ + public static function getCylinderInvInertia(mass:Number, r:Number, h:Number, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + + result.a = result.f = 1/(mass*(h*h/12 + r*r/4)); + result.k = 2/(mass*r*r); + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/altphysics.as b/0.0.9.0/src/alternativa/physics/altphysics.as new file mode 100644 index 0000000..2715307 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/altphysics.as @@ -0,0 +1,3 @@ +package alternativa.physics { + public namespace altphysics = "http://alternativaplatform.com/en/altphysics"; +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/.svn/all-wcprops b/0.0.9.0/src/alternativa/physics/collision/.svn/all-wcprops new file mode 100644 index 0000000..aefed7f --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/.svn/all-wcprops @@ -0,0 +1,65 @@ +K 25 +svn:wc:ra_dav:version-url +V 113 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/collision +END +ICollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 135 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/collision/ICollisionDetector.as +END +CollisionKdNode.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/collision/CollisionKdNode.as +END +CollisionKdTree2D.as +K 25 +svn:wc:ra_dav:version-url +V 134 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/collision/CollisionKdTree2D.as +END +IRayCollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 139 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/collision/IRayCollisionPredicate.as +END +IBodyCollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/collision/IBodyCollisionPredicate.as +END +CollisionPrimitive.as +K 25 +svn:wc:ra_dav:version-url +V 135 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/collision/CollisionPrimitive.as +END +KdTreeCollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/collision/KdTreeCollisionDetector.as +END +ICollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 136 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/collision/ICollisionPredicate.as +END +ICollider.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/collision/ICollider.as +END +CollisionKdTree.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/collision/CollisionKdTree.as +END diff --git a/0.0.9.0/src/alternativa/physics/collision/.svn/entries b/0.0.9.0/src/alternativa/physics/collision/.svn/entries new file mode 100644 index 0000000..5b2e320 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/.svn/entries @@ -0,0 +1,157 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/collision +http://svndev.alternativaplatform.com + + + +2010-02-12T12:34:25.192477Z +28424 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +colliders +dir + +ICollisionDetector.as +file + + + + +2010-10-28T04:32:43.000000Z +9f76503bdaf92b4170b05b6e66c45de1 +2010-02-12T12:34:25.192477Z +28424 +mike + +CollisionKdNode.as +file + + + + +2010-10-28T04:32:43.000000Z +a6fab491cd9bd45a5472bf4ffc321e08 +2010-02-12T12:34:25.192477Z +28424 +mike + +CollisionKdTree2D.as +file + + + + +2010-10-28T04:32:43.000000Z +82bb636d8b49cdfbc22694dd8b01e71d +2010-02-12T12:34:25.192477Z +28424 +mike + +IRayCollisionPredicate.as +file + + + + +2010-10-28T04:32:43.000000Z +32f9b1eaeacfcb40e7ae1927f52b8ab8 +2009-10-19T07:17:33.113306Z +22253 +mike + +types +dir + +IBodyCollisionPredicate.as +file + + + + +2010-10-28T04:32:43.000000Z +618f907a588979a8d06d4f4bcf6618a0 +2009-10-19T07:17:33.113306Z +22253 +mike + +CollisionPrimitive.as +file + + + + +2010-10-28T04:32:43.000000Z +8501ce34a3effa2380c472cc9b823afa +2009-11-24T08:44:15.941480Z +23758 +mike + +KdTreeCollisionDetector.as +file + + + + +2010-10-28T04:32:43.000000Z +ea670ad5d0f72f6a6df5163d65f204f9 +2010-02-12T12:34:25.192477Z +28424 +mike + +ICollisionPredicate.as +file + + + + +2010-10-28T04:32:43.000000Z +65f3250d36556fffc457c740fb7d38b3 +2009-10-19T07:17:33.113306Z +22253 +mike + +ICollider.as +file + + + + +2010-10-28T04:32:43.000000Z +128a7d7479e13caeccecb45161b27b22 +2009-10-19T07:17:33.113306Z +22253 +mike + +primitives +dir + +CollisionKdTree.as +file + + + + +2010-10-28T04:32:43.000000Z +8e815c1f1e527b0884b3f0124257a603 +2010-02-12T12:34:25.192477Z +28424 +mike + diff --git a/0.0.9.0/src/alternativa/physics/collision/.svn/format b/0.0.9.0/src/alternativa/physics/collision/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base b/0.0.9.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base new file mode 100644 index 0000000..f450f1e --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base @@ -0,0 +1,18 @@ +package alternativa.physics.collision { + + + import alternativa.physics.collision.types.BoundBox; + + public class CollisionKdNode { + public var indices:Vector.; + public var splitIndices:Vector.; + public var boundBox:BoundBox; + public var parent:CollisionKdNode; + public var splitTree:CollisionKdTree2D; + + public var axis:int = -1; // 0 - x, 1 - y, 2 - z + public var coord:Number; + public var positiveNode:CollisionKdNode; + public var negativeNode:CollisionKdNode; + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base b/0.0.9.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base new file mode 100644 index 0000000..d3dc307 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base @@ -0,0 +1,261 @@ +package alternativa.physics.collision { + + + + import alternativa.physics.collision.types.BoundBox; + + /** + * @author mike + */ + public class CollisionKdTree { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + public var rootNode:CollisionKdNode; + public var staticChildren:Vector.; + public var numStaticChildren:int; + public var staticBoundBoxes:Vector. = new Vector.(); + + private var splitAxis:int; + private var splitCoord:Number; + private var splitCost:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + + /** + * @param boundBox + */ + public function createTree(collisionPrimitives:Vector., boundBox:BoundBox = null):void { + staticChildren = collisionPrimitives.concat(); + numStaticChildren = staticChildren.length; + // Создаём корневую ноду + rootNode = new CollisionKdNode(); + rootNode.indices = new Vector.(); + // Расчитываем баунды объектов и рутовой ноды + var rootNodeBoundBox:BoundBox = rootNode.boundBox = (boundBox != null) ? boundBox : new BoundBox(); + for (var i:int = 0; i < numStaticChildren; ++i) { + var child:CollisionPrimitive = staticChildren[i]; + var childBoundBox:BoundBox = staticBoundBoxes[i] = child.calculateAABB(); + rootNodeBoundBox.addBoundBox(childBoundBox); + rootNode.indices[i] = i; + } + staticBoundBoxes.length = numStaticChildren; + // Разделяем рутовую ноду + splitNode(rootNode); + + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + var indices:Vector. = node.indices; + var numPrimitives:int = indices.length; + if (numPrimitives <= minPrimitivesPerNode) return; + + // Подготовка баунда с погрешностями + var nodeBoundBox:BoundBox = node.boundBox; + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + // Собираем опорные координаты + var i:int; + var j:int; + var numSplitCoordsX:int = 0 + var numSplitCoordsY:int = 0; + var numSplitCoordsZ:int = 0; + for (i = 0; i < numPrimitives; ++i) { + var boundBox:BoundBox = staticBoundBoxes[indices[i]]; + + if (boundBox.maxX - boundBox.minX <= doubleThreshold) { + if (boundBox.minX <= nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.minX; + else if (boundBox.maxX >= nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.maxX; + else splitCoordsX[numSplitCoordsX++] = (boundBox.minX + boundBox.maxX)*0.5; + } else { + if (boundBox.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = boundBox.minX; + if (boundBox.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = boundBox.maxX; + } + + if (boundBox.maxY - boundBox.minY <= doubleThreshold) { + if (boundBox.minY <= nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.minY; + else if (boundBox.maxY >= nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.maxY; + else splitCoordsY[numSplitCoordsY++] = (boundBox.minY + boundBox.maxY)*0.5; + } else { + if (boundBox.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = boundBox.minY; + if (boundBox.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = boundBox.maxY; + } + + if (boundBox.maxZ - boundBox.minZ <= doubleThreshold) { + if (boundBox.minZ <= nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.minZ; + else if (boundBox.maxZ >= nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.maxZ; + else splitCoordsZ[numSplitCoordsZ++] = (boundBox.minZ + boundBox.maxZ)*0.5; + } else { + if (boundBox.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.minZ; + if (boundBox.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numPrimitives; ++i) { + boundBox = staticBoundBoxes[indices[i]]; + var min:Number = axisX ? boundBox.minX : (axisY ? boundBox.minY : boundBox.minZ); + var max:Number = axisX ? boundBox.maxX : (axisY ? boundBox.maxY : boundBox.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(indices[i]); + indices[i] = -1; + } else { + if (node.splitIndices == null) node.splitIndices = new Vector.(); + node.splitIndices.push(indices[i]); + indices[i] = -1; + } + } else { + if (min >= coordMin) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(indices[i]); + indices[i] = -1; + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numPrimitives; ++i) { + if (indices[i] >= 0) indices[j++] = indices[i]; + } + if (j > 0) indices.length = j; + else node.indices = null; + + if (node.splitIndices != null) { + node.splitTree = new CollisionKdTree2D(this, node); + node.splitTree.createTree(); + } + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + /** + * + */ + public function traceTree():void { + traceNode("", rootNode); + } + + /** + * @param str + * @param node + */ + private function traceNode(str:String, node:CollisionKdNode):void { + if (node == null) return; + trace(str, node.axis == -1 ? "end" : ((node.axis == 0) ? "X" : ((node.axis == 1) ? "Y" : "Z")), "splitCoord=" + splitCoord, "bound", node.boundBox, "objs:", node.indices); + traceNode(str + "-", node.negativeNode); + traceNode(str + "+", node.positiveNode); + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base b/0.0.9.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base new file mode 100644 index 0000000..2621517 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base @@ -0,0 +1,226 @@ +package alternativa.physics.collision { + + + import alternativa.physics.collision.types.BoundBox; + + /** + * + */ + public class CollisionKdTree2D { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + + public var parentTree:CollisionKdTree; + public var parentNode:CollisionKdNode; + public var rootNode:CollisionKdNode; + + private var splitAxis:int; + private var splitCost:Number; + private var splitCoord:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + /** + * + * @param parentTree + * @param parentNode + */ + public function CollisionKdTree2D(parentTree:CollisionKdTree, parentNode:CollisionKdNode) { + this.parentTree = parentTree; + this.parentNode = parentNode; + } + + /** + * + */ + public function createTree():void { + rootNode = new CollisionKdNode(); + rootNode.boundBox = parentNode.boundBox.clone(); + rootNode.indices = new Vector.(); + var numObjects:int = parentNode.splitIndices.length; + for (var i:int = 0; i < numObjects; ++i) rootNode.indices[i] = parentNode.splitIndices[i]; + + splitNode(rootNode); + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + if (node.indices.length <= minPrimitivesPerNode) return; + + var objects:Vector. = node.indices; + var i:int; + var j:int; + + var nodeBoundBox:BoundBox = node.boundBox; + + // Подготовка баунда с погрешностями + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + // Собираем опорные координаты + var numSplitCoordsX:int; + var numSplitCoordsY:int; + var numSplitCoordsZ:int; + var numObjects:int = objects.length; + for (i = 0; i < numObjects; ++i) { + var bb:BoundBox = staticBoundBoxes[objects[i]]; + + if (parentNode.axis != 0) { + if (bb.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = bb.minX; + if (bb.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = bb.maxX; + } + + if (parentNode.axis != 1) { + if (bb.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = bb.minY; + if (bb.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = bb.maxY; + } + + if (parentNode.axis != 2) { + if (bb.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = bb.minZ; + if (bb.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = bb.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + if (parentNode.axis != 0) checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + if (parentNode.axis != 1) checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + if (parentNode.axis != 2) checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numObjects; ++i) { + bb = staticBoundBoxes[objects[i]]; + var min:Number = axisX ? bb.minX : (axisY ? bb.minY : bb.minZ); + var max:Number = axisX ? bb.maxX : (axisY ? bb.maxY : bb.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(objects[i]); + objects[i] = -1; + } + } else { + if (min >= coordMin) { + if (max > coordMax) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(objects[i]); + objects[i] = -1; + } + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numObjects; ++i) { + if (objects[i] >= 0) objects[j++] = objects[i]; + } + if (j > 0) objects.length = j; + else node.indices = null; + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/.svn/text-base/CollisionPrimitive.as.svn-base b/0.0.9.0/src/alternativa/physics/collision/.svn/text-base/CollisionPrimitive.as.svn-base new file mode 100644 index 0000000..e03cfee --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/.svn/text-base/CollisionPrimitive.as.svn-base @@ -0,0 +1,140 @@ +package alternativa.physics.collision { + + import alternativa.physics.Body; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * Базовый класс для примитивов, использующихся детектором столкновений. + */ + public class CollisionPrimitive { + // Константы типов примитива + public static const BOX:int = 1; + public static const PLANE:int = 2; + public static const SPHERE:int = 3; + public static const RECT:int = 4; + public static const TRIANGLE:int = 5; + + // Тип примитива + public var type:int; + // Группы примитива. Каждая группа определяется установленным битом. Столкновения проверяются только для примитивов, + // имеющих хотя бы одну общую группу. + public var collisionGroup:int; + // Предикат примитива, вызывающийся детектором при нахождении столкновения. В зависимости от возвращённого + // предикатом результата столкновение либо регистрируется, либо игнорируется. + // Nullable + public var postCollisionPredicate:ICollisionPredicate; + // Тело, владеющее примитивом. Поле сделано открытым для быстрого доступа на чтение. Установка значения должна выполняться вызовом метода setBody(). + // Nullable + public var body:Body; + // Трансформация примитива в системе координат тела, если оно указано. Не допускается масштабирование матрицы. + public var localTransform:Matrix4; + // Полная трансформация примитива. Не допускается масштабирование матрицы. + public var transform:Matrix4 = new Matrix4(); + // AABB в мировой системе координат. Расчитывается системой вызовом функции calculateBoundBox(). + public var aabb:BoundBox = new BoundBox(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param type тип примитива + * @param collisionGroup группа примитива + */ + public function CollisionPrimitive(type:int, collisionGroup:int) { + this.type = type; + this.collisionGroup = collisionGroup; + } + + /** + * Устанавливает тело, владеющее примитивом. + * + * @param body тело, которое владеет примитивом + * @param localTransform трансформация примитива в системе координат тела. Указание значения null равносильно + * заданию единичной матрицы, однако в первом случае не будет дополнительного умножения матриц при вычислении полной трансформации примитива. + */ + public function setBody(body:Body, localTransform:Matrix4 = null):void { + if (this.body == body) return; + this.body = body; + if (body != null) { + if (localTransform != null) { + if (this.localTransform == null) { + this.localTransform = new Matrix4(); + } + this.localTransform.copy(localTransform); + } else { + this.localTransform = null; + } + } + } + + /** + * Рассчитывает AABB примитива. Наследники должны переопределять этот метод, реализуя в нём корректный рассчёт. + * + * @return ссылка на свой AABB + */ + public function calculateAABB():BoundBox { + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + return -1; + } + + /** + * Клонирует примитив. Переопределять не рекомендуется. Вместо этого переопределяются методы createPrimitive() и copyFrom(). + * + * @return клон примитива + */ + public function clone():CollisionPrimitive { + var p:CollisionPrimitive = createPrimitive(); + return p.copyFrom(this); + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + if (source == null) { + throw new ArgumentError("Parameter source cannot be null"); + } + type = source.type; + transform.copy(source.transform); + collisionGroup = source.collisionGroup; + setBody(source.body, source.localTransform); + aabb.copyFrom(source.aabb); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + public function toString():String { + return "[CollisionPrimitive type=" + type + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + protected function createPrimitive():CollisionPrimitive { + return new CollisionPrimitive(type, collisionGroup); + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base b/0.0.9.0/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base new file mode 100644 index 0000000..4ace681 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.Body; + + public interface IBodyCollisionPredicate { + function considerBodies(body1:Body, body2:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base b/0.0.9.0/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base new file mode 100644 index 0000000..e6aac2a --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base @@ -0,0 +1,29 @@ +package alternativa.physics.collision { + + import alternativa.physics.Contact; + + /** + * Интерфейс определителя столкновений между двумя примитивами. + */ + public interface ICollider { + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base b/0.0.9.0/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base new file mode 100644 index 0000000..916f67b --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base @@ -0,0 +1,64 @@ +package alternativa.physics.collision { + import alternativa.physics.Contact; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.math.Vector3; + + /** + * Интерфейс детектора столкновений. + */ + public interface ICollisionDetector { + + /** + * Получает все столкновения в текущей конфигурации физической геометрии. + * + * @param contacts список контактов, в кторые будет записана информация о столкновении + * @return количество найденных столкновений + */ + function getAllContacts(contacts:Contact):Contact; + + /** + * Тестирует луч на пересечение с физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param direction направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRay(origin:Vector3, direction:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * + * @param origin + * @param direction + * @param collisionGroup + * @param maxTime + * @param predicate + * @param result + * @return + */ + function intersectRayWithStatic(origin:Vector3, direction:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @return + */ + function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base b/0.0.9.0/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base new file mode 100644 index 0000000..33d30ca --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base @@ -0,0 +1,8 @@ +package alternativa.physics.collision { + + public interface ICollisionPredicate { + + function considerCollision(primitive:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base b/0.0.9.0/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base new file mode 100644 index 0000000..b319f62 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.Body; + + public interface IRayCollisionPredicate { + function considerBody(body:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base b/0.0.9.0/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base new file mode 100644 index 0000000..9ca4c2a --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base @@ -0,0 +1,486 @@ +package alternativa.physics.collision { + + + 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.math.Vector3; + + use namespace altphysics; + + /** + * Детектор, хранящий статическую геометрию в kD-дереве и использующий дерево для ускорения тестов на пересечения. + */ + public class KdTreeCollisionDetector implements ICollisionDetector { + + altphysics var tree:CollisionKdTree; + altphysics var dynamicPrimitives:Vector.; + 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.(); + + 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.):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. = tree.staticChildren; +// var indices:Vector. = 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., indices:Vector., 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; + } \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/CollisionKdNode.as b/0.0.9.0/src/alternativa/physics/collision/CollisionKdNode.as new file mode 100644 index 0000000..f450f1e --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/CollisionKdNode.as @@ -0,0 +1,18 @@ +package alternativa.physics.collision { + + + import alternativa.physics.collision.types.BoundBox; + + public class CollisionKdNode { + public var indices:Vector.; + public var splitIndices:Vector.; + public var boundBox:BoundBox; + public var parent:CollisionKdNode; + public var splitTree:CollisionKdTree2D; + + public var axis:int = -1; // 0 - x, 1 - y, 2 - z + public var coord:Number; + public var positiveNode:CollisionKdNode; + public var negativeNode:CollisionKdNode; + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/CollisionKdTree.as b/0.0.9.0/src/alternativa/physics/collision/CollisionKdTree.as new file mode 100644 index 0000000..d3dc307 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/CollisionKdTree.as @@ -0,0 +1,261 @@ +package alternativa.physics.collision { + + + + import alternativa.physics.collision.types.BoundBox; + + /** + * @author mike + */ + public class CollisionKdTree { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + public var rootNode:CollisionKdNode; + public var staticChildren:Vector.; + public var numStaticChildren:int; + public var staticBoundBoxes:Vector. = new Vector.(); + + private var splitAxis:int; + private var splitCoord:Number; + private var splitCost:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + + /** + * @param boundBox + */ + public function createTree(collisionPrimitives:Vector., boundBox:BoundBox = null):void { + staticChildren = collisionPrimitives.concat(); + numStaticChildren = staticChildren.length; + // Создаём корневую ноду + rootNode = new CollisionKdNode(); + rootNode.indices = new Vector.(); + // Расчитываем баунды объектов и рутовой ноды + var rootNodeBoundBox:BoundBox = rootNode.boundBox = (boundBox != null) ? boundBox : new BoundBox(); + for (var i:int = 0; i < numStaticChildren; ++i) { + var child:CollisionPrimitive = staticChildren[i]; + var childBoundBox:BoundBox = staticBoundBoxes[i] = child.calculateAABB(); + rootNodeBoundBox.addBoundBox(childBoundBox); + rootNode.indices[i] = i; + } + staticBoundBoxes.length = numStaticChildren; + // Разделяем рутовую ноду + splitNode(rootNode); + + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + var indices:Vector. = node.indices; + var numPrimitives:int = indices.length; + if (numPrimitives <= minPrimitivesPerNode) return; + + // Подготовка баунда с погрешностями + var nodeBoundBox:BoundBox = node.boundBox; + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + // Собираем опорные координаты + var i:int; + var j:int; + var numSplitCoordsX:int = 0 + var numSplitCoordsY:int = 0; + var numSplitCoordsZ:int = 0; + for (i = 0; i < numPrimitives; ++i) { + var boundBox:BoundBox = staticBoundBoxes[indices[i]]; + + if (boundBox.maxX - boundBox.minX <= doubleThreshold) { + if (boundBox.minX <= nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.minX; + else if (boundBox.maxX >= nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.maxX; + else splitCoordsX[numSplitCoordsX++] = (boundBox.minX + boundBox.maxX)*0.5; + } else { + if (boundBox.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = boundBox.minX; + if (boundBox.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = boundBox.maxX; + } + + if (boundBox.maxY - boundBox.minY <= doubleThreshold) { + if (boundBox.minY <= nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.minY; + else if (boundBox.maxY >= nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.maxY; + else splitCoordsY[numSplitCoordsY++] = (boundBox.minY + boundBox.maxY)*0.5; + } else { + if (boundBox.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = boundBox.minY; + if (boundBox.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = boundBox.maxY; + } + + if (boundBox.maxZ - boundBox.minZ <= doubleThreshold) { + if (boundBox.minZ <= nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.minZ; + else if (boundBox.maxZ >= nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.maxZ; + else splitCoordsZ[numSplitCoordsZ++] = (boundBox.minZ + boundBox.maxZ)*0.5; + } else { + if (boundBox.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.minZ; + if (boundBox.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numPrimitives; ++i) { + boundBox = staticBoundBoxes[indices[i]]; + var min:Number = axisX ? boundBox.minX : (axisY ? boundBox.minY : boundBox.minZ); + var max:Number = axisX ? boundBox.maxX : (axisY ? boundBox.maxY : boundBox.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(indices[i]); + indices[i] = -1; + } else { + if (node.splitIndices == null) node.splitIndices = new Vector.(); + node.splitIndices.push(indices[i]); + indices[i] = -1; + } + } else { + if (min >= coordMin) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(indices[i]); + indices[i] = -1; + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numPrimitives; ++i) { + if (indices[i] >= 0) indices[j++] = indices[i]; + } + if (j > 0) indices.length = j; + else node.indices = null; + + if (node.splitIndices != null) { + node.splitTree = new CollisionKdTree2D(this, node); + node.splitTree.createTree(); + } + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + /** + * + */ + public function traceTree():void { + traceNode("", rootNode); + } + + /** + * @param str + * @param node + */ + private function traceNode(str:String, node:CollisionKdNode):void { + if (node == null) return; + trace(str, node.axis == -1 ? "end" : ((node.axis == 0) ? "X" : ((node.axis == 1) ? "Y" : "Z")), "splitCoord=" + splitCoord, "bound", node.boundBox, "objs:", node.indices); + traceNode(str + "-", node.negativeNode); + traceNode(str + "+", node.positiveNode); + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/CollisionKdTree2D.as b/0.0.9.0/src/alternativa/physics/collision/CollisionKdTree2D.as new file mode 100644 index 0000000..2621517 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/CollisionKdTree2D.as @@ -0,0 +1,226 @@ +package alternativa.physics.collision { + + + import alternativa.physics.collision.types.BoundBox; + + /** + * + */ + public class CollisionKdTree2D { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + + public var parentTree:CollisionKdTree; + public var parentNode:CollisionKdNode; + public var rootNode:CollisionKdNode; + + private var splitAxis:int; + private var splitCost:Number; + private var splitCoord:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + /** + * + * @param parentTree + * @param parentNode + */ + public function CollisionKdTree2D(parentTree:CollisionKdTree, parentNode:CollisionKdNode) { + this.parentTree = parentTree; + this.parentNode = parentNode; + } + + /** + * + */ + public function createTree():void { + rootNode = new CollisionKdNode(); + rootNode.boundBox = parentNode.boundBox.clone(); + rootNode.indices = new Vector.(); + var numObjects:int = parentNode.splitIndices.length; + for (var i:int = 0; i < numObjects; ++i) rootNode.indices[i] = parentNode.splitIndices[i]; + + splitNode(rootNode); + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + if (node.indices.length <= minPrimitivesPerNode) return; + + var objects:Vector. = node.indices; + var i:int; + var j:int; + + var nodeBoundBox:BoundBox = node.boundBox; + + // Подготовка баунда с погрешностями + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + // Собираем опорные координаты + var numSplitCoordsX:int; + var numSplitCoordsY:int; + var numSplitCoordsZ:int; + var numObjects:int = objects.length; + for (i = 0; i < numObjects; ++i) { + var bb:BoundBox = staticBoundBoxes[objects[i]]; + + if (parentNode.axis != 0) { + if (bb.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = bb.minX; + if (bb.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = bb.maxX; + } + + if (parentNode.axis != 1) { + if (bb.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = bb.minY; + if (bb.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = bb.maxY; + } + + if (parentNode.axis != 2) { + if (bb.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = bb.minZ; + if (bb.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = bb.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + if (parentNode.axis != 0) checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + if (parentNode.axis != 1) checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + if (parentNode.axis != 2) checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numObjects; ++i) { + bb = staticBoundBoxes[objects[i]]; + var min:Number = axisX ? bb.minX : (axisY ? bb.minY : bb.minZ); + var max:Number = axisX ? bb.maxX : (axisY ? bb.maxY : bb.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(objects[i]); + objects[i] = -1; + } + } else { + if (min >= coordMin) { + if (max > coordMax) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(objects[i]); + objects[i] = -1; + } + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numObjects; ++i) { + if (objects[i] >= 0) objects[j++] = objects[i]; + } + if (j > 0) objects.length = j; + else node.indices = null; + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/CollisionPrimitive.as b/0.0.9.0/src/alternativa/physics/collision/CollisionPrimitive.as new file mode 100644 index 0000000..e03cfee --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/CollisionPrimitive.as @@ -0,0 +1,140 @@ +package alternativa.physics.collision { + + import alternativa.physics.Body; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * Базовый класс для примитивов, использующихся детектором столкновений. + */ + public class CollisionPrimitive { + // Константы типов примитива + public static const BOX:int = 1; + public static const PLANE:int = 2; + public static const SPHERE:int = 3; + public static const RECT:int = 4; + public static const TRIANGLE:int = 5; + + // Тип примитива + public var type:int; + // Группы примитива. Каждая группа определяется установленным битом. Столкновения проверяются только для примитивов, + // имеющих хотя бы одну общую группу. + public var collisionGroup:int; + // Предикат примитива, вызывающийся детектором при нахождении столкновения. В зависимости от возвращённого + // предикатом результата столкновение либо регистрируется, либо игнорируется. + // Nullable + public var postCollisionPredicate:ICollisionPredicate; + // Тело, владеющее примитивом. Поле сделано открытым для быстрого доступа на чтение. Установка значения должна выполняться вызовом метода setBody(). + // Nullable + public var body:Body; + // Трансформация примитива в системе координат тела, если оно указано. Не допускается масштабирование матрицы. + public var localTransform:Matrix4; + // Полная трансформация примитива. Не допускается масштабирование матрицы. + public var transform:Matrix4 = new Matrix4(); + // AABB в мировой системе координат. Расчитывается системой вызовом функции calculateBoundBox(). + public var aabb:BoundBox = new BoundBox(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param type тип примитива + * @param collisionGroup группа примитива + */ + public function CollisionPrimitive(type:int, collisionGroup:int) { + this.type = type; + this.collisionGroup = collisionGroup; + } + + /** + * Устанавливает тело, владеющее примитивом. + * + * @param body тело, которое владеет примитивом + * @param localTransform трансформация примитива в системе координат тела. Указание значения null равносильно + * заданию единичной матрицы, однако в первом случае не будет дополнительного умножения матриц при вычислении полной трансформации примитива. + */ + public function setBody(body:Body, localTransform:Matrix4 = null):void { + if (this.body == body) return; + this.body = body; + if (body != null) { + if (localTransform != null) { + if (this.localTransform == null) { + this.localTransform = new Matrix4(); + } + this.localTransform.copy(localTransform); + } else { + this.localTransform = null; + } + } + } + + /** + * Рассчитывает AABB примитива. Наследники должны переопределять этот метод, реализуя в нём корректный рассчёт. + * + * @return ссылка на свой AABB + */ + public function calculateAABB():BoundBox { + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + return -1; + } + + /** + * Клонирует примитив. Переопределять не рекомендуется. Вместо этого переопределяются методы createPrimitive() и copyFrom(). + * + * @return клон примитива + */ + public function clone():CollisionPrimitive { + var p:CollisionPrimitive = createPrimitive(); + return p.copyFrom(this); + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + if (source == null) { + throw new ArgumentError("Parameter source cannot be null"); + } + type = source.type; + transform.copy(source.transform); + collisionGroup = source.collisionGroup; + setBody(source.body, source.localTransform); + aabb.copyFrom(source.aabb); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + public function toString():String { + return "[CollisionPrimitive type=" + type + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + protected function createPrimitive():CollisionPrimitive { + return new CollisionPrimitive(type, collisionGroup); + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/IBodyCollisionPredicate.as b/0.0.9.0/src/alternativa/physics/collision/IBodyCollisionPredicate.as new file mode 100644 index 0000000..4ace681 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/IBodyCollisionPredicate.as @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.Body; + + public interface IBodyCollisionPredicate { + function considerBodies(body1:Body, body2:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/ICollider.as b/0.0.9.0/src/alternativa/physics/collision/ICollider.as new file mode 100644 index 0000000..e6aac2a --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/ICollider.as @@ -0,0 +1,29 @@ +package alternativa.physics.collision { + + import alternativa.physics.Contact; + + /** + * Интерфейс определителя столкновений между двумя примитивами. + */ + public interface ICollider { + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/ICollisionDetector.as b/0.0.9.0/src/alternativa/physics/collision/ICollisionDetector.as new file mode 100644 index 0000000..916f67b --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/ICollisionDetector.as @@ -0,0 +1,64 @@ +package alternativa.physics.collision { + import alternativa.physics.Contact; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.math.Vector3; + + /** + * Интерфейс детектора столкновений. + */ + public interface ICollisionDetector { + + /** + * Получает все столкновения в текущей конфигурации физической геометрии. + * + * @param contacts список контактов, в кторые будет записана информация о столкновении + * @return количество найденных столкновений + */ + function getAllContacts(contacts:Contact):Contact; + + /** + * Тестирует луч на пересечение с физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param direction направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRay(origin:Vector3, direction:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * + * @param origin + * @param direction + * @param collisionGroup + * @param maxTime + * @param predicate + * @param result + * @return + */ + function intersectRayWithStatic(origin:Vector3, direction:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @return + */ + function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/ICollisionPredicate.as b/0.0.9.0/src/alternativa/physics/collision/ICollisionPredicate.as new file mode 100644 index 0000000..33d30ca --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/ICollisionPredicate.as @@ -0,0 +1,8 @@ +package alternativa.physics.collision { + + public interface ICollisionPredicate { + + function considerCollision(primitive:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/IRayCollisionPredicate.as b/0.0.9.0/src/alternativa/physics/collision/IRayCollisionPredicate.as new file mode 100644 index 0000000..b319f62 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/IRayCollisionPredicate.as @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.Body; + + public interface IRayCollisionPredicate { + function considerBody(body:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/KdTreeCollisionDetector.as b/0.0.9.0/src/alternativa/physics/collision/KdTreeCollisionDetector.as new file mode 100644 index 0000000..9ca4c2a --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/KdTreeCollisionDetector.as @@ -0,0 +1,486 @@ +package alternativa.physics.collision { + + + 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.math.Vector3; + + use namespace altphysics; + + /** + * Детектор, хранящий статическую геометрию в kD-дереве и использующий дерево для ускорения тестов на пересечения. + */ + public class KdTreeCollisionDetector implements ICollisionDetector { + + altphysics var tree:CollisionKdTree; + altphysics var dynamicPrimitives:Vector.; + 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.(); + + 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.):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. = tree.staticChildren; +// var indices:Vector. = 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., indices:Vector., 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; + } \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/colliders/.svn/all-wcprops b/0.0.9.0/src/alternativa/physics/collision/colliders/.svn/all-wcprops new file mode 100644 index 0000000..2c35da0 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/colliders/.svn/all-wcprops @@ -0,0 +1,53 @@ +K 25 +svn:wc:ra_dav:version-url +V 123 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/collision/colliders +END +BoxSphereCollider.as +K 25 +svn:wc:ra_dav:version-url +V 144 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/collision/colliders/BoxSphereCollider.as +END +BoxBoxCollider.as +K 25 +svn:wc:ra_dav:version-url +V 141 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/collision/colliders/BoxBoxCollider.as +END +BoxRectCollider.as +K 25 +svn:wc:ra_dav:version-url +V 142 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/collision/colliders/BoxRectCollider.as +END +SpherePlaneCollider.as +K 25 +svn:wc:ra_dav:version-url +V 146 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/collision/colliders/SpherePlaneCollider.as +END +BoxCollider.as +K 25 +svn:wc:ra_dav:version-url +V 138 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/collision/colliders/BoxCollider.as +END +BoxPlaneCollider.as +K 25 +svn:wc:ra_dav:version-url +V 143 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/collision/colliders/BoxPlaneCollider.as +END +SphereSphereCollider.as +K 25 +svn:wc:ra_dav:version-url +V 147 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/collision/colliders/SphereSphereCollider.as +END +BoxTriangleCollider.as +K 25 +svn:wc:ra_dav:version-url +V 146 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/collision/colliders/BoxTriangleCollider.as +END diff --git a/0.0.9.0/src/alternativa/physics/collision/colliders/.svn/entries b/0.0.9.0/src/alternativa/physics/collision/colliders/.svn/entries new file mode 100644 index 0000000..8b561b9 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/colliders/.svn/entries @@ -0,0 +1,124 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/collision/colliders +http://svndev.alternativaplatform.com + + + +2010-02-12T12:34:25.192477Z +28424 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +BoxSphereCollider.as +file + + + + +2010-10-28T04:32:43.000000Z +25170bcb9a1eec2a0941f4b50a06a554 +2009-12-08T15:53:11.094448Z +24506 +mike + +BoxBoxCollider.as +file + + + + +2010-10-28T04:32:43.000000Z +15da13b1383e9c1892edc205c8c055b0 +2010-02-12T12:34:25.192477Z +28424 +mike + +BoxRectCollider.as +file + + + + +2010-10-28T04:32:43.000000Z +457f0fdacef78377fe6ce772bbf3478c +2010-01-14T06:08:10.656848Z +26352 +mike + +SpherePlaneCollider.as +file + + + + +2010-10-28T04:32:43.000000Z +67621ed47d7e90b72498acf2802c4e2d +2009-11-24T08:44:15.941480Z +23758 +mike + +BoxCollider.as +file + + + + +2010-10-28T04:32:43.000000Z +52e72ed30a24a3a43ce16a19930546b5 +2010-02-12T12:34:25.192477Z +28424 +mike + +BoxPlaneCollider.as +file + + + + +2010-10-28T04:32:43.000000Z +204c7b02a9a1913269c558658bad4262 +2010-02-12T12:34:25.192477Z +28424 +mike + +SphereSphereCollider.as +file + + + + +2010-10-28T04:32:43.000000Z +301c7102f91a7f3ede83e35ca202db0b +2009-11-24T08:44:15.941480Z +23758 +mike + +BoxTriangleCollider.as +file + + + + +2010-10-28T04:32:43.000000Z +598db0f56227e526a4ba9b17b741d685 +2010-02-12T12:34:25.192477Z +28424 +mike + diff --git a/0.0.9.0/src/alternativa/physics/collision/colliders/.svn/format b/0.0.9.0/src/alternativa/physics/collision/colliders/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/colliders/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxBoxCollider.as.svn-base b/0.0.9.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxBoxCollider.as.svn-base new file mode 100644 index 0000000..2d3f04c --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxBoxCollider.as.svn-base @@ -0,0 +1,589 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * Расчитывает точки контакта двух боксов. Нормаль контакта направляется в сторону бокса с меньшим ID. + */ + public class BoxBoxCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + private var vectorToBox1:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var bestAxisIndex:int; + private var minOverlap:Number; + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + private var tmpPoints:Vector. = new Vector.(8, true); + private var pcount:int; + + /** + * + */ + public function BoxBoxCollider() { + for (var i:int = 0; i < 8; i++) { + tmpPoints[i] = new ContactPoint(); + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + + if (bestAxisIndex < 6) { + // Контакт грань-(грань|ребро|вершина) + if (!findFaceContactPoints(box1, box2, vectorToBox1, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 6; + findEdgesIntersection(box1, box2, vectorToBox1, int(bestAxisIndex/3), bestAxisIndex%3, contact); + } + contact.body1 = box1.body; + contact.body2 = box2.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + // Вектор из центра второго бокса в центр первого + vectorToBox1.x = transform1.d - transform2.d; + vectorToBox1.y = transform1.h - transform2.h; + vectorToBox1.z = transform1.l - transform2.l; + + // Проверка пересечения по основным осям + axis10.x = transform1.a; + axis10.y = transform1.e; + axis10.z = transform1.i; + if (!testMainAxis(box1, box2, axis10, 0, vectorToBox1)) return false; + axis11.x = transform1.b; + axis11.y = transform1.f; + axis11.z = transform1.j; + if (!testMainAxis(box1, box2, axis11, 1, vectorToBox1)) return false; + axis12.x = transform1.c; + axis12.y = transform1.g; + axis12.z = transform1.k; + if (!testMainAxis(box1, box2, axis12, 2, vectorToBox1)) return false; + + axis20.x = transform2.a; + axis20.y = transform2.e; + axis20.z = transform2.i; + if (!testMainAxis(box1, box2, axis20, 3, vectorToBox1)) return false; + axis21.x = transform2.b; + axis21.y = transform2.f; + axis21.z = transform2.j; + if (!testMainAxis(box1, box2, axis21, 4, vectorToBox1)) return false; + axis22.x = transform2.c; + axis22.y = transform2.g; + axis22.z = transform2.k; + if (!testMainAxis(box1, box2, axis22, 5, vectorToBox1)) return false; + + // Проверка производных осей + if (!testDerivedAxis(box1, box2, axis10, axis20, 6, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis21, 7, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis22, 8, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis11, axis20, 9, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis21, 10, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis22, 11, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis12, axis20, 12, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis21, 13, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis22, 14, vectorToBox1)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта грани одного бокса с гранью/ребром/вершиной другого. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param faceAxisIdx индекс оси первого бокса, перпендикулярной грани, с которой произошло столкновение + * @param contactInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var swapNormal:Boolean = false; + if (faceAxisIdx > 2) { + // Столкновение с гранью второго бокса. Для дальнейших расчётов боксы меняются местами, + // но нормаль контакта всё равно должна быть направлена в сторону первоначального box1 + var tmpBox:CollisionBox = box1; + box1 = box2; + box2 = tmpBox; + vectorToBox1.x = -vectorToBox1.x; + vectorToBox1.y = -vectorToBox1.y; + vectorToBox1.z = -vectorToBox1.z; + faceAxisIdx -= 3; + swapNormal = true; + } + + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + var colAxis:Vector3 = contact.normal; + + transform1.getAxis(faceAxisIdx, colAxis); + var negativeFace:Boolean = colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z > 0; + + var code:int = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box1.excludedFaces) != 0) return false; + + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + // Ищем ось второго бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + transform2.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisDot = dot; + incidentAxisIdx = axisIdx; + } + } + // Получаем список вершин грани второго бокса, переводим их в систему координат первого бокса и выполняем обрезку + // по грани первого бокса. Таким образом получается список потенциальных точек контакта. + transform2.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box2.hs, incidentAxisIdx, incidentAxisDot < 0, points1); + + // TODO: Вычислить результирующую матрицу, затем преобразовать векторы за один заход + transform2.transformVectorsN(points1, points2, 4); + transform1.transformVectorsInverseN(points2, points1, 4); + + var pnum:int = clip(box1.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points1[i]; + if ((pen = getPointBoxPenetration(box1.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + var cp:ContactPoint = tmpPoints[pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = transform1.a*v.x + transform1.b*v.y + transform1.c*v.z + transform1.d; + cpPos.y = transform1.e*v.x + transform1.f*v.y + transform1.g*v.z + transform1.h; + cpPos.z = transform1.i*v.x + transform1.j*v.y + transform1.k*v.z + transform1.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + + cp.penetration = pen; + } + } + if (swapNormal) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (pcount > 4) { + reducePoints(); + } + for (i = 0; i < pcount; i++) { + cp = contact.points[i]; + cp.copyFrom(tmpPoints[i]); + } + contact.pcount = pcount; + return true; + } + + /** + * + * @param contactInfo + */ + private function reducePoints():void { + var i:int; + var minIdx:int; + var cp1:ContactPoint; + var cp2:ContactPoint; + while (pcount > 4) { + var minLen:Number = 1e10; + var p1:Vector3 = (tmpPoints[int(pcount - 1)] as ContactPoint).pos; + var p2:Vector3; + for (i = 0; i < pcount; i++) { + p2 = (tmpPoints[i] as ContactPoint).pos; + var dx:Number = p2.x - p1.x; + var dy:Number = p2.y - p1.y; + var dz:Number = p2.z - p1.z; + var len:Number = dx*dx + dy*dy + dz*dz; + if (len < minLen) { + minLen = len; + minIdx = i; + } + p1 = p2; + } + var ii:int = minIdx == 0 ? (pcount - 1) : (minIdx - 1); + cp1 = tmpPoints[ii]; + cp2 = tmpPoints[minIdx]; + p1 = cp1.pos; + p2 = cp2.pos; + p2.x = 0.5*(p1.x + p2.x); + p2.y = 0.5*(p1.y + p2.y); + p2.z = 0.5*(p1.z + p2.z); + cp2.penetration = 0.5*(cp1.penetration + cp2.penetration); + if (minIdx > 0) { + for (i = minIdx; i < pcount; i++) { + tmpPoints[int(i - 1)] = tmpPoints[i]; + } + tmpPoints[int(pcount - 1)] = cp1; + } + pcount--; + } + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clip(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param contactInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):void { + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + transform1.getAxis(axisIdx1, axis10); + transform2.getAxis(axisIdx2, axis20); + + var colAxis:Vector3 = contact.normal; + + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону первого бокса + if (colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + + var tx:Number = box1.hs.x; + var ty:Number = box1.hs.y; + var tz:Number = box1.hs.z; + + var x2:Number = box2.hs.x; + var y2:Number = box2.hs.y; + var z2:Number = box2.hs.z; + // x + if (axisIdx1 == 0) { + tx = 0; + halfLen1 = box1.hs.x; + } else { + if (colAxis.x*transform1.a + colAxis.y*transform1.e + colAxis.z*transform1.i > 0) { + tx = -tx; + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = box2.hs.x; + } else { + if (colAxis.x*transform2.a + colAxis.y*transform2.e + colAxis.z*transform2.i < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + ty = 0; + halfLen1 = box1.hs.y; + } else { + if (colAxis.x*transform1.b + colAxis.y*transform1.f + colAxis.z*transform1.j > 0) { + ty = -ty; + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = box2.hs.y; + } else { + if (colAxis.x*transform2.b + colAxis.y*transform2.f + colAxis.z*transform2.j < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + tz = 0; + halfLen1 = box1.hs.z; + } else { + if (colAxis.x*transform1.c + colAxis.y*transform1.g + colAxis.z*transform1.k > 0) { + tz = -tz; + } + } + if (axisIdx2 == 2) { + z2 = 0; + halfLen2 = box2.hs.z; + } else { + if (colAxis.x*transform2.c + colAxis.y*transform2.g + colAxis.z*transform2.k < 0) { + z2 = -z2; + } + } + // Получаем глобальные координаты средних точек рёбер + var x1:Number = transform1.a*tx + transform1.b*ty + transform1.c*tz + transform1.d; + var y1:Number = transform1.e*tx + transform1.f*ty + transform1.g*tz + transform1.h; + var z1:Number = transform1.i*tx + transform1.j*ty + transform1.k*tz + transform1.l; + tx = x2; + ty = y2; + tz = z2; + x2 = transform2.a*tx + transform2.b*ty + transform2.c*tz + transform2.d; + y2 = transform2.e*tx + transform2.f*ty + transform2.g*tz + transform2.h; + z2 = transform2.i*tx + transform2.j*ty + transform2.k*tz + transform2.l; + + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + tx = x2 - x1; + ty = y2 - y1; + tz = z2 - z1; + var c1:Number = axis10.x*tx + axis10.y*ty + axis10.z*tz; + var c2:Number = axis20.x*tx + axis20.y*ty + axis20.z*tz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cp.pos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cp.pos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение боксов вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box1 + * @param box2 + * @param axis + * @param axisIndex + * @param toBox1 + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box1 + * @param box2 + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox1 + * @return + */ + private function testDerivedAxis(box1:CollisionBox, box2:CollisionBox, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия двух боксов вдоль заданной оси. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param axis ось + * @param vectorToBox1 вектор, соединяющий центр второго бокса с центром первого + * @return величина перекрытия боксов вдоль оси + */ + public function overlapOnAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, vectorToBox1:Vector3):Number { + var m:Matrix4 = box1.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box1.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box1.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box1.hs.z; + if (d < 0) d = -d; + projection += d; + + m = box2.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box2.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box2.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box2.hs.z; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox1.x*axis.x + vectorToBox1.y*axis.y + vectorToBox1.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} + import alternativa.math.Vector3; + + class CollisionPointTmp { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + } diff --git a/0.0.9.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxCollider.as.svn-base b/0.0.9.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxCollider.as.svn-base new file mode 100644 index 0000000..2429bb1 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxCollider.as.svn-base @@ -0,0 +1,427 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + /** + * + */ + public class BoxCollider implements ICollider { + + /** + * + */ + public function BoxCollider() { + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + return false; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + protected function getFaceVertsByAxis(hs:Vector3, axisIdx:int, negativeFace:Boolean, result:Vector.):void { + var v:Vector3; + switch (axisIdx) { + case 0: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + } + break; + case 1: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + } + break; + case 2: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + } else { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } + break; + } + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxPlaneCollider.as.svn-base b/0.0.9.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxPlaneCollider.as.svn-base new file mode 100644 index 0000000..3bebdc3 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxPlaneCollider.as.svn-base @@ -0,0 +1,88 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + public class BoxPlaneCollider implements ICollider { + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var normal:Vector3 = new Vector3(); + + public function BoxPlaneCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var box:RigidBox = body1 as RigidBox; +// var plane:RigidPlane; +// if (box == null) { +// box = body2 as RigidBox; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные координаты вершин бокса +// var sx:Number = box.halfSize.x; +// var sy:Number = box.halfSize.y; +// var sz:Number = box.halfSize.z; +// (verts1[0] as Vector3).reset(-sx, -sy, -sz); +// (verts1[1] as Vector3).reset(sx, -sy, -sz); +// (verts1[2] as Vector3).reset(sx, sy, -sz); +// (verts1[3] as Vector3).reset(-sx, sy, -sz); +// (verts1[4] as Vector3).reset(-sx, -sy, sz); +// (verts1[5] as Vector3).reset(sx, -sy, sz); +// (verts1[6] as Vector3).reset(sx, sy, sz); +// (verts1[7] as Vector3).reset(-sx, sy, sz); +// +// box.transform.transformVectors(verts1, verts2); +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// // Проверяем наличие столкновений с каждой вершиной +// collisionInfo.pcount = 0; +// for (var i:int = 0; i < 8; i++) { +// // Вершина добавляется в список точек столкновения, если лежит под плоскостью +// var dist:Number = (verts2[i] as Vector3).dot(normal); +// if (dist < offset) { +// var cp:ContactPoint; +// if (collisionInfo.pcount == collisionInfo.points.length) { +// cp = new ContactPoint(); +// collisionInfo.points[collisionInfo.pcount] = cp; +// } else { +// cp = collisionInfo.points[collisionInfo.pcount]; +// } +// cp.pos.copy(verts2[i]); +// cp.r1.diff(cp.pos, box.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); +// cp.penetration = offset - dist; +// collisionInfo.pcount++; +// } +// } +// if (collisionInfo.pcount > 0) { +// collisionInfo.body1 = box; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// return true; +// } + return false; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxRectCollider.as.svn-base b/0.0.9.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxRectCollider.as.svn-base new file mode 100644 index 0000000..ff5348a --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxRectCollider.as.svn-base @@ -0,0 +1,573 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * + */ + public class BoxRectCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + + private var vectorToBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxRectCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, rect, vectorToBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + if (!findEdgesIntersection(box, rect, vectorToBox, int(bestAxisIndex/2), bestAxisIndex%2, contact)) { + return false; + } + } + contact.body1 = box.body; + contact.body2 = rect.body; + + // Хак для танков, чтобы исключить утыкания танков в стыки статических примитивов + contact.normal.x = rect.transform.c; + contact.normal.y = rect.transform.g; + contact.normal.z = rect.transform.k; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + // Вектор из центра прямоугольника в центр бокса + vectorToBox.x = boxTransform.d - rectTransform.d; + vectorToBox.y = boxTransform.h - rectTransform.h; + vectorToBox.z = boxTransform.l - rectTransform.l; + + // Проверка пересечения по нормали прямоугольника + rectTransform.getAxis(2, axis22); + if (!testMainAxis(box, rect, axis22, 0, vectorToBox)) return false; + + // Проверка пересечения по основным осям бокса + boxTransform.getAxis(0, axis10); + if (!testMainAxis(box, rect, axis10, 1, vectorToBox)) return false; + boxTransform.getAxis(1, axis11); + if (!testMainAxis(box, rect, axis11, 2, vectorToBox)) return false; + boxTransform.getAxis(2, axis12); + if (!testMainAxis(box, rect, axis12, 3, vectorToBox)) return false; + + // Получаем направляющие рёбер прямоугольника + rectTransform.getAxis(0, axis20); + rectTransform.getAxis(1, axis21); + + // Проверка производных осей + if (!testDerivedAxis(box, rect, axis10, axis20, 4, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis10, axis21, 5, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis11, axis20, 6, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis11, axis21, 7, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis12, axis20, 8, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis12, axis21, 9, vectorToBox)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта бокса с прямоугольником. + * + * @param box бокс + * @param rect прямоугольник + * @param vectorToBox вектор, направленный из центра прямоугольника в центр бокса + * @param faceAxisIdx индекс оси, идентифицирующей полскость столкновения (грань бокса или полскость прямоугольника) + * @param colInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var pnum:int; + var i:int; + var v:Vector3; + var cp:ContactPoint; + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + var colAxis:Vector3 = contact.normal; + + var negativeFace:Boolean; + var code:int; + + if (faceAxisIdx == 0) { + // Столкновение с плоскостью прямоугольника + + // Проверим положение бокса относительно плоскости прямоугольника + colAxis.x = rectTransform.c; + colAxis.y = rectTransform.g; + colAxis.z = rectTransform.k; + +// var offset:Number = colAxis.x*rectTransform.d + colAxis.y*rectTransform.h + colAxis.z*rectTransform.l; +// if (bbPos.vDot(colAxis) < offset) return false; + + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisIdx = axisIdx; + incidentAxisDot = dot; + } + } + negativeFace = incidentAxisDot > 0; + + code = 1 << (incidentAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + + // Получаем список вершин грани бокса, переводим их в систему координат прямоугольника и выполняем обрезку + // по прямоугольнику. Таким образом получается список потенциальных точек контакта. + boxTransform.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box.hs, incidentAxisIdx, negativeFace, points1); + boxTransform.transformVectorsN(points1, points2, 4); + rectTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByRect(rect.hs); + // Проверяем каждую потенциальную точку на принадлежность нижней полуплоскости прямоугольника и добавляем такие точки в список контактов + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if (v.z < epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = -v.z; + var cpPos:Vector3 = cp.pos; + cpPos.x = rectTransform.a*v.x + rectTransform.b*v.y + rectTransform.c*v.z + rectTransform.d; + cpPos.y = rectTransform.e*v.x + rectTransform.f*v.y + rectTransform.g*v.z + rectTransform.h; + cpPos.z = rectTransform.i*v.x + rectTransform.j*v.y + rectTransform.k*v.z + rectTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } else { + // Столкновение с гранью бокса + faceAxisIdx--; + boxTransform.getAxis(faceAxisIdx, colAxis); + negativeFace = colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z > 0; + + code = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) { + return false; + } + + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (rectTransform.c*colAxis.x + rectTransform.g*colAxis.y + rectTransform.k*colAxis.z < 0) return false; + + getFaceVertsByAxis(rect.hs, 2, false, points1); + rectTransform.transformVectorsN(points1, points2, 4); + boxTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if ((pen = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = pen; + cpPos = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } + return true; + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * + * @param hs + * @return + */ + private function clipByRect(hs:Vector3):int { + var pnum:int = 4; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box первый бокс + * @param rect второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param colInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + boxTransform.getAxis(axisIdx1, axis10); + rectTransform.getAxis(axisIdx2, axis20); + var colAxis:Vector3 = contact.normal; + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону бокса + if (colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + var vx:Number = box.hs.x; + var vy:Number = box.hs.y; + var vz:Number = box.hs.z; + var x2:Number = rect.hs.x; + var y2:Number = rect.hs.y; + var z2:Number = rect.hs.z; + // x + if (axisIdx1 == 0) { + vx = 0; + halfLen1 = box.hs.x; + } else { + if (boxTransform.a*colAxis.x + boxTransform.e*colAxis.y + boxTransform.i*colAxis.z > 0) { + vx = -vx; + if ((box.excludedFaces & 2) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 1) != 0) { + return false; + } + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = rect.hs.x; + } else { + if (rectTransform.a*colAxis.x + rectTransform.e*colAxis.y + rectTransform.i*colAxis.z < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + vy = 0; + halfLen1 = box.hs.y; + } else { + if (boxTransform.b*colAxis.x + boxTransform.f*colAxis.y + boxTransform.j*colAxis.z > 0) { + vy = -vy; + if ((box.excludedFaces & 8) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 4) != 0) { + return false; + } + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = rect.hs.y; + } else { + if (rectTransform.b*colAxis.x + rectTransform.f*colAxis.y + rectTransform.j*colAxis.z < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + vz = 0; + halfLen1 = box.hs.z; + } else { + if (boxTransform.c*colAxis.x + boxTransform.g*colAxis.y + boxTransform.k*colAxis.z > 0) { + vz = -vz; + if ((box.excludedFaces & 32) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 16) != 0) { + return false; + } + } + } + // Получаем глобальные координаты средних точек рёбер + + var x1:Number = boxTransform.a*vx + boxTransform.b*vy + boxTransform.c*vz + boxTransform.d; + var y1:Number = boxTransform.e*vx + boxTransform.f*vy + boxTransform.g*vz + boxTransform.h; + var z1:Number = boxTransform.i*vx + boxTransform.j*vy + boxTransform.k*vz + boxTransform.l; + vx = x2; + vy = y2; + vz = z2; + x2 = rectTransform.a*vx + rectTransform.b*vy + rectTransform.c*vz + rectTransform.d; + y2 = rectTransform.e*vx + rectTransform.f*vy + rectTransform.g*vz + rectTransform.h; + z2 = rectTransform.i*vx + rectTransform.j*vy + rectTransform.k*vz + rectTransform.l; + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + vx = x2 - x1; + vy = y2 - y1; + vz = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cpPos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cpPos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cpPos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var v:Vector3 = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + return true; + } + + /** + * Проверяет пересечение вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box + * @param rect + * @param axis + * @param axisIndex + * @param vectorToBox + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param rect + * @param axis1 + * @param axis2 + * @param axisIndex + * @param vectorToBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, rect:CollisionRect, axis1:Vector3, axis2:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия вдоль заданной оси. + * + * @param box бокс + * @param rect прямоугольник + * @param axis ось + * @param vectorToBox вектор, соединяющий центр прямоугольника с центром бокса + * @return величина перекрытия вдоль оси + */ + public function overlapOnAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, vectorToBox:Vector3):Number { + var m:Matrix4 = box.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box.hs.z; + if (d < 0) d = -d; + projection += d; + + m = rect.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*rect.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*rect.hs.y; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox.x*axis.x + vectorToBox.y*axis.y + vectorToBox.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxSphereCollider.as.svn-base b/0.0.9.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxSphereCollider.as.svn-base new file mode 100644 index 0000000..da5e36d --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxSphereCollider.as.svn-base @@ -0,0 +1,168 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.math.Vector3; + use namespace altphysics; + /** + * + */ + public class BoxSphereCollider implements ICollider { + + private var center:Vector3 = new Vector3(); + private var closestPt:Vector3 = new Vector3(); + + private var bPos:Vector3 = new Vector3(); + private var sPos:Vector3 = new Vector3(); + + /** + * + */ + public function BoxSphereCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + // Зафиксированно столкновение + contact.body1 = sphere.body; + contact.body2 = box.body; + contact.normal.vCopy(closestPt).vTransformBy4(box.transform).vSubtract(sPos).vNormalize().vReverse(); + contact.pcount = 1; + + var cp:ContactPoint = contact.points[0]; + cp.penetration = sphere.r - Math.sqrt(distSqr); + cp.pos.vCopy(contact.normal).vScale(-sphere.r).vAdd(sPos); + cp.r1.vDiff(cp.pos, sPos); + cp.r2.vDiff(cp.pos, bPos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + + return true; + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxTriangleCollider.as.svn-base b/0.0.9.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxTriangleCollider.as.svn-base new file mode 100644 index 0000000..223860f --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxTriangleCollider.as.svn-base @@ -0,0 +1,723 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionTriangle; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * + */ + public class BoxTriangleCollider extends BoxCollider { + + public var epsilon:Number = 0.001; + + private var bestAxisIndex:int; + private var minOverlap:Number; + private var toBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var colNormal:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxTriangleCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, tri, toBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + if (!findEdgesIntersection(box, tri, toBox, bestAxisIndex%3, int(bestAxisIndex/3), contact)) return false; + } + + contact.body1 = box.body; + contact.body2 = tri.body; + + // Хак для танков, чтобы исключить утыкания танков в стыки статических примитивов + contact.normal.x = tri.transform.c; + contact.normal.y = tri.transform.g; + contact.normal.z = tri.transform.k; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + toBox.x = boxTransform.d - triTransform.d; + toBox.y = boxTransform.h - triTransform.h; + toBox.z = boxTransform.l - triTransform.l; + + minOverlap = 1e308; + + // Сначала проверяется нормаль треугольника + axis.x = triTransform.c; + axis.y = triTransform.g; + axis.z = triTransform.k; + if (!testMainAxis(box, tri, axis, 0, toBox)) return false; + + // Проверка основных осей бокса + axis10.x = boxTransform.a; + axis10.y = boxTransform.e; + axis10.z = boxTransform.i; + if (!testMainAxis(box, tri, axis10, 1, toBox)) return false; + axis11.x = boxTransform.b; + axis11.y = boxTransform.f; + axis11.z = boxTransform.j; + if (!testMainAxis(box, tri, axis11, 2, toBox)) return false; + axis12.x = boxTransform.c; + axis12.y = boxTransform.g; + axis12.z = boxTransform.k; + if (!testMainAxis(box, tri, axis12, 3, toBox)) return false; + + // Проверка производных осей + // TODO: заменить вычисления векторных произведений инлайнами + var v:Vector3 = tri.e0; + axis20.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis20.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis20.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis20, 4, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis20, 5, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis20, 6, toBox)) return false; + + v = tri.e1; + axis21.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis21.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis21.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis21, 7, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis21, 8, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis21, 9, toBox)) return false; + + v = tri.e2; + axis22.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis22.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis22.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis22, 10, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis22, 11, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis22, 12, toBox)) return false; + + return true; + } + + /** + * Тестирует пересечение примитивов вдоль заданной оси. + * + * @param box бокс + * @param tri треугольник + * @param axis ось + * @param axisIndex индекс оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return true, если примитивы имеют перекрытие проекций вдоль указанной оси, иначе false + */ + private function testMainAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, axisIndex:int, toBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param tri + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, tri:CollisionTriangle, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Рассчитывает величину перекрытия проекций бокса и треугольника на заданную ось. + * + * @param box бокс + * @param tri треугольник + * @param axis единичный направляющий вектор оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return величина перекрытия. Положительное значение указывает на наличие перекрытия, нулевое или отрицательное значение указывает на отсутствие перекрытия. + */ + private function overlapOnAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, toBox:Vector3):Number { + var t:Matrix4 = box.transform; + var projection:Number = (t.a*axis.x + t.e*axis.y + t.i*axis.z)*box.hs.x; + if (projection < 0) projection = -projection; + var d:Number = (t.b*axis.x + t.f*axis.y + t.j*axis.z)*box.hs.y; + projection += d < 0 ? -d : d; + d = (t.c*axis.x + t.g*axis.y + t.k*axis.z)*box.hs.z; + projection += d < 0 ? -d : d; + + var vectorProjection:Number = toBox.x*axis.x + toBox.y*axis.y + toBox.z*axis.z; + // Для оптимизации ось преобразуется в систему треугольника, а не его вершины в мировую систему + t = tri.transform; + var ax:Number = t.a*axis.x + t.e*axis.y + t.i*axis.z; + var ay:Number = t.b*axis.x + t.f*axis.y + t.j*axis.z; + var az:Number = t.c*axis.x + t.g*axis.y + t.k*axis.z; + var max:Number = 0; + if (vectorProjection < 0) { + vectorProjection = -vectorProjection; + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d < max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d < max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d < max) max = d; + max = -max; + } else { + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d > max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d > max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d > max) max = d; + } + + return projection + max - vectorProjection; + } + + /** + * Определяет точки контакта бокса и треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function findFaceContactPoints(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIndex:int, contact:Contact):Boolean { + if (faceAxisIndex == 0) { + // Столкновение с плоскостью треугольника + return getBoxToTriContact(box, tri, toBox, contact); + } else { + // Столкновение с гранью бокса + return getTriToBoxContact(box, tri, toBox, faceAxisIndex, contact); + } + } + + /** + * Определяет точки контакта бокса с плоскостью треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getBoxToTriContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + colNormal.x = triTransform.c; + colNormal.y = triTransform.g; + colNormal.z = triTransform.k; + + var over:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + if (!over) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var incAxisDot:Number = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colNormal.x + axis.y*colNormal.y + axis.z*colNormal.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incAxisDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Обрезка грани + var negativeFace:Boolean = incAxisDot > 0; + + var code:int = 1 << (incFaceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + getFaceVertsByAxis(box.hs, incFaceAxisIdx, negativeFace, points1); + boxTransform.transformVectorsN(points1, points2, 4); + triTransform.transformVectorsInverseN(points2, points1, 4); + var pnum:int = clipByTriangle(tri); + // Среди конечного списка точек определяются лежащие под плоскостью треугольника + var cp:ContactPoint; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points2[i]; + if ((over && v.z < 0) || (!over && v.z > 0)) { + cp = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z + triTransform.d; + cpPos.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z + triTransform.h; + cpPos.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z + triTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = over ? -v.z : v.z; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Оперделяет точки контакта треугольника с гранью бокса. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getTriToBoxContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + faceAxisIdx--; + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + boxTransform.getAxis(faceAxisIdx, colNormal); + var negativeFace:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + + var code:int = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + if (!negativeFace) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + var v:Vector3 = points1[0]; + v.x = tri.v0.x; + v.y = tri.v0.y; + v.z = tri.v0.z; + + v = points1[1]; + v.x = tri.v1.x; + v.y = tri.v1.y; + v.z = tri.v1.z; + + v = points1[2]; + v.x = tri.v2.x; + v.y = tri.v2.y; + v.z = tri.v2.z; + + triTransform.transformVectorsN(points1, points2, 3); + boxTransform.transformVectorsInverseN(points2, points1, 3); + + var pnum:int = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность боксу и добавляем такие точки в список контактов + var penetration:Number; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + v = points1[i]; + penetration = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace); + if (penetration > -epsilon) { + var cp:ContactPoint = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = penetration; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Вычисляет величину проникновения точки в бокс. + * + * @param hs вектор половинных размеров бокса + * @param p точка в системе координат бокса + * @param axisIndex индекс оси + * @param negativeFace если true, проверяется нижняя грань + * @return величина проникновения точки в бокс + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, negativeFace:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (negativeFace) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (negativeFace) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (negativeFace) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, вершины которой заданы в списке points1. Результат сохраняется в этом же списке. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 3; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Обрезает грань прямоугольника сторонами треугольника. Обрезка выполняется в системе координат и в проекции на + * плоскость треугольника. Входные вершины грани в количестве четырёх штук должны находиться в списке points1. + * Конечные вершины сохраняются в списке points2. + * + * @param tri треугольник + * @return количество врешин в конечном списке + */ + private function clipByTriangle(tri:CollisionTriangle):int { + var vnum:int = 4; + vnum = clipByLine(tri.v0, tri.e0, points1, vnum, points2); + if (vnum == 0) return 0; + vnum = clipByLine(tri.v1, tri.e1, points2, vnum, points1); + if (vnum == 0) return 0; + return clipByLine(tri.v2, tri.e2, points1, vnum, points2);; + } + + /** + * Обрезает полигон указанной прямой. + * + * @param linePoint точка на прямой + * @param lineDir единичный направляющий вектор прямой + * @param verticesIn список вершин исходного полигона + * @param vnum количество вершин исходного полигона + * @param verticesOut список, куда будут записаны вершины конечного полигона + * @return количество вершин конечного полигона + */ + private function clipByLine(linePoint:Vector3, lineDir:Vector3, verticesIn:Vector., vnum:int, verticesOut:Vector.):int { + var nx:Number = -lineDir.y; + var ny:Number = lineDir.x; + var offset:Number = linePoint.x*nx + linePoint.y*ny; + var v1:Vector3 = verticesIn[int(vnum - 1)]; + var offset1:Number = v1.x*nx + v1.y*ny; + var t:Number; + var v:Vector3; + var num:int = 0; + for (var i:int = 0; i < vnum; i++) { + var v2:Vector3 = verticesIn[i]; + var offset2:Number = v2.x*nx + v2.y*ny; + if (offset1 < offset) { + // Первая точка ребра во внешней полуплоскости + if (offset2 > offset) { + // Вторая точка ребра во внутренней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } else { + // Первая точка ребра во внутренней полуплоскости. Добавляем её в конечный список. + v = verticesOut[num]; + v.x = v1.x; + v.y = v1.y; + v.z = v1.z; + num++; + if (offset2 < offset) { + // Вторая точка ребра во внешней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } + v1 = v2; + offset1 = offset2; + } + return num; + } + + /** + * + * @param box + * @param tri + * @param toBox + * @param boxAxisIdx + * @param triAxisIdx + * @param contact + */ + private function findEdgesIntersection(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, boxAxisIdx:int, triAxisIdx:int, contact:Contact):Boolean { + // Определение точки и направляющего вектора ребра треугольника + var tmpx1:Number; + var tmpy1:Number; + var tmpz1:Number; + var tmpx2:Number; + var tmpy2:Number; + var tmpz2:Number; + switch (triAxisIdx) { + case 0: + tmpx1 = tri.e0.x; + tmpy1 = tri.e0.y; + tmpz1 = tri.e0.z; + tmpx2 = tri.v0.x; + tmpy2 = tri.v0.y; + tmpz2 = tri.v0.z; + break; + case 1: + tmpx1 = tri.e1.x; + tmpy1 = tri.e1.y; + tmpz1 = tri.e1.z; + tmpx2 = tri.v1.x; + tmpy2 = tri.v1.y; + tmpz2 = tri.v1.z; + break; + case 2: + tmpx1 = tri.e2.x; + tmpy1 = tri.e2.y; + tmpz1 = tri.e2.z; + tmpx2 = tri.v2.x; + tmpy2 = tri.v2.y; + tmpz2 = tri.v2.z; + break; + } + var triTransform:Matrix4 = tri.transform; + axis20.x = triTransform.a*tmpx1 + triTransform.b*tmpy1 + triTransform.c*tmpz1; + axis20.y = triTransform.e*tmpx1 + triTransform.f*tmpy1 + triTransform.g*tmpz1; + axis20.z = triTransform.i*tmpx1 + triTransform.j*tmpy1 + triTransform.k*tmpz1; + var x2:Number = triTransform.a*tmpx2 + triTransform.b*tmpy2 + triTransform.c*tmpz2 + triTransform.d; + var y2:Number = triTransform.e*tmpx2 + triTransform.f*tmpy2 + triTransform.g*tmpz2 + triTransform.h; + var z2:Number = triTransform.i*tmpx2 + triTransform.j*tmpy2 + triTransform.k*tmpz2 + triTransform.l; + + // Определение нормали контакта, точки и направляющего вектора ребра бокса + var boxTransform:Matrix4 = box.transform; + boxTransform.getAxis(boxAxisIdx, axis10); + + // Нормаль контакта + var v:Vector3 = contact.normal; + v.x = axis10.y*axis20.z - axis10.z*axis20.y; + v.y = axis10.z*axis20.x - axis10.x*axis20.z; + v.z = axis10.x*axis20.y - axis10.y*axis20.x; + k = 1/Math.sqrt(v.x*v.x + v.y*v.y + v.z*v.z); + v.x *= k; + v.y *= k; + v.z *= k; + // Разворот нормали в сторону бокса + if (v.x*toBox.x + v.y*toBox.y + v.z*toBox.z < 0) { + v.x = -v.x; + v.y = -v.y; + v.z = -v.z; + } + + var boxHalfLen:Number; + tmpx1 = box.hs.x; + tmpy1 = box.hs.y; + tmpz1 = box.hs.z; + // X + if (boxAxisIdx == 0) { + tmpx1 = 0; + boxHalfLen = box.hs.x; + } else { + if (boxTransform.a*v.x + boxTransform.e*v.y + boxTransform.i*v.z > 0) { + tmpx1 = -tmpx1; + if ((box.excludedFaces & 2) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 1) != 0) { + return false; + } + } + } + // Y + if (boxAxisIdx == 1) { + tmpy1 = 0; + boxHalfLen = box.hs.y; + } else { + if (boxTransform.b*v.x + boxTransform.f*v.y + boxTransform.j*v.z > 0) { + tmpy1 = -tmpy1; + if ((box.excludedFaces & 8) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 4) != 0) { + return false; + } + } + } + // Z + if (boxAxisIdx == 2) { + tmpz1 = 0; + boxHalfLen = box.hs.z; + } else { + if (boxTransform.c*v.x + boxTransform.g*v.y + boxTransform.k*v.z > 0) { + tmpz1 = -tmpz1; + if ((box.excludedFaces & 32) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 16) != 0) { + return false; + } + } + } + var x1:Number = boxTransform.a*tmpx1 + boxTransform.b*tmpy1 + boxTransform.c*tmpz1 + boxTransform.d; + var y1:Number = boxTransform.e*tmpx1 + boxTransform.f*tmpy1 + boxTransform.g*tmpz1 + boxTransform.h; + var z1:Number = boxTransform.i*tmpx1 + boxTransform.j*tmpy1 + boxTransform.k*tmpz1 + boxTransform.l; + + // Находим точку пересечения рёбер, решая систему уравнений + // axis10 - направляющий вектор ребра бокса + // x1, y1, z1 - средняя точка ребра бокса + // axis20 - направляющий вектор ребра треугольника + // x2, y2, z2 - начальная точка ребра треугольника + var k:Number = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + var vx:Number = x2 - x1; + var vy:Number = y2 - y1; + var vz:Number = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + + // Запись данных о контакте + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + v = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + v.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + v.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + v.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = v.x - boxTransform.d; + r.y = v.y - boxTransform.h; + r.z = v.z - boxTransform.l; + r = cp.r2; + r.x = v.x - triTransform.d; + r.y = v.y - triTransform.h; + r.z = v.z - triTransform.l; + return true; + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/colliders/.svn/text-base/SpherePlaneCollider.as.svn-base b/0.0.9.0/src/alternativa/physics/collision/colliders/.svn/text-base/SpherePlaneCollider.as.svn-base new file mode 100644 index 0000000..30cd55f --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/colliders/.svn/text-base/SpherePlaneCollider.as.svn-base @@ -0,0 +1,68 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + /** + * + */ + public class SpherePlaneCollider implements ICollider { + + private var normal:Vector3 = new Vector3(); + + /** + * + */ + public function SpherePlaneCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var sphere:RigidSphere = body1 as RigidSphere; +// var plane:RigidPlane; +// if (sphere == null) { +// sphere = body2 as RigidSphere; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// +// var dist:Number = sphere.state.pos.dot(normal) - offset; +// if (dist > sphere.r) return false; +// +// collisionInfo.body1 = sphere; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// collisionInfo.pcount = 1; +// +// var cp:ContactPoint = collisionInfo.points[0]; +// cp.penetration = sphere.r - dist; +// cp.pos.copy(normal).reverse().scale(sphere.r).add(sphere.state.pos); +// cp.r1.diff(cp.pos, sphere.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/colliders/.svn/text-base/SphereSphereCollider.as.svn-base b/0.0.9.0/src/alternativa/physics/collision/colliders/.svn/text-base/SphereSphereCollider.as.svn-base new file mode 100644 index 0000000..3332f3c --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/colliders/.svn/text-base/SphereSphereCollider.as.svn-base @@ -0,0 +1,70 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.math.Vector3; + use namespace altphysics; + + public class SphereSphereCollider implements ICollider { + + private var p1:Vector3 = new Vector3(); + private var p2:Vector3 = new Vector3(); + + public function SphereSphereCollider() { + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var s1:CollisionSphere; + var s2:CollisionSphere; + if (prim1.body != null) { + s1 = prim1 as CollisionSphere; + s2 = prim2 as CollisionSphere; + } else { + s1 = prim2 as CollisionSphere; + s2 = prim1 as CollisionSphere; + } + + s1.transform.getAxis(3, p1); + s2.transform.getAxis(3, p2); + var dx:Number = p1.x - p2.x; + var dy:Number = p1.y - p2.y; + var dz:Number = p1.z - p2.z; + var len:Number = dx*dx + dy*dy + dz*dz; + var sum:Number = s1.r + s2.r; + if (len > sum*sum) return false; + len = Math.sqrt(len); + dx /= len; + dy /= len; + dz /= len; + + contact.body1 = s1.body; + contact.body2 = s2.body; + contact.normal.x = dx; + contact.normal.y = dy; + contact.normal.z = dz; + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = sum - len; + cp.pos.x = p1.x - dx*s1.r; + cp.pos.y = p1.y - dy*s1.r; + cp.pos.z = p1.z - dz*s1.r; + cp.r1.vDiff(cp.pos, p1); + cp.r2.vDiff(cp.pos, p2); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/colliders/BoxBoxCollider.as b/0.0.9.0/src/alternativa/physics/collision/colliders/BoxBoxCollider.as new file mode 100644 index 0000000..2d3f04c --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/colliders/BoxBoxCollider.as @@ -0,0 +1,589 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * Расчитывает точки контакта двух боксов. Нормаль контакта направляется в сторону бокса с меньшим ID. + */ + public class BoxBoxCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + private var vectorToBox1:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var bestAxisIndex:int; + private var minOverlap:Number; + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + private var tmpPoints:Vector. = new Vector.(8, true); + private var pcount:int; + + /** + * + */ + public function BoxBoxCollider() { + for (var i:int = 0; i < 8; i++) { + tmpPoints[i] = new ContactPoint(); + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + + if (bestAxisIndex < 6) { + // Контакт грань-(грань|ребро|вершина) + if (!findFaceContactPoints(box1, box2, vectorToBox1, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 6; + findEdgesIntersection(box1, box2, vectorToBox1, int(bestAxisIndex/3), bestAxisIndex%3, contact); + } + contact.body1 = box1.body; + contact.body2 = box2.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + // Вектор из центра второго бокса в центр первого + vectorToBox1.x = transform1.d - transform2.d; + vectorToBox1.y = transform1.h - transform2.h; + vectorToBox1.z = transform1.l - transform2.l; + + // Проверка пересечения по основным осям + axis10.x = transform1.a; + axis10.y = transform1.e; + axis10.z = transform1.i; + if (!testMainAxis(box1, box2, axis10, 0, vectorToBox1)) return false; + axis11.x = transform1.b; + axis11.y = transform1.f; + axis11.z = transform1.j; + if (!testMainAxis(box1, box2, axis11, 1, vectorToBox1)) return false; + axis12.x = transform1.c; + axis12.y = transform1.g; + axis12.z = transform1.k; + if (!testMainAxis(box1, box2, axis12, 2, vectorToBox1)) return false; + + axis20.x = transform2.a; + axis20.y = transform2.e; + axis20.z = transform2.i; + if (!testMainAxis(box1, box2, axis20, 3, vectorToBox1)) return false; + axis21.x = transform2.b; + axis21.y = transform2.f; + axis21.z = transform2.j; + if (!testMainAxis(box1, box2, axis21, 4, vectorToBox1)) return false; + axis22.x = transform2.c; + axis22.y = transform2.g; + axis22.z = transform2.k; + if (!testMainAxis(box1, box2, axis22, 5, vectorToBox1)) return false; + + // Проверка производных осей + if (!testDerivedAxis(box1, box2, axis10, axis20, 6, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis21, 7, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis22, 8, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis11, axis20, 9, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis21, 10, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis22, 11, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis12, axis20, 12, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis21, 13, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis22, 14, vectorToBox1)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта грани одного бокса с гранью/ребром/вершиной другого. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param faceAxisIdx индекс оси первого бокса, перпендикулярной грани, с которой произошло столкновение + * @param contactInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var swapNormal:Boolean = false; + if (faceAxisIdx > 2) { + // Столкновение с гранью второго бокса. Для дальнейших расчётов боксы меняются местами, + // но нормаль контакта всё равно должна быть направлена в сторону первоначального box1 + var tmpBox:CollisionBox = box1; + box1 = box2; + box2 = tmpBox; + vectorToBox1.x = -vectorToBox1.x; + vectorToBox1.y = -vectorToBox1.y; + vectorToBox1.z = -vectorToBox1.z; + faceAxisIdx -= 3; + swapNormal = true; + } + + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + var colAxis:Vector3 = contact.normal; + + transform1.getAxis(faceAxisIdx, colAxis); + var negativeFace:Boolean = colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z > 0; + + var code:int = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box1.excludedFaces) != 0) return false; + + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + // Ищем ось второго бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + transform2.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisDot = dot; + incidentAxisIdx = axisIdx; + } + } + // Получаем список вершин грани второго бокса, переводим их в систему координат первого бокса и выполняем обрезку + // по грани первого бокса. Таким образом получается список потенциальных точек контакта. + transform2.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box2.hs, incidentAxisIdx, incidentAxisDot < 0, points1); + + // TODO: Вычислить результирующую матрицу, затем преобразовать векторы за один заход + transform2.transformVectorsN(points1, points2, 4); + transform1.transformVectorsInverseN(points2, points1, 4); + + var pnum:int = clip(box1.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points1[i]; + if ((pen = getPointBoxPenetration(box1.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + var cp:ContactPoint = tmpPoints[pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = transform1.a*v.x + transform1.b*v.y + transform1.c*v.z + transform1.d; + cpPos.y = transform1.e*v.x + transform1.f*v.y + transform1.g*v.z + transform1.h; + cpPos.z = transform1.i*v.x + transform1.j*v.y + transform1.k*v.z + transform1.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + + cp.penetration = pen; + } + } + if (swapNormal) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (pcount > 4) { + reducePoints(); + } + for (i = 0; i < pcount; i++) { + cp = contact.points[i]; + cp.copyFrom(tmpPoints[i]); + } + contact.pcount = pcount; + return true; + } + + /** + * + * @param contactInfo + */ + private function reducePoints():void { + var i:int; + var minIdx:int; + var cp1:ContactPoint; + var cp2:ContactPoint; + while (pcount > 4) { + var minLen:Number = 1e10; + var p1:Vector3 = (tmpPoints[int(pcount - 1)] as ContactPoint).pos; + var p2:Vector3; + for (i = 0; i < pcount; i++) { + p2 = (tmpPoints[i] as ContactPoint).pos; + var dx:Number = p2.x - p1.x; + var dy:Number = p2.y - p1.y; + var dz:Number = p2.z - p1.z; + var len:Number = dx*dx + dy*dy + dz*dz; + if (len < minLen) { + minLen = len; + minIdx = i; + } + p1 = p2; + } + var ii:int = minIdx == 0 ? (pcount - 1) : (minIdx - 1); + cp1 = tmpPoints[ii]; + cp2 = tmpPoints[minIdx]; + p1 = cp1.pos; + p2 = cp2.pos; + p2.x = 0.5*(p1.x + p2.x); + p2.y = 0.5*(p1.y + p2.y); + p2.z = 0.5*(p1.z + p2.z); + cp2.penetration = 0.5*(cp1.penetration + cp2.penetration); + if (minIdx > 0) { + for (i = minIdx; i < pcount; i++) { + tmpPoints[int(i - 1)] = tmpPoints[i]; + } + tmpPoints[int(pcount - 1)] = cp1; + } + pcount--; + } + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clip(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param contactInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):void { + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + transform1.getAxis(axisIdx1, axis10); + transform2.getAxis(axisIdx2, axis20); + + var colAxis:Vector3 = contact.normal; + + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону первого бокса + if (colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + + var tx:Number = box1.hs.x; + var ty:Number = box1.hs.y; + var tz:Number = box1.hs.z; + + var x2:Number = box2.hs.x; + var y2:Number = box2.hs.y; + var z2:Number = box2.hs.z; + // x + if (axisIdx1 == 0) { + tx = 0; + halfLen1 = box1.hs.x; + } else { + if (colAxis.x*transform1.a + colAxis.y*transform1.e + colAxis.z*transform1.i > 0) { + tx = -tx; + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = box2.hs.x; + } else { + if (colAxis.x*transform2.a + colAxis.y*transform2.e + colAxis.z*transform2.i < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + ty = 0; + halfLen1 = box1.hs.y; + } else { + if (colAxis.x*transform1.b + colAxis.y*transform1.f + colAxis.z*transform1.j > 0) { + ty = -ty; + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = box2.hs.y; + } else { + if (colAxis.x*transform2.b + colAxis.y*transform2.f + colAxis.z*transform2.j < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + tz = 0; + halfLen1 = box1.hs.z; + } else { + if (colAxis.x*transform1.c + colAxis.y*transform1.g + colAxis.z*transform1.k > 0) { + tz = -tz; + } + } + if (axisIdx2 == 2) { + z2 = 0; + halfLen2 = box2.hs.z; + } else { + if (colAxis.x*transform2.c + colAxis.y*transform2.g + colAxis.z*transform2.k < 0) { + z2 = -z2; + } + } + // Получаем глобальные координаты средних точек рёбер + var x1:Number = transform1.a*tx + transform1.b*ty + transform1.c*tz + transform1.d; + var y1:Number = transform1.e*tx + transform1.f*ty + transform1.g*tz + transform1.h; + var z1:Number = transform1.i*tx + transform1.j*ty + transform1.k*tz + transform1.l; + tx = x2; + ty = y2; + tz = z2; + x2 = transform2.a*tx + transform2.b*ty + transform2.c*tz + transform2.d; + y2 = transform2.e*tx + transform2.f*ty + transform2.g*tz + transform2.h; + z2 = transform2.i*tx + transform2.j*ty + transform2.k*tz + transform2.l; + + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + tx = x2 - x1; + ty = y2 - y1; + tz = z2 - z1; + var c1:Number = axis10.x*tx + axis10.y*ty + axis10.z*tz; + var c2:Number = axis20.x*tx + axis20.y*ty + axis20.z*tz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cp.pos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cp.pos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение боксов вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box1 + * @param box2 + * @param axis + * @param axisIndex + * @param toBox1 + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box1 + * @param box2 + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox1 + * @return + */ + private function testDerivedAxis(box1:CollisionBox, box2:CollisionBox, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия двух боксов вдоль заданной оси. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param axis ось + * @param vectorToBox1 вектор, соединяющий центр второго бокса с центром первого + * @return величина перекрытия боксов вдоль оси + */ + public function overlapOnAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, vectorToBox1:Vector3):Number { + var m:Matrix4 = box1.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box1.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box1.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box1.hs.z; + if (d < 0) d = -d; + projection += d; + + m = box2.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box2.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box2.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box2.hs.z; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox1.x*axis.x + vectorToBox1.y*axis.y + vectorToBox1.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} + import alternativa.math.Vector3; + + class CollisionPointTmp { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + } diff --git a/0.0.9.0/src/alternativa/physics/collision/colliders/BoxCollider.as b/0.0.9.0/src/alternativa/physics/collision/colliders/BoxCollider.as new file mode 100644 index 0000000..2429bb1 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/colliders/BoxCollider.as @@ -0,0 +1,427 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + /** + * + */ + public class BoxCollider implements ICollider { + + /** + * + */ + public function BoxCollider() { + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + return false; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + protected function getFaceVertsByAxis(hs:Vector3, axisIdx:int, negativeFace:Boolean, result:Vector.):void { + var v:Vector3; + switch (axisIdx) { + case 0: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + } + break; + case 1: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + } + break; + case 2: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + } else { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } + break; + } + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/colliders/BoxPlaneCollider.as b/0.0.9.0/src/alternativa/physics/collision/colliders/BoxPlaneCollider.as new file mode 100644 index 0000000..3bebdc3 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/colliders/BoxPlaneCollider.as @@ -0,0 +1,88 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + public class BoxPlaneCollider implements ICollider { + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var normal:Vector3 = new Vector3(); + + public function BoxPlaneCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var box:RigidBox = body1 as RigidBox; +// var plane:RigidPlane; +// if (box == null) { +// box = body2 as RigidBox; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные координаты вершин бокса +// var sx:Number = box.halfSize.x; +// var sy:Number = box.halfSize.y; +// var sz:Number = box.halfSize.z; +// (verts1[0] as Vector3).reset(-sx, -sy, -sz); +// (verts1[1] as Vector3).reset(sx, -sy, -sz); +// (verts1[2] as Vector3).reset(sx, sy, -sz); +// (verts1[3] as Vector3).reset(-sx, sy, -sz); +// (verts1[4] as Vector3).reset(-sx, -sy, sz); +// (verts1[5] as Vector3).reset(sx, -sy, sz); +// (verts1[6] as Vector3).reset(sx, sy, sz); +// (verts1[7] as Vector3).reset(-sx, sy, sz); +// +// box.transform.transformVectors(verts1, verts2); +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// // Проверяем наличие столкновений с каждой вершиной +// collisionInfo.pcount = 0; +// for (var i:int = 0; i < 8; i++) { +// // Вершина добавляется в список точек столкновения, если лежит под плоскостью +// var dist:Number = (verts2[i] as Vector3).dot(normal); +// if (dist < offset) { +// var cp:ContactPoint; +// if (collisionInfo.pcount == collisionInfo.points.length) { +// cp = new ContactPoint(); +// collisionInfo.points[collisionInfo.pcount] = cp; +// } else { +// cp = collisionInfo.points[collisionInfo.pcount]; +// } +// cp.pos.copy(verts2[i]); +// cp.r1.diff(cp.pos, box.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); +// cp.penetration = offset - dist; +// collisionInfo.pcount++; +// } +// } +// if (collisionInfo.pcount > 0) { +// collisionInfo.body1 = box; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// return true; +// } + return false; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/colliders/BoxRectCollider.as b/0.0.9.0/src/alternativa/physics/collision/colliders/BoxRectCollider.as new file mode 100644 index 0000000..ff5348a --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/colliders/BoxRectCollider.as @@ -0,0 +1,573 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * + */ + public class BoxRectCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + + private var vectorToBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxRectCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, rect, vectorToBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + if (!findEdgesIntersection(box, rect, vectorToBox, int(bestAxisIndex/2), bestAxisIndex%2, contact)) { + return false; + } + } + contact.body1 = box.body; + contact.body2 = rect.body; + + // Хак для танков, чтобы исключить утыкания танков в стыки статических примитивов + contact.normal.x = rect.transform.c; + contact.normal.y = rect.transform.g; + contact.normal.z = rect.transform.k; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + // Вектор из центра прямоугольника в центр бокса + vectorToBox.x = boxTransform.d - rectTransform.d; + vectorToBox.y = boxTransform.h - rectTransform.h; + vectorToBox.z = boxTransform.l - rectTransform.l; + + // Проверка пересечения по нормали прямоугольника + rectTransform.getAxis(2, axis22); + if (!testMainAxis(box, rect, axis22, 0, vectorToBox)) return false; + + // Проверка пересечения по основным осям бокса + boxTransform.getAxis(0, axis10); + if (!testMainAxis(box, rect, axis10, 1, vectorToBox)) return false; + boxTransform.getAxis(1, axis11); + if (!testMainAxis(box, rect, axis11, 2, vectorToBox)) return false; + boxTransform.getAxis(2, axis12); + if (!testMainAxis(box, rect, axis12, 3, vectorToBox)) return false; + + // Получаем направляющие рёбер прямоугольника + rectTransform.getAxis(0, axis20); + rectTransform.getAxis(1, axis21); + + // Проверка производных осей + if (!testDerivedAxis(box, rect, axis10, axis20, 4, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis10, axis21, 5, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis11, axis20, 6, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis11, axis21, 7, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis12, axis20, 8, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis12, axis21, 9, vectorToBox)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта бокса с прямоугольником. + * + * @param box бокс + * @param rect прямоугольник + * @param vectorToBox вектор, направленный из центра прямоугольника в центр бокса + * @param faceAxisIdx индекс оси, идентифицирующей полскость столкновения (грань бокса или полскость прямоугольника) + * @param colInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var pnum:int; + var i:int; + var v:Vector3; + var cp:ContactPoint; + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + var colAxis:Vector3 = contact.normal; + + var negativeFace:Boolean; + var code:int; + + if (faceAxisIdx == 0) { + // Столкновение с плоскостью прямоугольника + + // Проверим положение бокса относительно плоскости прямоугольника + colAxis.x = rectTransform.c; + colAxis.y = rectTransform.g; + colAxis.z = rectTransform.k; + +// var offset:Number = colAxis.x*rectTransform.d + colAxis.y*rectTransform.h + colAxis.z*rectTransform.l; +// if (bbPos.vDot(colAxis) < offset) return false; + + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisIdx = axisIdx; + incidentAxisDot = dot; + } + } + negativeFace = incidentAxisDot > 0; + + code = 1 << (incidentAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + + // Получаем список вершин грани бокса, переводим их в систему координат прямоугольника и выполняем обрезку + // по прямоугольнику. Таким образом получается список потенциальных точек контакта. + boxTransform.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box.hs, incidentAxisIdx, negativeFace, points1); + boxTransform.transformVectorsN(points1, points2, 4); + rectTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByRect(rect.hs); + // Проверяем каждую потенциальную точку на принадлежность нижней полуплоскости прямоугольника и добавляем такие точки в список контактов + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if (v.z < epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = -v.z; + var cpPos:Vector3 = cp.pos; + cpPos.x = rectTransform.a*v.x + rectTransform.b*v.y + rectTransform.c*v.z + rectTransform.d; + cpPos.y = rectTransform.e*v.x + rectTransform.f*v.y + rectTransform.g*v.z + rectTransform.h; + cpPos.z = rectTransform.i*v.x + rectTransform.j*v.y + rectTransform.k*v.z + rectTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } else { + // Столкновение с гранью бокса + faceAxisIdx--; + boxTransform.getAxis(faceAxisIdx, colAxis); + negativeFace = colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z > 0; + + code = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) { + return false; + } + + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (rectTransform.c*colAxis.x + rectTransform.g*colAxis.y + rectTransform.k*colAxis.z < 0) return false; + + getFaceVertsByAxis(rect.hs, 2, false, points1); + rectTransform.transformVectorsN(points1, points2, 4); + boxTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if ((pen = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = pen; + cpPos = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } + return true; + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * + * @param hs + * @return + */ + private function clipByRect(hs:Vector3):int { + var pnum:int = 4; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box первый бокс + * @param rect второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param colInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + boxTransform.getAxis(axisIdx1, axis10); + rectTransform.getAxis(axisIdx2, axis20); + var colAxis:Vector3 = contact.normal; + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону бокса + if (colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + var vx:Number = box.hs.x; + var vy:Number = box.hs.y; + var vz:Number = box.hs.z; + var x2:Number = rect.hs.x; + var y2:Number = rect.hs.y; + var z2:Number = rect.hs.z; + // x + if (axisIdx1 == 0) { + vx = 0; + halfLen1 = box.hs.x; + } else { + if (boxTransform.a*colAxis.x + boxTransform.e*colAxis.y + boxTransform.i*colAxis.z > 0) { + vx = -vx; + if ((box.excludedFaces & 2) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 1) != 0) { + return false; + } + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = rect.hs.x; + } else { + if (rectTransform.a*colAxis.x + rectTransform.e*colAxis.y + rectTransform.i*colAxis.z < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + vy = 0; + halfLen1 = box.hs.y; + } else { + if (boxTransform.b*colAxis.x + boxTransform.f*colAxis.y + boxTransform.j*colAxis.z > 0) { + vy = -vy; + if ((box.excludedFaces & 8) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 4) != 0) { + return false; + } + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = rect.hs.y; + } else { + if (rectTransform.b*colAxis.x + rectTransform.f*colAxis.y + rectTransform.j*colAxis.z < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + vz = 0; + halfLen1 = box.hs.z; + } else { + if (boxTransform.c*colAxis.x + boxTransform.g*colAxis.y + boxTransform.k*colAxis.z > 0) { + vz = -vz; + if ((box.excludedFaces & 32) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 16) != 0) { + return false; + } + } + } + // Получаем глобальные координаты средних точек рёбер + + var x1:Number = boxTransform.a*vx + boxTransform.b*vy + boxTransform.c*vz + boxTransform.d; + var y1:Number = boxTransform.e*vx + boxTransform.f*vy + boxTransform.g*vz + boxTransform.h; + var z1:Number = boxTransform.i*vx + boxTransform.j*vy + boxTransform.k*vz + boxTransform.l; + vx = x2; + vy = y2; + vz = z2; + x2 = rectTransform.a*vx + rectTransform.b*vy + rectTransform.c*vz + rectTransform.d; + y2 = rectTransform.e*vx + rectTransform.f*vy + rectTransform.g*vz + rectTransform.h; + z2 = rectTransform.i*vx + rectTransform.j*vy + rectTransform.k*vz + rectTransform.l; + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + vx = x2 - x1; + vy = y2 - y1; + vz = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cpPos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cpPos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cpPos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var v:Vector3 = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + return true; + } + + /** + * Проверяет пересечение вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box + * @param rect + * @param axis + * @param axisIndex + * @param vectorToBox + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param rect + * @param axis1 + * @param axis2 + * @param axisIndex + * @param vectorToBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, rect:CollisionRect, axis1:Vector3, axis2:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия вдоль заданной оси. + * + * @param box бокс + * @param rect прямоугольник + * @param axis ось + * @param vectorToBox вектор, соединяющий центр прямоугольника с центром бокса + * @return величина перекрытия вдоль оси + */ + public function overlapOnAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, vectorToBox:Vector3):Number { + var m:Matrix4 = box.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box.hs.z; + if (d < 0) d = -d; + projection += d; + + m = rect.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*rect.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*rect.hs.y; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox.x*axis.x + vectorToBox.y*axis.y + vectorToBox.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/colliders/BoxSphereCollider.as b/0.0.9.0/src/alternativa/physics/collision/colliders/BoxSphereCollider.as new file mode 100644 index 0000000..da5e36d --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/colliders/BoxSphereCollider.as @@ -0,0 +1,168 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.math.Vector3; + use namespace altphysics; + /** + * + */ + public class BoxSphereCollider implements ICollider { + + private var center:Vector3 = new Vector3(); + private var closestPt:Vector3 = new Vector3(); + + private var bPos:Vector3 = new Vector3(); + private var sPos:Vector3 = new Vector3(); + + /** + * + */ + public function BoxSphereCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + // Зафиксированно столкновение + contact.body1 = sphere.body; + contact.body2 = box.body; + contact.normal.vCopy(closestPt).vTransformBy4(box.transform).vSubtract(sPos).vNormalize().vReverse(); + contact.pcount = 1; + + var cp:ContactPoint = contact.points[0]; + cp.penetration = sphere.r - Math.sqrt(distSqr); + cp.pos.vCopy(contact.normal).vScale(-sphere.r).vAdd(sPos); + cp.r1.vDiff(cp.pos, sPos); + cp.r2.vDiff(cp.pos, bPos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + + return true; + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/colliders/BoxTriangleCollider.as b/0.0.9.0/src/alternativa/physics/collision/colliders/BoxTriangleCollider.as new file mode 100644 index 0000000..223860f --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/colliders/BoxTriangleCollider.as @@ -0,0 +1,723 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionTriangle; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * + */ + public class BoxTriangleCollider extends BoxCollider { + + public var epsilon:Number = 0.001; + + private var bestAxisIndex:int; + private var minOverlap:Number; + private var toBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var colNormal:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxTriangleCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, tri, toBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + if (!findEdgesIntersection(box, tri, toBox, bestAxisIndex%3, int(bestAxisIndex/3), contact)) return false; + } + + contact.body1 = box.body; + contact.body2 = tri.body; + + // Хак для танков, чтобы исключить утыкания танков в стыки статических примитивов + contact.normal.x = tri.transform.c; + contact.normal.y = tri.transform.g; + contact.normal.z = tri.transform.k; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + toBox.x = boxTransform.d - triTransform.d; + toBox.y = boxTransform.h - triTransform.h; + toBox.z = boxTransform.l - triTransform.l; + + minOverlap = 1e308; + + // Сначала проверяется нормаль треугольника + axis.x = triTransform.c; + axis.y = triTransform.g; + axis.z = triTransform.k; + if (!testMainAxis(box, tri, axis, 0, toBox)) return false; + + // Проверка основных осей бокса + axis10.x = boxTransform.a; + axis10.y = boxTransform.e; + axis10.z = boxTransform.i; + if (!testMainAxis(box, tri, axis10, 1, toBox)) return false; + axis11.x = boxTransform.b; + axis11.y = boxTransform.f; + axis11.z = boxTransform.j; + if (!testMainAxis(box, tri, axis11, 2, toBox)) return false; + axis12.x = boxTransform.c; + axis12.y = boxTransform.g; + axis12.z = boxTransform.k; + if (!testMainAxis(box, tri, axis12, 3, toBox)) return false; + + // Проверка производных осей + // TODO: заменить вычисления векторных произведений инлайнами + var v:Vector3 = tri.e0; + axis20.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis20.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis20.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis20, 4, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis20, 5, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis20, 6, toBox)) return false; + + v = tri.e1; + axis21.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis21.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis21.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis21, 7, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis21, 8, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis21, 9, toBox)) return false; + + v = tri.e2; + axis22.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis22.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis22.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis22, 10, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis22, 11, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis22, 12, toBox)) return false; + + return true; + } + + /** + * Тестирует пересечение примитивов вдоль заданной оси. + * + * @param box бокс + * @param tri треугольник + * @param axis ось + * @param axisIndex индекс оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return true, если примитивы имеют перекрытие проекций вдоль указанной оси, иначе false + */ + private function testMainAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, axisIndex:int, toBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param tri + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, tri:CollisionTriangle, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Рассчитывает величину перекрытия проекций бокса и треугольника на заданную ось. + * + * @param box бокс + * @param tri треугольник + * @param axis единичный направляющий вектор оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return величина перекрытия. Положительное значение указывает на наличие перекрытия, нулевое или отрицательное значение указывает на отсутствие перекрытия. + */ + private function overlapOnAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, toBox:Vector3):Number { + var t:Matrix4 = box.transform; + var projection:Number = (t.a*axis.x + t.e*axis.y + t.i*axis.z)*box.hs.x; + if (projection < 0) projection = -projection; + var d:Number = (t.b*axis.x + t.f*axis.y + t.j*axis.z)*box.hs.y; + projection += d < 0 ? -d : d; + d = (t.c*axis.x + t.g*axis.y + t.k*axis.z)*box.hs.z; + projection += d < 0 ? -d : d; + + var vectorProjection:Number = toBox.x*axis.x + toBox.y*axis.y + toBox.z*axis.z; + // Для оптимизации ось преобразуется в систему треугольника, а не его вершины в мировую систему + t = tri.transform; + var ax:Number = t.a*axis.x + t.e*axis.y + t.i*axis.z; + var ay:Number = t.b*axis.x + t.f*axis.y + t.j*axis.z; + var az:Number = t.c*axis.x + t.g*axis.y + t.k*axis.z; + var max:Number = 0; + if (vectorProjection < 0) { + vectorProjection = -vectorProjection; + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d < max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d < max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d < max) max = d; + max = -max; + } else { + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d > max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d > max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d > max) max = d; + } + + return projection + max - vectorProjection; + } + + /** + * Определяет точки контакта бокса и треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function findFaceContactPoints(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIndex:int, contact:Contact):Boolean { + if (faceAxisIndex == 0) { + // Столкновение с плоскостью треугольника + return getBoxToTriContact(box, tri, toBox, contact); + } else { + // Столкновение с гранью бокса + return getTriToBoxContact(box, tri, toBox, faceAxisIndex, contact); + } + } + + /** + * Определяет точки контакта бокса с плоскостью треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getBoxToTriContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + colNormal.x = triTransform.c; + colNormal.y = triTransform.g; + colNormal.z = triTransform.k; + + var over:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + if (!over) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var incAxisDot:Number = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colNormal.x + axis.y*colNormal.y + axis.z*colNormal.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incAxisDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Обрезка грани + var negativeFace:Boolean = incAxisDot > 0; + + var code:int = 1 << (incFaceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + getFaceVertsByAxis(box.hs, incFaceAxisIdx, negativeFace, points1); + boxTransform.transformVectorsN(points1, points2, 4); + triTransform.transformVectorsInverseN(points2, points1, 4); + var pnum:int = clipByTriangle(tri); + // Среди конечного списка точек определяются лежащие под плоскостью треугольника + var cp:ContactPoint; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points2[i]; + if ((over && v.z < 0) || (!over && v.z > 0)) { + cp = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z + triTransform.d; + cpPos.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z + triTransform.h; + cpPos.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z + triTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = over ? -v.z : v.z; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Оперделяет точки контакта треугольника с гранью бокса. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getTriToBoxContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + faceAxisIdx--; + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + boxTransform.getAxis(faceAxisIdx, colNormal); + var negativeFace:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + + var code:int = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + if (!negativeFace) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + var v:Vector3 = points1[0]; + v.x = tri.v0.x; + v.y = tri.v0.y; + v.z = tri.v0.z; + + v = points1[1]; + v.x = tri.v1.x; + v.y = tri.v1.y; + v.z = tri.v1.z; + + v = points1[2]; + v.x = tri.v2.x; + v.y = tri.v2.y; + v.z = tri.v2.z; + + triTransform.transformVectorsN(points1, points2, 3); + boxTransform.transformVectorsInverseN(points2, points1, 3); + + var pnum:int = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность боксу и добавляем такие точки в список контактов + var penetration:Number; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + v = points1[i]; + penetration = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace); + if (penetration > -epsilon) { + var cp:ContactPoint = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = penetration; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Вычисляет величину проникновения точки в бокс. + * + * @param hs вектор половинных размеров бокса + * @param p точка в системе координат бокса + * @param axisIndex индекс оси + * @param negativeFace если true, проверяется нижняя грань + * @return величина проникновения точки в бокс + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, negativeFace:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (negativeFace) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (negativeFace) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (negativeFace) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, вершины которой заданы в списке points1. Результат сохраняется в этом же списке. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 3; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Обрезает грань прямоугольника сторонами треугольника. Обрезка выполняется в системе координат и в проекции на + * плоскость треугольника. Входные вершины грани в количестве четырёх штук должны находиться в списке points1. + * Конечные вершины сохраняются в списке points2. + * + * @param tri треугольник + * @return количество врешин в конечном списке + */ + private function clipByTriangle(tri:CollisionTriangle):int { + var vnum:int = 4; + vnum = clipByLine(tri.v0, tri.e0, points1, vnum, points2); + if (vnum == 0) return 0; + vnum = clipByLine(tri.v1, tri.e1, points2, vnum, points1); + if (vnum == 0) return 0; + return clipByLine(tri.v2, tri.e2, points1, vnum, points2);; + } + + /** + * Обрезает полигон указанной прямой. + * + * @param linePoint точка на прямой + * @param lineDir единичный направляющий вектор прямой + * @param verticesIn список вершин исходного полигона + * @param vnum количество вершин исходного полигона + * @param verticesOut список, куда будут записаны вершины конечного полигона + * @return количество вершин конечного полигона + */ + private function clipByLine(linePoint:Vector3, lineDir:Vector3, verticesIn:Vector., vnum:int, verticesOut:Vector.):int { + var nx:Number = -lineDir.y; + var ny:Number = lineDir.x; + var offset:Number = linePoint.x*nx + linePoint.y*ny; + var v1:Vector3 = verticesIn[int(vnum - 1)]; + var offset1:Number = v1.x*nx + v1.y*ny; + var t:Number; + var v:Vector3; + var num:int = 0; + for (var i:int = 0; i < vnum; i++) { + var v2:Vector3 = verticesIn[i]; + var offset2:Number = v2.x*nx + v2.y*ny; + if (offset1 < offset) { + // Первая точка ребра во внешней полуплоскости + if (offset2 > offset) { + // Вторая точка ребра во внутренней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } else { + // Первая точка ребра во внутренней полуплоскости. Добавляем её в конечный список. + v = verticesOut[num]; + v.x = v1.x; + v.y = v1.y; + v.z = v1.z; + num++; + if (offset2 < offset) { + // Вторая точка ребра во внешней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } + v1 = v2; + offset1 = offset2; + } + return num; + } + + /** + * + * @param box + * @param tri + * @param toBox + * @param boxAxisIdx + * @param triAxisIdx + * @param contact + */ + private function findEdgesIntersection(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, boxAxisIdx:int, triAxisIdx:int, contact:Contact):Boolean { + // Определение точки и направляющего вектора ребра треугольника + var tmpx1:Number; + var tmpy1:Number; + var tmpz1:Number; + var tmpx2:Number; + var tmpy2:Number; + var tmpz2:Number; + switch (triAxisIdx) { + case 0: + tmpx1 = tri.e0.x; + tmpy1 = tri.e0.y; + tmpz1 = tri.e0.z; + tmpx2 = tri.v0.x; + tmpy2 = tri.v0.y; + tmpz2 = tri.v0.z; + break; + case 1: + tmpx1 = tri.e1.x; + tmpy1 = tri.e1.y; + tmpz1 = tri.e1.z; + tmpx2 = tri.v1.x; + tmpy2 = tri.v1.y; + tmpz2 = tri.v1.z; + break; + case 2: + tmpx1 = tri.e2.x; + tmpy1 = tri.e2.y; + tmpz1 = tri.e2.z; + tmpx2 = tri.v2.x; + tmpy2 = tri.v2.y; + tmpz2 = tri.v2.z; + break; + } + var triTransform:Matrix4 = tri.transform; + axis20.x = triTransform.a*tmpx1 + triTransform.b*tmpy1 + triTransform.c*tmpz1; + axis20.y = triTransform.e*tmpx1 + triTransform.f*tmpy1 + triTransform.g*tmpz1; + axis20.z = triTransform.i*tmpx1 + triTransform.j*tmpy1 + triTransform.k*tmpz1; + var x2:Number = triTransform.a*tmpx2 + triTransform.b*tmpy2 + triTransform.c*tmpz2 + triTransform.d; + var y2:Number = triTransform.e*tmpx2 + triTransform.f*tmpy2 + triTransform.g*tmpz2 + triTransform.h; + var z2:Number = triTransform.i*tmpx2 + triTransform.j*tmpy2 + triTransform.k*tmpz2 + triTransform.l; + + // Определение нормали контакта, точки и направляющего вектора ребра бокса + var boxTransform:Matrix4 = box.transform; + boxTransform.getAxis(boxAxisIdx, axis10); + + // Нормаль контакта + var v:Vector3 = contact.normal; + v.x = axis10.y*axis20.z - axis10.z*axis20.y; + v.y = axis10.z*axis20.x - axis10.x*axis20.z; + v.z = axis10.x*axis20.y - axis10.y*axis20.x; + k = 1/Math.sqrt(v.x*v.x + v.y*v.y + v.z*v.z); + v.x *= k; + v.y *= k; + v.z *= k; + // Разворот нормали в сторону бокса + if (v.x*toBox.x + v.y*toBox.y + v.z*toBox.z < 0) { + v.x = -v.x; + v.y = -v.y; + v.z = -v.z; + } + + var boxHalfLen:Number; + tmpx1 = box.hs.x; + tmpy1 = box.hs.y; + tmpz1 = box.hs.z; + // X + if (boxAxisIdx == 0) { + tmpx1 = 0; + boxHalfLen = box.hs.x; + } else { + if (boxTransform.a*v.x + boxTransform.e*v.y + boxTransform.i*v.z > 0) { + tmpx1 = -tmpx1; + if ((box.excludedFaces & 2) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 1) != 0) { + return false; + } + } + } + // Y + if (boxAxisIdx == 1) { + tmpy1 = 0; + boxHalfLen = box.hs.y; + } else { + if (boxTransform.b*v.x + boxTransform.f*v.y + boxTransform.j*v.z > 0) { + tmpy1 = -tmpy1; + if ((box.excludedFaces & 8) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 4) != 0) { + return false; + } + } + } + // Z + if (boxAxisIdx == 2) { + tmpz1 = 0; + boxHalfLen = box.hs.z; + } else { + if (boxTransform.c*v.x + boxTransform.g*v.y + boxTransform.k*v.z > 0) { + tmpz1 = -tmpz1; + if ((box.excludedFaces & 32) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 16) != 0) { + return false; + } + } + } + var x1:Number = boxTransform.a*tmpx1 + boxTransform.b*tmpy1 + boxTransform.c*tmpz1 + boxTransform.d; + var y1:Number = boxTransform.e*tmpx1 + boxTransform.f*tmpy1 + boxTransform.g*tmpz1 + boxTransform.h; + var z1:Number = boxTransform.i*tmpx1 + boxTransform.j*tmpy1 + boxTransform.k*tmpz1 + boxTransform.l; + + // Находим точку пересечения рёбер, решая систему уравнений + // axis10 - направляющий вектор ребра бокса + // x1, y1, z1 - средняя точка ребра бокса + // axis20 - направляющий вектор ребра треугольника + // x2, y2, z2 - начальная точка ребра треугольника + var k:Number = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + var vx:Number = x2 - x1; + var vy:Number = y2 - y1; + var vz:Number = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + + // Запись данных о контакте + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + v = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + v.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + v.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + v.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = v.x - boxTransform.d; + r.y = v.y - boxTransform.h; + r.z = v.z - boxTransform.l; + r = cp.r2; + r.x = v.x - triTransform.d; + r.y = v.y - triTransform.h; + r.z = v.z - triTransform.l; + return true; + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/colliders/SpherePlaneCollider.as b/0.0.9.0/src/alternativa/physics/collision/colliders/SpherePlaneCollider.as new file mode 100644 index 0000000..30cd55f --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/colliders/SpherePlaneCollider.as @@ -0,0 +1,68 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + /** + * + */ + public class SpherePlaneCollider implements ICollider { + + private var normal:Vector3 = new Vector3(); + + /** + * + */ + public function SpherePlaneCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var sphere:RigidSphere = body1 as RigidSphere; +// var plane:RigidPlane; +// if (sphere == null) { +// sphere = body2 as RigidSphere; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// +// var dist:Number = sphere.state.pos.dot(normal) - offset; +// if (dist > sphere.r) return false; +// +// collisionInfo.body1 = sphere; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// collisionInfo.pcount = 1; +// +// var cp:ContactPoint = collisionInfo.points[0]; +// cp.penetration = sphere.r - dist; +// cp.pos.copy(normal).reverse().scale(sphere.r).add(sphere.state.pos); +// cp.r1.diff(cp.pos, sphere.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/colliders/SphereSphereCollider.as b/0.0.9.0/src/alternativa/physics/collision/colliders/SphereSphereCollider.as new file mode 100644 index 0000000..3332f3c --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/colliders/SphereSphereCollider.as @@ -0,0 +1,70 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.math.Vector3; + use namespace altphysics; + + public class SphereSphereCollider implements ICollider { + + private var p1:Vector3 = new Vector3(); + private var p2:Vector3 = new Vector3(); + + public function SphereSphereCollider() { + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var s1:CollisionSphere; + var s2:CollisionSphere; + if (prim1.body != null) { + s1 = prim1 as CollisionSphere; + s2 = prim2 as CollisionSphere; + } else { + s1 = prim2 as CollisionSphere; + s2 = prim1 as CollisionSphere; + } + + s1.transform.getAxis(3, p1); + s2.transform.getAxis(3, p2); + var dx:Number = p1.x - p2.x; + var dy:Number = p1.y - p2.y; + var dz:Number = p1.z - p2.z; + var len:Number = dx*dx + dy*dy + dz*dz; + var sum:Number = s1.r + s2.r; + if (len > sum*sum) return false; + len = Math.sqrt(len); + dx /= len; + dy /= len; + dz /= len; + + contact.body1 = s1.body; + contact.body2 = s2.body; + contact.normal.x = dx; + contact.normal.y = dy; + contact.normal.z = dz; + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = sum - len; + cp.pos.x = p1.x - dx*s1.r; + cp.pos.y = p1.y - dy*s1.r; + cp.pos.z = p1.z - dz*s1.r; + cp.r1.vDiff(cp.pos, p1); + cp.r2.vDiff(cp.pos, p2); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/primitives/.svn/all-wcprops b/0.0.9.0/src/alternativa/physics/collision/primitives/.svn/all-wcprops new file mode 100644 index 0000000..e3c4b40 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/primitives/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 124 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/collision/primitives +END +CollisionTriangle.as +K 25 +svn:wc:ra_dav:version-url +V 145 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/collision/primitives/CollisionTriangle.as +END +CollisionSphere.as +K 25 +svn:wc:ra_dav:version-url +V 143 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/collision/primitives/CollisionSphere.as +END +CollisionBox.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/collision/primitives/CollisionBox.as +END +CollisionRect.as +K 25 +svn:wc:ra_dav:version-url +V 141 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/collision/primitives/CollisionRect.as +END diff --git a/0.0.9.0/src/alternativa/physics/collision/primitives/.svn/entries b/0.0.9.0/src/alternativa/physics/collision/primitives/.svn/entries new file mode 100644 index 0000000..4c8a49a --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/primitives/.svn/entries @@ -0,0 +1,76 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/collision/primitives +http://svndev.alternativaplatform.com + + + +2009-11-24T08:44:15.941480Z +23758 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +CollisionTriangle.as +file + + + + +2010-10-28T04:32:43.000000Z +23ebb96cda309378ce15ead4cf865446 +2009-11-24T08:44:15.941480Z +23758 +mike + +CollisionSphere.as +file + + + + +2010-10-28T04:32:43.000000Z +335b6b72039123d6d54fee4d6b17f74e +2009-11-24T08:44:15.941480Z +23758 +mike + +CollisionBox.as +file + + + + +2010-10-28T04:32:43.000000Z +65e81a3d0034265e568d549306df1406 +2009-11-24T08:44:15.941480Z +23758 +mike + +CollisionRect.as +file + + + + +2010-10-28T04:32:43.000000Z +91b913679dca5ae4baabafb09c5aa051 +2009-11-24T08:44:15.941480Z +23758 +mike + diff --git a/0.0.9.0/src/alternativa/physics/collision/primitives/.svn/format b/0.0.9.0/src/alternativa/physics/collision/primitives/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/primitives/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base b/0.0.9.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base new file mode 100644 index 0000000..1febfb4 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base @@ -0,0 +1,204 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * Ориентированный бокс. + */ + public class CollisionBox extends CollisionPrimitive { + + // Половинные размеры вдоль каждой из осей + public var hs:Vector3 = new Vector3(); + + public var excludedFaces:int; + + /** + * @param hs + * @param collisionGroup + */ + public function CollisionBox(hs:Vector3, collisionGroup:int) { + super(BOX, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + var t:Matrix4 = transform; + + var xx:Number; + var yy:Number; + var zz:Number; + + xx = t.a < 0 ? -t.a : t.a; + yy = t.b < 0 ? -t.b : t.b; + zz = t.c < 0 ? -t.c : t.c; + aabb.maxX = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minX = -aabb.maxX; + + xx = t.e < 0 ? -t.e : t.e; + yy = t.f < 0 ? -t.f : t.f; + zz = t.g < 0 ? -t.g : t.g; + aabb.maxY = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minY = -aabb.maxY; + + xx = t.i < 0 ? -t.i : t.i; + yy = t.j < 0 ? -t.j : t.j; + zz = t.k < 0 ? -t.k : t.k; + aabb.maxZ = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var box:CollisionBox = source as CollisionBox; + if (box == null) return this; + super.copyFrom(box); + hs.vCopy(box.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionBox(hs, collisionGroup); + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + var tMin:Number = -1; + var tMax:Number = 1e308; + var t1:Number; + var t2:Number; + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + // X + if (vx < epsilon && vx > -epsilon) { + if (ox < -hs.x || ox > hs.x) return -1; + } else { + t1 = (-hs.x - ox)/vx; + t2 = (hs.x - ox)/vx; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.x = -1; + normal.y = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.x = 1; + normal.y = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Y + if (vy < epsilon && vy > -epsilon) { + if (oy < -hs.y || oy > hs.y) return -1; + } else { + t1 = (-hs.y - oy)/vy; + t2 = (hs.y - oy)/vy; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.y = -1; + normal.x = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.y = 1; + normal.x = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Z + if (vz < epsilon && vz > -epsilon) { + if (oz < -hs.z || oz > hs.z) return -1; + } else { + t1 = (-hs.z - oz)/vz; + t2 = (hs.z - oz)/vz; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.z = -1; + normal.x = normal.y = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.z = 1; + normal.x = normal.y = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Перевод нормали в мировую систему координат + // Inlined normal.vDeltaTransformBy4(transform); + vx = normal.x; + vy = normal.y; + vz = normal.z; + normal.x = transform.a*vx + transform.b*vy + transform.c*vz; + normal.y = transform.e*vx + transform.f*vy + transform.g*vz; + normal.z = transform.i*vx + transform.j*vy + transform.k*vz; + + return tMin; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionBox hs=" + hs + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base b/0.0.9.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base new file mode 100644 index 0000000..7a33b79 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base @@ -0,0 +1,143 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * Ориентированный прямоугольник. Задаётся половинами размеров вдоль осей X (ширина) и Y (длина) локальной системы + * координат примитива. Таким образом, прямоугольник лежит в плоскости XY, его стороны параллельны осям этой + * плоскости, а нормаль направлена вдоль локальной оси Z. + * + * Прямоугольник может быть одно- или двусторонним. В случае одностороннего прямоугольника, + * столкновения с внутренней стороны не регистрируются. + */ + public class CollisionRect extends CollisionPrimitive { + // Половинные размеры прямоугольника вдоль осей X и Y. Нормаль направлена вдоль оси Z. + public var hs:Vector3 = new Vector3(); + // Флаг указывает, является примитив одно- или двусторонним + public var twoSided:Boolean = true; + + // Малое значение. Используется для указания фиктивной высоты примитива и в функции определения пересечения с сегментом. + private static const EPSILON:Number = 0.005; + + /** + * Создаёт новый экземпляр примитива. + * + * @param hs половинные размерв прямоугольника вдоль осей X и Y. Значение z игнорируется. + * @param collisionGroup группа примитива + */ + public function CollisionRect(hs:Vector3, collisionGroup:int) { + super(RECT, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * Расчитывает ограничивающий бокс прямоугольника. Для избежания проблем высота примитива принимается равной + * не нулю, а малому значению. + * + * @return + */ + override public function calculateAABB():BoundBox { + // Баунд бокс прямоугольника имеет минимальную высоту, отличную от нуля во избежание проблем с построением kd-дерева + var t:Matrix4 = transform; + var xx:Number = t.a < 0 ? -t.a : t.a; + var yy:Number = t.b < 0 ? -t.b : t.b; + var zz:Number = t.c < 0 ? -t.c : t.c; + aabb.maxX = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minX = -aabb.maxX; + + xx = t.e < 0 ? -t.e : t.e; + yy = t.f < 0 ? -t.f : t.f; + zz = t.g < 0 ? -t.g : t.g; + aabb.maxY = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minY = -aabb.maxY; + + xx = t.i < 0 ? -t.i : t.i; + yy = t.j < 0 ? -t.j : t.j; + zz = t.k < 0 ? -t.k : t.k; + aabb.maxZ = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var rect:CollisionRect = source as CollisionRect; + if (rect == null) return this; + super.copyFrom(rect); + hs.vCopy(rect.hs); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionRect hs=" + hs + "]"; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionRect(hs, collisionGroup); + } + + /** + * @param origin + * @param vector + * @param collisionGroup + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + + // Проверка параллельности сегмента и плоскости примитива + if (vz > -threshold && vz < threshold) return -1; + var t:Number = -oz/vz; + if (t < 0) return -1; + // Проверка вхождения точки пересечения в прямоугольник + ox += vx*t; + oy += vy*t; + oz = 0; + if (ox < (-hs.x - threshold) || ox > (hs.x + threshold) || oy < (-hs.y - threshold) || oy > (hs.y + threshold)) return -1; + + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + return t; + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base b/0.0.9.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base new file mode 100644 index 0000000..b912a5b --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base @@ -0,0 +1,81 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Vector3; + use namespace altphysics; + + + /** + * Сфера. + */ + public class CollisionSphere extends CollisionPrimitive { + + // Радиус сферы + public var r:Number = 0; + + /** + * + * @param r + * @param collisionGroup + */ + public function CollisionSphere(r:Number, collisionGroup:int) { + super(SPHERE, collisionGroup); + this.r = r; + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + aabb.maxX = transform.d + r; + aabb.minX = transform.d - r; + + aabb.maxY = transform.h + r; + aabb.minY = transform.h - r; + + aabb.maxZ = transform.l + r; + aabb.minZ = transform.l - r; + + return aabb; + } + + /** + * @param origin + * @param vector + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var px:Number = origin.x - transform.d; + var py:Number = origin.y - transform.h; + var pz:Number = origin.z - transform.l; + var k:Number = vector.x*px + vector.y*py + vector.z*pz; + if (k > 0) return -1; + var a:Number = vector.x*vector.x + vector.y*vector.y + vector.z*vector.z; + var D:Number = k*k - a*(px*px + py*py + pz*pz - r*r); + if (D < 0) return -1; + return -(k + Math.sqrt(D))/a; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var sphere:CollisionSphere = source as CollisionSphere; + if (sphere == null) return this; + super.copyFrom(sphere); + r = sphere.r; + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionSphere(r, collisionGroup); + } + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base b/0.0.9.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base new file mode 100644 index 0000000..bdc3166 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base @@ -0,0 +1,244 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * Примитив треугольник. + */ + public class CollisionTriangle extends CollisionPrimitive { + + public var v0:Vector3 = new Vector3(); + public var v1:Vector3 = new Vector3(); + public var v2:Vector3 = new Vector3(); + + public var e0:Vector3 = new Vector3(); + public var e1:Vector3 = new Vector3(); + public var e2:Vector3 = new Vector3(); + +// public var len0:Number; +// public var len1:Number; +// public var len2:Number; +// + /** + * + * @param v0 + * @param v1 + * @param v2 + * @param collisionGroup + */ + public function CollisionTriangle(v0:Vector3, v1:Vector3, v2:Vector3, collisionGroup:int) { + super(TRIANGLE, collisionGroup); + initVertices(v0, v1, v2); + } + + /** + * Рассчитывает AABB примитива. + * + * @return ссылка на свой AABB + */ + override public function calculateAABB():BoundBox { + var epsilon:Number = 0.005; + var a:Number; + var b:Number; + var eps_c:Number = epsilon*transform.c; + var eps_g:Number = epsilon*transform.g; + var eps_k:Number = epsilon*transform.k; + + // Вершина 0 + // Ось X + a = v0.x*transform.a + v0.y*transform.b; + aabb.minX = aabb.maxX = a + eps_c; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v0.x*transform.e + v0.y*transform.f; + aabb.minY = aabb.maxY = a + eps_g; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v0.x*transform.i + v0.y*transform.j; + aabb.minZ = aabb.maxZ = a + eps_k; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 1 + // Ось X + a = v1.x*transform.a + v1.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v1.x*transform.e + v1.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v1.x*transform.i + v1.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 2 + // Ось X + a = v2.x*transform.a + v2.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v2.x*transform.e + v2.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v2.x*transform.i + v2.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + aabb.minX += transform.d; + aabb.maxX += transform.d; + + aabb.minY += transform.h; + aabb.maxY += transform.h; + + aabb.minZ += transform.l; + aabb.maxZ += transform.l; + + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + // Луч трансформируется в систему координат примитива, затем проверяется пересечение + + var vz:Number = vector.x*transform.c + vector.y*transform.g + vector.z*transform.k; + // Если луч параллелен плоскости птимитива, то пересечения нет + if (vz < epsilon && vz > -epsilon) return -1; + + var tx:Number = origin.x - transform.d; + var ty:Number = origin.y - transform.h; + var tz:Number = origin.z - transform.l; + + var oz:Number = tx*transform.c + ty*transform.g + tz*transform.k; + var t:Number = -oz/vz; + if (t < 0) return -1; + + var ox:Number = tx*transform.a + ty*transform.e + tz*transform.i; + var oy:Number = tx*transform.b + ty*transform.f + tz*transform.j; + + tx = ox + t*(vector.x*transform.a + vector.y*transform.e + vector.z*transform.i); + ty = oy + t*(vector.x*transform.b + vector.y*transform.f + vector.z*transform.j); + tz = oz + t*vz; + + // Проверка вхождения точки в треугольник + if ((e0.x*(ty - v0.y) - e0.y*(tx - v0.x) < 0) || (e1.x*(ty - v1.y) - e1.y*(tx - v1.x) < 0) || (e2.x*(ty - v2.y) - e2.y*(tx - v2.x) < 0)) return -1; + + // Запись нормали + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + + return t; + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + super.copyFrom(source); + var tri:CollisionTriangle = source as CollisionTriangle; + if (tri != null) { + v0.vCopy(tri.v0); + v1.vCopy(tri.v1); + v2.vCopy(tri.v2); + + e0.vCopy(tri.e0); + e1.vCopy(tri.e1); + e2.vCopy(tri.e2); + +// len0 = tri.len0; +// len1 = tri.len1; +// len2 = tri.len2; + } + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionTriangle v0=" + v0 + ", v1=" + v1 + ", v2=" + v2 + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionTriangle(v0, v1, v2, collisionGroup); + } + + /** + * + * @param v0 + * @param v1 + * @param v2 + */ + private function initVertices(v0:Vector3, v1:Vector3, v2:Vector3):void { + this.v0.vCopy(v0); + this.v1.vCopy(v1); + this.v2.vCopy(v2); + + e0.vDiff(v1, v0); +// len0 = e0.vLength(); + e0.vNormalize(); + + e1.vDiff(v2, v1); +// len1 = e1.vLength(); + e1.vNormalize(); + + e2.vDiff(v0, v2); +// len2 = e2.vLength(); + e2.vNormalize(); + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/primitives/CollisionBox.as b/0.0.9.0/src/alternativa/physics/collision/primitives/CollisionBox.as new file mode 100644 index 0000000..1febfb4 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/primitives/CollisionBox.as @@ -0,0 +1,204 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * Ориентированный бокс. + */ + public class CollisionBox extends CollisionPrimitive { + + // Половинные размеры вдоль каждой из осей + public var hs:Vector3 = new Vector3(); + + public var excludedFaces:int; + + /** + * @param hs + * @param collisionGroup + */ + public function CollisionBox(hs:Vector3, collisionGroup:int) { + super(BOX, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + var t:Matrix4 = transform; + + var xx:Number; + var yy:Number; + var zz:Number; + + xx = t.a < 0 ? -t.a : t.a; + yy = t.b < 0 ? -t.b : t.b; + zz = t.c < 0 ? -t.c : t.c; + aabb.maxX = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minX = -aabb.maxX; + + xx = t.e < 0 ? -t.e : t.e; + yy = t.f < 0 ? -t.f : t.f; + zz = t.g < 0 ? -t.g : t.g; + aabb.maxY = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minY = -aabb.maxY; + + xx = t.i < 0 ? -t.i : t.i; + yy = t.j < 0 ? -t.j : t.j; + zz = t.k < 0 ? -t.k : t.k; + aabb.maxZ = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var box:CollisionBox = source as CollisionBox; + if (box == null) return this; + super.copyFrom(box); + hs.vCopy(box.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionBox(hs, collisionGroup); + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + var tMin:Number = -1; + var tMax:Number = 1e308; + var t1:Number; + var t2:Number; + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + // X + if (vx < epsilon && vx > -epsilon) { + if (ox < -hs.x || ox > hs.x) return -1; + } else { + t1 = (-hs.x - ox)/vx; + t2 = (hs.x - ox)/vx; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.x = -1; + normal.y = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.x = 1; + normal.y = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Y + if (vy < epsilon && vy > -epsilon) { + if (oy < -hs.y || oy > hs.y) return -1; + } else { + t1 = (-hs.y - oy)/vy; + t2 = (hs.y - oy)/vy; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.y = -1; + normal.x = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.y = 1; + normal.x = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Z + if (vz < epsilon && vz > -epsilon) { + if (oz < -hs.z || oz > hs.z) return -1; + } else { + t1 = (-hs.z - oz)/vz; + t2 = (hs.z - oz)/vz; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.z = -1; + normal.x = normal.y = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.z = 1; + normal.x = normal.y = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Перевод нормали в мировую систему координат + // Inlined normal.vDeltaTransformBy4(transform); + vx = normal.x; + vy = normal.y; + vz = normal.z; + normal.x = transform.a*vx + transform.b*vy + transform.c*vz; + normal.y = transform.e*vx + transform.f*vy + transform.g*vz; + normal.z = transform.i*vx + transform.j*vy + transform.k*vz; + + return tMin; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionBox hs=" + hs + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/primitives/CollisionRect.as b/0.0.9.0/src/alternativa/physics/collision/primitives/CollisionRect.as new file mode 100644 index 0000000..7a33b79 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/primitives/CollisionRect.as @@ -0,0 +1,143 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * Ориентированный прямоугольник. Задаётся половинами размеров вдоль осей X (ширина) и Y (длина) локальной системы + * координат примитива. Таким образом, прямоугольник лежит в плоскости XY, его стороны параллельны осям этой + * плоскости, а нормаль направлена вдоль локальной оси Z. + * + * Прямоугольник может быть одно- или двусторонним. В случае одностороннего прямоугольника, + * столкновения с внутренней стороны не регистрируются. + */ + public class CollisionRect extends CollisionPrimitive { + // Половинные размеры прямоугольника вдоль осей X и Y. Нормаль направлена вдоль оси Z. + public var hs:Vector3 = new Vector3(); + // Флаг указывает, является примитив одно- или двусторонним + public var twoSided:Boolean = true; + + // Малое значение. Используется для указания фиктивной высоты примитива и в функции определения пересечения с сегментом. + private static const EPSILON:Number = 0.005; + + /** + * Создаёт новый экземпляр примитива. + * + * @param hs половинные размерв прямоугольника вдоль осей X и Y. Значение z игнорируется. + * @param collisionGroup группа примитива + */ + public function CollisionRect(hs:Vector3, collisionGroup:int) { + super(RECT, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * Расчитывает ограничивающий бокс прямоугольника. Для избежания проблем высота примитива принимается равной + * не нулю, а малому значению. + * + * @return + */ + override public function calculateAABB():BoundBox { + // Баунд бокс прямоугольника имеет минимальную высоту, отличную от нуля во избежание проблем с построением kd-дерева + var t:Matrix4 = transform; + var xx:Number = t.a < 0 ? -t.a : t.a; + var yy:Number = t.b < 0 ? -t.b : t.b; + var zz:Number = t.c < 0 ? -t.c : t.c; + aabb.maxX = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minX = -aabb.maxX; + + xx = t.e < 0 ? -t.e : t.e; + yy = t.f < 0 ? -t.f : t.f; + zz = t.g < 0 ? -t.g : t.g; + aabb.maxY = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minY = -aabb.maxY; + + xx = t.i < 0 ? -t.i : t.i; + yy = t.j < 0 ? -t.j : t.j; + zz = t.k < 0 ? -t.k : t.k; + aabb.maxZ = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var rect:CollisionRect = source as CollisionRect; + if (rect == null) return this; + super.copyFrom(rect); + hs.vCopy(rect.hs); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionRect hs=" + hs + "]"; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionRect(hs, collisionGroup); + } + + /** + * @param origin + * @param vector + * @param collisionGroup + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + + // Проверка параллельности сегмента и плоскости примитива + if (vz > -threshold && vz < threshold) return -1; + var t:Number = -oz/vz; + if (t < 0) return -1; + // Проверка вхождения точки пересечения в прямоугольник + ox += vx*t; + oy += vy*t; + oz = 0; + if (ox < (-hs.x - threshold) || ox > (hs.x + threshold) || oy < (-hs.y - threshold) || oy > (hs.y + threshold)) return -1; + + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + return t; + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/primitives/CollisionSphere.as b/0.0.9.0/src/alternativa/physics/collision/primitives/CollisionSphere.as new file mode 100644 index 0000000..b912a5b --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/primitives/CollisionSphere.as @@ -0,0 +1,81 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Vector3; + use namespace altphysics; + + + /** + * Сфера. + */ + public class CollisionSphere extends CollisionPrimitive { + + // Радиус сферы + public var r:Number = 0; + + /** + * + * @param r + * @param collisionGroup + */ + public function CollisionSphere(r:Number, collisionGroup:int) { + super(SPHERE, collisionGroup); + this.r = r; + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + aabb.maxX = transform.d + r; + aabb.minX = transform.d - r; + + aabb.maxY = transform.h + r; + aabb.minY = transform.h - r; + + aabb.maxZ = transform.l + r; + aabb.minZ = transform.l - r; + + return aabb; + } + + /** + * @param origin + * @param vector + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var px:Number = origin.x - transform.d; + var py:Number = origin.y - transform.h; + var pz:Number = origin.z - transform.l; + var k:Number = vector.x*px + vector.y*py + vector.z*pz; + if (k > 0) return -1; + var a:Number = vector.x*vector.x + vector.y*vector.y + vector.z*vector.z; + var D:Number = k*k - a*(px*px + py*py + pz*pz - r*r); + if (D < 0) return -1; + return -(k + Math.sqrt(D))/a; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var sphere:CollisionSphere = source as CollisionSphere; + if (sphere == null) return this; + super.copyFrom(sphere); + r = sphere.r; + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionSphere(r, collisionGroup); + } + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/primitives/CollisionTriangle.as b/0.0.9.0/src/alternativa/physics/collision/primitives/CollisionTriangle.as new file mode 100644 index 0000000..bdc3166 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/primitives/CollisionTriangle.as @@ -0,0 +1,244 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * Примитив треугольник. + */ + public class CollisionTriangle extends CollisionPrimitive { + + public var v0:Vector3 = new Vector3(); + public var v1:Vector3 = new Vector3(); + public var v2:Vector3 = new Vector3(); + + public var e0:Vector3 = new Vector3(); + public var e1:Vector3 = new Vector3(); + public var e2:Vector3 = new Vector3(); + +// public var len0:Number; +// public var len1:Number; +// public var len2:Number; +// + /** + * + * @param v0 + * @param v1 + * @param v2 + * @param collisionGroup + */ + public function CollisionTriangle(v0:Vector3, v1:Vector3, v2:Vector3, collisionGroup:int) { + super(TRIANGLE, collisionGroup); + initVertices(v0, v1, v2); + } + + /** + * Рассчитывает AABB примитива. + * + * @return ссылка на свой AABB + */ + override public function calculateAABB():BoundBox { + var epsilon:Number = 0.005; + var a:Number; + var b:Number; + var eps_c:Number = epsilon*transform.c; + var eps_g:Number = epsilon*transform.g; + var eps_k:Number = epsilon*transform.k; + + // Вершина 0 + // Ось X + a = v0.x*transform.a + v0.y*transform.b; + aabb.minX = aabb.maxX = a + eps_c; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v0.x*transform.e + v0.y*transform.f; + aabb.minY = aabb.maxY = a + eps_g; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v0.x*transform.i + v0.y*transform.j; + aabb.minZ = aabb.maxZ = a + eps_k; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 1 + // Ось X + a = v1.x*transform.a + v1.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v1.x*transform.e + v1.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v1.x*transform.i + v1.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 2 + // Ось X + a = v2.x*transform.a + v2.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v2.x*transform.e + v2.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v2.x*transform.i + v2.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + aabb.minX += transform.d; + aabb.maxX += transform.d; + + aabb.minY += transform.h; + aabb.maxY += transform.h; + + aabb.minZ += transform.l; + aabb.maxZ += transform.l; + + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + // Луч трансформируется в систему координат примитива, затем проверяется пересечение + + var vz:Number = vector.x*transform.c + vector.y*transform.g + vector.z*transform.k; + // Если луч параллелен плоскости птимитива, то пересечения нет + if (vz < epsilon && vz > -epsilon) return -1; + + var tx:Number = origin.x - transform.d; + var ty:Number = origin.y - transform.h; + var tz:Number = origin.z - transform.l; + + var oz:Number = tx*transform.c + ty*transform.g + tz*transform.k; + var t:Number = -oz/vz; + if (t < 0) return -1; + + var ox:Number = tx*transform.a + ty*transform.e + tz*transform.i; + var oy:Number = tx*transform.b + ty*transform.f + tz*transform.j; + + tx = ox + t*(vector.x*transform.a + vector.y*transform.e + vector.z*transform.i); + ty = oy + t*(vector.x*transform.b + vector.y*transform.f + vector.z*transform.j); + tz = oz + t*vz; + + // Проверка вхождения точки в треугольник + if ((e0.x*(ty - v0.y) - e0.y*(tx - v0.x) < 0) || (e1.x*(ty - v1.y) - e1.y*(tx - v1.x) < 0) || (e2.x*(ty - v2.y) - e2.y*(tx - v2.x) < 0)) return -1; + + // Запись нормали + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + + return t; + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + super.copyFrom(source); + var tri:CollisionTriangle = source as CollisionTriangle; + if (tri != null) { + v0.vCopy(tri.v0); + v1.vCopy(tri.v1); + v2.vCopy(tri.v2); + + e0.vCopy(tri.e0); + e1.vCopy(tri.e1); + e2.vCopy(tri.e2); + +// len0 = tri.len0; +// len1 = tri.len1; +// len2 = tri.len2; + } + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionTriangle v0=" + v0 + ", v1=" + v1 + ", v2=" + v2 + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionTriangle(v0, v1, v2, collisionGroup); + } + + /** + * + * @param v0 + * @param v1 + * @param v2 + */ + private function initVertices(v0:Vector3, v1:Vector3, v2:Vector3):void { + this.v0.vCopy(v0); + this.v1.vCopy(v1); + this.v2.vCopy(v2); + + e0.vDiff(v1, v0); +// len0 = e0.vLength(); + e0.vNormalize(); + + e1.vDiff(v2, v1); +// len1 = e1.vLength(); + e1.vNormalize(); + + e2.vDiff(v0, v2); +// len2 = e2.vLength(); + e2.vNormalize(); + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/types/.svn/all-wcprops b/0.0.9.0/src/alternativa/physics/collision/types/.svn/all-wcprops new file mode 100644 index 0000000..7326f7d --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/types/.svn/all-wcprops @@ -0,0 +1,23 @@ +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/collision/types +END +BoundBox.as +K 25 +svn:wc:ra_dav:version-url +V 131 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/collision/types/BoundBox.as +END +RayIntersection.as +K 25 +svn:wc:ra_dav:version-url +V 138 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/collision/types/RayIntersection.as +END +Ray.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/collision/types/Ray.as +END diff --git a/0.0.9.0/src/alternativa/physics/collision/types/.svn/entries b/0.0.9.0/src/alternativa/physics/collision/types/.svn/entries new file mode 100644 index 0000000..0774ac8 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/types/.svn/entries @@ -0,0 +1,64 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/collision/types +http://svndev.alternativaplatform.com + + + +2009-11-24T08:44:15.941480Z +23758 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +BoundBox.as +file + + + + +2010-10-28T04:32:43.000000Z +d92d4ab3dde698feb674fb9488f59678 +2009-09-09T14:02:48.134744Z +19633 +mike + +RayIntersection.as +file + + + + +2010-10-28T04:32:43.000000Z +88c0836ead77b8eb0841f11159dfb836 +2009-11-24T08:44:15.941480Z +23758 +mike + +Ray.as +file + + + + +2010-10-28T04:32:43.000000Z +c65cc8bfae377f8f0520b5c79c8b3ab9 +2009-11-24T08:44:15.941480Z +23758 +mike + diff --git a/0.0.9.0/src/alternativa/physics/collision/types/.svn/format b/0.0.9.0/src/alternativa/physics/collision/types/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/types/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.0/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base b/0.0.9.0/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base new file mode 100644 index 0000000..b4d5278 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base @@ -0,0 +1,75 @@ +package alternativa.physics.collision.types { + + /** + * Ограничивающий параллелепипед. + */ + public class BoundBox { + + public var minX:Number = 1e308; + public var minY:Number = 1e308; + public var minZ:Number = 1e308; + public var maxX:Number = -1e308; + public var maxY:Number = -1e308; + public var maxZ:Number = -1e308; + + public function setSize(minX:Number, minY:Number, minZ:Number, maxX:Number, maxY:Number, maxZ:Number):void { + this.minX = minX; + this.minY = minY; + this.minZ = minZ; + this.maxX = maxX; + this.maxY = maxY; + this.maxZ = maxZ; + } + + public function addBoundBox(boundBox:BoundBox):void { + minX = (boundBox.minX < minX) ? boundBox.minX : minX; + minY = (boundBox.minY < minY) ? boundBox.minY : minY; + minZ = (boundBox.minZ < minZ) ? boundBox.minZ : minZ; + maxX = (boundBox.maxX > maxX) ? boundBox.maxX : maxX; + maxY = (boundBox.maxY > maxY) ? boundBox.maxY : maxY; + maxZ = (boundBox.maxZ > maxZ) ? boundBox.maxZ : maxZ; + } + + public function addPoint(x:Number, y:Number, z:Number):void { + if (x < minX) minX = x; + if (x > maxX) maxX = x; + if (y < minY) minY = y; + if (y > maxY) maxY = y; + if (z < minZ) minZ = z; + if (z > maxZ) maxZ = z; + } + + public function infinity():void { + minX = 1e308; + minY = 1e308; + minZ = 1e308; + maxX = -1e308; + maxY = -1e308; + maxZ = -1e308; + } + + public function intersects(bb:BoundBox, epsilon:Number):Boolean { + return !(minX > bb.maxX + epsilon || maxX < bb.minX - epsilon || minY > bb.maxY + epsilon || maxY < bb.minY - epsilon || minZ > bb.maxZ + epsilon || maxZ < bb.minZ - epsilon); + } + + public function copyFrom(boundBox:BoundBox):void { + minX = boundBox.minX; + minY = boundBox.minY; + minZ = boundBox.minZ; + maxX = boundBox.maxX; + maxY = boundBox.maxY; + maxZ = boundBox.maxZ; + } + + public function clone():BoundBox { + var clone:BoundBox = new BoundBox(); + clone.copyFrom(this); + return clone; + } + + public function toString():String { + return "BoundBox [" + minX + ", " + minY + ", " + minZ + " : " + maxX + ", " + maxY + ", " + maxZ + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base b/0.0.9.0/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base new file mode 100644 index 0000000..fb2ffbc --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base @@ -0,0 +1,13 @@ +package alternativa.physics.collision.types { + import alternativa.math.Vector3; + + /** + * Структура, описывающая луч и сегмент прямой в пространстве. + */ + public class Ray { + // Начало луча. Начальная точка сегмента на прямой луча. + public var origin:Vector3 = new Vector3(); + // Направляющий вектор луча. Длина вектора задаёт длину сегмента на луче. Конечная точка сегмента вычисляется как origin + dir. + public var dir:Vector3 = new Vector3(); + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base b/0.0.9.0/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base new file mode 100644 index 0000000..21e9290 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base @@ -0,0 +1,25 @@ +package alternativa.physics.collision.types { + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.math.Vector3; + + /** + * Структура описывает точку пересечения луча с физической геометрией. + */ + public class RayIntersection { + // Физический примитив, с которым пересекается луч + public var primitive:CollisionPrimitive; + // Положенеи точки пересечения + public var pos:Vector3 = new Vector3(); + // Нормаль поверхности физического примитива в точке пересечения + public var normal:Vector3 = new Vector3(); + // Время пересечения (|pos - ray.origin|/|ray.dir|) + public var t:Number = 0; + + public function copy(source:RayIntersection):void { + primitive = source.primitive; + pos.vCopy(source.pos); + normal.vCopy(source.normal); + t = source.t; + } + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/types/BoundBox.as b/0.0.9.0/src/alternativa/physics/collision/types/BoundBox.as new file mode 100644 index 0000000..b4d5278 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/types/BoundBox.as @@ -0,0 +1,75 @@ +package alternativa.physics.collision.types { + + /** + * Ограничивающий параллелепипед. + */ + public class BoundBox { + + public var minX:Number = 1e308; + public var minY:Number = 1e308; + public var minZ:Number = 1e308; + public var maxX:Number = -1e308; + public var maxY:Number = -1e308; + public var maxZ:Number = -1e308; + + public function setSize(minX:Number, minY:Number, minZ:Number, maxX:Number, maxY:Number, maxZ:Number):void { + this.minX = minX; + this.minY = minY; + this.minZ = minZ; + this.maxX = maxX; + this.maxY = maxY; + this.maxZ = maxZ; + } + + public function addBoundBox(boundBox:BoundBox):void { + minX = (boundBox.minX < minX) ? boundBox.minX : minX; + minY = (boundBox.minY < minY) ? boundBox.minY : minY; + minZ = (boundBox.minZ < minZ) ? boundBox.minZ : minZ; + maxX = (boundBox.maxX > maxX) ? boundBox.maxX : maxX; + maxY = (boundBox.maxY > maxY) ? boundBox.maxY : maxY; + maxZ = (boundBox.maxZ > maxZ) ? boundBox.maxZ : maxZ; + } + + public function addPoint(x:Number, y:Number, z:Number):void { + if (x < minX) minX = x; + if (x > maxX) maxX = x; + if (y < minY) minY = y; + if (y > maxY) maxY = y; + if (z < minZ) minZ = z; + if (z > maxZ) maxZ = z; + } + + public function infinity():void { + minX = 1e308; + minY = 1e308; + minZ = 1e308; + maxX = -1e308; + maxY = -1e308; + maxZ = -1e308; + } + + public function intersects(bb:BoundBox, epsilon:Number):Boolean { + return !(minX > bb.maxX + epsilon || maxX < bb.minX - epsilon || minY > bb.maxY + epsilon || maxY < bb.minY - epsilon || minZ > bb.maxZ + epsilon || maxZ < bb.minZ - epsilon); + } + + public function copyFrom(boundBox:BoundBox):void { + minX = boundBox.minX; + minY = boundBox.minY; + minZ = boundBox.minZ; + maxX = boundBox.maxX; + maxY = boundBox.maxY; + maxZ = boundBox.maxZ; + } + + public function clone():BoundBox { + var clone:BoundBox = new BoundBox(); + clone.copyFrom(this); + return clone; + } + + public function toString():String { + return "BoundBox [" + minX + ", " + minY + ", " + minZ + " : " + maxX + ", " + maxY + ", " + maxZ + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/types/Ray.as b/0.0.9.0/src/alternativa/physics/collision/types/Ray.as new file mode 100644 index 0000000..fb2ffbc --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/types/Ray.as @@ -0,0 +1,13 @@ +package alternativa.physics.collision.types { + import alternativa.math.Vector3; + + /** + * Структура, описывающая луч и сегмент прямой в пространстве. + */ + public class Ray { + // Начало луча. Начальная точка сегмента на прямой луча. + public var origin:Vector3 = new Vector3(); + // Направляющий вектор луча. Длина вектора задаёт длину сегмента на луче. Конечная точка сегмента вычисляется как origin + dir. + public var dir:Vector3 = new Vector3(); + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/collision/types/RayIntersection.as b/0.0.9.0/src/alternativa/physics/collision/types/RayIntersection.as new file mode 100644 index 0000000..21e9290 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/collision/types/RayIntersection.as @@ -0,0 +1,25 @@ +package alternativa.physics.collision.types { + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.math.Vector3; + + /** + * Структура описывает точку пересечения луча с физической геометрией. + */ + public class RayIntersection { + // Физический примитив, с которым пересекается луч + public var primitive:CollisionPrimitive; + // Положенеи точки пересечения + public var pos:Vector3 = new Vector3(); + // Нормаль поверхности физического примитива в точке пересечения + public var normal:Vector3 = new Vector3(); + // Время пересечения (|pos - ray.origin|/|ray.dir|) + public var t:Number = 0; + + public function copy(source:RayIntersection):void { + primitive = source.primitive; + pos.vCopy(source.pos); + normal.vCopy(source.normal); + t = source.t; + } + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/constraints/.svn/all-wcprops b/0.0.9.0/src/alternativa/physics/constraints/.svn/all-wcprops new file mode 100644 index 0000000..951baeb --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/constraints/.svn/all-wcprops @@ -0,0 +1,17 @@ +K 25 +svn:wc:ra_dav:version-url +V 115 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/constraints +END +Constraint.as +K 25 +svn:wc:ra_dav:version-url +V 129 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/constraints/Constraint.as +END +MaxDistanceConstraint.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/constraints/MaxDistanceConstraint.as +END diff --git a/0.0.9.0/src/alternativa/physics/constraints/.svn/entries b/0.0.9.0/src/alternativa/physics/constraints/.svn/entries new file mode 100644 index 0000000..b653655 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/constraints/.svn/entries @@ -0,0 +1,52 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/constraints +http://svndev.alternativaplatform.com + + + +2010-01-20T17:53:18.555251Z +26849 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Constraint.as +file + + + + +2010-10-28T04:32:43.000000Z +02bdbd40eda515549956d6393551a63b +2010-01-20T17:53:18.555251Z +26849 +mike + +MaxDistanceConstraint.as +file + + + + +2010-10-28T04:32:43.000000Z +dbf792986b9dd7e2a1c720cf4a578f42 +2009-11-24T08:44:15.941480Z +23758 +mike + diff --git a/0.0.9.0/src/alternativa/physics/constraints/.svn/format b/0.0.9.0/src/alternativa/physics/constraints/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/constraints/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.0/src/alternativa/physics/constraints/.svn/text-base/Constraint.as.svn-base b/0.0.9.0/src/alternativa/physics/constraints/.svn/text-base/Constraint.as.svn-base new file mode 100644 index 0000000..8607442 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/constraints/.svn/text-base/Constraint.as.svn-base @@ -0,0 +1,35 @@ +package alternativa.physics.constraints { + + import alternativa.physics.PhysicsScene; + import alternativa.physics.altphysics; + + use namespace altphysics; + + /** + * + */ + public class Constraint { + + altphysics var satisfied:Boolean; + altphysics var world:PhysicsScene; + + /** + * + */ + public function Constraint() { + } + + /** + * @param dt + */ + public function preProcess(dt:Number):void { + } + + /** + * @param dt + */ + public function apply(dt:Number):void { + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base b/0.0.9.0/src/alternativa/physics/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base new file mode 100644 index 0000000..d29f518 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base @@ -0,0 +1,183 @@ +package alternativa.physics.constraints { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * + */ + public class MaxDistanceConstraint extends Constraint { + // Первое тело ограничения + altphysics var body1:Body; + // Второе тело ограничения. Может быть null. + altphysics var body2:Body; + // Первая точка крепления в системе координат первого тела + altphysics var r1:Vector3; + // Вторая точка привязки в системе координат второго тела. В случае отсутствия второго тела, координаты задаются в мировой системе. + altphysics var r2:Vector3; + // Максимально допустимое расстояние между точками крепления + altphysics var maxDistance:Number; + // Радиус-вектор первой точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr1:Vector3 = new Vector3(); + // Радиус-вектор второй точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr2:Vector3 = new Vector3(); + // Минимальная скорость сближения для удовлетворения ограничения + private var minClosingVel:Number; + // Изменение относительной скорости на единицу импульса + private var velByUnitImpulseN:Number; + // Направление корректирующего импульса + private var impulseDirection:Vector3 = new Vector3(); + + // Вспомогательные переменные +// private static var _v:Vector3 = new Vector3(); +// private static var _v1:Vector3 = new Vector3(); +// private static var _v2:Vector3 = new Vector3(); + + /** + * @param body1 + * @param body2 + * @param r1 + * @param r2 + * @param maxDistance + */ + public function MaxDistanceConstraint(body1:Body, body2:Body, r1:Vector3, r2:Vector3, maxDistance:Number) { + super(); + this.body1 = body1; + this.body2 = body2; + this.r1 = r1.vClone(); + this.r2 = r2.vClone(); + this.maxDistance = maxDistance; + } + + /** + * @param dt + */ + override public function preProcess(dt:Number):void { + // Вычислим расстояние между точками + var m:Matrix3 = body1.baseMatrix; + wr1.x = m.a*r1.x + m.b*r1.y + m.c*r1.z; + wr1.y = m.e*r1.x + m.f*r1.y + m.g*r1.z; + wr1.z = m.i*r1.x + m.j*r1.y + m.k*r1.z; + if (body2 != null) { + m = body2.baseMatrix; + wr2.x = m.a*r2.x + m.b*r2.y + m.c*r2.z; + wr2.y = m.e*r2.x + m.f*r2.y + m.g*r2.z; + wr2.z = m.i*r2.x + m.j*r2.y + m.k*r2.z; + } else { + wr2.x = r2.x; + wr2.y = r2.y; + wr2.z = r2.z; + } + var p1:Vector3 = body1.state.pos; + impulseDirection.x = wr2.x - wr1.x - p1.x; + impulseDirection.y = wr2.y - wr1.y - p1.y; + impulseDirection.z = wr2.z - wr1.z - p1.z; + if (body2 != null) { + var p2:Vector3 = body2.state.pos; + impulseDirection.x += p2.x; + impulseDirection.y += p2.y; + impulseDirection.z += p2.z; + } + + var len:Number = Math.sqrt(impulseDirection.x*impulseDirection.x + impulseDirection.y*impulseDirection.y + impulseDirection.z*impulseDirection.z); + var delta:Number = len - maxDistance; + if (delta > 0) { + satisfied = false; + if (len < 0.001) { + impulseDirection.x = 1; + } else { + len = 1/len; + impulseDirection.x *= len; + impulseDirection.y *= len; + impulseDirection.z *= len; + } + minClosingVel = delta/(world.penResolutionSteps*dt); + if (minClosingVel > world.maxPenResolutionSpeed) { + minClosingVel = world.maxPenResolutionSpeed; + } + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + var x:Number; + var y:Number; + var z:Number; + var vx:Number; + var vy:Number; + var vz:Number; + velByUnitImpulseN = 0; + if (body1.movable) { +// velByUnitImpulseN += body1.invMass + _v.vCross2(wr1, impulseDirection).vTransformBy3(body1.invInertiaWorld).vCross(wr1).vDot(impulseDirection); + vx = wr1.y*impulseDirection.z - wr1.z*impulseDirection.y; + vy = wr1.z*impulseDirection.x - wr1.x*impulseDirection.z; + vz = wr1.x*impulseDirection.y - wr1.y*impulseDirection.x; + + m = body1.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr1.z - z*wr1.y; + vy = z*wr1.x - x*wr1.z; + vz = x*wr1.y - y*wr1.x; + velByUnitImpulseN += body1.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + if (body2 != null && body2.movable) { +// velByUnitImpulseN += body2.invMass + _v.vCross2(wr2, impulseDirection).vTransformBy3(body2.invInertiaWorld).vCross(wr2).vDot(impulseDirection); + vx = wr2.y*impulseDirection.z - wr2.z*impulseDirection.y; + vy = wr2.z*impulseDirection.x - wr2.x*impulseDirection.z; + vz = wr2.x*impulseDirection.y - wr2.y*impulseDirection.x; + + m = body2.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr2.z - z*wr2.y; + vy = z*wr2.x - x*wr2.z; + vz = x*wr2.y - y*wr2.x; + velByUnitImpulseN += body2.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + } else { + satisfied = true; + } + } + + /** + * @param dt + */ + override public function apply(dt:Number):void { + if (satisfied) return; + // Расчитываем проекцию скорости относительной скорости + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 + var vel:Vector3 = body1.state.velocity; + var rot:Vector3 = body1.state.rotation; +// v1Cross2(state.rotation, wr1); + var vx:Number = vel.x + rot.y*wr1.z - rot.z*wr1.y; + var vy:Number = vel.y + rot.z*wr1.x - rot.x*wr1.z; + var vz:Number = vel.z + rot.x*wr1.y - rot.y*wr1.x; + // V2 = V2_c + w2%r2 + if (body2 != null) { + vel = body2.state.velocity; + rot = body2.state.rotation; +// _v2.vCross2(state.rotation, wr2); + vx -= vel.x + rot.y*wr2.z - rot.z*wr2.y; + vy -= vel.y + rot.z*wr2.x - rot.x*wr2.z; + vz -= vel.z + rot.x*wr2.y - rot.y*wr2.x; + } + var closingVel:Number = vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + if (closingVel > minClosingVel) return; + var impulse:Number = (minClosingVel - closingVel)/velByUnitImpulseN; + if (body1.movable) { + body1.applyRelPosWorldImpulse(wr1, impulseDirection, impulse); + } + if (body2 != null && body2.movable) { + body2.applyRelPosWorldImpulse(wr2, impulseDirection, -impulse); + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/constraints/Constraint.as b/0.0.9.0/src/alternativa/physics/constraints/Constraint.as new file mode 100644 index 0000000..8607442 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/constraints/Constraint.as @@ -0,0 +1,35 @@ +package alternativa.physics.constraints { + + import alternativa.physics.PhysicsScene; + import alternativa.physics.altphysics; + + use namespace altphysics; + + /** + * + */ + public class Constraint { + + altphysics var satisfied:Boolean; + altphysics var world:PhysicsScene; + + /** + * + */ + public function Constraint() { + } + + /** + * @param dt + */ + public function preProcess(dt:Number):void { + } + + /** + * @param dt + */ + public function apply(dt:Number):void { + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/constraints/MaxDistanceConstraint.as b/0.0.9.0/src/alternativa/physics/constraints/MaxDistanceConstraint.as new file mode 100644 index 0000000..d29f518 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/constraints/MaxDistanceConstraint.as @@ -0,0 +1,183 @@ +package alternativa.physics.constraints { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * + */ + public class MaxDistanceConstraint extends Constraint { + // Первое тело ограничения + altphysics var body1:Body; + // Второе тело ограничения. Может быть null. + altphysics var body2:Body; + // Первая точка крепления в системе координат первого тела + altphysics var r1:Vector3; + // Вторая точка привязки в системе координат второго тела. В случае отсутствия второго тела, координаты задаются в мировой системе. + altphysics var r2:Vector3; + // Максимально допустимое расстояние между точками крепления + altphysics var maxDistance:Number; + // Радиус-вектор первой точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr1:Vector3 = new Vector3(); + // Радиус-вектор второй точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr2:Vector3 = new Vector3(); + // Минимальная скорость сближения для удовлетворения ограничения + private var minClosingVel:Number; + // Изменение относительной скорости на единицу импульса + private var velByUnitImpulseN:Number; + // Направление корректирующего импульса + private var impulseDirection:Vector3 = new Vector3(); + + // Вспомогательные переменные +// private static var _v:Vector3 = new Vector3(); +// private static var _v1:Vector3 = new Vector3(); +// private static var _v2:Vector3 = new Vector3(); + + /** + * @param body1 + * @param body2 + * @param r1 + * @param r2 + * @param maxDistance + */ + public function MaxDistanceConstraint(body1:Body, body2:Body, r1:Vector3, r2:Vector3, maxDistance:Number) { + super(); + this.body1 = body1; + this.body2 = body2; + this.r1 = r1.vClone(); + this.r2 = r2.vClone(); + this.maxDistance = maxDistance; + } + + /** + * @param dt + */ + override public function preProcess(dt:Number):void { + // Вычислим расстояние между точками + var m:Matrix3 = body1.baseMatrix; + wr1.x = m.a*r1.x + m.b*r1.y + m.c*r1.z; + wr1.y = m.e*r1.x + m.f*r1.y + m.g*r1.z; + wr1.z = m.i*r1.x + m.j*r1.y + m.k*r1.z; + if (body2 != null) { + m = body2.baseMatrix; + wr2.x = m.a*r2.x + m.b*r2.y + m.c*r2.z; + wr2.y = m.e*r2.x + m.f*r2.y + m.g*r2.z; + wr2.z = m.i*r2.x + m.j*r2.y + m.k*r2.z; + } else { + wr2.x = r2.x; + wr2.y = r2.y; + wr2.z = r2.z; + } + var p1:Vector3 = body1.state.pos; + impulseDirection.x = wr2.x - wr1.x - p1.x; + impulseDirection.y = wr2.y - wr1.y - p1.y; + impulseDirection.z = wr2.z - wr1.z - p1.z; + if (body2 != null) { + var p2:Vector3 = body2.state.pos; + impulseDirection.x += p2.x; + impulseDirection.y += p2.y; + impulseDirection.z += p2.z; + } + + var len:Number = Math.sqrt(impulseDirection.x*impulseDirection.x + impulseDirection.y*impulseDirection.y + impulseDirection.z*impulseDirection.z); + var delta:Number = len - maxDistance; + if (delta > 0) { + satisfied = false; + if (len < 0.001) { + impulseDirection.x = 1; + } else { + len = 1/len; + impulseDirection.x *= len; + impulseDirection.y *= len; + impulseDirection.z *= len; + } + minClosingVel = delta/(world.penResolutionSteps*dt); + if (minClosingVel > world.maxPenResolutionSpeed) { + minClosingVel = world.maxPenResolutionSpeed; + } + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + var x:Number; + var y:Number; + var z:Number; + var vx:Number; + var vy:Number; + var vz:Number; + velByUnitImpulseN = 0; + if (body1.movable) { +// velByUnitImpulseN += body1.invMass + _v.vCross2(wr1, impulseDirection).vTransformBy3(body1.invInertiaWorld).vCross(wr1).vDot(impulseDirection); + vx = wr1.y*impulseDirection.z - wr1.z*impulseDirection.y; + vy = wr1.z*impulseDirection.x - wr1.x*impulseDirection.z; + vz = wr1.x*impulseDirection.y - wr1.y*impulseDirection.x; + + m = body1.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr1.z - z*wr1.y; + vy = z*wr1.x - x*wr1.z; + vz = x*wr1.y - y*wr1.x; + velByUnitImpulseN += body1.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + if (body2 != null && body2.movable) { +// velByUnitImpulseN += body2.invMass + _v.vCross2(wr2, impulseDirection).vTransformBy3(body2.invInertiaWorld).vCross(wr2).vDot(impulseDirection); + vx = wr2.y*impulseDirection.z - wr2.z*impulseDirection.y; + vy = wr2.z*impulseDirection.x - wr2.x*impulseDirection.z; + vz = wr2.x*impulseDirection.y - wr2.y*impulseDirection.x; + + m = body2.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr2.z - z*wr2.y; + vy = z*wr2.x - x*wr2.z; + vz = x*wr2.y - y*wr2.x; + velByUnitImpulseN += body2.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + } else { + satisfied = true; + } + } + + /** + * @param dt + */ + override public function apply(dt:Number):void { + if (satisfied) return; + // Расчитываем проекцию скорости относительной скорости + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 + var vel:Vector3 = body1.state.velocity; + var rot:Vector3 = body1.state.rotation; +// v1Cross2(state.rotation, wr1); + var vx:Number = vel.x + rot.y*wr1.z - rot.z*wr1.y; + var vy:Number = vel.y + rot.z*wr1.x - rot.x*wr1.z; + var vz:Number = vel.z + rot.x*wr1.y - rot.y*wr1.x; + // V2 = V2_c + w2%r2 + if (body2 != null) { + vel = body2.state.velocity; + rot = body2.state.rotation; +// _v2.vCross2(state.rotation, wr2); + vx -= vel.x + rot.y*wr2.z - rot.z*wr2.y; + vy -= vel.y + rot.z*wr2.x - rot.x*wr2.z; + vz -= vel.z + rot.x*wr2.y - rot.y*wr2.x; + } + var closingVel:Number = vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + if (closingVel > minClosingVel) return; + var impulse:Number = (minClosingVel - closingVel)/velByUnitImpulseN; + if (body1.movable) { + body1.applyRelPosWorldImpulse(wr1, impulseDirection, impulse); + } + if (body2 != null && body2.movable) { + body2.applyRelPosWorldImpulse(wr2, impulseDirection, -impulse); + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/primitives/.svn/all-wcprops b/0.0.9.0/src/alternativa/physics/primitives/.svn/all-wcprops new file mode 100644 index 0000000..e2be0c4 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/primitives/.svn/all-wcprops @@ -0,0 +1,35 @@ +K 25 +svn:wc:ra_dav:version-url +V 114 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/primitives +END +RigidPlane.as +K 25 +svn:wc:ra_dav:version-url +V 128 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/primitives/RigidPlane.as +END +RigidSphere.as +K 25 +svn:wc:ra_dav:version-url +V 129 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/primitives/RigidSphere.as +END +RigidBox.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/primitives/RigidBox.as +END +RigidCylinder.as +K 25 +svn:wc:ra_dav:version-url +V 131 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/primitives/RigidCylinder.as +END +RigidRect.as +K 25 +svn:wc:ra_dav:version-url +V 127 +/!svn/ver/28758/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/primitives/RigidRect.as +END diff --git a/0.0.9.0/src/alternativa/physics/primitives/.svn/entries b/0.0.9.0/src/alternativa/physics/primitives/.svn/entries new file mode 100644 index 0000000..252b697 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/primitives/.svn/entries @@ -0,0 +1,88 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.0/src/alternativa/physics/primitives +http://svndev.alternativaplatform.com + + + +2009-11-24T08:44:15.941480Z +23758 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +RigidPlane.as +file + + + + +2010-10-28T04:32:43.000000Z +8faf607cf544d7388852668f5cd5672f +2009-11-24T08:44:15.941480Z +23758 +mike + +RigidSphere.as +file + + + + +2010-10-28T04:32:43.000000Z +774b0c23b084bfffb3381eb40e616deb +2009-11-24T08:44:15.941480Z +23758 +mike + +RigidBox.as +file + + + + +2010-10-28T04:32:43.000000Z +7b93f349a2818f22b76d9590cb05f84a +2009-11-24T08:44:15.941480Z +23758 +mike + +RigidCylinder.as +file + + + + +2010-10-28T04:32:43.000000Z +e9ce0eeb457428727cc10d2e53a0f164 +2009-11-24T08:44:15.941480Z +23758 +mike + +RigidRect.as +file + + + + +2010-10-28T04:32:43.000000Z +2eb6d0ebde8c2b46c4b0f810b1d64e1e +2009-11-24T08:44:15.941480Z +23758 +mike + diff --git a/0.0.9.0/src/alternativa/physics/primitives/.svn/format b/0.0.9.0/src/alternativa/physics/primitives/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/primitives/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.0/src/alternativa/physics/primitives/.svn/text-base/RigidBox.as.svn-base b/0.0.9.0/src/alternativa/physics/primitives/.svn/text-base/RigidBox.as.svn-base new file mode 100644 index 0000000..24e9b9f --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/primitives/.svn/text-base/RigidBox.as.svn-base @@ -0,0 +1,56 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * + */ + public class RigidBox extends Body { + + /** + * + * @param halfSize + * @param mass + */ + public function RigidBox(halfSize:Vector3, mass:Number) { + super(0, Matrix3.ZERO); + addCollisionPrimitive(new CollisionBox(halfSize, 1)); + setParams(halfSize, mass); + } + + /** + * @param halfSize + * @param mass + */ + public function setParams(halfSize:Vector3, mass:Number):void { + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + invInertia.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + invInertia.a = 3*invMass/(yy + zz); + invInertia.f = 3*invMass/(zz + xx); + invInertia.k = 3*invMass/(xx + yy); + } + var prim:CollisionBox = CollisionBox(collisionPrimitives.head.primitive); + prim.hs.vCopy(halfSize); + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/primitives/.svn/text-base/RigidCylinder.as.svn-base b/0.0.9.0/src/alternativa/physics/primitives/.svn/text-base/RigidCylinder.as.svn-base new file mode 100644 index 0000000..4ff65dd --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/primitives/.svn/text-base/RigidCylinder.as.svn-base @@ -0,0 +1,32 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.math.Matrix3; + + use namespace altphysics; + + public class RigidCylinder extends Body { + + altphysics var r:Number; + altphysics var h:Number; + + public function RigidCylinder(mass:Number, radius:Number, height:Number) { + super(0, Matrix3.ZERO); + setParams(mass, radius, height); + } + + public function setParams(mass:Number, radius:Number, height:Number):void { + r = radius; + h = height; + invInertiaWorld.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + invInertia.a = invInertia.f = mass*(h*h/12 + r*r/4); + invInertia.k = 0.5*mass*r*r; + invInertia.invert(); + } + } + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/primitives/.svn/text-base/RigidPlane.as.svn-base b/0.0.9.0/src/alternativa/physics/primitives/.svn/text-base/RigidPlane.as.svn-base new file mode 100644 index 0000000..d9ebf9b --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/primitives/.svn/text-base/RigidPlane.as.svn-base @@ -0,0 +1,23 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + public class RigidPlane extends Body { + + public var normal:Vector3 = new Vector3(); + public var offset:Number; + + public function RigidPlane() { + super(0, Matrix3.ZERO); + this.normal.vCopy(Vector3.Z_AXIS); + this.offset = 0; + movable = false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/primitives/.svn/text-base/RigidRect.as.svn-base b/0.0.9.0/src/alternativa/physics/primitives/.svn/text-base/RigidRect.as.svn-base new file mode 100644 index 0000000..4591931 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/primitives/.svn/text-base/RigidRect.as.svn-base @@ -0,0 +1,20 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + public class RigidRect extends Body { + + public function RigidRect(halfWidth:Number, halfLength:Number) { + super(0, Matrix3.ZERO); + movable = false; + addCollisionPrimitive(new CollisionRect(new Vector3(halfWidth, halfLength, 0), 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/primitives/.svn/text-base/RigidSphere.as.svn-base b/0.0.9.0/src/alternativa/physics/primitives/.svn/text-base/RigidSphere.as.svn-base new file mode 100644 index 0000000..8ad180c --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/primitives/.svn/text-base/RigidSphere.as.svn-base @@ -0,0 +1,29 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.math.Matrix3; + + use namespace altphysics; + + public class RigidSphere extends Body { + + public function RigidSphere(radius:Number, mass:Number) { + var m:Matrix3; + if (mass == Infinity) { + mass = 0; + m = Matrix3.ZERO; + } else { + mass = 1/mass; + var r2:Number = radius*radius; + m = new Matrix3(); + m.a = m.f = m.k = 2.5*mass/r2; + } + super(mass, m); + if (mass == 0) movable = false; + addCollisionPrimitive(new CollisionSphere(radius, 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/primitives/RigidBox.as b/0.0.9.0/src/alternativa/physics/primitives/RigidBox.as new file mode 100644 index 0000000..24e9b9f --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/primitives/RigidBox.as @@ -0,0 +1,56 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * + */ + public class RigidBox extends Body { + + /** + * + * @param halfSize + * @param mass + */ + public function RigidBox(halfSize:Vector3, mass:Number) { + super(0, Matrix3.ZERO); + addCollisionPrimitive(new CollisionBox(halfSize, 1)); + setParams(halfSize, mass); + } + + /** + * @param halfSize + * @param mass + */ + public function setParams(halfSize:Vector3, mass:Number):void { + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + invInertia.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + invInertia.a = 3*invMass/(yy + zz); + invInertia.f = 3*invMass/(zz + xx); + invInertia.k = 3*invMass/(xx + yy); + } + var prim:CollisionBox = CollisionBox(collisionPrimitives.head.primitive); + prim.hs.vCopy(halfSize); + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/primitives/RigidCylinder.as b/0.0.9.0/src/alternativa/physics/primitives/RigidCylinder.as new file mode 100644 index 0000000..4ff65dd --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/primitives/RigidCylinder.as @@ -0,0 +1,32 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.math.Matrix3; + + use namespace altphysics; + + public class RigidCylinder extends Body { + + altphysics var r:Number; + altphysics var h:Number; + + public function RigidCylinder(mass:Number, radius:Number, height:Number) { + super(0, Matrix3.ZERO); + setParams(mass, radius, height); + } + + public function setParams(mass:Number, radius:Number, height:Number):void { + r = radius; + h = height; + invInertiaWorld.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + invInertia.a = invInertia.f = mass*(h*h/12 + r*r/4); + invInertia.k = 0.5*mass*r*r; + invInertia.invert(); + } + } + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/primitives/RigidPlane.as b/0.0.9.0/src/alternativa/physics/primitives/RigidPlane.as new file mode 100644 index 0000000..d9ebf9b --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/primitives/RigidPlane.as @@ -0,0 +1,23 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + public class RigidPlane extends Body { + + public var normal:Vector3 = new Vector3(); + public var offset:Number; + + public function RigidPlane() { + super(0, Matrix3.ZERO); + this.normal.vCopy(Vector3.Z_AXIS); + this.offset = 0; + movable = false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/primitives/RigidRect.as b/0.0.9.0/src/alternativa/physics/primitives/RigidRect.as new file mode 100644 index 0000000..4591931 --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/primitives/RigidRect.as @@ -0,0 +1,20 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + public class RigidRect extends Body { + + public function RigidRect(halfWidth:Number, halfLength:Number) { + super(0, Matrix3.ZERO); + movable = false; + addCollisionPrimitive(new CollisionRect(new Vector3(halfWidth, halfLength, 0), 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.9.0/src/alternativa/physics/primitives/RigidSphere.as b/0.0.9.0/src/alternativa/physics/primitives/RigidSphere.as new file mode 100644 index 0000000..8ad180c --- /dev/null +++ b/0.0.9.0/src/alternativa/physics/primitives/RigidSphere.as @@ -0,0 +1,29 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.math.Matrix3; + + use namespace altphysics; + + public class RigidSphere extends Body { + + public function RigidSphere(radius:Number, mass:Number) { + var m:Matrix3; + if (mass == Infinity) { + mass = 0; + m = Matrix3.ZERO; + } else { + mass = 1/mass; + var r2:Number = radius*radius; + m = new Matrix3(); + m.a = m.f = m.k = 2.5*mass/r2; + } + super(mass, m); + if (mass == 0) movable = false; + addCollisionPrimitive(new CollisionSphere(radius, 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/.actionScriptProperties b/0.0.9.1/.actionScriptProperties new file mode 100644 index 0000000..f73adc8 --- /dev/null +++ b/0.0.9.1/.actionScriptProperties @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/0.0.9.1/.flexLibProperties b/0.0.9.1/.flexLibProperties new file mode 100644 index 0000000..0b65396 --- /dev/null +++ b/0.0.9.1/.flexLibProperties @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/0.0.9.1/.project b/0.0.9.1/.project new file mode 100644 index 0000000..1f1425f --- /dev/null +++ b/0.0.9.1/.project @@ -0,0 +1,19 @@ + + + AlternativaPhysics + + + + + + com.adobe.flexbuilder.project.flexbuilder + + + + + + org.maven.ide.eclipse.maven2Nature + com.adobe.flexbuilder.project.flexlibnature + com.adobe.flexbuilder.project.actionscriptnature + + diff --git a/0.0.9.1/.settings/.svn/all-wcprops b/0.0.9.1/.settings/.svn/all-wcprops new file mode 100644 index 0000000..a26e495 --- /dev/null +++ b/0.0.9.1/.settings/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 89 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/.settings +END +org.eclipse.core.resources.prefs +K 25 +svn:wc:ra_dav:version-url +V 122 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/.settings/org.eclipse.core.resources.prefs +END diff --git a/0.0.9.1/.settings/.svn/entries b/0.0.9.1/.settings/.svn/entries new file mode 100644 index 0000000..8a147b2 --- /dev/null +++ b/0.0.9.1/.settings/.svn/entries @@ -0,0 +1,40 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/.settings +http://svndev.alternativaplatform.com + + + +2009-04-01T12:25:29.638016Z +10301 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +org.eclipse.core.resources.prefs +file + + + + +2010-10-28T04:32:44.000000Z +4f70b476f3e5075e399505021df2f89b +2009-04-01T12:25:29.638016Z +10301 +mike + diff --git a/0.0.9.1/.settings/.svn/format b/0.0.9.1/.settings/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.1/.settings/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.1/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base b/0.0.9.1/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base new file mode 100644 index 0000000..b3d0d49 --- /dev/null +++ b/0.0.9.1/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base @@ -0,0 +1,3 @@ +#Fri Mar 20 00:19:33 YEKT 2009 +eclipse.preferences.version=1 +encoding/=UTF-8 diff --git a/0.0.9.1/.settings/org.eclipse.core.resources.prefs b/0.0.9.1/.settings/org.eclipse.core.resources.prefs new file mode 100644 index 0000000..b3d0d49 --- /dev/null +++ b/0.0.9.1/.settings/org.eclipse.core.resources.prefs @@ -0,0 +1,3 @@ +#Fri Mar 20 00:19:33 YEKT 2009 +eclipse.preferences.version=1 +encoding/=UTF-8 diff --git a/0.0.9.1/.svn/all-wcprops b/0.0.9.1/.svn/all-wcprops new file mode 100644 index 0000000..9190fd2 --- /dev/null +++ b/0.0.9.1/.svn/all-wcprops @@ -0,0 +1,35 @@ +K 25 +svn:wc:ra_dav:version-url +V 79 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1 +END +.flexLibProperties +K 25 +svn:wc:ra_dav:version-url +V 98 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/.flexLibProperties +END +.project +K 25 +svn:wc:ra_dav:version-url +V 88 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/.project +END +pom.xml +K 25 +svn:wc:ra_dav:version-url +V 87 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/pom.xml +END +.actionScriptProperties +K 25 +svn:wc:ra_dav:version-url +V 103 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/.actionScriptProperties +END +AlternativaPhysics.iml +K 25 +svn:wc:ra_dav:version-url +V 102 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/AlternativaPhysics.iml +END diff --git a/0.0.9.1/.svn/dir-prop-base b/0.0.9.1/.svn/dir-prop-base new file mode 100644 index 0000000..c31ca28 --- /dev/null +++ b/0.0.9.1/.svn/dir-prop-base @@ -0,0 +1,7 @@ +K 13 +svn:mergeinfo +V 278 +/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized:19806-19860 +/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.7.0.LinkedList:20360-22217,22226-22252 +/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D:13892-14098 +END diff --git a/0.0.9.1/.svn/entries b/0.0.9.1/.svn/entries new file mode 100644 index 0000000..bb8ad20 --- /dev/null +++ b/0.0.9.1/.svn/entries @@ -0,0 +1,97 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1 +http://svndev.alternativaplatform.com + + + +2010-03-06T17:29:47.415653Z +29407 +mike +has-props + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +META-INF +dir + +.flexLibProperties +file + + + + +2010-10-28T04:32:44.000000Z +b170dccb732dcb12462ac421cdca9723 +2010-02-12T12:34:25.192477Z +28424 +mike + +.project +file + + + + +2010-10-28T04:32:44.000000Z +1e90cc68b52b93173b2b5de88eb5c3a3 +2009-04-20T17:15:55.260110Z +11541 +mike + +src +dir + +pom.xml +file + + + + +2010-10-28T04:32:44.000000Z +98288e4a3065f5d7f4fd9a89f2742990 +2010-03-06T17:29:47.415653Z +29407 +mike + +.actionScriptProperties +file + + + + +2010-10-28T04:32:44.000000Z +b8656f882fff2baedd57a92db86fcf8c +2010-01-14T07:05:07.823716Z +26358 +mike + +AlternativaPhysics.iml +file + + + + +2010-10-28T04:32:44.000000Z +5162d434bd39d50f59a8057c1fa2c961 +2010-02-19T13:52:11.808507Z +28758 +mike + +.settings +dir + diff --git a/0.0.9.1/.svn/format b/0.0.9.1/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.1/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.1/.svn/text-base/.actionScriptProperties.svn-base b/0.0.9.1/.svn/text-base/.actionScriptProperties.svn-base new file mode 100644 index 0000000..f73adc8 --- /dev/null +++ b/0.0.9.1/.svn/text-base/.actionScriptProperties.svn-base @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/0.0.9.1/.svn/text-base/.flexLibProperties.svn-base b/0.0.9.1/.svn/text-base/.flexLibProperties.svn-base new file mode 100644 index 0000000..0b65396 --- /dev/null +++ b/0.0.9.1/.svn/text-base/.flexLibProperties.svn-base @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/0.0.9.1/.svn/text-base/.project.svn-base b/0.0.9.1/.svn/text-base/.project.svn-base new file mode 100644 index 0000000..1f1425f --- /dev/null +++ b/0.0.9.1/.svn/text-base/.project.svn-base @@ -0,0 +1,19 @@ + + + AlternativaPhysics + + + + + + com.adobe.flexbuilder.project.flexbuilder + + + + + + org.maven.ide.eclipse.maven2Nature + com.adobe.flexbuilder.project.flexlibnature + com.adobe.flexbuilder.project.actionscriptnature + + diff --git a/0.0.9.1/.svn/text-base/AlternativaPhysics.iml.svn-base b/0.0.9.1/.svn/text-base/AlternativaPhysics.iml.svn-base new file mode 100644 index 0000000..7783af3 --- /dev/null +++ b/0.0.9.1/.svn/text-base/AlternativaPhysics.iml.svn-base @@ -0,0 +1,65 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/0.0.9.1/.svn/text-base/pom.xml.svn-base b/0.0.9.1/.svn/text-base/pom.xml.svn-base new file mode 100644 index 0000000..1138f57 --- /dev/null +++ b/0.0.9.1/.svn/text-base/pom.xml.svn-base @@ -0,0 +1,17 @@ + + 4.0.0 + platform.clients.fp10.libraries + AlternativaPhysics + swc + 0.0.9.1 + + platform.tools.maven + FlashBasePom + 2.0.0.0 + + + scm:svn:http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1 + + + + diff --git a/0.0.9.1/AlternativaPhysics.iml b/0.0.9.1/AlternativaPhysics.iml new file mode 100644 index 0000000..7783af3 --- /dev/null +++ b/0.0.9.1/AlternativaPhysics.iml @@ -0,0 +1,65 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/0.0.9.1/META-INF/.svn/all-wcprops b/0.0.9.1/META-INF/.svn/all-wcprops new file mode 100644 index 0000000..0dfcccf --- /dev/null +++ b/0.0.9.1/META-INF/.svn/all-wcprops @@ -0,0 +1,17 @@ +K 25 +svn:wc:ra_dav:version-url +V 88 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/META-INF +END +security.sspj +K 25 +svn:wc:ra_dav:version-url +V 102 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/META-INF/security.sspj +END +MANIFEST.MF +K 25 +svn:wc:ra_dav:version-url +V 100 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/META-INF/MANIFEST.MF +END diff --git a/0.0.9.1/META-INF/.svn/entries b/0.0.9.1/META-INF/.svn/entries new file mode 100644 index 0000000..94ac647 --- /dev/null +++ b/0.0.9.1/META-INF/.svn/entries @@ -0,0 +1,52 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/META-INF +http://svndev.alternativaplatform.com + + + +2009-12-25T05:53:34.807593Z +25872 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +security.sspj +file + + + + +2010-10-28T04:32:43.000000Z +4684eb6f77fe6999c75cd467c18cf5d6 +2009-12-25T05:53:34.807593Z +25872 +mike + +MANIFEST.MF +file + + + + +2010-10-28T04:32:43.000000Z +7b64dceb50a6905850ff00a0bfbe77eb +2009-04-20T17:15:55.260110Z +11541 +mike + diff --git a/0.0.9.1/META-INF/.svn/format b/0.0.9.1/META-INF/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.1/META-INF/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.1/META-INF/.svn/text-base/MANIFEST.MF.svn-base b/0.0.9.1/META-INF/.svn/text-base/MANIFEST.MF.svn-base new file mode 100644 index 0000000..5066a55 --- /dev/null +++ b/0.0.9.1/META-INF/.svn/text-base/MANIFEST.MF.svn-base @@ -0,0 +1 @@ +Bundle-Name: platform.clients.fp10.libraries.AlternativaPhysics diff --git a/0.0.9.1/META-INF/.svn/text-base/security.sspj.svn-base b/0.0.9.1/META-INF/.svn/text-base/security.sspj.svn-base new file mode 100644 index 0000000..e463c72 --- /dev/null +++ b/0.0.9.1/META-INF/.svn/text-base/security.sspj.svn-base @@ -0,0 +1,11923 @@ + + + + + true + + + true + + + true + + + 3.4 + + + true + + + true + + + true + + + + + + + + + + + true + + + FFFFFF + + + D:\work\eclipse\ws_projects\TanksTestingTool\lib\AlternativaPhysics.swc + + + AlternativaPhysics.swc + + + 1 + + + 24.0 + + + Not Present + + + 500 X 375 + + + + + + + true + + + + + + + + + + + + + + + + append + + + true + + + 10 + + + Public Function + + + + + + + getEulerAngles + + + true + + + 10 + + + Public Function + + + + + + + invert + + + true + + + 10 + + + Public Function + + + + + + + prepend + + + true + + + 10 + + + Public Function + + + + + + + setRotationMatrix + + + true + + + 10 + + + Public Function + + + + + + + subtract + + + true + + + 10 + + + Public Function + + + + + + + toIdentity + + + true + + + 10 + + + Public Function + + + + + + + transformVector + + + true + + + 10 + + + Public Function + + + + + + + transformVector3To3D + + + true + + + 10 + + + Public Function + + + + + + + transpose + + + true + + + 10 + + + Public Function + + + + + + + a + + + true + + + 10 + + + Public Variable + + + + + + + b + + + true + + + 10 + + + Public Variable + + + + + + + c + + + true + + + 10 + + + Public Variable + + + + + + + clone + + + true + + + 10 + + + Public Function + + + + + + + e + + + true + + + 10 + + + Public Variable + + + + + + + f + + + true + + + 10 + + + Public Variable + + + + + + + g + + + true + + + 10 + + + Public Variable + + + + + + + add + + + true + + + 10 + + + Public Function + + + + + + + i + + + true + + + 10 + + + Public Variable + + + + + + + j + + + true + + + 10 + + + Public Variable + + + + + + + k + + + true + + + 10 + + + Public Variable + + + + + + + copy + + + true + + + 10 + + + Public Function + + + + + + + createSkewSymmetric + + + true + + + 10 + + + Public Function + + + + + + + fromAxisAngle + + + true + + + 10 + + + Public Function + + + + + + + prependTransposed + + + true + + + 10 + + + Public Function + + + + + + + toString + + + true + + + 10 + + + Public Function + + + + + + + transformVectorInverse + + + true + + + 10 + + + Public Function + + + + + + + IDENTITY + + + true + + + 10 + + + Public Variable + + + + + + + ZERO + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + Matrix3 + + + true + + + 10 + + + Class + + + + + + + + + + + append + + + true + + + 10 + + + Public Function + + + + + + + deltaTransformVectorInverse + + + true + + + 10 + + + Public Function + + + + + + + getAxis + + + true + + + 10 + + + Public Function + + + + + + + invert + + + true + + + 10 + + + Public Function + + + + + + + prepend + + + true + + + 10 + + + Public Function + + + + + + + setFromMatrix3 + + + true + + + 10 + + + Public Function + + + + + + + subtract + + + true + + + 10 + + + Public Function + + + + + + + toIdentity + + + true + + + 10 + + + Public Function + + + + + + + transformVector + + + true + + + 10 + + + Public Function + + + + + + + transformVectors + + + true + + + 10 + + + Public Function + + + + + + + transformVectorsInverse + + + true + + + 10 + + + Public Function + + + + + + + transformVectorsInverseN + + + true + + + 10 + + + Public Function + + + + + + + transformVectorsN + + + true + + + 10 + + + Public Function + + + + + + + a + + + true + + + 10 + + + Public Variable + + + + + + + b + + + true + + + 10 + + + Public Variable + + + + + + + c + + + true + + + 10 + + + Public Variable + + + + + + + d + + + true + + + 10 + + + Public Variable + + + + + + + e + + + true + + + 10 + + + Public Variable + + + + + + + f + + + true + + + 10 + + + Public Variable + + + + + + + g + + + true + + + 10 + + + Public Variable + + + + + + + h + + + true + + + 10 + + + Public Variable + + + + + + + i + + + true + + + 10 + + + Public Variable + + + + + + + j + + + true + + + 10 + + + Public Variable + + + + + + + k + + + true + + + 10 + + + Public Variable + + + + + + + l + + + true + + + 10 + + + Public Variable + + + + + + + add + + + true + + + 10 + + + Public Function + + + + + + + clone + + + true + + + 10 + + + Public Function + + + + + + + copy + + + true + + + 10 + + + Public Function + + + + + + + deltaTransformVector + + + true + + + 10 + + + Public Function + + + + + + + getEulerAngles + + + true + + + 10 + + + Public Function + + + + + + + setOrientationFromMatrix3 + + + true + + + 10 + + + Public Function + + + + + + + toString + + + true + + + 10 + + + Public Function + + + + + + + transformVectorInverse + + + true + + + 10 + + + Public Function + + + + + + + IDENTITY + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + Matrix4 + + + true + + + 10 + + + Class + + + + + + + + + + + append + + + true + + + 10 + + + Public Function + + + + + + + clone + + + true + + + 10 + + + Public Function + + + + + + + conjugate + + + true + + + 10 + + + Public Function + + + + + + + diff + + + true + + + 10 + + + Public Function + + + + + + + getEulerAngles + + + true + + + 10 + + + Public Function + + + + + + + length + + + true + + + 10 + + + Public Function + + + + + + + lengthSqr + + + true + + + 10 + + + Public Function + + + + + + + nlerp + + + true + + + 10 + + + Public Function + + + + + + + normalize + + + true + + + 10 + + + Public Function + + + + + + + prepend + + + true + + + 10 + + + Public Function + + + + + + + reset + + + true + + + 10 + + + Public Function + + + + + + + rotateByVector + + + true + + + 10 + + + Public Function + + + + + + + setFromAxisAngle + + + true + + + 10 + + + Public Function + + + + + + + setFromAxisAngleComponents + + + true + + + 10 + + + Public Function + + + + + + + setFromEulerAnglesXYZ + + + true + + + 10 + + + Public Function + + + + + + + slerp + + + true + + + 10 + + + Public Function + + + + + + + subtract + + + true + + + 10 + + + Public Function + + + + + + + toAxisVector + + + true + + + 10 + + + Public Function + + + + + + + toMatrix3 + + + true + + + 10 + + + Public Function + + + + + + + toString + + + true + + + 10 + + + Public Function + + + + + + + toVector3D + + + true + + + 10 + + + Public Function + + + + + + + w + + + true + + + 10 + + + Public Variable + + + + + + + x + + + true + + + 10 + + + Public Variable + + + + + + + y + + + true + + + 10 + + + Public Variable + + + + + + + z + + + true + + + 10 + + + Public Variable + + + + + + + addScaledVector + + + true + + + 10 + + + Public Function + + + + + + + copy + + + true + + + 10 + + + Public Function + + + + + + + createFromAxisAngle + + + true + + + 10 + + + Public Function + + + + + + + createFromAxisAngleComponents + + + true + + + 10 + + + Public Function + + + + + + + multiply + + + true + + + 10 + + + Public Function + + + + + + + _q + + + + + + true + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + Quaternion + + + true + + + 10 + + + Class + + + + + + + + + + + copyFromVector3D + + + true + + + 10 + + + Public Function + + + + + + + toString + + + true + + + 10 + + + Public Function + + + + + + + toVector3D + + + true + + + 10 + + + Public Function + + + + + + + vAdd + + + true + + + 10 + + + Public Function + + + + + + + vAddScaled + + + true + + + 10 + + + Public Function + + + + + + + vClone + + + true + + + 10 + + + Public Function + + + + + + + vCopy + + + true + + + 10 + + + Public Function + + + + + + + vCross + + + true + + + 10 + + + Public Function + + + + + + + vCross2 + + + true + + + 10 + + + Public Function + + + + + + + vDeltaTransformBy4 + + + true + + + 10 + + + Public Function + + + + + + + vDiff + + + true + + + 10 + + + Public Function + + + + + + + vDot + + + true + + + 10 + + + Public Function + + + + + + + vLength + + + true + + + 10 + + + Public Function + + + + + + + vLengthSqr + + + true + + + 10 + + + Public Function + + + + + + + vNormalize + + + true + + + 10 + + + Public Function + + + + + + + vReset + + + true + + + 10 + + + Public Function + + + + + + + vReverse + + + true + + + 10 + + + Public Function + + + + + + + vScale + + + true + + + 10 + + + Public Function + + + + + + + vSetLength + + + true + + + 10 + + + Public Function + + + + + + + vSubtract + + + true + + + 10 + + + Public Function + + + + + + + vSum + + + true + + + 10 + + + Public Function + + + + + + + vTransformBy3 + + + true + + + 10 + + + Public Function + + + + + + + vTransformBy3Tr + + + true + + + 10 + + + Public Function + + + + + + + vTransformBy4 + + + true + + + 10 + + + Public Function + + + + + + + vTransformInverseBy4 + + + true + + + 10 + + + Public Function + + + + + + + X_AXIS + + + true + + + 10 + + + Public Variable + + + + + + + Y_AXIS + + + true + + + 10 + + + Public Variable + + + + + + + Z_AXIS + + + true + + + 10 + + + Public Variable + + + + + + + ZERO + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + Vector3 + + + true + + + 10 + + + Class + + + + + + + alternativa.math + + + 10 + + + Package + + + + + + + + + + + + + + + interpolate + + + true + + + 10 + + + Public Function + + + + + + + setRotation + + + true + + + 10 + + + Public Function + + + + + + + canFreeze + + + true + + + 10 + + + Public Variable + + + + + + + invInertiaWorld + + + true + + + 10 + + + Public Variable + + + + + + + invMass + + + true + + + 10 + + + Public Variable + + + + + + + setRotationXYZ + + + true + + + 10 + + + Public Function + + + + + + + invInertia + + + true + + + 10 + + + Public Variable + + + + + + + name + + + true + + + 10 + + + Public Variable + + + + + + + state + + + true + + + 10 + + + Public Variable + + + + + + + world + + + true + + + 10 + + + Public Variable + + + + + + + addTorque + + + true + + + 10 + + + Public Function + + + + + + + true + + + saveState + + + + + + Public Function + + + + + + + setOrientation + + + true + + + 10 + + + Public Function + + + + + + + id + + + true + + + 10 + + + Public Variable + + + + + + + applyRelPosWorldImpulse + + + true + + + 10 + + + Public Function + + + + + + + true + + + integratePosition + + + + + + Public Function + + + + + + + frozen + + + true + + + 10 + + + Public Variable + + + + + + + prevState + + + true + + + 10 + + + Public Variable + + + + + + + true + + + calcAccelerations + + + + + + Public Function + + + + + + + true + + + restoreState + + + + + + Public Function + + + + + + + material + + + true + + + 10 + + + Public Variable + + + + + + + true + + + clearAccumulators + + + + + + Public Function + + + + + + + setPosition + + + true + + + 10 + + + Public Function + + + + + + + contactsNum + + + true + + + 10 + + + Public Variable + + + + + + + forceAccum + + + true + + + 10 + + + Public Variable + + + + + + + addWorldForceAtLocalPoint + + + true + + + 10 + + + Public Function + + + + + + + beforePhysicsStep + + + true + + + 10 + + + Public Function + + + + + + + true + + + integrateVelocity + + + + + + Public Function + + + + + + + baseMatrix + + + true + + + 10 + + + Public Variable + + + + + + + collisionPrimitives + + + true + + + 10 + + + Public Variable + + + + + + + addLocalForce + + + true + + + 10 + + + Public Function + + + + + + + addWorldForceXYZ + + + true + + + 10 + + + Public Function + + + + + + + calcDerivedData + + + true + + + 10 + + + Public Function + + + + + + + setVelocityXYZ + + + true + + + 10 + + + Public Function + + + + + + + angleAccel + + + true + + + 10 + + + Public Variable + + + + + + + contacts + + + true + + + 10 + + + Public Variable + + + + + + + addCollisionPrimitive + + + true + + + 10 + + + Public Function + + + + + + + addForce + + + true + + + 10 + + + Public Function + + + + + + + setVelocity + + + true + + + 10 + + + Public Function + + + + + + + aabb + + + true + + + 10 + + + Public Variable + + + + + + + accel + + + true + + + 10 + + + Public Variable + + + + + + + freezeCounter + + + true + + + 10 + + + Public Variable + + + + + + + movable + + + true + + + 10 + + + Public Variable + + + + + + + postCollisionPredicate + + + true + + + 10 + + + Public Variable + + + + + + + torqueAccum + + + true + + + 10 + + + Public Variable + + + + + + + addWorldForce + + + true + + + 10 + + + Public Function + + + + + + + addWorldForceScaled + + + true + + + 10 + + + Public Function + + + + + + + MAX_CONTACTS + + + true + + + 10 + + + Public Variable + + + + + + + addForceXYZ + + + true + + + 10 + + + Public Function + + + + + + + removeCollisionPrimitive + + + true + + + 10 + + + Public Function + + + + + + + setPositionXYZ + + + true + + + 10 + + + Public Function + + + + + + + linDamping + + + true + + + 10 + + + Public Variable + + + + + + + _f + + + + + + true + + + Private Variable + + + + + + + _r + + + + + + true + + + Private Variable + + + + + + + rotDamping + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + Body + + + true + + + 10 + + + Class + + + + + + + + + + + head + + + true + + + 10 + + + Public Variable + + + + + + + size + + + true + + + 10 + + + Public Variable + + + + + + + remove + + + true + + + 10 + + + Public Function + + + + + + + tail + + + true + + + 10 + + + Public Variable + + + + + + + append + + + true + + + 10 + + + Public Function + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + findItem + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + BodyList + + + true + + + 10 + + + Class + + + + + + + + + + + body + + + true + + + 10 + + + Public Variable + + + + + + + next + + + true + + + 10 + + + Public Variable + + + + + + + prev + + + true + + + 10 + + + Public Variable + + + + + + + clearPool + + + true + + + 10 + + + Public Function + + + + + + + dispose + + + true + + + 10 + + + Public Function + + + + + + + poolTop + + + + + + true + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + create + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + BodyListItem + + + true + + + 10 + + + Class + + + + + + + + + + + friction + + + true + + + 10 + + + Public Variable + + + + + + + restitution + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + BodyMaterial + + + true + + + 10 + + + Class + + + + + + + + + + + pos + + + true + + + 10 + + + Public Variable + + + + + + + copy + + + true + + + 10 + + + Public Function + + + + + + + orientation + + + true + + + 10 + + + Public Variable + + + + + + + rotation + + + true + + + 10 + + + Public Variable + + + + + + + velocity + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + BodyState + + + true + + + 10 + + + Class + + + + + + + + + + + head + + + true + + + 10 + + + Public Variable + + + + + + + size + + + true + + + 10 + + + Public Variable + + + + + + + remove + + + true + + + 10 + + + Public Function + + + + + + + tail + + + true + + + 10 + + + Public Variable + + + + + + + append + + + true + + + 10 + + + Public Function + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + clear + + + true + + + 10 + + + Public Function + + + + + + + findItem + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + CollisionPrimitiveList + + + true + + + 10 + + + Class + + + + + + + + + + + next + + + true + + + 10 + + + Public Variable + + + + + + + prev + + + true + + + 10 + + + Public Variable + + + + + + + primitive + + + true + + + 10 + + + Public Variable + + + + + + + clearPool + + + true + + + 10 + + + Public Function + + + + + + + dispose + + + true + + + 10 + + + Public Function + + + + + + + poolTop + + + + + + true + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + create + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + CollisionPrimitiveListItem + + + true + + + 10 + + + Class + + + + + + + + + + + body1 + + + true + + + 10 + + + Public Variable + + + + + + + body2 + + + true + + + 10 + + + Public Variable + + + + + + + friction + + + true + + + 10 + + + Public Variable + + + + + + + MAX_POINTS + + + + + + true + + + Private Variable + + + + + + + index + + + true + + + 10 + + + Public Variable + + + + + + + maxPenetration + + + true + + + 10 + + + Public Variable + + + + + + + next + + + true + + + 10 + + + Public Variable + + + + + + + true + + + normal + + + true + + + 10 + + + Public Variable + + + + + + + true + + + pcount + + + true + + + 10 + + + Public Variable + + + + + + + points + + + true + + + 10 + + + Public Variable + + + + + + + restitution + + + true + + + 10 + + + Public Variable + + + + + + + satisfied + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + Contact + + + true + + + 10 + + + Class + + + + + + + + + + + accumImpulseN + + + true + + + 10 + + + Public Variable + + + + + + + angularInertia2 + + + true + + + 10 + + + Public Variable + + + + + + + feature1 + + + true + + + 10 + + + Public Variable + + + + + + + feature2 + + + true + + + 10 + + + Public Variable + + + + + + + minSepVel + + + true + + + 10 + + + Public Variable + + + + + + + normalVel + + + true + + + 10 + + + Public Variable + + + + + + + pos + + + true + + + 10 + + + Public Variable + + + + + + + r1 + + + true + + + 10 + + + Public Variable + + + + + + + r2 + + + true + + + 10 + + + Public Variable + + + + + + + copyFrom + + + true + + + 10 + + + Public Function + + + + + + + angularInertia1 + + + true + + + 10 + + + Public Variable + + + + + + + penetration + + + true + + + 10 + + + Public Variable + + + + + + + satisfied + + + true + + + 10 + + + Public Variable + + + + + + + true + + + velByUnitImpulseN + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + ContactPoint + + + true + + + 10 + + + Class + + + + + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + getBoxInvInertia + + + true + + + 10 + + + Public Function + + + + + + + getCylinderInvInertia + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + PhysicsUtils + + + true + + + 10 + + + Class + + + + + + + + + + + removeConstraint + + + true + + + 10 + + + Public Function + + + + + + + _gravityMagnitude + + + true + + + 10 + + + Public Variable + + + + + + + allowedPenetration + + + true + + + 10 + + + Public Variable + + + + + + + collisionIterations + + + true + + + 10 + + + Public Variable + + + + + + + penResolutionSteps + + + true + + + 10 + + + Public Variable + + + + + + + timeStamp + + + true + + + 10 + + + Public Variable + + + + + + + detectCollisions + + + + + + true + + + Private Function + + + + + + + intergateVelocities + + + + + + true + + + Private Function + + + + + + + resolveInterpenetration + + + + + + true + + + Private Function + + + + + + + _gravity + + + true + + + 10 + + + Public Variable + + + + + + + postPhysics + + + + + + true + + + Private Function + + + + + + + resolveContact + + + + + + true + + + Private Function + + + + + + + separateContactPoint + + + + + + true + + + Private Function + + + + + + + angSpeedFreezeLimit + + + true + + + 10 + + + Public Variable + + + + + + + collisionDetector + + + true + + + 10 + + + Public Variable + + + + + + + _v1 + + + + + + true + + + Private Variable + + + + + + + _v2 + + + + + + true + + + Private Variable + + + + + + + maxPenResolutionSpeed + + + true + + + 10 + + + Public Variable + + + + + + + staticSeparationIterations + + + true + + + 10 + + + Public Variable + + + + + + + usePrediction + + + true + + + 10 + + + Public Variable + + + + + + + gravity + + + true + + + 10 + + + Public Function + + + + + + + applyForces + + + + + + true + + + Private Function + + + + + + + true + + + constraints + + + + + + Public Variable + + + + + + + staticSeparationSteps + + + true + + + 10 + + + Public Variable + + + + + + + integratePositions + + + + + + true + + + Private Function + + + + + + + removeBody + + + true + + + 10 + + + Public Function + + + + + + + runPhysics + + + true + + + 10 + + + Public Function + + + + + + + preProcessContacts + + + + + + true + + + Private Function + + + + + + + bodies + + + true + + + 10 + + + Public Variable + + + + + + + true + + + contacts + + + + + + Public Variable + + + + + + + freezeSteps + + + true + + + 10 + + + Public Variable + + + + + + + linSpeedFreezeLimit + + + true + + + 10 + + + Public Variable + + + + + + + maxAngleMove + + + true + + + 10 + + + Public Variable + + + + + + + time + + + true + + + 10 + + + Public Variable + + + + + + + gravity + + + true + + + 10 + + + Public Function + + + + + + + borderContact + + + + + + true + + + Private Variable + + + + + + + addConstraint + + + true + + + 10 + + + Public Function + + + + + + + processContacts + + + + + + true + + + Private Function + + + + + + + addBody + + + true + + + 10 + + + Public Function + + + + + + + calcSepVelocity + + + + + + true + + + Private Function + + + + + + + performStaticSeparation + + + + + + true + + + Private Function + + + + + + + resolveContactPoint + + + + + + true + + + Private Function + + + + + + + true + + + constraintsNum + + + + + + Public Variable + + + + + + + true + + + MAX_CONTACTS + + + + + + Public Variable + + + + + + + _r + + + + + + true + + + Private Variable + + + + + + + _t + + + + + + true + + + Private Variable + + + + + + + _v + + + + + + true + + + Private Variable + + + + + + + contactIterations + + + true + + + 10 + + + Public Variable + + + + + + + useStaticSeparation + + + true + + + 10 + + + Public Variable + + + + + + + lastBodyId + + + + + + true + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + World + + + true + + + 10 + + + Class + + + + + + + alternativa.physics + + + 10 + + + Package + + + + + + + + + + + + + + + true + + + axis + + + true + + + 10 + + + Public Variable + + + + + + + boundBox + + + true + + + 10 + + + Public Variable + + + + + + + coord + + + true + + + 10 + + + Public Variable + + + + + + + indices + + + true + + + 10 + + + Public Variable + + + + + + + negativeNode + + + true + + + 10 + + + Public Variable + + + + + + + parent + + + true + + + 10 + + + Public Variable + + + + + + + positiveNode + + + true + + + 10 + + + Public Variable + + + + + + + splitIndices + + + true + + + 10 + + + Public Variable + + + + + + + splitTree + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + CollisionKdNode + + + true + + + 10 + + + Class + + + + + + + + + + + minPrimitivesPerNode + + + true + + + 10 + + + Public Variable + + + + + + + splitNode + + + + + + true + + + Private Function + + + + + + + rootNode + + + true + + + 10 + + + Public Variable + + + + + + + splitCost + + + + + + true + + + Private Variable + + + + + + + staticBoundBoxes + + + true + + + 10 + + + Public Variable + + + + + + + splitCoord + + + + + + true + + + Private Variable + + + + + + + staticChildren + + + true + + + 10 + + + Public Variable + + + + + + + threshold + + + true + + + 10 + + + Public Variable + + + + + + + traceTree + + + true + + + 10 + + + Public Function + + + + + + + numStaticChildren + + + true + + + 10 + + + Public Variable + + + + + + + checkNodeAxis + + + + + + true + + + Private Function + + + + + + + _bb + + + + + + true + + + Private Variable + + + + + + + _nodeBB + + + + + + true + + + Private Variable + + + + + + + nodeBoundBoxThreshold + + + + + + true + + + Private Variable + + + + + + + splitAxis + + + + + + true + + + Private Variable + + + + + + + createTree + + + true + + + 10 + + + Public Function + + + + + + + traceNode + + + + + + true + + + Private Function + + + + + + + splitCoordsX + + + + + + true + + + Private Variable + + + + + + + splitCoordsY + + + + + + true + + + Private Variable + + + + + + + splitCoordsZ + + + + + + true + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + CollisionKdTree + + + true + + + 10 + + + Class + + + + + + + + + + + minPrimitivesPerNode + + + true + + + 10 + + + Public Variable + + + + + + + rootNode + + + true + + + 10 + + + Public Variable + + + + + + + splitCoord + + + + + + true + + + Private Variable + + + + + + + splitCost + + + + + + true + + + Private Variable + + + + + + + parentNode + + + true + + + 10 + + + Public Variable + + + + + + + parentTree + + + true + + + 10 + + + Public Variable + + + + + + + threshold + + + true + + + 10 + + + Public Variable + + + + + + + checkNodeAxis + + + + + + true + + + Private Function + + + + + + + createTree + + + true + + + 10 + + + Public Function + + + + + + + _bb + + + + + + true + + + Private Variable + + + + + + + nodeBoundBoxThreshold + + + + + + true + + + Private Variable + + + + + + + splitAxis + + + + + + true + + + Private Variable + + + + + + + splitNode + + + + + + true + + + Private Function + + + + + + + _nodeBB + + + + + + true + + + Private Variable + + + + + + + splitCoordsX + + + + + + true + + + Private Variable + + + + + + + splitCoordsY + + + + + + true + + + Private Variable + + + + + + + splitCoordsZ + + + + + + true + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + CollisionKdTree2D + + + true + + + 10 + + + Class + + + + + + + + + + + aabb + + + true + + + 10 + + + Public Variable + + + + + + + body + + + true + + + 10 + + + Public Variable + + + + + + + localTransform + + + true + + + 10 + + + Public Variable + + + + + + + postCollisionPredicate + + + true + + + 10 + + + Public Variable + + + + + + + calculateAABB + + + true + + + 10 + + + Public Function + + + + + + + setBody + + + true + + + 10 + + + Public Function + + + + + + + collisionGroup + + + true + + + 10 + + + Public Variable + + + + + + + transform + + + true + + + 10 + + + Public Variable + + + + + + + true + + + createPrimitive + + + + + + true + + + 10 + + + Private Function + + + + + + + clone + + + true + + + 10 + + + Public Function + + + + + + + BOX + + + true + + + 10 + + + Public Variable + + + + + + + PLANE + + + true + + + 10 + + + Public Variable + + + + + + + SPHERE + + + true + + + 10 + + + Public Variable + + + + + + + TRIANGLE + + + true + + + 10 + + + Public Variable + + + + + + + type + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + copyFrom + + + true + + + 10 + + + Public Function + + + + + + + getRayIntersection + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + toString + + + true + + + 10 + + + Public Function + + + + + + + RECT + + + true + + + 10 + + + Public Variable + + + + + + + CollisionPrimitive + + + true + + + 10 + + + Class + + + + + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + true + + + considerBodies + + + + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + IBodyCollisionPredicate + + + true + + + 10 + + + Class + + + + + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + getContact + + + + + + 10 + + + Public Function + + + + + + + haveCollision + + + + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + ICollider + + + true + + + 10 + + + Class + + + + + + + true + + + + + + + true + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + true + + + getAllContacts + + + + + + Public Function + + + + + + + true + + + getContact + + + + + + 10 + + + Public Function + + + + + + + true + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + true + + + intersectRay + + + + + + Public Function + + + + + + + true + + + testCollision + + + + + + Public Function + + + + + + + ICollisionDetector + + + true + + + 10 + + + Class + + + + + + + true + + + + + + + true + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + true + + + considerCollision + + + + + + Public Function + + + + + + + true + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + ICollisionPredicate + + + true + + + 10 + + + Class + + + + + + + true + + + + + + + true + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + true + + + considerBody + + + + + + Public Function + + + + + + + true + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + IRayCollisionPredicate + + + true + + + 10 + + + Class + + + + + + + + + + + addPrimitive + + + true + + + 10 + + + Public Function + + + + + + + removePrimitive + + + true + + + 10 + + + Public Function + + + + + + + testCollision + + + true + + + 10 + + + Public Function + + + + + + + true + + + dynamicPrimitives + + + + + + Public Variable + + + + + + + init + + + true + + + 10 + + + Public Function + + + + + + + intersectRayWithStatic + + + true + + + 10 + + + Public Function + + + + + + + intersectRayWithDynamic + + + + + + true + + + Private Function + + + + + + + intersectRay + + + true + + + 10 + + + Public Function + + + + + + + testBodyPrimitiveCollision + + + true + + + 10 + + + Public Function + + + + + + + getRayNodeIntersection + + + + + + true + + + Private Function + + + + + + + _dynamicIntersection + + + + + + true + + + Private Variable + + + + + + + getContact + + + true + + + 10 + + + Public Function + + + + + + + true + + + threshold + + + + + + Public Variable + + + + + + + _time + + + + + + true + + + Private Variable + + + + + + + getAllContacts + + + true + + + 10 + + + Public Function + + + + + + + _n + + + + + + true + + + Private Variable + + + + + + + _o + + + + + + true + + + Private Variable + + + + + + + addCollider + + + + + + true + + + Private Function + + + + + + + getPrimitiveNodeCollisions + + + + + + true + + + Private Function + + + + + + + true + + + tree + + + + + + Public Variable + + + + + + + getRayBoundBoxIntersection + + + + + + true + + + Private Function + + + + + + + testRayAgainstNode + + + + + + true + + + Private Function + + + + + + + true + + + dynamicPrimitivesNum + + + + + + Public Variable + + + + + + + _rayAABB + + + + + + true + + + Private Variable + + + + + + + colliders + + + + + + true + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + KdTreeCollisionDetector + + + true + + + 10 + + + Class + + + + + + + alternativa.physics.collision + + + 10 + + + Package + + + + + + + + + + + + + + + axis10 + + + + + + true + + + Private Variable + + + + + + + axis11 + + + + + + true + + + Private Variable + + + + + + + findEdgesIntersection + + + + + + true + + + Private Function + + + + + + + axis12 + + + + + + true + + + Private Variable + + + + + + + true + + + vectorToBox1 + + + + + + Private Variable + + + + + + + findFaceContactPoints + + + + + + true + + + Private Function + + + + + + + true + + + overlapOnAxis + + + true + + + 10 + + + Public Function + + + + + + + clip + + + + + + true + + + Private Function + + + + + + + minOverlap + + + + + + true + + + Private Variable + + + + + + + haveCollision + + + true + + + 10 + + + Public Function + + + + + + + true + + + axis + + + + + + Private Variable + + + + + + + axis20 + + + + + + true + + + Private Variable + + + + + + + axis21 + + + + + + true + + + Private Variable + + + + + + + axis22 + + + + + + true + + + Private Variable + + + + + + + true + + + epsilon + + + + + + Private Variable + + + + + + + true + + + pcount + + + + + + Private Variable + + + + + + + tmpPoints + + + + + + true + + + Private Variable + + + + + + + testMainAxis + + + + + + true + + + Private Function + + + + + + + bestAxisIndex + + + + + + true + + + Private Variable + + + + + + + points1 + + + + + + true + + + Private Variable + + + + + + + points2 + + + + + + true + + + Private Variable + + + + + + + reducePoints + + + + + + true + + + Private Function + + + + + + + getContact + + + true + + + 10 + + + Public Function + + + + + + + getPointBoxPenetration + + + + + + true + + + Private Function + + + + + + + testDerivedAxis + + + + + + true + + + Private Function + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + BoxBoxCollider + + + true + + + 10 + + + Class + + + + + + + + + + + getContact + + + true + + + 10 + + + Public Function + + + + + + + clipHighX + + + + + + true + + + Private Function + + + + + + + clipHighZ + + + + + + true + + + Private Function + + + + + + + clipLowX + + + + + + true + + + Private Function + + + + + + + clipLowY + + + + + + true + + + Private Function + + + + + + + getFaceVertsByAxis + + + + + + true + + + Private Function + + + + + + + haveCollision + + + true + + + 10 + + + Public Function + + + + + + + clipHighY + + + + + + true + + + Private Function + + + + + + + clipLowZ + + + + + + true + + + Private Function + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + BoxCollider + + + true + + + 10 + + + Class + + + + + + + + + + + true + + + normal + + + + + + Private Variable + + + + + + + verts1 + + + + + + true + + + Private Variable + + + + + + + verts2 + + + + + + true + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + getContact + + + true + + + 10 + + + Public Function + + + + + + + haveCollision + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + BoxPlaneCollider + + + true + + + 10 + + + Class + + + + + + + + + + + true + + + axis + + + + + + Private Variable + + + + + + + axis10 + + + + + + true + + + Private Variable + + + + + + + axis11 + + + + + + true + + + Private Variable + + + + + + + findEdgesIntersection + + + + + + true + + + Private Function + + + + + + + axis12 + + + + + + true + + + Private Variable + + + + + + + clipByBox + + + + + + true + + + Private Function + + + + + + + true + + + vectorToBox + + + + + + Private Variable + + + + + + + true + + + overlapOnAxis + + + true + + + 10 + + + Public Function + + + + + + + clipByRect + + + + + + true + + + Private Function + + + + + + + minOverlap + + + + + + true + + + Private Variable + + + + + + + haveCollision + + + true + + + 10 + + + Public Function + + + + + + + axis20 + + + + + + true + + + Private Variable + + + + + + + axis21 + + + + + + true + + + Private Variable + + + + + + + findFaceContactPoints + + + + + + true + + + Private Function + + + + + + + axis22 + + + + + + true + + + Private Variable + + + + + + + true + + + epsilon + + + + + + Private Variable + + + + + + + testMainAxis + + + + + + true + + + Private Function + + + + + + + bestAxisIndex + + + + + + true + + + Private Variable + + + + + + + points1 + + + + + + true + + + Private Variable + + + + + + + points2 + + + + + + true + + + Private Variable + + + + + + + getContact + + + true + + + 10 + + + Public Function + + + + + + + getPointBoxPenetration + + + + + + true + + + Private Function + + + + + + + testDerivedAxis + + + + + + true + + + Private Function + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + BoxRectCollider + + + true + + + 10 + + + Class + + + + + + + + + + + sPos + + + + + + true + + + Private Variable + + + + + + + getContact + + + true + + + 10 + + + Public Function + + + + + + + haveCollision + + + true + + + 10 + + + Public Function + + + + + + + bPos + + + + + + true + + + Private Variable + + + + + + + center + + + + + + true + + + Private Variable + + + + + + + closestPt + + + + + + true + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + BoxSphereCollider + + + true + + + 10 + + + Class + + + + + + + + + + + axis11 + + + + + + true + + + Private Variable + + + + + + + axis12 + + + + + + true + + + Private Variable + + + + + + + true + + + vector + + + + + + Private Variable + + + + + + + findFaceContactPoints + + + + + + true + + + Private Function + + + + + + + getBoxToTriContact + + + + + + true + + + Private Function + + + + + + + axis10 + + + + + + true + + + Private Variable + + + + + + + colNormal + + + + + + true + + + Private Variable + + + + + + + true + + + toBox + + + + + + Private Variable + + + + + + + clipByBox + + + + + + true + + + Private Function + + + + + + + minOverlap + + + + + + true + + + Private Variable + + + + + + + true + + + overlapOnAxis + + + + + + Private Function + + + + + + + haveCollision + + + true + + + 10 + + + Public Function + + + + + + + clipByLine + + + + + + true + + + Private Function + + + + + + + getTriToBoxContact + + + + + + true + + + Private Function + + + + + + + true + + + axis + + + + + + Private Variable + + + + + + + axis20 + + + + + + true + + + Private Variable + + + + + + + axis21 + + + + + + true + + + Private Variable + + + + + + + axis22 + + + + + + true + + + Private Variable + + + + + + + point2 + + + + + + true + + + Private Variable + + + + + + + true + + + epsilon + + + true + + + 10 + + + Public Variable + + + + + + + testMainAxis + + + + + + true + + + Private Function + + + + + + + bestAxisIndex + + + + + + true + + + Private Variable + + + + + + + point1 + + + + + + true + + + Private Variable + + + + + + + points1 + + + + + + true + + + Private Variable + + + + + + + clipByTriangle + + + + + + true + + + Private Function + + + + + + + points2 + + + + + + true + + + Private Variable + + + + + + + getContact + + + true + + + 10 + + + Public Function + + + + + + + findEdgesIntersection + + + + + + true + + + Private Function + + + + + + + getPointBoxPenetration + + + + + + true + + + Private Function + + + + + + + testDerivedAxis + + + + + + true + + + Private Function + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + BoxTriangleCollider + + + true + + + 10 + + + Class + + + + + + + + + + + getContact + + + true + + + 10 + + + Public Function + + + + + + + true + + + normal + + + + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + haveCollision + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + SpherePlaneCollider + + + true + + + 10 + + + Class + + + + + + + + + + + p1 + + + + + + true + + + Private Variable + + + + + + + haveCollision + + + true + + + 10 + + + Public Function + + + + + + + p2 + + + + + + true + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + getContact + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + SphereSphereCollider + + + true + + + 10 + + + Class + + + + + + + alternativa.physics.collision.colliders + + + 10 + + + Package + + + + + + + + + + + + + + + true + + + createPrimitive + + + + + + true + + + 10 + + + Private Function + + + + + + + calculateAABB + + + true + + + 10 + + + Public Function + + + + + + + getRayIntersection + + + true + + + 10 + + + Public Function + + + + + + + excludedFaces + + + true + + + 10 + + + Public Variable + + + + + + + hs + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + copyFrom + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + toString + + + true + + + 10 + + + Public Function + + + + + + + CollisionBox + + + true + + + 10 + + + Class + + + + + + + + + + + twoSided + + + true + + + 10 + + + Public Variable + + + + + + + calculateAABB + + + true + + + 10 + + + Public Function + + + + + + + getRayIntersection + + + true + + + 10 + + + Public Function + + + + + + + true + + + createPrimitive + + + + + + true + + + 10 + + + Private Function + + + + + + + hs + + + true + + + 10 + + + Public Variable + + + + + + + copyFrom + + + true + + + 10 + + + Public Function + + + + + + + toString + + + true + + + 10 + + + Public Function + + + + + + + EPSILON + + + + + + true + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + CollisionRect + + + true + + + 10 + + + Class + + + + + + + + + + + r + + + true + + + 10 + + + Public Variable + + + + + + + calculateAABB + + + true + + + 10 + + + Public Function + + + + + + + true + + + createPrimitive + + + + + + true + + + 10 + + + Private Function + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + copyFrom + + + true + + + 10 + + + Public Function + + + + + + + getRayIntersection + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + CollisionSphere + + + true + + + 10 + + + Class + + + + + + + + + + + v0 + + + true + + + 10 + + + Public Variable + + + + + + + calculateAABB + + + true + + + 10 + + + Public Function + + + + + + + getRayIntersection + + + true + + + 10 + + + Public Function + + + + + + + initVertices + + + + + + true + + + Private Function + + + + + + + e0 + + + true + + + 10 + + + Public Variable + + + + + + + e1 + + + true + + + 10 + + + Public Variable + + + + + + + e2 + + + true + + + 10 + + + Public Variable + + + + + + + copyFrom + + + true + + + 10 + + + Public Function + + + + + + + true + + + createPrimitive + + + + + + true + + + 10 + + + Private Function + + + + + + + toString + + + true + + + 10 + + + Public Function + + + + + + + v1 + + + true + + + 10 + + + Public Variable + + + + + + + v2 + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + CollisionTriangle + + + true + + + 10 + + + Class + + + + + + + alternativa.physics.collision.primitives + + + 10 + + + Package + + + + + + + + + + + + + + + clone + + + true + + + 10 + + + Public Function + + + + + + + setSize + + + true + + + 10 + + + Public Function + + + + + + + maxX + + + true + + + 10 + + + Public Variable + + + + + + + maxY + + + true + + + 10 + + + Public Variable + + + + + + + maxZ + + + true + + + 10 + + + Public Variable + + + + + + + minX + + + true + + + 10 + + + Public Variable + + + + + + + minY + + + true + + + 10 + + + Public Variable + + + + + + + minZ + + + true + + + 10 + + + Public Variable + + + + + + + addBoundBox + + + true + + + 10 + + + Public Function + + + + + + + addPoint + + + true + + + 10 + + + Public Function + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + copyFrom + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + infinity + + + true + + + 10 + + + Public Function + + + + + + + intersects + + + true + + + 10 + + + Public Function + + + + + + + toString + + + true + + + 10 + + + Public Function + + + + + + + BoundBox + + + true + + + 10 + + + Class + + + + + + + + + + + dir + + + true + + + 10 + + + Public Variable + + + + + + + origin + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + Ray + + + true + + + 10 + + + Class + + + + + + + + + + + true + + + normal + + + true + + + 10 + + + Public Variable + + + + + + + pos + + + true + + + 10 + + + Public Variable + + + + + + + primitive + + + true + + + 10 + + + Public Variable + + + + + + + t + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + copy + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + RayIntersection + + + true + + + 10 + + + Class + + + + + + + alternativa.physics.collision.types + + + 10 + + + Package + + + + + + + + + + + true + + + + + + + true + + + satisfied + + + + + + Public Variable + + + + + + + true + + + world + + + + + + Public Variable + + + + + + + true + + + apply + + + true + + + 10 + + + Public Function + + + + + + + true + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + true + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + true + + + preProcess + + + true + + + 10 + + + Public Function + + + + + + + Constraint + + + true + + + 10 + + + Class + + + + + + + + + + + preProcess + + + true + + + 10 + + + Public Function + + + + + + + impulseDirection + + + + + + true + + + Private Variable + + + + + + + true + + + body1 + + + + + + Public Variable + + + + + + + true + + + body2 + + + + + + Public Variable + + + + + + + true + + + velByUnitImpulseN + + + + + + Private Variable + + + + + + + apply + + + true + + + 10 + + + Public Function + + + + + + + true + + + maxDistance + + + + + + Public Variable + + + + + + + true + + + r2 + + + + + + Public Variable + + + + + + + true + + + wr1 + + + + + + Public Variable + + + + + + + true + + + wr2 + + + + + + Public Variable + + + + + + + minClosingVel + + + + + + true + + + Private Variable + + + + + + + true + + + r1 + + + + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + MaxDistanceConstraint + + + true + + + 10 + + + Class + + + + + + + alternativa.physics.constraints + + + 10 + + + Package + + + + + + + + + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + setParams + + + true + + + 10 + + + Public Function + + + + + + + RigidBox + + + true + + + 10 + + + Class + + + + + + + true + + + + + + + true + + + h + + + + + + Public Variable + + + + + + + true + + + r + + + + + + Public Variable + + + + + + + true + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + true + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + true + + + setParams + + + true + + + 10 + + + Public Function + + + + + + + RigidCylinder + + + true + + + 10 + + + Class + + + + + + + + + + + true + + + normal + + + true + + + 10 + + + Public Variable + + + + + + + offset + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + RigidPlane + + + true + + + 10 + + + Class + + + + + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + RigidRect + + + true + + + 10 + + + Class + + + + + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + RigidSphere + + + true + + + 10 + + + Class + + + + + + + alternativa.physics.primitives + + + 10 + + + Package + + + + + + + FFFFFF + + + true + + + D:\work\eclipse\ws_projects\TanksTestingTool\lib\AlternativaPhysics.swc + + + AlternativaPhysics.swc + + + 1 + + + 24.0 + + + Not Present + + + + + + + Time Line + + + + + + + 500 X 375 + + + 157458 + + + 10 + + + + + + + 157458 + + + 10 + + + + + + + diff --git a/0.0.9.1/META-INF/MANIFEST.MF b/0.0.9.1/META-INF/MANIFEST.MF new file mode 100644 index 0000000..5066a55 --- /dev/null +++ b/0.0.9.1/META-INF/MANIFEST.MF @@ -0,0 +1 @@ +Bundle-Name: platform.clients.fp10.libraries.AlternativaPhysics diff --git a/0.0.9.1/META-INF/security.sspj b/0.0.9.1/META-INF/security.sspj new file mode 100644 index 0000000..e463c72 --- /dev/null +++ b/0.0.9.1/META-INF/security.sspj @@ -0,0 +1,11923 @@ + + + + + true + + + true + + + true + + + 3.4 + + + true + + + true + + + true + + + + + + + + + + + true + + + FFFFFF + + + D:\work\eclipse\ws_projects\TanksTestingTool\lib\AlternativaPhysics.swc + + + AlternativaPhysics.swc + + + 1 + + + 24.0 + + + Not Present + + + 500 X 375 + + + + + + + true + + + + + + + + + + + + + + + + append + + + true + + + 10 + + + Public Function + + + + + + + getEulerAngles + + + true + + + 10 + + + Public Function + + + + + + + invert + + + true + + + 10 + + + Public Function + + + + + + + prepend + + + true + + + 10 + + + Public Function + + + + + + + setRotationMatrix + + + true + + + 10 + + + Public Function + + + + + + + subtract + + + true + + + 10 + + + Public Function + + + + + + + toIdentity + + + true + + + 10 + + + Public Function + + + + + + + transformVector + + + true + + + 10 + + + Public Function + + + + + + + transformVector3To3D + + + true + + + 10 + + + Public Function + + + + + + + transpose + + + true + + + 10 + + + Public Function + + + + + + + a + + + true + + + 10 + + + Public Variable + + + + + + + b + + + true + + + 10 + + + Public Variable + + + + + + + c + + + true + + + 10 + + + Public Variable + + + + + + + clone + + + true + + + 10 + + + Public Function + + + + + + + e + + + true + + + 10 + + + Public Variable + + + + + + + f + + + true + + + 10 + + + Public Variable + + + + + + + g + + + true + + + 10 + + + Public Variable + + + + + + + add + + + true + + + 10 + + + Public Function + + + + + + + i + + + true + + + 10 + + + Public Variable + + + + + + + j + + + true + + + 10 + + + Public Variable + + + + + + + k + + + true + + + 10 + + + Public Variable + + + + + + + copy + + + true + + + 10 + + + Public Function + + + + + + + createSkewSymmetric + + + true + + + 10 + + + Public Function + + + + + + + fromAxisAngle + + + true + + + 10 + + + Public Function + + + + + + + prependTransposed + + + true + + + 10 + + + Public Function + + + + + + + toString + + + true + + + 10 + + + Public Function + + + + + + + transformVectorInverse + + + true + + + 10 + + + Public Function + + + + + + + IDENTITY + + + true + + + 10 + + + Public Variable + + + + + + + ZERO + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + Matrix3 + + + true + + + 10 + + + Class + + + + + + + + + + + append + + + true + + + 10 + + + Public Function + + + + + + + deltaTransformVectorInverse + + + true + + + 10 + + + Public Function + + + + + + + getAxis + + + true + + + 10 + + + Public Function + + + + + + + invert + + + true + + + 10 + + + Public Function + + + + + + + prepend + + + true + + + 10 + + + Public Function + + + + + + + setFromMatrix3 + + + true + + + 10 + + + Public Function + + + + + + + subtract + + + true + + + 10 + + + Public Function + + + + + + + toIdentity + + + true + + + 10 + + + Public Function + + + + + + + transformVector + + + true + + + 10 + + + Public Function + + + + + + + transformVectors + + + true + + + 10 + + + Public Function + + + + + + + transformVectorsInverse + + + true + + + 10 + + + Public Function + + + + + + + transformVectorsInverseN + + + true + + + 10 + + + Public Function + + + + + + + transformVectorsN + + + true + + + 10 + + + Public Function + + + + + + + a + + + true + + + 10 + + + Public Variable + + + + + + + b + + + true + + + 10 + + + Public Variable + + + + + + + c + + + true + + + 10 + + + Public Variable + + + + + + + d + + + true + + + 10 + + + Public Variable + + + + + + + e + + + true + + + 10 + + + Public Variable + + + + + + + f + + + true + + + 10 + + + Public Variable + + + + + + + g + + + true + + + 10 + + + Public Variable + + + + + + + h + + + true + + + 10 + + + Public Variable + + + + + + + i + + + true + + + 10 + + + Public Variable + + + + + + + j + + + true + + + 10 + + + Public Variable + + + + + + + k + + + true + + + 10 + + + Public Variable + + + + + + + l + + + true + + + 10 + + + Public Variable + + + + + + + add + + + true + + + 10 + + + Public Function + + + + + + + clone + + + true + + + 10 + + + Public Function + + + + + + + copy + + + true + + + 10 + + + Public Function + + + + + + + deltaTransformVector + + + true + + + 10 + + + Public Function + + + + + + + getEulerAngles + + + true + + + 10 + + + Public Function + + + + + + + setOrientationFromMatrix3 + + + true + + + 10 + + + Public Function + + + + + + + toString + + + true + + + 10 + + + Public Function + + + + + + + transformVectorInverse + + + true + + + 10 + + + Public Function + + + + + + + IDENTITY + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + Matrix4 + + + true + + + 10 + + + Class + + + + + + + + + + + append + + + true + + + 10 + + + Public Function + + + + + + + clone + + + true + + + 10 + + + Public Function + + + + + + + conjugate + + + true + + + 10 + + + Public Function + + + + + + + diff + + + true + + + 10 + + + Public Function + + + + + + + getEulerAngles + + + true + + + 10 + + + Public Function + + + + + + + length + + + true + + + 10 + + + Public Function + + + + + + + lengthSqr + + + true + + + 10 + + + Public Function + + + + + + + nlerp + + + true + + + 10 + + + Public Function + + + + + + + normalize + + + true + + + 10 + + + Public Function + + + + + + + prepend + + + true + + + 10 + + + Public Function + + + + + + + reset + + + true + + + 10 + + + Public Function + + + + + + + rotateByVector + + + true + + + 10 + + + Public Function + + + + + + + setFromAxisAngle + + + true + + + 10 + + + Public Function + + + + + + + setFromAxisAngleComponents + + + true + + + 10 + + + Public Function + + + + + + + setFromEulerAnglesXYZ + + + true + + + 10 + + + Public Function + + + + + + + slerp + + + true + + + 10 + + + Public Function + + + + + + + subtract + + + true + + + 10 + + + Public Function + + + + + + + toAxisVector + + + true + + + 10 + + + Public Function + + + + + + + toMatrix3 + + + true + + + 10 + + + Public Function + + + + + + + toString + + + true + + + 10 + + + Public Function + + + + + + + toVector3D + + + true + + + 10 + + + Public Function + + + + + + + w + + + true + + + 10 + + + Public Variable + + + + + + + x + + + true + + + 10 + + + Public Variable + + + + + + + y + + + true + + + 10 + + + Public Variable + + + + + + + z + + + true + + + 10 + + + Public Variable + + + + + + + addScaledVector + + + true + + + 10 + + + Public Function + + + + + + + copy + + + true + + + 10 + + + Public Function + + + + + + + createFromAxisAngle + + + true + + + 10 + + + Public Function + + + + + + + createFromAxisAngleComponents + + + true + + + 10 + + + Public Function + + + + + + + multiply + + + true + + + 10 + + + Public Function + + + + + + + _q + + + + + + true + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + Quaternion + + + true + + + 10 + + + Class + + + + + + + + + + + copyFromVector3D + + + true + + + 10 + + + Public Function + + + + + + + toString + + + true + + + 10 + + + Public Function + + + + + + + toVector3D + + + true + + + 10 + + + Public Function + + + + + + + vAdd + + + true + + + 10 + + + Public Function + + + + + + + vAddScaled + + + true + + + 10 + + + Public Function + + + + + + + vClone + + + true + + + 10 + + + Public Function + + + + + + + vCopy + + + true + + + 10 + + + Public Function + + + + + + + vCross + + + true + + + 10 + + + Public Function + + + + + + + vCross2 + + + true + + + 10 + + + Public Function + + + + + + + vDeltaTransformBy4 + + + true + + + 10 + + + Public Function + + + + + + + vDiff + + + true + + + 10 + + + Public Function + + + + + + + vDot + + + true + + + 10 + + + Public Function + + + + + + + vLength + + + true + + + 10 + + + Public Function + + + + + + + vLengthSqr + + + true + + + 10 + + + Public Function + + + + + + + vNormalize + + + true + + + 10 + + + Public Function + + + + + + + vReset + + + true + + + 10 + + + Public Function + + + + + + + vReverse + + + true + + + 10 + + + Public Function + + + + + + + vScale + + + true + + + 10 + + + Public Function + + + + + + + vSetLength + + + true + + + 10 + + + Public Function + + + + + + + vSubtract + + + true + + + 10 + + + Public Function + + + + + + + vSum + + + true + + + 10 + + + Public Function + + + + + + + vTransformBy3 + + + true + + + 10 + + + Public Function + + + + + + + vTransformBy3Tr + + + true + + + 10 + + + Public Function + + + + + + + vTransformBy4 + + + true + + + 10 + + + Public Function + + + + + + + vTransformInverseBy4 + + + true + + + 10 + + + Public Function + + + + + + + X_AXIS + + + true + + + 10 + + + Public Variable + + + + + + + Y_AXIS + + + true + + + 10 + + + Public Variable + + + + + + + Z_AXIS + + + true + + + 10 + + + Public Variable + + + + + + + ZERO + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + Vector3 + + + true + + + 10 + + + Class + + + + + + + alternativa.math + + + 10 + + + Package + + + + + + + + + + + + + + + interpolate + + + true + + + 10 + + + Public Function + + + + + + + setRotation + + + true + + + 10 + + + Public Function + + + + + + + canFreeze + + + true + + + 10 + + + Public Variable + + + + + + + invInertiaWorld + + + true + + + 10 + + + Public Variable + + + + + + + invMass + + + true + + + 10 + + + Public Variable + + + + + + + setRotationXYZ + + + true + + + 10 + + + Public Function + + + + + + + invInertia + + + true + + + 10 + + + Public Variable + + + + + + + name + + + true + + + 10 + + + Public Variable + + + + + + + state + + + true + + + 10 + + + Public Variable + + + + + + + world + + + true + + + 10 + + + Public Variable + + + + + + + addTorque + + + true + + + 10 + + + Public Function + + + + + + + true + + + saveState + + + + + + Public Function + + + + + + + setOrientation + + + true + + + 10 + + + Public Function + + + + + + + id + + + true + + + 10 + + + Public Variable + + + + + + + applyRelPosWorldImpulse + + + true + + + 10 + + + Public Function + + + + + + + true + + + integratePosition + + + + + + Public Function + + + + + + + frozen + + + true + + + 10 + + + Public Variable + + + + + + + prevState + + + true + + + 10 + + + Public Variable + + + + + + + true + + + calcAccelerations + + + + + + Public Function + + + + + + + true + + + restoreState + + + + + + Public Function + + + + + + + material + + + true + + + 10 + + + Public Variable + + + + + + + true + + + clearAccumulators + + + + + + Public Function + + + + + + + setPosition + + + true + + + 10 + + + Public Function + + + + + + + contactsNum + + + true + + + 10 + + + Public Variable + + + + + + + forceAccum + + + true + + + 10 + + + Public Variable + + + + + + + addWorldForceAtLocalPoint + + + true + + + 10 + + + Public Function + + + + + + + beforePhysicsStep + + + true + + + 10 + + + Public Function + + + + + + + true + + + integrateVelocity + + + + + + Public Function + + + + + + + baseMatrix + + + true + + + 10 + + + Public Variable + + + + + + + collisionPrimitives + + + true + + + 10 + + + Public Variable + + + + + + + addLocalForce + + + true + + + 10 + + + Public Function + + + + + + + addWorldForceXYZ + + + true + + + 10 + + + Public Function + + + + + + + calcDerivedData + + + true + + + 10 + + + Public Function + + + + + + + setVelocityXYZ + + + true + + + 10 + + + Public Function + + + + + + + angleAccel + + + true + + + 10 + + + Public Variable + + + + + + + contacts + + + true + + + 10 + + + Public Variable + + + + + + + addCollisionPrimitive + + + true + + + 10 + + + Public Function + + + + + + + addForce + + + true + + + 10 + + + Public Function + + + + + + + setVelocity + + + true + + + 10 + + + Public Function + + + + + + + aabb + + + true + + + 10 + + + Public Variable + + + + + + + accel + + + true + + + 10 + + + Public Variable + + + + + + + freezeCounter + + + true + + + 10 + + + Public Variable + + + + + + + movable + + + true + + + 10 + + + Public Variable + + + + + + + postCollisionPredicate + + + true + + + 10 + + + Public Variable + + + + + + + torqueAccum + + + true + + + 10 + + + Public Variable + + + + + + + addWorldForce + + + true + + + 10 + + + Public Function + + + + + + + addWorldForceScaled + + + true + + + 10 + + + Public Function + + + + + + + MAX_CONTACTS + + + true + + + 10 + + + Public Variable + + + + + + + addForceXYZ + + + true + + + 10 + + + Public Function + + + + + + + removeCollisionPrimitive + + + true + + + 10 + + + Public Function + + + + + + + setPositionXYZ + + + true + + + 10 + + + Public Function + + + + + + + linDamping + + + true + + + 10 + + + Public Variable + + + + + + + _f + + + + + + true + + + Private Variable + + + + + + + _r + + + + + + true + + + Private Variable + + + + + + + rotDamping + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + Body + + + true + + + 10 + + + Class + + + + + + + + + + + head + + + true + + + 10 + + + Public Variable + + + + + + + size + + + true + + + 10 + + + Public Variable + + + + + + + remove + + + true + + + 10 + + + Public Function + + + + + + + tail + + + true + + + 10 + + + Public Variable + + + + + + + append + + + true + + + 10 + + + Public Function + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + findItem + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + BodyList + + + true + + + 10 + + + Class + + + + + + + + + + + body + + + true + + + 10 + + + Public Variable + + + + + + + next + + + true + + + 10 + + + Public Variable + + + + + + + prev + + + true + + + 10 + + + Public Variable + + + + + + + clearPool + + + true + + + 10 + + + Public Function + + + + + + + dispose + + + true + + + 10 + + + Public Function + + + + + + + poolTop + + + + + + true + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + create + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + BodyListItem + + + true + + + 10 + + + Class + + + + + + + + + + + friction + + + true + + + 10 + + + Public Variable + + + + + + + restitution + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + BodyMaterial + + + true + + + 10 + + + Class + + + + + + + + + + + pos + + + true + + + 10 + + + Public Variable + + + + + + + copy + + + true + + + 10 + + + Public Function + + + + + + + orientation + + + true + + + 10 + + + Public Variable + + + + + + + rotation + + + true + + + 10 + + + Public Variable + + + + + + + velocity + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + BodyState + + + true + + + 10 + + + Class + + + + + + + + + + + head + + + true + + + 10 + + + Public Variable + + + + + + + size + + + true + + + 10 + + + Public Variable + + + + + + + remove + + + true + + + 10 + + + Public Function + + + + + + + tail + + + true + + + 10 + + + Public Variable + + + + + + + append + + + true + + + 10 + + + Public Function + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + clear + + + true + + + 10 + + + Public Function + + + + + + + findItem + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + CollisionPrimitiveList + + + true + + + 10 + + + Class + + + + + + + + + + + next + + + true + + + 10 + + + Public Variable + + + + + + + prev + + + true + + + 10 + + + Public Variable + + + + + + + primitive + + + true + + + 10 + + + Public Variable + + + + + + + clearPool + + + true + + + 10 + + + Public Function + + + + + + + dispose + + + true + + + 10 + + + Public Function + + + + + + + poolTop + + + + + + true + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + create + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + CollisionPrimitiveListItem + + + true + + + 10 + + + Class + + + + + + + + + + + body1 + + + true + + + 10 + + + Public Variable + + + + + + + body2 + + + true + + + 10 + + + Public Variable + + + + + + + friction + + + true + + + 10 + + + Public Variable + + + + + + + MAX_POINTS + + + + + + true + + + Private Variable + + + + + + + index + + + true + + + 10 + + + Public Variable + + + + + + + maxPenetration + + + true + + + 10 + + + Public Variable + + + + + + + next + + + true + + + 10 + + + Public Variable + + + + + + + true + + + normal + + + true + + + 10 + + + Public Variable + + + + + + + true + + + pcount + + + true + + + 10 + + + Public Variable + + + + + + + points + + + true + + + 10 + + + Public Variable + + + + + + + restitution + + + true + + + 10 + + + Public Variable + + + + + + + satisfied + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + Contact + + + true + + + 10 + + + Class + + + + + + + + + + + accumImpulseN + + + true + + + 10 + + + Public Variable + + + + + + + angularInertia2 + + + true + + + 10 + + + Public Variable + + + + + + + feature1 + + + true + + + 10 + + + Public Variable + + + + + + + feature2 + + + true + + + 10 + + + Public Variable + + + + + + + minSepVel + + + true + + + 10 + + + Public Variable + + + + + + + normalVel + + + true + + + 10 + + + Public Variable + + + + + + + pos + + + true + + + 10 + + + Public Variable + + + + + + + r1 + + + true + + + 10 + + + Public Variable + + + + + + + r2 + + + true + + + 10 + + + Public Variable + + + + + + + copyFrom + + + true + + + 10 + + + Public Function + + + + + + + angularInertia1 + + + true + + + 10 + + + Public Variable + + + + + + + penetration + + + true + + + 10 + + + Public Variable + + + + + + + satisfied + + + true + + + 10 + + + Public Variable + + + + + + + true + + + velByUnitImpulseN + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + ContactPoint + + + true + + + 10 + + + Class + + + + + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + getBoxInvInertia + + + true + + + 10 + + + Public Function + + + + + + + getCylinderInvInertia + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + PhysicsUtils + + + true + + + 10 + + + Class + + + + + + + + + + + removeConstraint + + + true + + + 10 + + + Public Function + + + + + + + _gravityMagnitude + + + true + + + 10 + + + Public Variable + + + + + + + allowedPenetration + + + true + + + 10 + + + Public Variable + + + + + + + collisionIterations + + + true + + + 10 + + + Public Variable + + + + + + + penResolutionSteps + + + true + + + 10 + + + Public Variable + + + + + + + timeStamp + + + true + + + 10 + + + Public Variable + + + + + + + detectCollisions + + + + + + true + + + Private Function + + + + + + + intergateVelocities + + + + + + true + + + Private Function + + + + + + + resolveInterpenetration + + + + + + true + + + Private Function + + + + + + + _gravity + + + true + + + 10 + + + Public Variable + + + + + + + postPhysics + + + + + + true + + + Private Function + + + + + + + resolveContact + + + + + + true + + + Private Function + + + + + + + separateContactPoint + + + + + + true + + + Private Function + + + + + + + angSpeedFreezeLimit + + + true + + + 10 + + + Public Variable + + + + + + + collisionDetector + + + true + + + 10 + + + Public Variable + + + + + + + _v1 + + + + + + true + + + Private Variable + + + + + + + _v2 + + + + + + true + + + Private Variable + + + + + + + maxPenResolutionSpeed + + + true + + + 10 + + + Public Variable + + + + + + + staticSeparationIterations + + + true + + + 10 + + + Public Variable + + + + + + + usePrediction + + + true + + + 10 + + + Public Variable + + + + + + + gravity + + + true + + + 10 + + + Public Function + + + + + + + applyForces + + + + + + true + + + Private Function + + + + + + + true + + + constraints + + + + + + Public Variable + + + + + + + staticSeparationSteps + + + true + + + 10 + + + Public Variable + + + + + + + integratePositions + + + + + + true + + + Private Function + + + + + + + removeBody + + + true + + + 10 + + + Public Function + + + + + + + runPhysics + + + true + + + 10 + + + Public Function + + + + + + + preProcessContacts + + + + + + true + + + Private Function + + + + + + + bodies + + + true + + + 10 + + + Public Variable + + + + + + + true + + + contacts + + + + + + Public Variable + + + + + + + freezeSteps + + + true + + + 10 + + + Public Variable + + + + + + + linSpeedFreezeLimit + + + true + + + 10 + + + Public Variable + + + + + + + maxAngleMove + + + true + + + 10 + + + Public Variable + + + + + + + time + + + true + + + 10 + + + Public Variable + + + + + + + gravity + + + true + + + 10 + + + Public Function + + + + + + + borderContact + + + + + + true + + + Private Variable + + + + + + + addConstraint + + + true + + + 10 + + + Public Function + + + + + + + processContacts + + + + + + true + + + Private Function + + + + + + + addBody + + + true + + + 10 + + + Public Function + + + + + + + calcSepVelocity + + + + + + true + + + Private Function + + + + + + + performStaticSeparation + + + + + + true + + + Private Function + + + + + + + resolveContactPoint + + + + + + true + + + Private Function + + + + + + + true + + + constraintsNum + + + + + + Public Variable + + + + + + + true + + + MAX_CONTACTS + + + + + + Public Variable + + + + + + + _r + + + + + + true + + + Private Variable + + + + + + + _t + + + + + + true + + + Private Variable + + + + + + + _v + + + + + + true + + + Private Variable + + + + + + + contactIterations + + + true + + + 10 + + + Public Variable + + + + + + + useStaticSeparation + + + true + + + 10 + + + Public Variable + + + + + + + lastBodyId + + + + + + true + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + World + + + true + + + 10 + + + Class + + + + + + + alternativa.physics + + + 10 + + + Package + + + + + + + + + + + + + + + true + + + axis + + + true + + + 10 + + + Public Variable + + + + + + + boundBox + + + true + + + 10 + + + Public Variable + + + + + + + coord + + + true + + + 10 + + + Public Variable + + + + + + + indices + + + true + + + 10 + + + Public Variable + + + + + + + negativeNode + + + true + + + 10 + + + Public Variable + + + + + + + parent + + + true + + + 10 + + + Public Variable + + + + + + + positiveNode + + + true + + + 10 + + + Public Variable + + + + + + + splitIndices + + + true + + + 10 + + + Public Variable + + + + + + + splitTree + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + CollisionKdNode + + + true + + + 10 + + + Class + + + + + + + + + + + minPrimitivesPerNode + + + true + + + 10 + + + Public Variable + + + + + + + splitNode + + + + + + true + + + Private Function + + + + + + + rootNode + + + true + + + 10 + + + Public Variable + + + + + + + splitCost + + + + + + true + + + Private Variable + + + + + + + staticBoundBoxes + + + true + + + 10 + + + Public Variable + + + + + + + splitCoord + + + + + + true + + + Private Variable + + + + + + + staticChildren + + + true + + + 10 + + + Public Variable + + + + + + + threshold + + + true + + + 10 + + + Public Variable + + + + + + + traceTree + + + true + + + 10 + + + Public Function + + + + + + + numStaticChildren + + + true + + + 10 + + + Public Variable + + + + + + + checkNodeAxis + + + + + + true + + + Private Function + + + + + + + _bb + + + + + + true + + + Private Variable + + + + + + + _nodeBB + + + + + + true + + + Private Variable + + + + + + + nodeBoundBoxThreshold + + + + + + true + + + Private Variable + + + + + + + splitAxis + + + + + + true + + + Private Variable + + + + + + + createTree + + + true + + + 10 + + + Public Function + + + + + + + traceNode + + + + + + true + + + Private Function + + + + + + + splitCoordsX + + + + + + true + + + Private Variable + + + + + + + splitCoordsY + + + + + + true + + + Private Variable + + + + + + + splitCoordsZ + + + + + + true + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + CollisionKdTree + + + true + + + 10 + + + Class + + + + + + + + + + + minPrimitivesPerNode + + + true + + + 10 + + + Public Variable + + + + + + + rootNode + + + true + + + 10 + + + Public Variable + + + + + + + splitCoord + + + + + + true + + + Private Variable + + + + + + + splitCost + + + + + + true + + + Private Variable + + + + + + + parentNode + + + true + + + 10 + + + Public Variable + + + + + + + parentTree + + + true + + + 10 + + + Public Variable + + + + + + + threshold + + + true + + + 10 + + + Public Variable + + + + + + + checkNodeAxis + + + + + + true + + + Private Function + + + + + + + createTree + + + true + + + 10 + + + Public Function + + + + + + + _bb + + + + + + true + + + Private Variable + + + + + + + nodeBoundBoxThreshold + + + + + + true + + + Private Variable + + + + + + + splitAxis + + + + + + true + + + Private Variable + + + + + + + splitNode + + + + + + true + + + Private Function + + + + + + + _nodeBB + + + + + + true + + + Private Variable + + + + + + + splitCoordsX + + + + + + true + + + Private Variable + + + + + + + splitCoordsY + + + + + + true + + + Private Variable + + + + + + + splitCoordsZ + + + + + + true + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + CollisionKdTree2D + + + true + + + 10 + + + Class + + + + + + + + + + + aabb + + + true + + + 10 + + + Public Variable + + + + + + + body + + + true + + + 10 + + + Public Variable + + + + + + + localTransform + + + true + + + 10 + + + Public Variable + + + + + + + postCollisionPredicate + + + true + + + 10 + + + Public Variable + + + + + + + calculateAABB + + + true + + + 10 + + + Public Function + + + + + + + setBody + + + true + + + 10 + + + Public Function + + + + + + + collisionGroup + + + true + + + 10 + + + Public Variable + + + + + + + transform + + + true + + + 10 + + + Public Variable + + + + + + + true + + + createPrimitive + + + + + + true + + + 10 + + + Private Function + + + + + + + clone + + + true + + + 10 + + + Public Function + + + + + + + BOX + + + true + + + 10 + + + Public Variable + + + + + + + PLANE + + + true + + + 10 + + + Public Variable + + + + + + + SPHERE + + + true + + + 10 + + + Public Variable + + + + + + + TRIANGLE + + + true + + + 10 + + + Public Variable + + + + + + + type + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + copyFrom + + + true + + + 10 + + + Public Function + + + + + + + getRayIntersection + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + toString + + + true + + + 10 + + + Public Function + + + + + + + RECT + + + true + + + 10 + + + Public Variable + + + + + + + CollisionPrimitive + + + true + + + 10 + + + Class + + + + + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + true + + + considerBodies + + + + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + IBodyCollisionPredicate + + + true + + + 10 + + + Class + + + + + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + getContact + + + + + + 10 + + + Public Function + + + + + + + haveCollision + + + + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + ICollider + + + true + + + 10 + + + Class + + + + + + + true + + + + + + + true + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + true + + + getAllContacts + + + + + + Public Function + + + + + + + true + + + getContact + + + + + + 10 + + + Public Function + + + + + + + true + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + true + + + intersectRay + + + + + + Public Function + + + + + + + true + + + testCollision + + + + + + Public Function + + + + + + + ICollisionDetector + + + true + + + 10 + + + Class + + + + + + + true + + + + + + + true + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + true + + + considerCollision + + + + + + Public Function + + + + + + + true + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + ICollisionPredicate + + + true + + + 10 + + + Class + + + + + + + true + + + + + + + true + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + true + + + considerBody + + + + + + Public Function + + + + + + + true + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + IRayCollisionPredicate + + + true + + + 10 + + + Class + + + + + + + + + + + addPrimitive + + + true + + + 10 + + + Public Function + + + + + + + removePrimitive + + + true + + + 10 + + + Public Function + + + + + + + testCollision + + + true + + + 10 + + + Public Function + + + + + + + true + + + dynamicPrimitives + + + + + + Public Variable + + + + + + + init + + + true + + + 10 + + + Public Function + + + + + + + intersectRayWithStatic + + + true + + + 10 + + + Public Function + + + + + + + intersectRayWithDynamic + + + + + + true + + + Private Function + + + + + + + intersectRay + + + true + + + 10 + + + Public Function + + + + + + + testBodyPrimitiveCollision + + + true + + + 10 + + + Public Function + + + + + + + getRayNodeIntersection + + + + + + true + + + Private Function + + + + + + + _dynamicIntersection + + + + + + true + + + Private Variable + + + + + + + getContact + + + true + + + 10 + + + Public Function + + + + + + + true + + + threshold + + + + + + Public Variable + + + + + + + _time + + + + + + true + + + Private Variable + + + + + + + getAllContacts + + + true + + + 10 + + + Public Function + + + + + + + _n + + + + + + true + + + Private Variable + + + + + + + _o + + + + + + true + + + Private Variable + + + + + + + addCollider + + + + + + true + + + Private Function + + + + + + + getPrimitiveNodeCollisions + + + + + + true + + + Private Function + + + + + + + true + + + tree + + + + + + Public Variable + + + + + + + getRayBoundBoxIntersection + + + + + + true + + + Private Function + + + + + + + testRayAgainstNode + + + + + + true + + + Private Function + + + + + + + true + + + dynamicPrimitivesNum + + + + + + Public Variable + + + + + + + _rayAABB + + + + + + true + + + Private Variable + + + + + + + colliders + + + + + + true + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + KdTreeCollisionDetector + + + true + + + 10 + + + Class + + + + + + + alternativa.physics.collision + + + 10 + + + Package + + + + + + + + + + + + + + + axis10 + + + + + + true + + + Private Variable + + + + + + + axis11 + + + + + + true + + + Private Variable + + + + + + + findEdgesIntersection + + + + + + true + + + Private Function + + + + + + + axis12 + + + + + + true + + + Private Variable + + + + + + + true + + + vectorToBox1 + + + + + + Private Variable + + + + + + + findFaceContactPoints + + + + + + true + + + Private Function + + + + + + + true + + + overlapOnAxis + + + true + + + 10 + + + Public Function + + + + + + + clip + + + + + + true + + + Private Function + + + + + + + minOverlap + + + + + + true + + + Private Variable + + + + + + + haveCollision + + + true + + + 10 + + + Public Function + + + + + + + true + + + axis + + + + + + Private Variable + + + + + + + axis20 + + + + + + true + + + Private Variable + + + + + + + axis21 + + + + + + true + + + Private Variable + + + + + + + axis22 + + + + + + true + + + Private Variable + + + + + + + true + + + epsilon + + + + + + Private Variable + + + + + + + true + + + pcount + + + + + + Private Variable + + + + + + + tmpPoints + + + + + + true + + + Private Variable + + + + + + + testMainAxis + + + + + + true + + + Private Function + + + + + + + bestAxisIndex + + + + + + true + + + Private Variable + + + + + + + points1 + + + + + + true + + + Private Variable + + + + + + + points2 + + + + + + true + + + Private Variable + + + + + + + reducePoints + + + + + + true + + + Private Function + + + + + + + getContact + + + true + + + 10 + + + Public Function + + + + + + + getPointBoxPenetration + + + + + + true + + + Private Function + + + + + + + testDerivedAxis + + + + + + true + + + Private Function + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + BoxBoxCollider + + + true + + + 10 + + + Class + + + + + + + + + + + getContact + + + true + + + 10 + + + Public Function + + + + + + + clipHighX + + + + + + true + + + Private Function + + + + + + + clipHighZ + + + + + + true + + + Private Function + + + + + + + clipLowX + + + + + + true + + + Private Function + + + + + + + clipLowY + + + + + + true + + + Private Function + + + + + + + getFaceVertsByAxis + + + + + + true + + + Private Function + + + + + + + haveCollision + + + true + + + 10 + + + Public Function + + + + + + + clipHighY + + + + + + true + + + Private Function + + + + + + + clipLowZ + + + + + + true + + + Private Function + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + BoxCollider + + + true + + + 10 + + + Class + + + + + + + + + + + true + + + normal + + + + + + Private Variable + + + + + + + verts1 + + + + + + true + + + Private Variable + + + + + + + verts2 + + + + + + true + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + getContact + + + true + + + 10 + + + Public Function + + + + + + + haveCollision + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + BoxPlaneCollider + + + true + + + 10 + + + Class + + + + + + + + + + + true + + + axis + + + + + + Private Variable + + + + + + + axis10 + + + + + + true + + + Private Variable + + + + + + + axis11 + + + + + + true + + + Private Variable + + + + + + + findEdgesIntersection + + + + + + true + + + Private Function + + + + + + + axis12 + + + + + + true + + + Private Variable + + + + + + + clipByBox + + + + + + true + + + Private Function + + + + + + + true + + + vectorToBox + + + + + + Private Variable + + + + + + + true + + + overlapOnAxis + + + true + + + 10 + + + Public Function + + + + + + + clipByRect + + + + + + true + + + Private Function + + + + + + + minOverlap + + + + + + true + + + Private Variable + + + + + + + haveCollision + + + true + + + 10 + + + Public Function + + + + + + + axis20 + + + + + + true + + + Private Variable + + + + + + + axis21 + + + + + + true + + + Private Variable + + + + + + + findFaceContactPoints + + + + + + true + + + Private Function + + + + + + + axis22 + + + + + + true + + + Private Variable + + + + + + + true + + + epsilon + + + + + + Private Variable + + + + + + + testMainAxis + + + + + + true + + + Private Function + + + + + + + bestAxisIndex + + + + + + true + + + Private Variable + + + + + + + points1 + + + + + + true + + + Private Variable + + + + + + + points2 + + + + + + true + + + Private Variable + + + + + + + getContact + + + true + + + 10 + + + Public Function + + + + + + + getPointBoxPenetration + + + + + + true + + + Private Function + + + + + + + testDerivedAxis + + + + + + true + + + Private Function + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + BoxRectCollider + + + true + + + 10 + + + Class + + + + + + + + + + + sPos + + + + + + true + + + Private Variable + + + + + + + getContact + + + true + + + 10 + + + Public Function + + + + + + + haveCollision + + + true + + + 10 + + + Public Function + + + + + + + bPos + + + + + + true + + + Private Variable + + + + + + + center + + + + + + true + + + Private Variable + + + + + + + closestPt + + + + + + true + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + BoxSphereCollider + + + true + + + 10 + + + Class + + + + + + + + + + + axis11 + + + + + + true + + + Private Variable + + + + + + + axis12 + + + + + + true + + + Private Variable + + + + + + + true + + + vector + + + + + + Private Variable + + + + + + + findFaceContactPoints + + + + + + true + + + Private Function + + + + + + + getBoxToTriContact + + + + + + true + + + Private Function + + + + + + + axis10 + + + + + + true + + + Private Variable + + + + + + + colNormal + + + + + + true + + + Private Variable + + + + + + + true + + + toBox + + + + + + Private Variable + + + + + + + clipByBox + + + + + + true + + + Private Function + + + + + + + minOverlap + + + + + + true + + + Private Variable + + + + + + + true + + + overlapOnAxis + + + + + + Private Function + + + + + + + haveCollision + + + true + + + 10 + + + Public Function + + + + + + + clipByLine + + + + + + true + + + Private Function + + + + + + + getTriToBoxContact + + + + + + true + + + Private Function + + + + + + + true + + + axis + + + + + + Private Variable + + + + + + + axis20 + + + + + + true + + + Private Variable + + + + + + + axis21 + + + + + + true + + + Private Variable + + + + + + + axis22 + + + + + + true + + + Private Variable + + + + + + + point2 + + + + + + true + + + Private Variable + + + + + + + true + + + epsilon + + + true + + + 10 + + + Public Variable + + + + + + + testMainAxis + + + + + + true + + + Private Function + + + + + + + bestAxisIndex + + + + + + true + + + Private Variable + + + + + + + point1 + + + + + + true + + + Private Variable + + + + + + + points1 + + + + + + true + + + Private Variable + + + + + + + clipByTriangle + + + + + + true + + + Private Function + + + + + + + points2 + + + + + + true + + + Private Variable + + + + + + + getContact + + + true + + + 10 + + + Public Function + + + + + + + findEdgesIntersection + + + + + + true + + + Private Function + + + + + + + getPointBoxPenetration + + + + + + true + + + Private Function + + + + + + + testDerivedAxis + + + + + + true + + + Private Function + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + BoxTriangleCollider + + + true + + + 10 + + + Class + + + + + + + + + + + getContact + + + true + + + 10 + + + Public Function + + + + + + + true + + + normal + + + + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + haveCollision + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + SpherePlaneCollider + + + true + + + 10 + + + Class + + + + + + + + + + + p1 + + + + + + true + + + Private Variable + + + + + + + haveCollision + + + true + + + 10 + + + Public Function + + + + + + + p2 + + + + + + true + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + getContact + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + SphereSphereCollider + + + true + + + 10 + + + Class + + + + + + + alternativa.physics.collision.colliders + + + 10 + + + Package + + + + + + + + + + + + + + + true + + + createPrimitive + + + + + + true + + + 10 + + + Private Function + + + + + + + calculateAABB + + + true + + + 10 + + + Public Function + + + + + + + getRayIntersection + + + true + + + 10 + + + Public Function + + + + + + + excludedFaces + + + true + + + 10 + + + Public Variable + + + + + + + hs + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + copyFrom + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + toString + + + true + + + 10 + + + Public Function + + + + + + + CollisionBox + + + true + + + 10 + + + Class + + + + + + + + + + + twoSided + + + true + + + 10 + + + Public Variable + + + + + + + calculateAABB + + + true + + + 10 + + + Public Function + + + + + + + getRayIntersection + + + true + + + 10 + + + Public Function + + + + + + + true + + + createPrimitive + + + + + + true + + + 10 + + + Private Function + + + + + + + hs + + + true + + + 10 + + + Public Variable + + + + + + + copyFrom + + + true + + + 10 + + + Public Function + + + + + + + toString + + + true + + + 10 + + + Public Function + + + + + + + EPSILON + + + + + + true + + + Private Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + CollisionRect + + + true + + + 10 + + + Class + + + + + + + + + + + r + + + true + + + 10 + + + Public Variable + + + + + + + calculateAABB + + + true + + + 10 + + + Public Function + + + + + + + true + + + createPrimitive + + + + + + true + + + 10 + + + Private Function + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + copyFrom + + + true + + + 10 + + + Public Function + + + + + + + getRayIntersection + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + CollisionSphere + + + true + + + 10 + + + Class + + + + + + + + + + + v0 + + + true + + + 10 + + + Public Variable + + + + + + + calculateAABB + + + true + + + 10 + + + Public Function + + + + + + + getRayIntersection + + + true + + + 10 + + + Public Function + + + + + + + initVertices + + + + + + true + + + Private Function + + + + + + + e0 + + + true + + + 10 + + + Public Variable + + + + + + + e1 + + + true + + + 10 + + + Public Variable + + + + + + + e2 + + + true + + + 10 + + + Public Variable + + + + + + + copyFrom + + + true + + + 10 + + + Public Function + + + + + + + true + + + createPrimitive + + + + + + true + + + 10 + + + Private Function + + + + + + + toString + + + true + + + 10 + + + Public Function + + + + + + + v1 + + + true + + + 10 + + + Public Variable + + + + + + + v2 + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + CollisionTriangle + + + true + + + 10 + + + Class + + + + + + + alternativa.physics.collision.primitives + + + 10 + + + Package + + + + + + + + + + + + + + + clone + + + true + + + 10 + + + Public Function + + + + + + + setSize + + + true + + + 10 + + + Public Function + + + + + + + maxX + + + true + + + 10 + + + Public Variable + + + + + + + maxY + + + true + + + 10 + + + Public Variable + + + + + + + maxZ + + + true + + + 10 + + + Public Variable + + + + + + + minX + + + true + + + 10 + + + Public Variable + + + + + + + minY + + + true + + + 10 + + + Public Variable + + + + + + + minZ + + + true + + + 10 + + + Public Variable + + + + + + + addBoundBox + + + true + + + 10 + + + Public Function + + + + + + + addPoint + + + true + + + 10 + + + Public Function + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + copyFrom + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + infinity + + + true + + + 10 + + + Public Function + + + + + + + intersects + + + true + + + 10 + + + Public Function + + + + + + + toString + + + true + + + 10 + + + Public Function + + + + + + + BoundBox + + + true + + + 10 + + + Class + + + + + + + + + + + dir + + + true + + + 10 + + + Public Variable + + + + + + + origin + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + Ray + + + true + + + 10 + + + Class + + + + + + + + + + + true + + + normal + + + true + + + 10 + + + Public Variable + + + + + + + pos + + + true + + + 10 + + + Public Variable + + + + + + + primitive + + + true + + + 10 + + + Public Variable + + + + + + + t + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + copy + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + RayIntersection + + + true + + + 10 + + + Class + + + + + + + alternativa.physics.collision.types + + + 10 + + + Package + + + + + + + + + + + true + + + + + + + true + + + satisfied + + + + + + Public Variable + + + + + + + true + + + world + + + + + + Public Variable + + + + + + + true + + + apply + + + true + + + 10 + + + Public Function + + + + + + + true + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + true + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + true + + + preProcess + + + true + + + 10 + + + Public Function + + + + + + + Constraint + + + true + + + 10 + + + Class + + + + + + + + + + + preProcess + + + true + + + 10 + + + Public Function + + + + + + + impulseDirection + + + + + + true + + + Private Variable + + + + + + + true + + + body1 + + + + + + Public Variable + + + + + + + true + + + body2 + + + + + + Public Variable + + + + + + + true + + + velByUnitImpulseN + + + + + + Private Variable + + + + + + + apply + + + true + + + 10 + + + Public Function + + + + + + + true + + + maxDistance + + + + + + Public Variable + + + + + + + true + + + r2 + + + + + + Public Variable + + + + + + + true + + + wr1 + + + + + + Public Variable + + + + + + + true + + + wr2 + + + + + + Public Variable + + + + + + + minClosingVel + + + + + + true + + + Private Variable + + + + + + + true + + + r1 + + + + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + MaxDistanceConstraint + + + true + + + 10 + + + Class + + + + + + + alternativa.physics.constraints + + + 10 + + + Package + + + + + + + + + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + setParams + + + true + + + 10 + + + Public Function + + + + + + + RigidBox + + + true + + + 10 + + + Class + + + + + + + true + + + + + + + true + + + h + + + + + + Public Variable + + + + + + + true + + + r + + + + + + Public Variable + + + + + + + true + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + true + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + true + + + setParams + + + true + + + 10 + + + Public Function + + + + + + + RigidCylinder + + + true + + + 10 + + + Class + + + + + + + + + + + true + + + normal + + + true + + + 10 + + + Public Variable + + + + + + + offset + + + true + + + 10 + + + Public Variable + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + RigidPlane + + + true + + + 10 + + + Class + + + + + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + RigidRect + + + true + + + 10 + + + Class + + + + + + + + + + + c-ctr + + + true + + + 10 + + + Public Function + + + + + + + i-ctr + + + true + + + 10 + + + Public Function + + + + + + + RigidSphere + + + true + + + 10 + + + Class + + + + + + + alternativa.physics.primitives + + + 10 + + + Package + + + + + + + FFFFFF + + + true + + + D:\work\eclipse\ws_projects\TanksTestingTool\lib\AlternativaPhysics.swc + + + AlternativaPhysics.swc + + + 1 + + + 24.0 + + + Not Present + + + + + + + Time Line + + + + + + + 500 X 375 + + + 157458 + + + 10 + + + + + + + 157458 + + + 10 + + + + + + + diff --git a/0.0.9.1/pom.xml b/0.0.9.1/pom.xml new file mode 100644 index 0000000..1138f57 --- /dev/null +++ b/0.0.9.1/pom.xml @@ -0,0 +1,17 @@ + + 4.0.0 + platform.clients.fp10.libraries + AlternativaPhysics + swc + 0.0.9.1 + + platform.tools.maven + FlashBasePom + 2.0.0.0 + + + scm:svn:http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1 + + + + diff --git a/0.0.9.1/src/.svn/all-wcprops b/0.0.9.1/src/.svn/all-wcprops new file mode 100644 index 0000000..26322f7 --- /dev/null +++ b/0.0.9.1/src/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 83 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src +END diff --git a/0.0.9.1/src/.svn/entries b/0.0.9.1/src/.svn/entries new file mode 100644 index 0000000..fcf3e51 --- /dev/null +++ b/0.0.9.1/src/.svn/entries @@ -0,0 +1,31 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src +http://svndev.alternativaplatform.com + + + +2010-03-06T16:51:19.781174Z +29404 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +alternativa +dir + diff --git a/0.0.9.1/src/.svn/format b/0.0.9.1/src/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.1/src/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.1/src/alternativa/.svn/all-wcprops b/0.0.9.1/src/alternativa/.svn/all-wcprops new file mode 100644 index 0000000..f7bdbdc --- /dev/null +++ b/0.0.9.1/src/alternativa/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 95 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa +END diff --git a/0.0.9.1/src/alternativa/.svn/entries b/0.0.9.1/src/alternativa/.svn/entries new file mode 100644 index 0000000..2fe9793 --- /dev/null +++ b/0.0.9.1/src/alternativa/.svn/entries @@ -0,0 +1,34 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa +http://svndev.alternativaplatform.com + + + +2010-03-06T16:51:19.781174Z +29404 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +physics +dir + +math +dir + diff --git a/0.0.9.1/src/alternativa/.svn/format b/0.0.9.1/src/alternativa/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.1/src/alternativa/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.1/src/alternativa/math/.svn/all-wcprops b/0.0.9.1/src/alternativa/math/.svn/all-wcprops new file mode 100644 index 0000000..72058d6 --- /dev/null +++ b/0.0.9.1/src/alternativa/math/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 100 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/math +END +Quaternion.as +K 25 +svn:wc:ra_dav:version-url +V 114 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/math/Quaternion.as +END +Vector3.as +K 25 +svn:wc:ra_dav:version-url +V 111 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/math/Vector3.as +END +Matrix3.as +K 25 +svn:wc:ra_dav:version-url +V 111 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/math/Matrix3.as +END +Matrix4.as +K 25 +svn:wc:ra_dav:version-url +V 111 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/math/Matrix4.as +END diff --git a/0.0.9.1/src/alternativa/math/.svn/entries b/0.0.9.1/src/alternativa/math/.svn/entries new file mode 100644 index 0000000..e15c0f0 --- /dev/null +++ b/0.0.9.1/src/alternativa/math/.svn/entries @@ -0,0 +1,76 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/math +http://svndev.alternativaplatform.com + + + +2010-02-12T12:34:25.192477Z +28424 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Quaternion.as +file + + + + +2010-10-28T04:32:44.000000Z +7332c9620420bd10062d913decfb9501 +2010-02-12T12:34:25.192477Z +28424 +mike + +Vector3.as +file + + + + +2010-10-28T04:32:44.000000Z +a1571b527786783cef1a9f56db3f2af6 +2010-02-12T12:34:25.192477Z +28424 +mike + +Matrix3.as +file + + + + +2010-10-28T04:32:44.000000Z +0054bd91fe694474a28c8ec17697292b +2010-02-12T12:34:25.192477Z +28424 +mike + +Matrix4.as +file + + + + +2010-10-28T04:32:44.000000Z +47b31d4350499ebe77a5973a938e4b44 +2010-02-12T12:34:25.192477Z +28424 +mike + diff --git a/0.0.9.1/src/alternativa/math/.svn/format b/0.0.9.1/src/alternativa/math/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.1/src/alternativa/math/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.1/src/alternativa/math/.svn/text-base/Matrix3.as.svn-base b/0.0.9.1/src/alternativa/math/.svn/text-base/Matrix3.as.svn-base new file mode 100644 index 0000000..244cc98 --- /dev/null +++ b/0.0.9.1/src/alternativa/math/.svn/text-base/Matrix3.as.svn-base @@ -0,0 +1,390 @@ +package alternativa.math { + import flash.geom.Vector3D; + + /** + * + */ + public class Matrix3 { + + public static const ZERO:Matrix3 = new Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0); + public static const IDENTITY:Matrix3 = new Matrix3(); + + public var a:Number; + public var b:Number; + public var c:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix3(a:Number = 1, b:Number = 0, c:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1) { + this.a = a; + this.b = b; + this.c = c; + + this.e = e; + this.f = f; + this.g = g; + + this.i = i; + this.j = j; + this.k = k; + } + + /** + * + * @return + */ + public function toIdentity():Matrix3 { + a = f = k = 1; + b = c = e = g = i = j = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + var det:Number = 1/(-cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk); + + a = (ff*kk - gg*jj)*det; + b = (cc*jj - bb*kk)*det; + c = (bb*gg - cc*ff)*det; + e = (gg*ii - ee*kk)*det; + f = (aa*kk - cc*ii)*det; + g = (cc*ee - aa*gg)*det; + i = (ee*jj - ff*ii)*det; + j = (bb*ii - aa*jj)*det; + k = (aa*ff - bb*ee)*det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prependTransposed(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.b + cc*m.c; + b = aa*m.e + bb*m.f + cc*m.g; + c = aa*m.i + bb*m.j + cc*m.k; + e = ee*m.a + ff*m.b + gg*m.c; + f = ee*m.e + ff*m.f + gg*m.g; + g = ee*m.i + ff*m.j + gg*m.k; + i = ii*m.a + jj*m.b + kk*m.c; + j = ii*m.e + jj*m.f + kk*m.g; + k = ii*m.i + jj*m.j + kk*m.k; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix3):Matrix3 { + a += m.a; + b += m.b; + c += m.c; + e += m.e; + f += m.f; + g += m.g; + i += m.i; + j += m.j; + k += m.k; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix3):Matrix3 { + a -= m.a; + b -= m.b; + c -= m.c; + e -= m.e; + f -= m.f; + g -= m.g; + i -= m.i; + j -= m.j; + k -= m.k; + + return this; + } + + /** + * + * @return + */ + public function transpose():Matrix3 { + var tmp:Number = b; + b = e; + e = tmp; + tmp = c; + c = i; + i = tmp; + tmp = g; + g = j; + j = tmp; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector3To3D(vin:Vector3, vout:Vector3D):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * + * @param matrix + */ + public function createSkewSymmetric(v:Vector3):Matrix3 { + a = f = k = 0; + b = -v.z; + c = v.y; + e = v.z; + g = -v.x; + i = -v.y; + j = v.x; + return this; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix3):Matrix3 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @return + */ + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):Matrix3 { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + + return this; + } + + /** + * Метод формирует матрицу поворота на заданный угол относительно заданной оси. + * + * @param axis нормализованный вектор, задающий ось, относительно которой выполняется поворот + * @param angle угол поворота в радианах + */ + public function fromAxisAngle(axis:Vector3, angle:Number):void { + var c1:Number = Math.cos(angle); + var s:Number = Math.sin(angle); + var t:Number = 1 - c1; + var x:Number = axis.x; + var y:Number = axis.y; + var z:Number = axis.z; + + a = t*x*x + c1; + b = t*x*y - z*s; + c = t*x*z + y*s; + + e = t*x*y + z*s; + f = t*y*y + c1; + g = t*y*z - x*s; + + i = t*x*z - y*s; + j = t*y*z + x*s; + k = t*z*z + c1; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix3 { + return new Matrix3(a, b, c, e, f, g, i, j, k); + } + + /** + * + * @return + */ + public function toString():String { + return "[Matrix3 (" + a + ", " + b + ", " + c + "), (" + e + ", " + f + ", " + g + "), (" + i + ", " + j + ", " + k + ")]"; + } + + /** + * @param angles + */ + public function getEulerAngles(angles:Vector3):void { + if (-1 < i && i < 1) { + angles.x = Math.atan2(j, k); + angles.y = -Math.asin(i); + angles.z = Math.atan2(e, a); + } else { + angles.x = 0; + angles.y = (i <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-b, f); + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/math/.svn/text-base/Matrix4.as.svn-base b/0.0.9.1/src/alternativa/math/.svn/text-base/Matrix4.as.svn-base new file mode 100644 index 0000000..d06eb5f --- /dev/null +++ b/0.0.9.1/src/alternativa/math/.svn/text-base/Matrix4.as.svn-base @@ -0,0 +1,475 @@ +package alternativa.math { + + /** + * + */ + public class Matrix4 { + + public static const IDENTITY:Matrix4 = new Matrix4(); + + public var a:Number; + public var b:Number; + public var c:Number; + public var d:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + public var h:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + public var l:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix4(a:Number = 1, b:Number = 0, c:Number = 0, d:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, h:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1, l:Number = 0) { + this.a = a; + this.b = b; + this.c = c; + this.d = d; + + this.e = e; + this.f = f; + this.g = g; + this.h = h; + + this.i = i; + this.j = j; + this.k = k; + this.l = l; + } + + /** + * + * @return + */ + public function toIdentity():Matrix4 { + a = f = k = 1; + b = c = e = g = i = j = d = h = l = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + var det:Number = -cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk; + + a = (-gg*jj + ff*kk)/det; + b = (cc*jj - bb*kk)/det; + c = (-cc*ff + bb*gg)/det; + d = (dd*gg*jj - cc*hh*jj - dd*ff*kk + bb*hh*kk + cc*ff*ll - bb*gg*ll)/det; + e = (gg*ii - ee*kk)/det; + f = (-cc*ii + aa*kk)/det; + g = (cc*ee - aa*gg)/det; + h = (cc*hh*ii - dd*gg*ii + dd*ee*kk - aa*hh*kk - cc*ee*ll + aa*gg*ll)/det; + i = (-ff*ii + ee*jj)/det; + j = (bb*ii - aa*jj)/det; + k = (-bb*ee + aa*ff)/det; + l = (dd*ff*ii - bb*hh*ii - dd*ee*jj + aa*hh*jj + bb*ee*ll - aa*ff*ll)/det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + d = m.a*dd + m.b*hh + m.c*ll + m.d; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + h = m.e*dd + m.f*hh + m.g*ll + m.h; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + l = m.i*dd + m.j*hh + m.k*ll + m.l; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + d = aa*m.d + bb*m.h + cc*m.l + dd; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + h = ee*m.d + ff*m.h + gg*m.l + hh; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + l = ii*m.d + jj*m.h + kk*m.l + ll; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix4):Matrix4 { + a += m.a; + b += m.b; + c += m.c; + d += m.d; + e += m.e; + f += m.f; + g += m.g; + h += m.h; + i += m.i; + j += m.j; + k += m.k; + l += m.l; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix4):Matrix4 { + a -= m.a; + b -= m.b; + c -= m.c; + d -= m.d; + e -= m.e; + f -= m.f; + g -= m.g; + h -= m.h; + i -= m.i; + j -= m.j; + k -= m.k; + l -= m.l; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectors(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + * @param len + */ + public function transformVectorsN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverse(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverseN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(idx:int, axis:Vector3):void { + switch (idx) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + case 3: + axis.x = d; + axis.y = h; + axis.z = l; + return; + } + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix4):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = m.d; + e = m.e; + f = m.f; + g = m.g; + h = m.h; + i = m.i; + j = m.j; + k = m.k; + l = m.l; + + return this; + } + + public function setFromMatrix3(m:Matrix3, offset:Vector3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = offset.x; + e = m.e; + f = m.f; + g = m.g; + h = offset.y; + i = m.i; + j = m.j; + k = m.k; + l = offset.z; + + return this; + } + + public function setOrientationFromMatrix3(m:Matrix3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @param angles + */ + public function getEulerAngles(angles:Vector3):void { + if (-1 < i && i < 1) { + angles.x = Math.atan2(j, k); + angles.y = -Math.asin(i); + angles.z = Math.atan2(e, a); + } else { + angles.x = 0; + angles.y = (i <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-b, f); + } + } + + /** + * Устанавливает координаты матрицы. + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + d = pos.x; + h = pos.y; + l = pos.z; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix4 { + return new Matrix4(a, b, c, d, e, f, g, h, i, j, k, l); + } + + /** + * + * @return + */ + public function toString():String { + return "[Matrix4 [" + a.toFixed(3) + " " + b.toFixed(3) + " " + c.toFixed(3) + " " + d.toFixed(3) + "] [" + e.toFixed(3) + " " + f.toFixed(3) + " " + g.toFixed(3) + " " + h.toFixed(3) + "] [" + i.toFixed(3) + " " + j.toFixed(3) + " " + k.toFixed(3) + " " + l.toFixed(3) + "]]"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/math/.svn/text-base/Quaternion.as.svn-base b/0.0.9.1/src/alternativa/math/.svn/text-base/Quaternion.as.svn-base new file mode 100644 index 0000000..ddc85f5 --- /dev/null +++ b/0.0.9.1/src/alternativa/math/.svn/text-base/Quaternion.as.svn-base @@ -0,0 +1,458 @@ +package alternativa.math { + import flash.geom.Vector3D; + + public class Quaternion { + + public var w:Number; + public var x:Number; + public var y:Number; + public var z:Number; + + public static function multiply(q1:Quaternion, q2:Quaternion, result:Quaternion):void { + result.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z; + result.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y; + result.y = q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z; + result.z = q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x; + } + + public static function createFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngle(axis, angle); + return q; + } + + public static function createFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngleComponents(x, y, z, angle); + return q; + } + + private static var _q:Quaternion = new Quaternion(); + + public function Quaternion(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0) { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + } + + public function reset(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0):Quaternion { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + return this; + } + + public function normalize():Quaternion { + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * Умножает на указанный кватернион слева: this * q + * + * @param q множитель + */ + public function prepend(q:Quaternion):Quaternion { + var ww:Number = w*q.w - x*q.x - y*q.y - z*q.z; + var xx:Number = w*q.x + x*q.w + y*q.z - z*q.y; + var yy:Number = w*q.y + y*q.w + z*q.x - x*q.z; + var zz:Number = w*q.z + z*q.w + x*q.y - y*q.x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Умножает на указанный кватернион справа: q * this + * + * @param q множитель + */ + public function append(q:Quaternion):Quaternion { + var ww:Number = q.w*w - q.x*x - q.y*y - q.z*z; + var xx:Number = q.w*x + q.x*w + q.y*z - q.z*y; + var yy:Number = q.w*y + q.y*w + q.z*x - q.x*z; + var zz:Number = q.w*z + q.z*w + q.x*y - q.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * + * @param vector + */ + public function rotateByVector(v:Vector3):Quaternion { + var ww:Number = -v.x*x - v.y*y - v.z*z; + var xx:Number = v.x*w + v.y*z - v.z*y; + var yy:Number = v.y*w + v.z*x - v.x*z; + var zz:Number = v.z*w + v.x*y - v.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Добавляет вращение, приданное вектором угловой скорости за указанное время. + * + * @param v + * @param scale + */ + public function addScaledVector(v:Vector3, scale:Number):Quaternion { + var vx:Number = v.x*scale; + var vy:Number = v.y*scale; + var vz:Number = v.z*scale; + var ww:Number = -x*vx - y*vy - z*vz; + var xx:Number = vx*w + vy*z - vz*y; + var yy:Number = vy*w + vz*x - vx*z; + var zz:Number = vz*w + vx*y - vy*x; + w += 0.5*ww; + x += 0.5*xx; + y += 0.5*yy; + z += 0.5*zz; + // inlined normalize + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param m + * @return + */ + public function toMatrix3(m:Matrix3):Quaternion { + var xx2:Number = 2.0*x*x; + var yy2:Number = 2.0*y*y; + var zz2:Number = 2.0*z*z; + var xy2:Number = 2.0*x*y; + var yz2:Number = 2.0*y*z; + var zx2:Number = 2.0*z*x; + var wx2:Number = 2.0*w*x; + var wy2:Number = 2.0*w*y; + var wz2:Number = 2.0*w*z; + + m.a = 1.0 - yy2 - zz2; + m.b = xy2 - wz2; + m.c = zx2 + wy2; + + m.e = xy2 + wz2; + m.f = 1.0 - xx2 - zz2; + m.g = yz2 - wx2; + + m.i = zx2 - wy2; + m.j = yz2 + wx2; + m.k = 1.0 - xx2 - yy2; + return this; + } + + /** + * + * @return + */ + public function length():Number { + return Math.sqrt(w*w + x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function lengthSqr():Number { + return w*w + x*x + y*y + z*z; + } + + /** + * + * @param axis + * @param angle + * @return + */ + public function setFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z); + x = axis.x*k; + y = axis.y*k; + z = axis.z*k; + return this; + } + + /** + * + * @param x + * @param y + * @param z + * @param angle + */ + public function setFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(x*x + y*y + z*z); + this.x = x*k; + this.y = y*k; + this.z = z*k; + return this; + } + + /** + * + * @param vector + */ + public function toAxisVector(v:Vector3 = null):Vector3 { + if (w < -1 || w > 1) { + normalize(); + } + if (v == null) { + v = new Vector3(); + } + if (w > -1 && w < 1) { + if (w == 0) { + v.x = x; + v.y = y; + v.z = z; + } else { + var angle:Number = 2*Math.acos(w); + var coeff:Number = 1/Math.sqrt(1 - w*w); + v.x = x*coeff*angle; + v.y = y*coeff*angle; + v.z = z*coeff*angle; + } + } else { + v.x = 0; + v.y = 0; + v.z = 0; + } + return v; + } + + /** + * + * @param rotations + */ + public function getEulerAngles(angles:Vector3):Vector3 { + var qi2:Number = 2*x*x; + var qj2:Number = 2*y*y; + var qk2:Number = 2*z*z; + var qij:Number = 2*x*y; + var qjk:Number = 2*y*z; + var qki:Number = 2*z*x; + var qri:Number = 2*w*x; + var qrj:Number = 2*w*y; + var qrk:Number = 2*w*z; + + var aa:Number = 1 - qj2 - qk2; + var bb:Number = qij - qrk; + var ee:Number = qij + qrk; + var ff:Number = 1 - qi2 - qk2; + var ii:Number = qki - qrj; + var jj:Number = qjk + qri; + var kk:Number = 1 - qi2 - qj2; + + if (-1 < ii && ii < 1) { + if (angles == null) { + angles = new Vector3(Math.atan2(jj, kk), -Math.asin(ii), Math.atan2(ee, aa)); + } else { + angles.x = Math.atan2(jj, kk); + angles.y = -Math.asin(ii); + angles.z = Math.atan2(ee, aa); + } + } else { + if (angles == null) { + angles = new Vector3(0, 0.5*((ii <= -1) ? Math.PI : -Math.PI), Math.atan2(-bb, ff)); + } else { + angles.x = 0; + angles.y = (ii <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-bb, ff); + } + } + return angles; + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setFromEulerAnglesXYZ(x:Number, y:Number, z:Number):void { + setFromAxisAngleComponents(1, 0, 0, x); + + _q.setFromAxisAngleComponents(0, 1, 0, y); + append(_q); + normalize(); + + _q.setFromAxisAngleComponents(0, 0, 1, z); + append(_q); + normalize(); + } + + /** + * + */ + public function conjugate():void { + x = -x; + y = -y; + z = -z; + } + + /** + * Выполняет линейную интерполяцию. + * + * @param q1 начало отрезка + * @param q2 конец отрезка + * @param t время, обычно задаётся в интервале [0, 1] + */ + public function nlerp(q1:Quaternion, q2:Quaternion, t:Number):Quaternion { + var d:Number = 1 - t; + w = q1.w*d + q2.w*t; + x = q1.x*d + q2.x*t; + y = q1.y*d + q2.y*t; + z = q1.z*d + q2.z*t; + // inlined normalize + d = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param q + * @return + */ + public function subtract(q:Quaternion):Quaternion { + w -= q.w; + x -= q.x; + y -= q.y; + z -= q.z; + return this; + } + + /** + * + * @param q1 + * @param q2 + * @return + */ + public function diff(q1:Quaternion, q2:Quaternion):Quaternion { + w = q2.w - q1.w; + x = q2.x - q1.x; + y = q2.y - q1.y; + z = q2.z - q1.z; + return this; + } + + /** + * + * @param q + * @return + */ + public function copy(q:Quaternion):Quaternion { + w = q.w; + x = q.x; + y = q.y; + z = q.z; + return this; + } + + /** + * @param reslult + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + result.w = w; + return result; + } + + /** + * + * @return + */ + public function clone():Quaternion { + return new Quaternion(w, x, y, z); + } + + /** + * + * @return + */ + public function toString():String { + return "[" + w + ", " + x + ", " + y + ", " + z + "]"; + } + + /** + * Выполняет сферическую интерполяцию между двумя заданными кватернионами по наименьшему расстоянию. + * + * @param a первый кватерион + * @param b второй кватернион + * @param t параметр интерполяции, обычно принадлежит отрезку [0, 1] + * @return this + */ + public function slerp(a:Quaternion, b:Quaternion, t:Number):Quaternion { + var flip:Number = 1; + // Так как одна и та же ориентация представляется двумя значениями q и -q, нужно сменить знак одного из кватернионов + // если скалярное произведение отрицательно. Иначе будет получено интерполированное значение по наибольшему расстоянию. + var cosine:Number = a.w*b.w + a.x*b.x + a.y*b.y + a.z*b.z; + if (cosine < 0) { + cosine = -cosine; + flip = -1; + } + + if ((1 - cosine) < 0.001) { + // Вблизи нуля используется линейная интерполяция + var k1:Number = 1 - t; + var k2:Number = t*flip; + w = a.w*k1 + b.w*k2; + x = a.x*k1 + b.x*k2; + y = a.y*k1 + b.y*k2; + z = a.z*k1 + b.z*k2; + normalize(); + } else { + var theta:Number = Math.acos(cosine); + var sine:Number = Math.sin(theta); + var beta:Number = Math.sin((1 - t)*theta)/sine; + var alpha:Number = Math.sin(t*theta)/sine*flip; + w = a.w*beta + b.w*alpha; + x = a.x*beta + b.x*alpha; + y = a.y*beta + b.y*alpha; + z = a.z*beta + b.z*alpha; + } + + return this; + } + + } +} diff --git a/0.0.9.1/src/alternativa/math/.svn/text-base/Vector3.as.svn-base b/0.0.9.1/src/alternativa/math/.svn/text-base/Vector3.as.svn-base new file mode 100644 index 0000000..89010a8 --- /dev/null +++ b/0.0.9.1/src/alternativa/math/.svn/text-base/Vector3.as.svn-base @@ -0,0 +1,334 @@ +package alternativa.math { + import flash.geom.Vector3D; + + /** + * + */ + public class Vector3 extends Vector3D { + + public static const ZERO:Vector3 = new Vector3(0, 0, 0); + public static const X_AXIS:Vector3 = new Vector3(1, 0, 0); + public static const Y_AXIS:Vector3 = new Vector3(0, 1, 0); + public static const Z_AXIS:Vector3 = new Vector3(0, 0, 1); + + /** + * + * @param x + * @param y + * @param z + */ + public function Vector3(x:Number = 0, y:Number = 0, z:Number = 0) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * + * @return + */ + public function vLength():Number { + return Math.sqrt(x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function vLengthSqr():Number { + return x*x + y*y + z*z; + } + + /** + * + * @param length + * @return + */ + public function vSetLength(length:Number):Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = length; + } else { + var k:Number = length/Math.sqrt(x*x + y*y + z*z); + x *= k; + y *= k; + z *= k; + } + return this; + } + + /** + * + * @return + */ + public function vNormalize():Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = 1; + } else { + d = Math.sqrt(d); + x /= d; + y /= d; + z /= d; + } + return this; + } + + /** + * Сложение координат. + * + * @param v точка, координаты которой прибавляются к собственным + */ + public function vAdd(v:Vector3):Vector3 { + x += v.x; + y += v.y; + z += v.z; + return this; + } + + /** + * + * @param k + * @param v + * @return + */ + public function vAddScaled(k:Number, v:Vector3):Vector3 { + x += k*v.x; + y += k*v.y; + z += k*v.z; + return this; + } + + /** + * Вычитание координат. + * + * @param v точка, координаты которой вычитаются из собственных + */ + public function vSubtract(v:Vector3):Vector3 { + x -= v.x; + y -= v.y; + z -= v.z; + return this; + } + + /** + * Вычисляет сумму векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + * @return this + */ + public function vSum(a:Vector3, b:Vector3):Vector3 { + x = a.x + b.x; + y = a.y + b.y; + z = a.z + b.z; + return this; + } + + /** + * Вычисление разности векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + */ + public function vDiff(a:Vector3, b:Vector3):Vector3 { + x = a.x - b.x; + y = a.y - b.y; + z = a.z - b.z; + return this; + } + + /** + * Умножение на скаляр. + * + * @param k число, на которое умножаются координаты + */ + public function vScale(k:Number):Vector3 { + x *= k; + y *= k; + z *= k; + return this; + } + + /** + * Инвертирование вектора. + */ + public function vReverse():Vector3 { + x = -x; + y = -y; + z = -z; + return this; + } + + /** + * + * @param v + * @return + */ + public function vDot(v:Vector3):Number { + return x*v.x + y*v.y + z*v.z; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross(v:Vector3):Vector3 { + var xx:Number = y*v.z - z*v.y; + var yy:Number = z*v.x - x*v.z; + var zz:Number = x*v.y - y*v.x; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross2(a:Vector3, b:Vector3):Vector3 { + x = a.y*b.z - a.z*b.y; + y = a.z*b.x - a.x*b.z; + z = a.x*b.y - a.y*b.x; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy3(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * + * @param matrix + */ + public function vTransformBy3Tr(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz + m.d; + y = m.e*xx + m.f*yy + m.g*zz + m.h; + z = m.i*xx + m.j*yy + m.k*zz + m.l; + return this; + } + + /** + * + * @param m + * @return + */ + public function vTransformInverseBy4(m:Matrix4):Vector3 { + var xx:Number = x - m.d; + var yy:Number = y - m.h; + var zz:Number = z - m.l; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * + * @param m + */ + public function vDeltaTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * Установка координат. + */ + public function vReset(x:Number = 0, y:Number = 0, z:Number = 0):Vector3 { + this.x = x; + this.y = y; + this.z = z; + return this; + } + + /** + * Копирование координат точки. + * + * @param v точка, координаты которой копируются + */ + public function vCopy(v:Vector3):Vector3 { + x = v.x; + y = v.y; + z = v.z; + return this; + } + + /** + * Клонирование точки. + * + * @return клонированная точка + */ + public function vClone():Vector3 { + return new Vector3(x, y, z); + } + + /** + * Копирует координаты вектора в экземпляр класса Vector3D. + * + * @param result экземпляр, в который копируются координаты + * @return переданный параметр + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + return result; + } + + /** + * @param source + * @return + */ + public function copyFromVector3D(source:Vector3D):Vector3 { + x = source.x; + y = source.y; + z = source.z; + return this; + } + + override public function toString():String { + return "Vector3(" + x + "," + y + ", " + z + ")"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/math/Matrix3.as b/0.0.9.1/src/alternativa/math/Matrix3.as new file mode 100644 index 0000000..244cc98 --- /dev/null +++ b/0.0.9.1/src/alternativa/math/Matrix3.as @@ -0,0 +1,390 @@ +package alternativa.math { + import flash.geom.Vector3D; + + /** + * + */ + public class Matrix3 { + + public static const ZERO:Matrix3 = new Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0); + public static const IDENTITY:Matrix3 = new Matrix3(); + + public var a:Number; + public var b:Number; + public var c:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix3(a:Number = 1, b:Number = 0, c:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1) { + this.a = a; + this.b = b; + this.c = c; + + this.e = e; + this.f = f; + this.g = g; + + this.i = i; + this.j = j; + this.k = k; + } + + /** + * + * @return + */ + public function toIdentity():Matrix3 { + a = f = k = 1; + b = c = e = g = i = j = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + var det:Number = 1/(-cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk); + + a = (ff*kk - gg*jj)*det; + b = (cc*jj - bb*kk)*det; + c = (bb*gg - cc*ff)*det; + e = (gg*ii - ee*kk)*det; + f = (aa*kk - cc*ii)*det; + g = (cc*ee - aa*gg)*det; + i = (ee*jj - ff*ii)*det; + j = (bb*ii - aa*jj)*det; + k = (aa*ff - bb*ee)*det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prependTransposed(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.b + cc*m.c; + b = aa*m.e + bb*m.f + cc*m.g; + c = aa*m.i + bb*m.j + cc*m.k; + e = ee*m.a + ff*m.b + gg*m.c; + f = ee*m.e + ff*m.f + gg*m.g; + g = ee*m.i + ff*m.j + gg*m.k; + i = ii*m.a + jj*m.b + kk*m.c; + j = ii*m.e + jj*m.f + kk*m.g; + k = ii*m.i + jj*m.j + kk*m.k; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix3):Matrix3 { + a += m.a; + b += m.b; + c += m.c; + e += m.e; + f += m.f; + g += m.g; + i += m.i; + j += m.j; + k += m.k; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix3):Matrix3 { + a -= m.a; + b -= m.b; + c -= m.c; + e -= m.e; + f -= m.f; + g -= m.g; + i -= m.i; + j -= m.j; + k -= m.k; + + return this; + } + + /** + * + * @return + */ + public function transpose():Matrix3 { + var tmp:Number = b; + b = e; + e = tmp; + tmp = c; + c = i; + i = tmp; + tmp = g; + g = j; + j = tmp; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector3To3D(vin:Vector3, vout:Vector3D):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * + * @param matrix + */ + public function createSkewSymmetric(v:Vector3):Matrix3 { + a = f = k = 0; + b = -v.z; + c = v.y; + e = v.z; + g = -v.x; + i = -v.y; + j = v.x; + return this; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix3):Matrix3 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @return + */ + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):Matrix3 { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + + return this; + } + + /** + * Метод формирует матрицу поворота на заданный угол относительно заданной оси. + * + * @param axis нормализованный вектор, задающий ось, относительно которой выполняется поворот + * @param angle угол поворота в радианах + */ + public function fromAxisAngle(axis:Vector3, angle:Number):void { + var c1:Number = Math.cos(angle); + var s:Number = Math.sin(angle); + var t:Number = 1 - c1; + var x:Number = axis.x; + var y:Number = axis.y; + var z:Number = axis.z; + + a = t*x*x + c1; + b = t*x*y - z*s; + c = t*x*z + y*s; + + e = t*x*y + z*s; + f = t*y*y + c1; + g = t*y*z - x*s; + + i = t*x*z - y*s; + j = t*y*z + x*s; + k = t*z*z + c1; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix3 { + return new Matrix3(a, b, c, e, f, g, i, j, k); + } + + /** + * + * @return + */ + public function toString():String { + return "[Matrix3 (" + a + ", " + b + ", " + c + "), (" + e + ", " + f + ", " + g + "), (" + i + ", " + j + ", " + k + ")]"; + } + + /** + * @param angles + */ + public function getEulerAngles(angles:Vector3):void { + if (-1 < i && i < 1) { + angles.x = Math.atan2(j, k); + angles.y = -Math.asin(i); + angles.z = Math.atan2(e, a); + } else { + angles.x = 0; + angles.y = (i <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-b, f); + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/math/Matrix4.as b/0.0.9.1/src/alternativa/math/Matrix4.as new file mode 100644 index 0000000..d06eb5f --- /dev/null +++ b/0.0.9.1/src/alternativa/math/Matrix4.as @@ -0,0 +1,475 @@ +package alternativa.math { + + /** + * + */ + public class Matrix4 { + + public static const IDENTITY:Matrix4 = new Matrix4(); + + public var a:Number; + public var b:Number; + public var c:Number; + public var d:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + public var h:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + public var l:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix4(a:Number = 1, b:Number = 0, c:Number = 0, d:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, h:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1, l:Number = 0) { + this.a = a; + this.b = b; + this.c = c; + this.d = d; + + this.e = e; + this.f = f; + this.g = g; + this.h = h; + + this.i = i; + this.j = j; + this.k = k; + this.l = l; + } + + /** + * + * @return + */ + public function toIdentity():Matrix4 { + a = f = k = 1; + b = c = e = g = i = j = d = h = l = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + var det:Number = -cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk; + + a = (-gg*jj + ff*kk)/det; + b = (cc*jj - bb*kk)/det; + c = (-cc*ff + bb*gg)/det; + d = (dd*gg*jj - cc*hh*jj - dd*ff*kk + bb*hh*kk + cc*ff*ll - bb*gg*ll)/det; + e = (gg*ii - ee*kk)/det; + f = (-cc*ii + aa*kk)/det; + g = (cc*ee - aa*gg)/det; + h = (cc*hh*ii - dd*gg*ii + dd*ee*kk - aa*hh*kk - cc*ee*ll + aa*gg*ll)/det; + i = (-ff*ii + ee*jj)/det; + j = (bb*ii - aa*jj)/det; + k = (-bb*ee + aa*ff)/det; + l = (dd*ff*ii - bb*hh*ii - dd*ee*jj + aa*hh*jj + bb*ee*ll - aa*ff*ll)/det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + d = m.a*dd + m.b*hh + m.c*ll + m.d; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + h = m.e*dd + m.f*hh + m.g*ll + m.h; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + l = m.i*dd + m.j*hh + m.k*ll + m.l; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + d = aa*m.d + bb*m.h + cc*m.l + dd; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + h = ee*m.d + ff*m.h + gg*m.l + hh; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + l = ii*m.d + jj*m.h + kk*m.l + ll; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix4):Matrix4 { + a += m.a; + b += m.b; + c += m.c; + d += m.d; + e += m.e; + f += m.f; + g += m.g; + h += m.h; + i += m.i; + j += m.j; + k += m.k; + l += m.l; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix4):Matrix4 { + a -= m.a; + b -= m.b; + c -= m.c; + d -= m.d; + e -= m.e; + f -= m.f; + g -= m.g; + h -= m.h; + i -= m.i; + j -= m.j; + k -= m.k; + l -= m.l; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectors(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + * @param len + */ + public function transformVectorsN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverse(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverseN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(idx:int, axis:Vector3):void { + switch (idx) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + case 3: + axis.x = d; + axis.y = h; + axis.z = l; + return; + } + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix4):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = m.d; + e = m.e; + f = m.f; + g = m.g; + h = m.h; + i = m.i; + j = m.j; + k = m.k; + l = m.l; + + return this; + } + + public function setFromMatrix3(m:Matrix3, offset:Vector3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = offset.x; + e = m.e; + f = m.f; + g = m.g; + h = offset.y; + i = m.i; + j = m.j; + k = m.k; + l = offset.z; + + return this; + } + + public function setOrientationFromMatrix3(m:Matrix3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @param angles + */ + public function getEulerAngles(angles:Vector3):void { + if (-1 < i && i < 1) { + angles.x = Math.atan2(j, k); + angles.y = -Math.asin(i); + angles.z = Math.atan2(e, a); + } else { + angles.x = 0; + angles.y = (i <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-b, f); + } + } + + /** + * Устанавливает координаты матрицы. + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + d = pos.x; + h = pos.y; + l = pos.z; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix4 { + return new Matrix4(a, b, c, d, e, f, g, h, i, j, k, l); + } + + /** + * + * @return + */ + public function toString():String { + return "[Matrix4 [" + a.toFixed(3) + " " + b.toFixed(3) + " " + c.toFixed(3) + " " + d.toFixed(3) + "] [" + e.toFixed(3) + " " + f.toFixed(3) + " " + g.toFixed(3) + " " + h.toFixed(3) + "] [" + i.toFixed(3) + " " + j.toFixed(3) + " " + k.toFixed(3) + " " + l.toFixed(3) + "]]"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/math/Quaternion.as b/0.0.9.1/src/alternativa/math/Quaternion.as new file mode 100644 index 0000000..ddc85f5 --- /dev/null +++ b/0.0.9.1/src/alternativa/math/Quaternion.as @@ -0,0 +1,458 @@ +package alternativa.math { + import flash.geom.Vector3D; + + public class Quaternion { + + public var w:Number; + public var x:Number; + public var y:Number; + public var z:Number; + + public static function multiply(q1:Quaternion, q2:Quaternion, result:Quaternion):void { + result.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z; + result.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y; + result.y = q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z; + result.z = q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x; + } + + public static function createFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngle(axis, angle); + return q; + } + + public static function createFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngleComponents(x, y, z, angle); + return q; + } + + private static var _q:Quaternion = new Quaternion(); + + public function Quaternion(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0) { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + } + + public function reset(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0):Quaternion { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + return this; + } + + public function normalize():Quaternion { + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * Умножает на указанный кватернион слева: this * q + * + * @param q множитель + */ + public function prepend(q:Quaternion):Quaternion { + var ww:Number = w*q.w - x*q.x - y*q.y - z*q.z; + var xx:Number = w*q.x + x*q.w + y*q.z - z*q.y; + var yy:Number = w*q.y + y*q.w + z*q.x - x*q.z; + var zz:Number = w*q.z + z*q.w + x*q.y - y*q.x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Умножает на указанный кватернион справа: q * this + * + * @param q множитель + */ + public function append(q:Quaternion):Quaternion { + var ww:Number = q.w*w - q.x*x - q.y*y - q.z*z; + var xx:Number = q.w*x + q.x*w + q.y*z - q.z*y; + var yy:Number = q.w*y + q.y*w + q.z*x - q.x*z; + var zz:Number = q.w*z + q.z*w + q.x*y - q.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * + * @param vector + */ + public function rotateByVector(v:Vector3):Quaternion { + var ww:Number = -v.x*x - v.y*y - v.z*z; + var xx:Number = v.x*w + v.y*z - v.z*y; + var yy:Number = v.y*w + v.z*x - v.x*z; + var zz:Number = v.z*w + v.x*y - v.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Добавляет вращение, приданное вектором угловой скорости за указанное время. + * + * @param v + * @param scale + */ + public function addScaledVector(v:Vector3, scale:Number):Quaternion { + var vx:Number = v.x*scale; + var vy:Number = v.y*scale; + var vz:Number = v.z*scale; + var ww:Number = -x*vx - y*vy - z*vz; + var xx:Number = vx*w + vy*z - vz*y; + var yy:Number = vy*w + vz*x - vx*z; + var zz:Number = vz*w + vx*y - vy*x; + w += 0.5*ww; + x += 0.5*xx; + y += 0.5*yy; + z += 0.5*zz; + // inlined normalize + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param m + * @return + */ + public function toMatrix3(m:Matrix3):Quaternion { + var xx2:Number = 2.0*x*x; + var yy2:Number = 2.0*y*y; + var zz2:Number = 2.0*z*z; + var xy2:Number = 2.0*x*y; + var yz2:Number = 2.0*y*z; + var zx2:Number = 2.0*z*x; + var wx2:Number = 2.0*w*x; + var wy2:Number = 2.0*w*y; + var wz2:Number = 2.0*w*z; + + m.a = 1.0 - yy2 - zz2; + m.b = xy2 - wz2; + m.c = zx2 + wy2; + + m.e = xy2 + wz2; + m.f = 1.0 - xx2 - zz2; + m.g = yz2 - wx2; + + m.i = zx2 - wy2; + m.j = yz2 + wx2; + m.k = 1.0 - xx2 - yy2; + return this; + } + + /** + * + * @return + */ + public function length():Number { + return Math.sqrt(w*w + x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function lengthSqr():Number { + return w*w + x*x + y*y + z*z; + } + + /** + * + * @param axis + * @param angle + * @return + */ + public function setFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z); + x = axis.x*k; + y = axis.y*k; + z = axis.z*k; + return this; + } + + /** + * + * @param x + * @param y + * @param z + * @param angle + */ + public function setFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(x*x + y*y + z*z); + this.x = x*k; + this.y = y*k; + this.z = z*k; + return this; + } + + /** + * + * @param vector + */ + public function toAxisVector(v:Vector3 = null):Vector3 { + if (w < -1 || w > 1) { + normalize(); + } + if (v == null) { + v = new Vector3(); + } + if (w > -1 && w < 1) { + if (w == 0) { + v.x = x; + v.y = y; + v.z = z; + } else { + var angle:Number = 2*Math.acos(w); + var coeff:Number = 1/Math.sqrt(1 - w*w); + v.x = x*coeff*angle; + v.y = y*coeff*angle; + v.z = z*coeff*angle; + } + } else { + v.x = 0; + v.y = 0; + v.z = 0; + } + return v; + } + + /** + * + * @param rotations + */ + public function getEulerAngles(angles:Vector3):Vector3 { + var qi2:Number = 2*x*x; + var qj2:Number = 2*y*y; + var qk2:Number = 2*z*z; + var qij:Number = 2*x*y; + var qjk:Number = 2*y*z; + var qki:Number = 2*z*x; + var qri:Number = 2*w*x; + var qrj:Number = 2*w*y; + var qrk:Number = 2*w*z; + + var aa:Number = 1 - qj2 - qk2; + var bb:Number = qij - qrk; + var ee:Number = qij + qrk; + var ff:Number = 1 - qi2 - qk2; + var ii:Number = qki - qrj; + var jj:Number = qjk + qri; + var kk:Number = 1 - qi2 - qj2; + + if (-1 < ii && ii < 1) { + if (angles == null) { + angles = new Vector3(Math.atan2(jj, kk), -Math.asin(ii), Math.atan2(ee, aa)); + } else { + angles.x = Math.atan2(jj, kk); + angles.y = -Math.asin(ii); + angles.z = Math.atan2(ee, aa); + } + } else { + if (angles == null) { + angles = new Vector3(0, 0.5*((ii <= -1) ? Math.PI : -Math.PI), Math.atan2(-bb, ff)); + } else { + angles.x = 0; + angles.y = (ii <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-bb, ff); + } + } + return angles; + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setFromEulerAnglesXYZ(x:Number, y:Number, z:Number):void { + setFromAxisAngleComponents(1, 0, 0, x); + + _q.setFromAxisAngleComponents(0, 1, 0, y); + append(_q); + normalize(); + + _q.setFromAxisAngleComponents(0, 0, 1, z); + append(_q); + normalize(); + } + + /** + * + */ + public function conjugate():void { + x = -x; + y = -y; + z = -z; + } + + /** + * Выполняет линейную интерполяцию. + * + * @param q1 начало отрезка + * @param q2 конец отрезка + * @param t время, обычно задаётся в интервале [0, 1] + */ + public function nlerp(q1:Quaternion, q2:Quaternion, t:Number):Quaternion { + var d:Number = 1 - t; + w = q1.w*d + q2.w*t; + x = q1.x*d + q2.x*t; + y = q1.y*d + q2.y*t; + z = q1.z*d + q2.z*t; + // inlined normalize + d = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param q + * @return + */ + public function subtract(q:Quaternion):Quaternion { + w -= q.w; + x -= q.x; + y -= q.y; + z -= q.z; + return this; + } + + /** + * + * @param q1 + * @param q2 + * @return + */ + public function diff(q1:Quaternion, q2:Quaternion):Quaternion { + w = q2.w - q1.w; + x = q2.x - q1.x; + y = q2.y - q1.y; + z = q2.z - q1.z; + return this; + } + + /** + * + * @param q + * @return + */ + public function copy(q:Quaternion):Quaternion { + w = q.w; + x = q.x; + y = q.y; + z = q.z; + return this; + } + + /** + * @param reslult + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + result.w = w; + return result; + } + + /** + * + * @return + */ + public function clone():Quaternion { + return new Quaternion(w, x, y, z); + } + + /** + * + * @return + */ + public function toString():String { + return "[" + w + ", " + x + ", " + y + ", " + z + "]"; + } + + /** + * Выполняет сферическую интерполяцию между двумя заданными кватернионами по наименьшему расстоянию. + * + * @param a первый кватерион + * @param b второй кватернион + * @param t параметр интерполяции, обычно принадлежит отрезку [0, 1] + * @return this + */ + public function slerp(a:Quaternion, b:Quaternion, t:Number):Quaternion { + var flip:Number = 1; + // Так как одна и та же ориентация представляется двумя значениями q и -q, нужно сменить знак одного из кватернионов + // если скалярное произведение отрицательно. Иначе будет получено интерполированное значение по наибольшему расстоянию. + var cosine:Number = a.w*b.w + a.x*b.x + a.y*b.y + a.z*b.z; + if (cosine < 0) { + cosine = -cosine; + flip = -1; + } + + if ((1 - cosine) < 0.001) { + // Вблизи нуля используется линейная интерполяция + var k1:Number = 1 - t; + var k2:Number = t*flip; + w = a.w*k1 + b.w*k2; + x = a.x*k1 + b.x*k2; + y = a.y*k1 + b.y*k2; + z = a.z*k1 + b.z*k2; + normalize(); + } else { + var theta:Number = Math.acos(cosine); + var sine:Number = Math.sin(theta); + var beta:Number = Math.sin((1 - t)*theta)/sine; + var alpha:Number = Math.sin(t*theta)/sine*flip; + w = a.w*beta + b.w*alpha; + x = a.x*beta + b.x*alpha; + y = a.y*beta + b.y*alpha; + z = a.z*beta + b.z*alpha; + } + + return this; + } + + } +} diff --git a/0.0.9.1/src/alternativa/math/Vector3.as b/0.0.9.1/src/alternativa/math/Vector3.as new file mode 100644 index 0000000..89010a8 --- /dev/null +++ b/0.0.9.1/src/alternativa/math/Vector3.as @@ -0,0 +1,334 @@ +package alternativa.math { + import flash.geom.Vector3D; + + /** + * + */ + public class Vector3 extends Vector3D { + + public static const ZERO:Vector3 = new Vector3(0, 0, 0); + public static const X_AXIS:Vector3 = new Vector3(1, 0, 0); + public static const Y_AXIS:Vector3 = new Vector3(0, 1, 0); + public static const Z_AXIS:Vector3 = new Vector3(0, 0, 1); + + /** + * + * @param x + * @param y + * @param z + */ + public function Vector3(x:Number = 0, y:Number = 0, z:Number = 0) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * + * @return + */ + public function vLength():Number { + return Math.sqrt(x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function vLengthSqr():Number { + return x*x + y*y + z*z; + } + + /** + * + * @param length + * @return + */ + public function vSetLength(length:Number):Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = length; + } else { + var k:Number = length/Math.sqrt(x*x + y*y + z*z); + x *= k; + y *= k; + z *= k; + } + return this; + } + + /** + * + * @return + */ + public function vNormalize():Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = 1; + } else { + d = Math.sqrt(d); + x /= d; + y /= d; + z /= d; + } + return this; + } + + /** + * Сложение координат. + * + * @param v точка, координаты которой прибавляются к собственным + */ + public function vAdd(v:Vector3):Vector3 { + x += v.x; + y += v.y; + z += v.z; + return this; + } + + /** + * + * @param k + * @param v + * @return + */ + public function vAddScaled(k:Number, v:Vector3):Vector3 { + x += k*v.x; + y += k*v.y; + z += k*v.z; + return this; + } + + /** + * Вычитание координат. + * + * @param v точка, координаты которой вычитаются из собственных + */ + public function vSubtract(v:Vector3):Vector3 { + x -= v.x; + y -= v.y; + z -= v.z; + return this; + } + + /** + * Вычисляет сумму векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + * @return this + */ + public function vSum(a:Vector3, b:Vector3):Vector3 { + x = a.x + b.x; + y = a.y + b.y; + z = a.z + b.z; + return this; + } + + /** + * Вычисление разности векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + */ + public function vDiff(a:Vector3, b:Vector3):Vector3 { + x = a.x - b.x; + y = a.y - b.y; + z = a.z - b.z; + return this; + } + + /** + * Умножение на скаляр. + * + * @param k число, на которое умножаются координаты + */ + public function vScale(k:Number):Vector3 { + x *= k; + y *= k; + z *= k; + return this; + } + + /** + * Инвертирование вектора. + */ + public function vReverse():Vector3 { + x = -x; + y = -y; + z = -z; + return this; + } + + /** + * + * @param v + * @return + */ + public function vDot(v:Vector3):Number { + return x*v.x + y*v.y + z*v.z; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross(v:Vector3):Vector3 { + var xx:Number = y*v.z - z*v.y; + var yy:Number = z*v.x - x*v.z; + var zz:Number = x*v.y - y*v.x; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross2(a:Vector3, b:Vector3):Vector3 { + x = a.y*b.z - a.z*b.y; + y = a.z*b.x - a.x*b.z; + z = a.x*b.y - a.y*b.x; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy3(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * + * @param matrix + */ + public function vTransformBy3Tr(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz + m.d; + y = m.e*xx + m.f*yy + m.g*zz + m.h; + z = m.i*xx + m.j*yy + m.k*zz + m.l; + return this; + } + + /** + * + * @param m + * @return + */ + public function vTransformInverseBy4(m:Matrix4):Vector3 { + var xx:Number = x - m.d; + var yy:Number = y - m.h; + var zz:Number = z - m.l; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * + * @param m + */ + public function vDeltaTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * Установка координат. + */ + public function vReset(x:Number = 0, y:Number = 0, z:Number = 0):Vector3 { + this.x = x; + this.y = y; + this.z = z; + return this; + } + + /** + * Копирование координат точки. + * + * @param v точка, координаты которой копируются + */ + public function vCopy(v:Vector3):Vector3 { + x = v.x; + y = v.y; + z = v.z; + return this; + } + + /** + * Клонирование точки. + * + * @return клонированная точка + */ + public function vClone():Vector3 { + return new Vector3(x, y, z); + } + + /** + * Копирует координаты вектора в экземпляр класса Vector3D. + * + * @param result экземпляр, в который копируются координаты + * @return переданный параметр + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + return result; + } + + /** + * @param source + * @return + */ + public function copyFromVector3D(source:Vector3D):Vector3 { + x = source.x; + y = source.y; + z = source.z; + return this; + } + + override public function toString():String { + return "Vector3(" + x + "," + y + ", " + z + ")"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/.svn/all-wcprops b/0.0.9.1/src/alternativa/physics/.svn/all-wcprops new file mode 100644 index 0000000..8cb9e68 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/.svn/all-wcprops @@ -0,0 +1,77 @@ +K 25 +svn:wc:ra_dav:version-url +V 103 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics +END +Body.as +K 25 +svn:wc:ra_dav:version-url +V 111 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/Body.as +END +BodyState.as +K 25 +svn:wc:ra_dav:version-url +V 116 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/BodyState.as +END +CollisionPrimitiveListItem.as +K 25 +svn:wc:ra_dav:version-url +V 133 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/CollisionPrimitiveListItem.as +END +CollisionPrimitiveList.as +K 25 +svn:wc:ra_dav:version-url +V 129 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/CollisionPrimitiveList.as +END +PhysicsScene.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/PhysicsScene.as +END +PhysicsUtils.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/PhysicsUtils.as +END +altphysics.as +K 25 +svn:wc:ra_dav:version-url +V 117 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/altphysics.as +END +ContactPoint.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/ContactPoint.as +END +BodyListItem.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/BodyListItem.as +END +BodyList.as +K 25 +svn:wc:ra_dav:version-url +V 115 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/BodyList.as +END +Contact.as +K 25 +svn:wc:ra_dav:version-url +V 114 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/Contact.as +END +BodyMaterial.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/BodyMaterial.as +END diff --git a/0.0.9.1/src/alternativa/physics/.svn/entries b/0.0.9.1/src/alternativa/physics/.svn/entries new file mode 100644 index 0000000..4e8d26a --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/.svn/entries @@ -0,0 +1,181 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics +http://svndev.alternativaplatform.com + + + +2010-03-06T16:51:19.781174Z +29404 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +CollisionPrimitiveList.as +file + + + + +2010-10-28T04:32:44.000000Z +5752b1f4909c5146720b58fbfffed468 +2009-11-01T12:35:52.270571Z +22955 +mike + +altphysics.as +file + + + + +2010-10-28T04:32:44.000000Z +04fbe40a27502dbbd0dba02a76b2df50 +2009-04-20T20:09:14.715403Z +11579 +mike + +collision +dir + +primitives +dir + +BodyList.as +file + + + + +2010-10-28T04:32:44.000000Z +08a90332660bd0273728006d1402dd25 +2009-11-01T12:35:52.270571Z +22955 +mike + +Contact.as +file + + + + +2010-10-28T04:32:44.000000Z +eb89f4f37bcf2d21eadc733f244fcf64 +2010-02-12T12:34:25.192477Z +28424 +mike + +Body.as +file + + + + +2010-10-28T04:32:44.000000Z +355d0fb70dbb4946d4090a7c94c0d58a +2010-02-12T12:34:25.192477Z +28424 +mike + +BodyState.as +file + + + + +2010-10-28T04:32:44.000000Z +5be68820e6f6185d62b3928c39dde285 +2009-11-24T08:44:15.941480Z +23758 +mike + +CollisionPrimitiveListItem.as +file + + + + +2010-10-28T04:32:44.000000Z +cec5dbfefef812d87481478455f367b1 +2009-10-19T07:17:33.113306Z +22253 +mike + +PhysicsScene.as +file + + + + +2010-10-28T04:32:44.000000Z +96c74784bb71c06141cc469eec57ccb8 +2010-02-19T13:09:00.412754Z +28731 +mike + +PhysicsUtils.as +file + + + + +2010-10-28T04:32:44.000000Z +dce0a94f8a861291a2a8942ed0ba54a5 +2009-11-24T08:44:15.941480Z +23758 +mike + +constraints +dir + +ContactPoint.as +file + + + + +2010-10-28T04:32:44.000000Z +14f23db94810328dedd7da2992812ced +2009-11-24T08:44:15.941480Z +23758 +mike + +BodyListItem.as +file + + + + +2010-10-28T04:32:44.000000Z +2fc2b598310b24aef3f8b90e14a8ba4f +2009-10-19T07:17:33.113306Z +22253 +mike + +BodyMaterial.as +file + + + + +2010-10-28T04:32:44.000000Z +9b700692ebb8017882f3e8cec095e1a9 +2009-10-19T07:17:33.113306Z +22253 +mike + diff --git a/0.0.9.1/src/alternativa/physics/.svn/format b/0.0.9.1/src/alternativa/physics/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.1/src/alternativa/physics/.svn/text-base/Body.as.svn-base b/0.0.9.1/src/alternativa/physics/.svn/text-base/Body.as.svn-base new file mode 100644 index 0000000..cd22c5b --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/.svn/text-base/Body.as.svn-base @@ -0,0 +1,405 @@ +package alternativa.physics { + + import alternativa.math.Matrix3; + import alternativa.math.Matrix4; + import alternativa.math.Quaternion; + import alternativa.math.Vector3; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.IBodyCollisionPredicate; + import alternativa.physics.collision.types.BoundBox; + + use namespace altphysics; + + /** + * + */ + public class Body { + + public static var linDamping:Number = 0.997; + public static var rotDamping:Number = 0.997; + + // Идентификатор тела, уникальный в пределах мира + public var id:int; + // Имя тела + public var name:String; + // Мир, в котором находится тело + public var world:PhysicsScene; + // Флаг подвижности тела + public var movable:Boolean = true; + // Флаг указывает, может ли тело быть заморожено + public var canFreeze:Boolean = false; + + public var freezeCounter:int; + public var frozen:Boolean = false; + // Ограничивающий бокс тела + public var aabb:BoundBox = new BoundBox(); + // Предикат, через который тело получает сообщения о столкновениях + public var postCollisionPredicate:IBodyCollisionPredicate; + + // Текущее состояние тела + public var state:BodyState = new BodyState(); + // Предыдущее состояние тела + public var prevState:BodyState = new BodyState(); + // Линейное ускорение тела на текущем шаге симуляции + public var accel:Vector3 = new Vector3(); + // Угловое ускорение тела на текущем шаге симуляции + public var angleAccel:Vector3 = new Vector3(); + // Физический материал тела + public var material:BodyMaterial = new BodyMaterial(); + // Обратная масса тела + public var invMass:Number = 1; + // Обратная матрица тензора инерции в локальных координатах + public var invInertia:Matrix3 = new Matrix3(); + // Обратная матрица тензора инерции в мировых координатах + public var invInertiaWorld:Matrix3 = new Matrix3(); + // Базисная матрица тела в мировых координатах + public var baseMatrix:Matrix3 = new Matrix3(); + + public const MAX_CONTACTS:int = 20; + public var contacts:Vector. = new Vector.(MAX_CONTACTS); + public var contactsNum:int; + + public var collisionPrimitives:CollisionPrimitiveList; + + // Аккумулятор сил + public var forceAccum:Vector3 = new Vector3(); + // Аккумулятор моментов + public var torqueAccum:Vector3 = new Vector3(); + + // Внутренние переменные для избежания создания экземпляров + private static var _r:Vector3 = new Vector3(); + private static var _f:Vector3 = new Vector3(); + + /** + * + * @param invMass + * @param invInertia + */ + public function Body(invMass:Number, invInertia:Matrix3) { + this.invMass = invMass; + this.invInertia.copy(invInertia); + } + + /** + * @param primitive + * @param localTransform + */ + public function addCollisionPrimitive(primitive:CollisionPrimitive, localTransform:Matrix4 = null):void { + if (primitive == null) { + throw new ArgumentError("Primitive cannot be null"); + } + if (collisionPrimitives == null) { + collisionPrimitives = new CollisionPrimitiveList(); + } + collisionPrimitives.append(primitive); + primitive.setBody(this, localTransform); + } + + /** + * + * @param primitive + */ + public function removeCollisionPrimitive(primitive:CollisionPrimitive):void { + if (collisionPrimitives == null) return; + primitive.setBody(null); + collisionPrimitives.remove(primitive); + if (collisionPrimitives.size == 0) { + collisionPrimitives = null; + } + } + + /** + * @param t + * @param result + */ + public function interpolate(t:Number, pos:Vector3, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.w = prevState.orientation.w*t1 + state.orientation.w*t; + orientation.x = prevState.orientation.x*t1 + state.orientation.x*t; + orientation.y = prevState.orientation.y*t1 + state.orientation.y*t; + orientation.z = prevState.orientation.z*t1 + state.orientation.z*t; + } + + /** + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + state.pos.vCopy(pos); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setPositionXYZ(x:Number, y:Number, z:Number):void { + state.pos.vReset(x, y, z); + } + + /** + * + * @param vel + */ + public function setVelocity(vel:Vector3):void { + state.velocity.vCopy(vel); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setVelocityXYZ(x:Number, y:Number, z:Number):void { + state.velocity.vReset(x, y, z); + } + + /** + * + * @param rot + */ + public function setRotation(rot:Vector3):void { + state.rotation.vCopy(rot); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setRotationXYZ(x:Number, y:Number, z:Number):void { + state.rotation.vReset(x, y, z); + } + + /** + * + * @param q + */ + public function setOrientation(q:Quaternion):void { + state.orientation.copy(q); + } + + /** + * @param r + * @param dir + * @param magnitude + */ + public function applyRelPosWorldImpulse(r:Vector3, dir:Vector3, magnitude:Number):void { + var d:Number = magnitude*invMass; + // Линейная часть + state.velocity.x += d*dir.x; + state.velocity.y += d*dir.y; + state.velocity.z += d*dir.z; + + // Вращательная часть + var x:Number = (r.y*dir.z - r.z*dir.y)*magnitude; + var y:Number = (r.z*dir.x - r.x*dir.z)*magnitude; + var z:Number = (r.x*dir.y - r.y*dir.x)*magnitude; + + state.rotation.x += invInertiaWorld.a*x + invInertiaWorld.b*y + invInertiaWorld.c*z; + state.rotation.y += invInertiaWorld.e*x + invInertiaWorld.f*y + invInertiaWorld.g*z; + state.rotation.z += invInertiaWorld.i*x + invInertiaWorld.j*y + invInertiaWorld.k*z; + } + + /** + * @param f + */ + public function addForce(f:Vector3):void { + forceAccum.vAdd(f); + } + + /** + * + * @param fx + * @param fy + * @param fz + */ + public function addForceXYZ(fx:Number, fy:Number, fz:Number):void { + forceAccum.x += fx; + forceAccum.y += fy; + forceAccum.z += fz; + } + + /** + * @param pos + * @param f + */ + public function addWorldForceXYZ(px:Number, py:Number, pz:Number, fx:Number, fy:Number, fz:Number):void { + forceAccum.x += fx; + forceAccum.y += fy; + forceAccum.z += fz; + + var pos:Vector3 = state.pos; + var rx:Number = px - pos.x; + var ry:Number = py - pos.y; + var rz:Number = pz - pos.z; + +// var x:Number = ry*fz - rz*fy; +// var y:Number = rz*fx - rx*fz; +// var z:Number = rx*fy - ry*fx; + + torqueAccum.x += ry*fz - rz*fy; + torqueAccum.y += rz*fx - rx*fz; + torqueAccum.z += rx*fy - ry*fx; + } + + /** + * @param pos + * @param f + */ + public function addWorldForce(pos:Vector3, force:Vector3):void { + forceAccum.vAdd(force); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(force)); + } + + /** + * @param pos + * @param f + */ + public function addWorldForceScaled(pos:Vector3, force:Vector3, scale:Number):void { + _f.x = scale*force.x; + _f.y = scale*force.y; + _f.z = scale*force.z; + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(_f)); + } + + /** + * @param pos + * @param f + */ + public function addLocalForce(pos:Vector3, force:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(pos, _r); + // Трансформируем вектор силы в мировую систему + baseMatrix.transformVector(force, _f); + // Добавляем силу и момент + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vCross(_f)); + } + + /** + * + * @param localPos + * @param worldForce + */ + public function addWorldForceAtLocalPoint(localPos:Vector3, worldForce:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(localPos, _r); + // Добавляем силу и момент + forceAccum.vAdd(worldForce); + torqueAccum.vAdd(_r.vCross(worldForce)); + } + + /** + * @param dt + */ + public function beforePhysicsStep(dt:Number):void { + } + + /** + * @param t + */ + public function addTorque(t:Vector3):void { + torqueAccum.vAdd(t); + } + + /** + * + */ + altphysics function clearAccumulators():void { + forceAccum.x = forceAccum.y = forceAccum.z = 0; + torqueAccum.x = torqueAccum.y = torqueAccum.z = 0; + } + + /** + * + */ + altphysics function calcAccelerations():void { + accel.x = forceAccum.x*invMass; + accel.y = forceAccum.y*invMass; + accel.z = forceAccum.z*invMass; + angleAccel.x = invInertiaWorld.a*torqueAccum.x + invInertiaWorld.b*torqueAccum.y + invInertiaWorld.c*torqueAccum.z; + angleAccel.y = invInertiaWorld.e*torqueAccum.x + invInertiaWorld.f*torqueAccum.y + invInertiaWorld.g*torqueAccum.z; + angleAccel.z = invInertiaWorld.i*torqueAccum.x + invInertiaWorld.j*torqueAccum.y + invInertiaWorld.k*torqueAccum.z; + } + + /** + * Вычисляет производные данные. + */ + public function calcDerivedData():void { + // Вычисление базисной матрицы и обратного тензора инерции в мировых координатах + state.orientation.toMatrix3(baseMatrix); + invInertiaWorld.copy(invInertia).append(baseMatrix).prependTransposed(baseMatrix); + if (collisionPrimitives != null) { + aabb.infinity(); + var item:CollisionPrimitiveListItem = collisionPrimitives.head; + while (item != null) { + var primitive:CollisionPrimitive = item.primitive; + primitive.transform.setFromMatrix3(baseMatrix, state.pos); + if (primitive.localTransform != null) { + primitive.transform.prepend(primitive.localTransform); + } + primitive.calculateAABB(); + aabb.addBoundBox(primitive.aabb); + item = item.next; + } + } + } + + /** + * + */ + altphysics function saveState():void { + prevState.copy(state); + } + + /** + * + */ + altphysics function restoreState():void { + state.copy(prevState); + } + + /** + * @param dt + */ + altphysics function integrateVelocity(dt:Number):void { + // v = v + a*t + state.velocity.x += accel.x*dt; + state.velocity.y += accel.y*dt; + state.velocity.z += accel.z*dt; + // rot = rot + eps*t + state.rotation.x += angleAccel.x*dt; + state.rotation.y += angleAccel.y*dt; + state.rotation.z += angleAccel.z*dt; + + state.velocity.x *= linDamping; + state.velocity.y *= linDamping; + state.velocity.z *= linDamping; + + state.rotation.x *= rotDamping; + state.rotation.y *= rotDamping; + state.rotation.z *= rotDamping; + } + + /** + * + */ + altphysics function integratePosition(dt:Number):void { + // pos = pos + v*t + state.pos.x += state.velocity.x*dt; + state.pos.y += state.velocity.y*dt; + state.pos.z += state.velocity.z*dt; + // q = q + 0.5*rot*q + state.orientation.addScaledVector(state.rotation, dt); + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/.svn/text-base/BodyList.as.svn-base b/0.0.9.1/src/alternativa/physics/.svn/text-base/BodyList.as.svn-base new file mode 100644 index 0000000..aa8f79f --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/.svn/text-base/BodyList.as.svn-base @@ -0,0 +1,76 @@ +package alternativa.physics { + + /** + * + */ + public class BodyList { + + public var head:BodyListItem; + public var tail:BodyListItem; + public var size:int; + + /** + * + */ + public function BodyList() { + } + + /** + * + * @param body + */ + public function append(body:Body):void { + var item:BodyListItem = BodyListItem.create(body); + if (head == null) { + head = tail = item; + } else { + tail.next = item; + item.prev = tail; + tail = item; + } + size++; + } + + /** + * + * @param body + */ + public function remove(body:Body):Boolean { + var item:BodyListItem = findItem(body); + if (item == null) return false; + if (item == head) { + if (size == 1) { + head = tail = null; + } else { + head = item.next; + head.prev = null; + } + } else { + if (item == tail) { + tail = item.prev; + tail.next = null; + } else { + item.prev.next = item.next; + item.next.prev = item.prev; + } + } + item.dispose(); + size--; + return true; + } + + /** + * + * @param body + * @return + */ + public function findItem(body:Body):BodyListItem { + var item:BodyListItem = head; + while (item != null && item.body != body) { + item = item.next; + } + return item; + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/.svn/text-base/BodyListItem.as.svn-base b/0.0.9.1/src/alternativa/physics/.svn/text-base/BodyListItem.as.svn-base new file mode 100644 index 0000000..382e11c --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/.svn/text-base/BodyListItem.as.svn-base @@ -0,0 +1,74 @@ +package alternativa.physics { + + /** + * + */ + public class BodyListItem { + + // Верхний элемент хранилища + private static var poolTop:BodyListItem; + + /** + * Создаёт новый элемент списка. + * + * @param primitive примитив, содержащийся в элементе + * @return новый элемент списка + */ + public static function create(body:Body):BodyListItem { + var item:BodyListItem; + if (poolTop == null) { + item = new BodyListItem(body); + } else { + item = poolTop; + poolTop = item.next; + item.next = null; + item.body = body; + } + return item; + } + + /** + * Очищает хранилище. + */ + public static function clearPool():void { + var item:BodyListItem = poolTop; + while (item != null) { + poolTop = item.next; + item.next = null; + item = poolTop; + } + } + + /** + * + */ + public var body:Body; + /** + * + */ + public var next:BodyListItem; + /** + * + */ + public var prev:BodyListItem; + + /** + * + * @param body + */ + public function BodyListItem(body:Body) { + this.body = body; + } + + /** + * + */ + public function dispose():void { + body = null; + prev = null; + next = poolTop; + poolTop = this; + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/.svn/text-base/BodyMaterial.as.svn-base b/0.0.9.1/src/alternativa/physics/.svn/text-base/BodyMaterial.as.svn-base new file mode 100644 index 0000000..7a7c47d --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/.svn/text-base/BodyMaterial.as.svn-base @@ -0,0 +1,11 @@ +package alternativa.physics { + + public class BodyMaterial { + + public var restitution:Number = 0; + public var friction:Number = 0.3; +// public var dynamicFriction:Number = 0.2; +// public var dynamicFriction:Number = 0.2; + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/.svn/text-base/BodyState.as.svn-base b/0.0.9.1/src/alternativa/physics/.svn/text-base/BodyState.as.svn-base new file mode 100644 index 0000000..3987388 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/.svn/text-base/BodyState.as.svn-base @@ -0,0 +1,39 @@ +package alternativa.physics { + import alternativa.math.Quaternion; + import alternativa.math.Vector3; + + /** + * Класс описывает состояние твёрдого тела. + */ + public class BodyState { + /** + * Положение тела. + */ + public var pos:Vector3 = new Vector3(); + /** + * Ориентация тела. + */ + public var orientation:Quaternion = new Quaternion(); + /** + * Скорость тела. + */ + public var velocity:Vector3 = new Vector3(); + /** + * Угловая скорость тела. + */ + public var rotation:Vector3 = new Vector3(); + + /** + * Копирует значение указанного объекта. + * + * @param state + */ + public function copy(state:BodyState):void { + pos.vCopy(state.pos); + orientation.copy(state.orientation); + velocity.vCopy(state.velocity); + rotation.vCopy(state.rotation); + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/.svn/text-base/CollisionPrimitiveList.as.svn-base b/0.0.9.1/src/alternativa/physics/.svn/text-base/CollisionPrimitiveList.as.svn-base new file mode 100644 index 0000000..5d5304f --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/.svn/text-base/CollisionPrimitiveList.as.svn-base @@ -0,0 +1,89 @@ +package alternativa.physics { + import alternativa.physics.collision.CollisionPrimitive; + + /** + * + */ + public class CollisionPrimitiveList { + + public var head:CollisionPrimitiveListItem; + public var tail:CollisionPrimitiveListItem; + public var size:int; + + /** + * + */ + public function CollisionPrimitiveList() { + } + + /** + * + * @param primitive + */ + public function append(primitive:CollisionPrimitive):void { + var item:CollisionPrimitiveListItem = CollisionPrimitiveListItem.create(primitive); + if (head == null) { + head = tail = item; + } else { + tail.next = item; + item.prev = tail; + tail = item; + } + size++; + } + + /** + * + * @param primitve + */ + public function remove(primitve:CollisionPrimitive):void { + var item:CollisionPrimitiveListItem = findItem(primitve); + if (item == null) return; + if (item == head) { + if (size == 1) { + head = tail = null; + } else { + head = item.next; + head.prev = null; + } + } else { + if (item == tail) { + tail = tail.prev; + tail.next = null; + } else { + item.prev.next = item.next; + item.next.prev = item.prev; + } + } + item.dispose(); + size--; + } + + /** + * + * @param primitive + * @return + */ + public function findItem(primitive:CollisionPrimitive):CollisionPrimitiveListItem { + var item:CollisionPrimitiveListItem = head; + while (item != null && item.primitive != primitive) { + item = item.next; + } + return item; + } + + /** + * + */ + public function clear():void { + while (head != null) { + var item:CollisionPrimitiveListItem = head; + head = head.next; + item.dispose(); + } + tail = null; + size = 0; + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/.svn/text-base/CollisionPrimitiveListItem.as.svn-base b/0.0.9.1/src/alternativa/physics/.svn/text-base/CollisionPrimitiveListItem.as.svn-base new file mode 100644 index 0000000..d204532 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/.svn/text-base/CollisionPrimitiveListItem.as.svn-base @@ -0,0 +1,76 @@ +package alternativa.physics { + import alternativa.physics.collision.CollisionPrimitive; + + /** + * Элемент списка примитивов. + */ + public class CollisionPrimitiveListItem { + + // Верхний элемент хранилища + private static var poolTop:CollisionPrimitiveListItem; + + /** + * Создаёт новый элемент списка. + * + * @param primitive примитив, содержащийся в элементе + * @return новый элемент списка + */ + public static function create(primitive:CollisionPrimitive):CollisionPrimitiveListItem { + var item:CollisionPrimitiveListItem; + if (poolTop == null) { + item = new CollisionPrimitiveListItem(primitive); + } else { + item = poolTop; + item.primitive = primitive; + poolTop = item.next; + item.next = null; + } + return item; + } + + /** + * Очищает хранилище. + */ + public static function clearPool():void { + var curr:CollisionPrimitiveListItem = poolTop; + while (curr != null) { + poolTop = curr.next; + curr.next = null; + curr = poolTop; + } + } + + /** + * Примитив, хранящийся в элементе списка. + */ + public var primitive:CollisionPrimitive; + /** + * Ссылка не следующий элемент списка. + */ + public var next:CollisionPrimitiveListItem; + /** + * Ссылка не предыдущий элемент списка. + */ + public var prev:CollisionPrimitiveListItem; + + /** + * Создаёт новый экземпляр. + * + * @param primitive примитив, хранящийся в элементе списка + */ + public function CollisionPrimitiveListItem(primitive:CollisionPrimitive) { + this.primitive = primitive; + } + + /** + * Очищает внутренние ссылки и помещает элемент в хранилище для дальнейшего использования. + */ + public function dispose():void { + primitive = null; + prev = null; + next = poolTop; + poolTop = this; + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/.svn/text-base/Contact.as.svn-base b/0.0.9.1/src/alternativa/physics/.svn/text-base/Contact.as.svn-base new file mode 100644 index 0000000..3f010ec --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/.svn/text-base/Contact.as.svn-base @@ -0,0 +1,69 @@ +package alternativa.physics { + + + import alternativa.math.Vector3; + + /** + * Описывает контакт одного или двух тел. + */ + public class Contact { + /** + * Первое тело контакта, не может быть равно null. + */ + public var body1:Body; + /** + * Второе тело контакта, может быть равно null. + */ + public var body2:Body; + /** + * Предрассчитанный взаимный коэффициент отскока. + */ + public var restitution:Number; + /** + * Предрассчитанный взаимный коэффициент трения. + */ + public var friction:Number; + /** + * Нормаль контакта. Направлена к первому телу. + */ + public var normal:Vector3 = new Vector3(); + /** + * Список точек контакта. + */ + public var points:Vector. = new Vector.(MAX_POINTS, true); + /** + * Количество точек контакта. + */ + public var pcount:int; + /** + * Максимальная глубина пересечения. + */ + public var maxPenetration:Number = 0; + /** + * Флаг показывает, разрешён контакт или нет. + */ + public var satisfied:Boolean; + /** + * Следующий контакт в списке. + */ + public var next:Contact; + /** + * Индекс контакта. Первый контакт в списке имеет индекс 0. + */ + public var index:int; + + // Максимальное количество точек контакта + private const MAX_POINTS:int = 8; + + /** + * + */ + public function Contact(index:int) { + this.index = index; + for (var i:int = 0; i < MAX_POINTS; i++) { + points[i] = new ContactPoint(); + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/.svn/text-base/ContactPoint.as.svn-base b/0.0.9.1/src/alternativa/physics/.svn/text-base/ContactPoint.as.svn-base new file mode 100644 index 0000000..395d2dc --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/.svn/text-base/ContactPoint.as.svn-base @@ -0,0 +1,55 @@ +package alternativa.physics { + import alternativa.math.Vector3; + + /** + * + */ + public class ContactPoint { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + + // Величины, расчитываемые перед началом фазы решения контактов + + // Требуемая проекция конечной скорости на нормаль для упругого контакта + public var normalVel:Number; + // Минимальная скорость разделения неупругого контакта + public var minSepVel:Number; + // Изменение проекции скорости на единицу нормального импульса + public var velByUnitImpulseN:Number; + + public var angularInertia1:Number; + public var angularInertia2:Number; + + // Радиус-вектор точки контакта относительно центра первого тела + public var r1:Vector3 = new Vector3(); + // Радиус-вектор точки контакта относительно центра второго тела + public var r2:Vector3 = new Vector3(); + + // Величины, накапливаемые во время фазы решения контактов + + // Накопленный импульс, применяемый для получения требуемой относительной скорости в точке контакта + public var accumImpulseN:Number; + // Накопленный импульс, применяемый для разделения тел в точке контакта. Не создаёт момента. +// public var accumSepImpulse:Number; + + public var satisfied:Boolean; + + /** + * + * @param cp + */ + public function copyFrom(cp:ContactPoint):void { + pos.vCopy(cp.pos); + penetration = cp.penetration; + feature1 = cp.feature1; + feature2 = cp.feature2; + r1.vCopy(cp.r1); + r2.vCopy(cp.r2); + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/.svn/text-base/PhysicsScene.as.svn-base b/0.0.9.1/src/alternativa/physics/.svn/text-base/PhysicsScene.as.svn-base new file mode 100644 index 0000000..abdc762 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/.svn/text-base/PhysicsScene.as.svn-base @@ -0,0 +1,743 @@ +package alternativa.physics { + + + import alternativa.physics.collision.ICollisionDetector; + import alternativa.physics.collision.KdTreeCollisionDetector; + import alternativa.physics.constraints.Constraint; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * Класс реализует физическую симуляцию поведения твёрдых тел. + */ + public class PhysicsScene { + + private static var lastBodyId:int; + + // Максимальное количество контактов + public const MAX_CONTACTS:int = 1000; + + ////////////////////////////////////////////////////////////////// + // Настроечные параметры симуляции. Могут быть изменены в любой + // момент времени без нарушения корректной работы симуляции. + ////////////////////////////////////////////////////////////////// + // Количество шагов, за которое пересекающиеся тела должны разделиться + public var penResolutionSteps:int = 10; + // Величина допустимой глубины пересечения + public var allowedPenetration:Number = 0.1; + // Максимальная скорость, добавляемая с целью разделения тел + public var maxPenResolutionSpeed:Number = 0.5; + // Количество итераций для обработки упругих контактов + public var collisionIterations:int = 5; + // Количество итераций для обработки неупругих контактов + public var contactIterations:int = 5; + // Флаг использования предсказания состояний + public var usePrediction:Boolean = false; + + public var freezeSteps:int = 10; + public var linSpeedFreezeLimit:Number = 1; + public var angSpeedFreezeLimit:Number = 0.01; + + // Переменные для процедуры разделения тел путём непосредственного их перемещенеия. + // !!!!!!!!!!!!!!!!!!!!!!!! + // !!! Экспериментально !!! + // !!!!!!!!!!!!!!!!!!!!!!!! + public var staticSeparationIterations:int = 10; + public var staticSeparationSteps:int = 10; + public var maxAngleMove:Number = 10; + public var useStaticSeparation:Boolean = false; + + // Вектор гравитации + public var _gravity:Vector3 = new Vector3(0, 0, -9.8); + // Модуль вектора гравитации + public var _gravityMagnitude:Number = 9.8; + + // Использующийся детектор столкновений + public var collisionDetector:ICollisionDetector; + + // Список тел, участвующих в симуляции + public var bodies:BodyList = new BodyList(); + + // Список контактов на текущем шаге симуляции + altphysics var contacts:Contact; + // Количество контактов на текущем шаге симуляции +// altphysics var contactsNum:int; + // Список ограничений + altphysics var constraints:Vector. = new Vector.(); + // Количество ограничений + altphysics var constraintsNum:int; + // Временная метка. Число прошедших шагов с начала симуляции. + public var timeStamp:int; + // Время с начала симуляции, мс + public var time:int; + // Первый неиспользованный контакт на текущем шаге симуляции + private var borderContact:Contact; + + // Временные переменные для избежания создания экземпляров + private var _r:Vector3 = new Vector3(); + private var _t:Vector3 = new Vector3(); + private var _v:Vector3 = new Vector3(); + private var _v1:Vector3 = new Vector3(); + private var _v2:Vector3 = new Vector3(); + + /** + * + */ + public function PhysicsScene() { + contacts = new Contact(0); + var contact:Contact = contacts; + for (var i:int = 1; i < MAX_CONTACTS; i++) { + contact.next = new Contact(i); + contact = contact.next; + } + collisionDetector = new KdTreeCollisionDetector(); + } + + /** + * Вектор гравитации. + */ + public function get gravity():Vector3 { + return _gravity.vClone(); + } + + /** + * @private + */ + public function set gravity(value:Vector3):void { + _gravity.vCopy(value); + _gravityMagnitude = _gravity.vLength(); + } + + /** + * Добавляет тело в симуляцию. + * + * @param body + */ + public function addBody(body:Body):void { + body.id = lastBodyId++; + body.world = this; + bodies.append(body); + } + + /** + * Удаляет тело из симуляции. + * @param body + * @return + */ + public function removeBody(body:Body):void { + if (bodies.remove(body)) { + body.world = null; + } + } + + /** + * Добавляет ограничение. + * @param c + */ + public function addConstraint(c:Constraint):void { + constraints[constraintsNum++] = c; + c.world = this; + } + + /** + * Удаляет ограничение. + * @param c + */ + public function removeConstraint(c:Constraint):Boolean { + var idx:int = constraints.indexOf(c); + if (idx < 0) return false; + constraints.splice(idx, 1) + constraintsNum--; + c.world = null; + return true; + } + + /** + * Применяет к телам действующте на них силы. Аккумуляторы сил и моментов тел очищаются после завершения шага + * симуляции, поэтому на момент вызова метода могут уже содержать некоторые значения. + * + * @param dt промежуток времени, в течении которого действуют силы + */ + private function applyForces(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + body.beforePhysicsStep(dt); + body.calcAccelerations(); + // Ускорение свободного падения применяется только к подвижным телам во избежание некорректного изменения + // фиктивной скорости неподвижных тел. + if (body.movable && !body.frozen) { + body.accel.x += _gravity.x; + body.accel.y += _gravity.y; + body.accel.z += _gravity.z; + } + item = item.next; + } + } + + /** + * Определяет все столкновения на текущем шаге симуляции и заполняет список получившихся контактов. + * + * @param dt длительность шага симуляции + */ + private function detectCollisions(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + if (!body.frozen) { + body.contactsNum = 0; + body.saveState(); + // При включённом режиме предсказания состояние тел интегрируется на один шаг вперёд + if (usePrediction) { + body.integrateVelocity(dt); + body.integratePosition(dt); + } + body.calcDerivedData(); + } + item = item.next; + } + + borderContact = collisionDetector.getAllContacts(contacts); + + // Расчёт относительных векторов точки контакта вынесен сюда из-за необходимости учитывать + // положение тел в предсказанном состоянии + var contact:Contact = contacts; + while (contact != borderContact) { + var b1:Body = contact.body1; + var b2:Body = contact.body2; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + var bPos:Vector3 = b1.state.pos; + cp.r1.x = cp.pos.x - bPos.x; + cp.r1.y = cp.pos.y - bPos.y; + cp.r1.z = cp.pos.z - bPos.z; + if (b2 != null) { + bPos = b2.state.pos; + cp.r2.x = cp.pos.x - bPos.x; + cp.r2.y = cp.pos.y - bPos.y; + cp.r2.z = cp.pos.z - bPos.z; + } + } + contact = contact.next; + } + + // Восстановление состояния тел + if (usePrediction) { + item = bodies.head; + while (item != null) { + body = item.body; + if (!body.frozen) { + body.restoreState(); + body.calcDerivedData(); + } + item = item.next; + } + } + } + + /** + * Подготваливает полученные из детектора столкновений контакты, расчитывая значения, не меняющиеся + * в ходе шага симуляции. + */ + private function preProcessContacts(dt:Number):void { + var contact:Contact = contacts; + while (contact != borderContact) { + var b1:Body = contact.body1; + var b2:Body = contact.body2; + // Столкнувшиеся тела размораживаются + if (b1.frozen) { + b1.frozen = false; + b1.freezeCounter = 0; + } + if (b2 != null && b2.frozen) { + b2.frozen = false; + b2.freezeCounter = 0; + } + contact.restitution = b1.material.restitution; + if (b2 != null && b2.material.restitution < contact.restitution) contact.restitution = b2.material.restitution; + contact.friction = b1.material.friction; + if (b2 != null && b2.material.friction < contact.friction) contact.friction = b2.material.friction; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.accumImpulseN = 0; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + cp.velByUnitImpulseN = 0; + if (b1.movable) { + cp.angularInertia1 = _v.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1).vDot(contact.normal); + cp.velByUnitImpulseN += b1.invMass + cp.angularInertia1; + } + if (b2 != null && b2.movable) { + cp.angularInertia2 = _v.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2).vDot(contact.normal); + cp.velByUnitImpulseN += b2.invMass + cp.angularInertia2; + } + // Расчёт требуемой конечной скорости для упругого контакта + calcSepVelocity(b1, b2, cp, _v); + cp.normalVel = _v.vDot(contact.normal); + if (cp.normalVel < 0) cp.normalVel = - contact.restitution*cp.normalVel; + // Скорость разделения неупругого контакта + cp.minSepVel = cp.penetration > allowedPenetration ? (cp.penetration - allowedPenetration)/(penResolutionSteps*dt) : 0; + if (cp.minSepVel > maxPenResolutionSpeed) cp.minSepVel = maxPenResolutionSpeed; + } + contact = contact.next; + } + for (var i:int = 0; i < constraintsNum; i++) { + var constraint:Constraint = constraints[i]; + constraint.preProcess(dt); + } + } + + /** + * + * @param dt + * @param forceInelastic + */ + private function processContacts(dt:Number, forceInelastic:Boolean):void { + var iterNum:int = forceInelastic ? contactIterations : collisionIterations; + var i:int; + var forwardLoop:Boolean = false; + for (var iter:int = 0; iter < iterNum; iter++) { + forwardLoop = !forwardLoop; + var contact:Contact = contacts; + while (contact != borderContact) { + resolveContact(contact, forceInelastic, forwardLoop); + contact = contact.next; + } + // Ограничения + for (i = 0; i < constraintsNum; i++) { + var constraint:Constraint = constraints[i]; + constraint.apply(dt); + } + } + + // Разделение контактов путём непосредственного изменения координат и ориентации. Экспериментально. +// if (forceInelastic && useStaticSeparation) performStaticSeparation(); + } + + /** + * + */ + private function resolveContact(contactInfo:Contact, forceInelastic:Boolean, forwardLoop:Boolean):void { + var b1:Body = contactInfo.body1; + var b2:Body = contactInfo.body2; + var normal:Vector3 = contactInfo.normal; + var i:int; + if (forwardLoop) { + for (i = 0; i < contactInfo.pcount; i++) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } else { + for (i = contactInfo.pcount - 1; i >= 0; i--) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } + } + + /** + * + * @param idx + * @param b1 + * @param b2 + * @param colInfo + * @param normal + * @param forceInelastic + */ + private function resolveContactPoint(idx:int, b1:Body, b2:Body, contact:Contact, normal:Vector3, forceInelastic:Boolean):void { + var cp:ContactPoint = contact.points[idx]; + if (!forceInelastic) { + cp.satisfied = true; + } + + var newVel:Number = 0; + calcSepVelocity(b1, b2, cp, _v); + var cnormal:Vector3 = contact.normal; + var sepVel:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + if (forceInelastic) { + var minSpeVel:Number = useStaticSeparation ? 0 : cp.minSepVel; + if (sepVel < minSpeVel) { + cp.satisfied = false; + } else if (cp.satisfied) return; + newVel = minSpeVel; + } else { + newVel = cp.normalVel; + } + var deltaVel:Number = newVel - sepVel; + var impulse:Number = deltaVel/cp.velByUnitImpulseN; + var accumImpulse:Number = cp.accumImpulseN + impulse; + if (accumImpulse < 0) { + accumImpulse = 0; + } + var deltaImpulse:Number = accumImpulse - cp.accumImpulseN; + cp.accumImpulseN = accumImpulse; + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, normal, deltaImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, normal, -deltaImpulse); + } + + // Учёт силы трения + calcSepVelocity(b1, b2, cp, _v); + // Расчитываем изменение касательной скорости на единицу касательного импульса + var tanSpeedByUnitImpulse:Number = 0; + +// _v.vAddScaled(-_v.vDot(contact.normal), contact.normal); + var dot:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + _v.x -= dot*cnormal.x; + _v.y -= dot*cnormal.y; + _v.z -= dot*cnormal.z; + + var tanSpeed:Number = _v.vLength(); + if (tanSpeed < 0.001) return; + +// _t.vCopy(_v).vNormalize().vReverse(); + _t.x = -_v.x; + _t.y = -_v.y; + _t.z = -_v.z; + _t.vNormalize(); + + var r:Vector3; + var m:Matrix3; + var xx:Number; + var yy:Number; + var zz:Number; + // dV = b.invMass + ((invI * (r % t)) % r) * t + if (b1.movable) { +// _v.vCross2(cp.r1, _t).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1); + r = cp.r1; + m = b1.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b1.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + if (b2 != null && b2.movable) { +// _v.vCross2(cp.r2, _t).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2); + + r = cp.r2; + m = b2.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b2.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + + var tanImpulse:Number = tanSpeed/tanSpeedByUnitImpulse; + var max:Number = contact.friction*cp.accumImpulseN; + if (max < 0) { + if (tanImpulse < max) tanImpulse = max; + } else { + if (tanImpulse > max) tanImpulse = max; + } + + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, _t, tanImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, _t, -tanImpulse); + } + } + + /** + * + * @param cp + * @param normal + * @return + */ + private function calcSepVelocity(body1:Body, body2:Body, cp:ContactPoint, result:Vector3):void { + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 +// result.vCopy(body1.state.velocity).vAdd(_v1.vCross2(body1.state.rotation, cp.r1)); + var rot:Vector3 = body1.state.rotation; + var v:Vector3 = cp.r1; + var x:Number = rot.y*v.z - rot.z*v.y; + var y:Number = rot.z*v.x - rot.x*v.z; + var z:Number = rot.x*v.y - rot.y*v.x; + v = body1.state.velocity; + result.x = v.x + x; + result.y = v.y + y; + result.z = v.z + z; + // V2 = V2_c + w2%r2 + if (body2 != null) { +// result.vSubtract(body2.state.velocity).vSubtract(_v2.vCross2(body2.state.rotation, cp.r2)); + rot = body2.state.rotation; + v = cp.r2; + x = rot.y*v.z - rot.z*v.y; + y = rot.z*v.x - rot.x*v.z; + z = rot.x*v.y - rot.y*v.x; + v = body2.state.velocity; + result.x -= v.x + x; + result.y -= v.y + y; + result.z -= v.z + z; + } + } + + /** + * + * @param dt + */ + private function intergateVelocities(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + item.body.integrateVelocity(dt); + item = item.next; + } + } + + /** + * + * @param dt + */ + private function integratePositions(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + if (body.movable && !body.frozen) { + body.integratePosition(dt); + } + item = item.next; + } + } + + /** + * + */ + private function performStaticSeparation():void { + var iterNum:int = staticSeparationIterations; +// iterNum = 100; + + // 1. В начале каждой итерации для всех контактов сбрасывается флаг satisfied с одновременным поиском наихудшего контакта, т.е. имеющего наибольшую величину пересечения тел. + // 2. Если найденный контакт имеет величину пересечения меньше предельно допустимой процедура прерывается, т.к. разделение контактов не требуется. + // 3. Если процедура продолжается, то выполняется разделение найденного контакта и установка его флага satisfied в true, после чего запускается внутренний цикл, состоящий + // из contactsNum - 1 итераций. На каждой итерации ищется наихудший контакт среди оставшихся (satisfied == false). Если такой контакт найден, то выполняется его разделение + // и переход к следующей итерации внутреннего цикла, иначе внутренний цикл прерывается и выполняется переход к следующей итерации внешнего цикла (шаг 1.). +// for (var iter:int = 0; iter < iterNum; iter++) { +// // Ищем контакт с максимальной величиной пересечения тел, одновременно сбрасывая флаги satisfied в false +// var worstContact:Contact = contacts[0]; +// var i:int; +// for (i = 1; i < contactsNum; i++) { +// var contact:Contact = contacts[i]; +// contact.satisfied = false; +// if (contact.maxPenetration > worstContact.maxPenetration) worstContact = contact; +// } +// if (worstContact.maxPenetration <= allowedPenetration) return; +// +// resolveInterpenetration(worstContact); +// // Внутренний цикл по оставшимся контактам +// for (i = 1; i < contactsNum; i++) { +// worstContact = getWorstContact(); +// if (worstContact == null) break; +// resolveInterpenetration(worstContact); +// } +// } + } + + /** + * + */ +// private function getWorstContact():Contact { +// var maxPen:Number = 0; +// var worst:Contact = null; +// for (var i:int = 0; i < contactsNum; i++) { +// var c:Contact = contacts[i]; +// if (!c.satisfied && c.maxPenetration > maxPen) { +// worst = c; +// maxPen = c.maxPenetration; +// } +// } +// return maxPen > allowedPenetration ? worst : null; +// } + + /** + * Разделяет указанный контакт, выполняя staticSeparationSteps итераций по списку точек контакта. + * 1. В начале каждой итерации ищется точка с наибольшим пересечением, одновременно сбрасываются флаги satisfied у точек. + * 2. Для найденной точки выполняется процедура разделения. + * 3. После выполняется pcount - 1 итерация по списку точек, каждый раз ищется наихудшая среди имеющих satisfied == false, для которой выполняется процедура разделения. + * Если наихудшая точка не найдена, то выполнется переход к следующей итерации внешнего цикла (шаг 1). + * + * В конце процедуры разделения обновляются значения пересечений для остальных точек контакта, а также для всех прочих контактов, относящихся к телам текущего. + * + * @param contact контакт для разделения + */ + private function resolveInterpenetration(contact:Contact):void { + contact.satisfied = true; + + for (var step:int = 0; step < staticSeparationSteps; step++) { + var worstCp:ContactPoint = contact.points[0]; + var cp:ContactPoint; + var i:int; + for (i = 1; i < contact.pcount; i++) { + cp = contact.points[i]; + cp.satisfied = false; + if (cp.penetration > worstCp.penetration) worstCp = cp; + } + if (worstCp.penetration <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + // Разделяем оставшиеся точки + var maxPen:Number = 0; + for (i = 1; i < contact.pcount; i++) { + // Поиск наихудшей точки + for (var j:int = 0; j < contact.pcount; j++) { + cp = contact.points[j]; + if (cp.satisfied) continue; + if (cp.penetration > maxPen) { + maxPen = cp.penetration; + worstCp = cp; + } + } + if (maxPen <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + } + } + } + + /** + * + * @param cp + * @param contact + */ + private function separateContactPoint(cp:ContactPoint, contact:Contact):void { +// cp.satisfied = true; +// +// var b1:Body = contact.body1; +// var b2:Body = contact.body2; +// var totalMove:Number = cp.penetration - allowedPenetration; +// var moveCoeff:Number = totalMove/cp.velByUnitImpulseN; +// var linMove1:Number; +// var angleMove1:Number; +// if (b1.movable) { +// linMove1 = b1.invMass*moveCoeff; +// angleMove1 = cp.angularInertia1*moveCoeff; +// if (angleMove1 > maxAngleMove) { +// linMove1 += angleMove1 - maxAngleMove; +// angleMove1 -= maxAngleMove; +// } +// b1.state.pos.vAddScaled(linMove1, contact.normal); +// _v1.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vScale(angleMove1); +// b1.state.orientation.addScaledVector(_v1, 1); +// } +// var linMove2:Number; +// var angleMove2:Number; +// if (b2 != null && b2.movable) { +// linMove2 = b2.invMass*moveCoeff; +// angleMove2 = cp.angularInertia2*moveCoeff; +// if (angleMove2 > maxAngleMove) { +// linMove2 += angleMove2 - maxAngleMove; +// angleMove2 -= maxAngleMove; +// } +// b2.state.pos.vAddScaled(-linMove2, contact.normal); +// _v2.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vScale(angleMove2); +// _v2.vReverse(); +// b2.state.orientation.addScaledVector(_v2, 1); +// } +// cp.penetration = allowedPenetration; +// // Обновляем пересечения в других точках +// var i:int; +// for (i = 0; i < contact.pcount; i++) { +// var cp1:ContactPoint = contact.points[i]; +// if (cp1 == cp) continue; +// var angularMove:Number; +// if (b1.movable) { +// angularMove = _v.vCross2(_v1, cp1.r1).vDot(contact.normal); +// cp1.penetration -= linMove1 + angularMove; +// } +// if (b2 != null && b2.movable) { +// angularMove = _v.vCross2(_v2, cp1.r2).vDot(contact.normal); +// cp1.penetration -= linMove2 - angularMove; +// } +// // Обновление максимального значения для контакта +// if (cp1.penetration > contact.maxPenetration) contact.maxPenetration = cp1.penetration; +// } +// // Обновляем пересечения для других контактов +// var c:Contact; +// var j:int; +// if (b1.movable) { +// for (i = 0; i < b1.contactsNum; i++) { +// c = b1.contacts[i]; +// if (c == contact) continue; +// for (j = 0; j < c.pcount; j++) { +// cp1 = c.points[j]; +// if (b1 == c.body1) cp1.penetration -= linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r1).vDot(c.normal); +// else cp1.penetration += linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r2).vDot(c.normal); +// if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; +// } +// } +// } +// if (b2 != null && b2.movable) { +// for (i = 0; i < b2.contactsNum; i++) { +// c = b2.contacts[i]; +// if (c == contact) continue; +// for (j = 0; j < c.pcount; j++) { +// cp1 = c.points[j]; +// if (b2 == c.body1) cp1.penetration -= linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r1).vDot(c.normal); +// else cp1.penetration += linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r2).vDot(c.normal); +// if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; +// } +// } +// } + } + + /** + * + */ + private function postPhysics():void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + body.clearAccumulators(); + body.calcDerivedData(); + if (body.canFreeze) { + if (body.state.velocity.vLength() < linSpeedFreezeLimit && body.state.rotation.vLength() < angSpeedFreezeLimit) { + if (!body.frozen) { + body.freezeCounter++; + if (body.freezeCounter >= freezeSteps) body.frozen = true; + } + } else { + body.freezeCounter = 0; + body.frozen = false; + } + } + item = item.next; + } + } + + /** + * + * @param delta + */ + public function update(delta:int):void { + timeStamp++; + time += delta; + var dt:Number = 0.001*delta; + applyForces(dt); + detectCollisions(dt); + preProcessContacts(dt); + processContacts(dt, false); + intergateVelocities(dt); + processContacts(dt, true); + integratePositions(dt); + postPhysics(); + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/.svn/text-base/PhysicsUtils.as.svn-base b/0.0.9.1/src/alternativa/physics/.svn/text-base/PhysicsUtils.as.svn-base new file mode 100644 index 0000000..5a2e46f --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/.svn/text-base/PhysicsUtils.as.svn-base @@ -0,0 +1,49 @@ +package alternativa.physics { + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + public class PhysicsUtils { + + /** + * @param mass + * @param halfSize + * @param result + */ + public static function getBoxInvInertia(mass:Number, halfSize:Vector3, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + result.a = 3/(mass*(yy + zz)); + result.f = 3/(mass*(zz + xx)); + result.k = 3/(mass*(xx + yy)); + } + + /** + * @param mass + * @param r + * @param h + * @param result + */ + public static function getCylinderInvInertia(mass:Number, r:Number, h:Number, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + + result.a = result.f = 1/(mass*(h*h/12 + r*r/4)); + result.k = 2/(mass*r*r); + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base b/0.0.9.1/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base new file mode 100644 index 0000000..2715307 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base @@ -0,0 +1,3 @@ +package alternativa.physics { + public namespace altphysics = "http://alternativaplatform.com/en/altphysics"; +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/Body.as b/0.0.9.1/src/alternativa/physics/Body.as new file mode 100644 index 0000000..cd22c5b --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/Body.as @@ -0,0 +1,405 @@ +package alternativa.physics { + + import alternativa.math.Matrix3; + import alternativa.math.Matrix4; + import alternativa.math.Quaternion; + import alternativa.math.Vector3; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.IBodyCollisionPredicate; + import alternativa.physics.collision.types.BoundBox; + + use namespace altphysics; + + /** + * + */ + public class Body { + + public static var linDamping:Number = 0.997; + public static var rotDamping:Number = 0.997; + + // Идентификатор тела, уникальный в пределах мира + public var id:int; + // Имя тела + public var name:String; + // Мир, в котором находится тело + public var world:PhysicsScene; + // Флаг подвижности тела + public var movable:Boolean = true; + // Флаг указывает, может ли тело быть заморожено + public var canFreeze:Boolean = false; + + public var freezeCounter:int; + public var frozen:Boolean = false; + // Ограничивающий бокс тела + public var aabb:BoundBox = new BoundBox(); + // Предикат, через который тело получает сообщения о столкновениях + public var postCollisionPredicate:IBodyCollisionPredicate; + + // Текущее состояние тела + public var state:BodyState = new BodyState(); + // Предыдущее состояние тела + public var prevState:BodyState = new BodyState(); + // Линейное ускорение тела на текущем шаге симуляции + public var accel:Vector3 = new Vector3(); + // Угловое ускорение тела на текущем шаге симуляции + public var angleAccel:Vector3 = new Vector3(); + // Физический материал тела + public var material:BodyMaterial = new BodyMaterial(); + // Обратная масса тела + public var invMass:Number = 1; + // Обратная матрица тензора инерции в локальных координатах + public var invInertia:Matrix3 = new Matrix3(); + // Обратная матрица тензора инерции в мировых координатах + public var invInertiaWorld:Matrix3 = new Matrix3(); + // Базисная матрица тела в мировых координатах + public var baseMatrix:Matrix3 = new Matrix3(); + + public const MAX_CONTACTS:int = 20; + public var contacts:Vector. = new Vector.(MAX_CONTACTS); + public var contactsNum:int; + + public var collisionPrimitives:CollisionPrimitiveList; + + // Аккумулятор сил + public var forceAccum:Vector3 = new Vector3(); + // Аккумулятор моментов + public var torqueAccum:Vector3 = new Vector3(); + + // Внутренние переменные для избежания создания экземпляров + private static var _r:Vector3 = new Vector3(); + private static var _f:Vector3 = new Vector3(); + + /** + * + * @param invMass + * @param invInertia + */ + public function Body(invMass:Number, invInertia:Matrix3) { + this.invMass = invMass; + this.invInertia.copy(invInertia); + } + + /** + * @param primitive + * @param localTransform + */ + public function addCollisionPrimitive(primitive:CollisionPrimitive, localTransform:Matrix4 = null):void { + if (primitive == null) { + throw new ArgumentError("Primitive cannot be null"); + } + if (collisionPrimitives == null) { + collisionPrimitives = new CollisionPrimitiveList(); + } + collisionPrimitives.append(primitive); + primitive.setBody(this, localTransform); + } + + /** + * + * @param primitive + */ + public function removeCollisionPrimitive(primitive:CollisionPrimitive):void { + if (collisionPrimitives == null) return; + primitive.setBody(null); + collisionPrimitives.remove(primitive); + if (collisionPrimitives.size == 0) { + collisionPrimitives = null; + } + } + + /** + * @param t + * @param result + */ + public function interpolate(t:Number, pos:Vector3, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.w = prevState.orientation.w*t1 + state.orientation.w*t; + orientation.x = prevState.orientation.x*t1 + state.orientation.x*t; + orientation.y = prevState.orientation.y*t1 + state.orientation.y*t; + orientation.z = prevState.orientation.z*t1 + state.orientation.z*t; + } + + /** + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + state.pos.vCopy(pos); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setPositionXYZ(x:Number, y:Number, z:Number):void { + state.pos.vReset(x, y, z); + } + + /** + * + * @param vel + */ + public function setVelocity(vel:Vector3):void { + state.velocity.vCopy(vel); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setVelocityXYZ(x:Number, y:Number, z:Number):void { + state.velocity.vReset(x, y, z); + } + + /** + * + * @param rot + */ + public function setRotation(rot:Vector3):void { + state.rotation.vCopy(rot); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setRotationXYZ(x:Number, y:Number, z:Number):void { + state.rotation.vReset(x, y, z); + } + + /** + * + * @param q + */ + public function setOrientation(q:Quaternion):void { + state.orientation.copy(q); + } + + /** + * @param r + * @param dir + * @param magnitude + */ + public function applyRelPosWorldImpulse(r:Vector3, dir:Vector3, magnitude:Number):void { + var d:Number = magnitude*invMass; + // Линейная часть + state.velocity.x += d*dir.x; + state.velocity.y += d*dir.y; + state.velocity.z += d*dir.z; + + // Вращательная часть + var x:Number = (r.y*dir.z - r.z*dir.y)*magnitude; + var y:Number = (r.z*dir.x - r.x*dir.z)*magnitude; + var z:Number = (r.x*dir.y - r.y*dir.x)*magnitude; + + state.rotation.x += invInertiaWorld.a*x + invInertiaWorld.b*y + invInertiaWorld.c*z; + state.rotation.y += invInertiaWorld.e*x + invInertiaWorld.f*y + invInertiaWorld.g*z; + state.rotation.z += invInertiaWorld.i*x + invInertiaWorld.j*y + invInertiaWorld.k*z; + } + + /** + * @param f + */ + public function addForce(f:Vector3):void { + forceAccum.vAdd(f); + } + + /** + * + * @param fx + * @param fy + * @param fz + */ + public function addForceXYZ(fx:Number, fy:Number, fz:Number):void { + forceAccum.x += fx; + forceAccum.y += fy; + forceAccum.z += fz; + } + + /** + * @param pos + * @param f + */ + public function addWorldForceXYZ(px:Number, py:Number, pz:Number, fx:Number, fy:Number, fz:Number):void { + forceAccum.x += fx; + forceAccum.y += fy; + forceAccum.z += fz; + + var pos:Vector3 = state.pos; + var rx:Number = px - pos.x; + var ry:Number = py - pos.y; + var rz:Number = pz - pos.z; + +// var x:Number = ry*fz - rz*fy; +// var y:Number = rz*fx - rx*fz; +// var z:Number = rx*fy - ry*fx; + + torqueAccum.x += ry*fz - rz*fy; + torqueAccum.y += rz*fx - rx*fz; + torqueAccum.z += rx*fy - ry*fx; + } + + /** + * @param pos + * @param f + */ + public function addWorldForce(pos:Vector3, force:Vector3):void { + forceAccum.vAdd(force); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(force)); + } + + /** + * @param pos + * @param f + */ + public function addWorldForceScaled(pos:Vector3, force:Vector3, scale:Number):void { + _f.x = scale*force.x; + _f.y = scale*force.y; + _f.z = scale*force.z; + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(_f)); + } + + /** + * @param pos + * @param f + */ + public function addLocalForce(pos:Vector3, force:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(pos, _r); + // Трансформируем вектор силы в мировую систему + baseMatrix.transformVector(force, _f); + // Добавляем силу и момент + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vCross(_f)); + } + + /** + * + * @param localPos + * @param worldForce + */ + public function addWorldForceAtLocalPoint(localPos:Vector3, worldForce:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(localPos, _r); + // Добавляем силу и момент + forceAccum.vAdd(worldForce); + torqueAccum.vAdd(_r.vCross(worldForce)); + } + + /** + * @param dt + */ + public function beforePhysicsStep(dt:Number):void { + } + + /** + * @param t + */ + public function addTorque(t:Vector3):void { + torqueAccum.vAdd(t); + } + + /** + * + */ + altphysics function clearAccumulators():void { + forceAccum.x = forceAccum.y = forceAccum.z = 0; + torqueAccum.x = torqueAccum.y = torqueAccum.z = 0; + } + + /** + * + */ + altphysics function calcAccelerations():void { + accel.x = forceAccum.x*invMass; + accel.y = forceAccum.y*invMass; + accel.z = forceAccum.z*invMass; + angleAccel.x = invInertiaWorld.a*torqueAccum.x + invInertiaWorld.b*torqueAccum.y + invInertiaWorld.c*torqueAccum.z; + angleAccel.y = invInertiaWorld.e*torqueAccum.x + invInertiaWorld.f*torqueAccum.y + invInertiaWorld.g*torqueAccum.z; + angleAccel.z = invInertiaWorld.i*torqueAccum.x + invInertiaWorld.j*torqueAccum.y + invInertiaWorld.k*torqueAccum.z; + } + + /** + * Вычисляет производные данные. + */ + public function calcDerivedData():void { + // Вычисление базисной матрицы и обратного тензора инерции в мировых координатах + state.orientation.toMatrix3(baseMatrix); + invInertiaWorld.copy(invInertia).append(baseMatrix).prependTransposed(baseMatrix); + if (collisionPrimitives != null) { + aabb.infinity(); + var item:CollisionPrimitiveListItem = collisionPrimitives.head; + while (item != null) { + var primitive:CollisionPrimitive = item.primitive; + primitive.transform.setFromMatrix3(baseMatrix, state.pos); + if (primitive.localTransform != null) { + primitive.transform.prepend(primitive.localTransform); + } + primitive.calculateAABB(); + aabb.addBoundBox(primitive.aabb); + item = item.next; + } + } + } + + /** + * + */ + altphysics function saveState():void { + prevState.copy(state); + } + + /** + * + */ + altphysics function restoreState():void { + state.copy(prevState); + } + + /** + * @param dt + */ + altphysics function integrateVelocity(dt:Number):void { + // v = v + a*t + state.velocity.x += accel.x*dt; + state.velocity.y += accel.y*dt; + state.velocity.z += accel.z*dt; + // rot = rot + eps*t + state.rotation.x += angleAccel.x*dt; + state.rotation.y += angleAccel.y*dt; + state.rotation.z += angleAccel.z*dt; + + state.velocity.x *= linDamping; + state.velocity.y *= linDamping; + state.velocity.z *= linDamping; + + state.rotation.x *= rotDamping; + state.rotation.y *= rotDamping; + state.rotation.z *= rotDamping; + } + + /** + * + */ + altphysics function integratePosition(dt:Number):void { + // pos = pos + v*t + state.pos.x += state.velocity.x*dt; + state.pos.y += state.velocity.y*dt; + state.pos.z += state.velocity.z*dt; + // q = q + 0.5*rot*q + state.orientation.addScaledVector(state.rotation, dt); + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/BodyList.as b/0.0.9.1/src/alternativa/physics/BodyList.as new file mode 100644 index 0000000..aa8f79f --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/BodyList.as @@ -0,0 +1,76 @@ +package alternativa.physics { + + /** + * + */ + public class BodyList { + + public var head:BodyListItem; + public var tail:BodyListItem; + public var size:int; + + /** + * + */ + public function BodyList() { + } + + /** + * + * @param body + */ + public function append(body:Body):void { + var item:BodyListItem = BodyListItem.create(body); + if (head == null) { + head = tail = item; + } else { + tail.next = item; + item.prev = tail; + tail = item; + } + size++; + } + + /** + * + * @param body + */ + public function remove(body:Body):Boolean { + var item:BodyListItem = findItem(body); + if (item == null) return false; + if (item == head) { + if (size == 1) { + head = tail = null; + } else { + head = item.next; + head.prev = null; + } + } else { + if (item == tail) { + tail = item.prev; + tail.next = null; + } else { + item.prev.next = item.next; + item.next.prev = item.prev; + } + } + item.dispose(); + size--; + return true; + } + + /** + * + * @param body + * @return + */ + public function findItem(body:Body):BodyListItem { + var item:BodyListItem = head; + while (item != null && item.body != body) { + item = item.next; + } + return item; + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/BodyListItem.as b/0.0.9.1/src/alternativa/physics/BodyListItem.as new file mode 100644 index 0000000..382e11c --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/BodyListItem.as @@ -0,0 +1,74 @@ +package alternativa.physics { + + /** + * + */ + public class BodyListItem { + + // Верхний элемент хранилища + private static var poolTop:BodyListItem; + + /** + * Создаёт новый элемент списка. + * + * @param primitive примитив, содержащийся в элементе + * @return новый элемент списка + */ + public static function create(body:Body):BodyListItem { + var item:BodyListItem; + if (poolTop == null) { + item = new BodyListItem(body); + } else { + item = poolTop; + poolTop = item.next; + item.next = null; + item.body = body; + } + return item; + } + + /** + * Очищает хранилище. + */ + public static function clearPool():void { + var item:BodyListItem = poolTop; + while (item != null) { + poolTop = item.next; + item.next = null; + item = poolTop; + } + } + + /** + * + */ + public var body:Body; + /** + * + */ + public var next:BodyListItem; + /** + * + */ + public var prev:BodyListItem; + + /** + * + * @param body + */ + public function BodyListItem(body:Body) { + this.body = body; + } + + /** + * + */ + public function dispose():void { + body = null; + prev = null; + next = poolTop; + poolTop = this; + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/BodyMaterial.as b/0.0.9.1/src/alternativa/physics/BodyMaterial.as new file mode 100644 index 0000000..7a7c47d --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/BodyMaterial.as @@ -0,0 +1,11 @@ +package alternativa.physics { + + public class BodyMaterial { + + public var restitution:Number = 0; + public var friction:Number = 0.3; +// public var dynamicFriction:Number = 0.2; +// public var dynamicFriction:Number = 0.2; + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/BodyState.as b/0.0.9.1/src/alternativa/physics/BodyState.as new file mode 100644 index 0000000..3987388 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/BodyState.as @@ -0,0 +1,39 @@ +package alternativa.physics { + import alternativa.math.Quaternion; + import alternativa.math.Vector3; + + /** + * Класс описывает состояние твёрдого тела. + */ + public class BodyState { + /** + * Положение тела. + */ + public var pos:Vector3 = new Vector3(); + /** + * Ориентация тела. + */ + public var orientation:Quaternion = new Quaternion(); + /** + * Скорость тела. + */ + public var velocity:Vector3 = new Vector3(); + /** + * Угловая скорость тела. + */ + public var rotation:Vector3 = new Vector3(); + + /** + * Копирует значение указанного объекта. + * + * @param state + */ + public function copy(state:BodyState):void { + pos.vCopy(state.pos); + orientation.copy(state.orientation); + velocity.vCopy(state.velocity); + rotation.vCopy(state.rotation); + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/CollisionPrimitiveList.as b/0.0.9.1/src/alternativa/physics/CollisionPrimitiveList.as new file mode 100644 index 0000000..5d5304f --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/CollisionPrimitiveList.as @@ -0,0 +1,89 @@ +package alternativa.physics { + import alternativa.physics.collision.CollisionPrimitive; + + /** + * + */ + public class CollisionPrimitiveList { + + public var head:CollisionPrimitiveListItem; + public var tail:CollisionPrimitiveListItem; + public var size:int; + + /** + * + */ + public function CollisionPrimitiveList() { + } + + /** + * + * @param primitive + */ + public function append(primitive:CollisionPrimitive):void { + var item:CollisionPrimitiveListItem = CollisionPrimitiveListItem.create(primitive); + if (head == null) { + head = tail = item; + } else { + tail.next = item; + item.prev = tail; + tail = item; + } + size++; + } + + /** + * + * @param primitve + */ + public function remove(primitve:CollisionPrimitive):void { + var item:CollisionPrimitiveListItem = findItem(primitve); + if (item == null) return; + if (item == head) { + if (size == 1) { + head = tail = null; + } else { + head = item.next; + head.prev = null; + } + } else { + if (item == tail) { + tail = tail.prev; + tail.next = null; + } else { + item.prev.next = item.next; + item.next.prev = item.prev; + } + } + item.dispose(); + size--; + } + + /** + * + * @param primitive + * @return + */ + public function findItem(primitive:CollisionPrimitive):CollisionPrimitiveListItem { + var item:CollisionPrimitiveListItem = head; + while (item != null && item.primitive != primitive) { + item = item.next; + } + return item; + } + + /** + * + */ + public function clear():void { + while (head != null) { + var item:CollisionPrimitiveListItem = head; + head = head.next; + item.dispose(); + } + tail = null; + size = 0; + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/CollisionPrimitiveListItem.as b/0.0.9.1/src/alternativa/physics/CollisionPrimitiveListItem.as new file mode 100644 index 0000000..d204532 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/CollisionPrimitiveListItem.as @@ -0,0 +1,76 @@ +package alternativa.physics { + import alternativa.physics.collision.CollisionPrimitive; + + /** + * Элемент списка примитивов. + */ + public class CollisionPrimitiveListItem { + + // Верхний элемент хранилища + private static var poolTop:CollisionPrimitiveListItem; + + /** + * Создаёт новый элемент списка. + * + * @param primitive примитив, содержащийся в элементе + * @return новый элемент списка + */ + public static function create(primitive:CollisionPrimitive):CollisionPrimitiveListItem { + var item:CollisionPrimitiveListItem; + if (poolTop == null) { + item = new CollisionPrimitiveListItem(primitive); + } else { + item = poolTop; + item.primitive = primitive; + poolTop = item.next; + item.next = null; + } + return item; + } + + /** + * Очищает хранилище. + */ + public static function clearPool():void { + var curr:CollisionPrimitiveListItem = poolTop; + while (curr != null) { + poolTop = curr.next; + curr.next = null; + curr = poolTop; + } + } + + /** + * Примитив, хранящийся в элементе списка. + */ + public var primitive:CollisionPrimitive; + /** + * Ссылка не следующий элемент списка. + */ + public var next:CollisionPrimitiveListItem; + /** + * Ссылка не предыдущий элемент списка. + */ + public var prev:CollisionPrimitiveListItem; + + /** + * Создаёт новый экземпляр. + * + * @param primitive примитив, хранящийся в элементе списка + */ + public function CollisionPrimitiveListItem(primitive:CollisionPrimitive) { + this.primitive = primitive; + } + + /** + * Очищает внутренние ссылки и помещает элемент в хранилище для дальнейшего использования. + */ + public function dispose():void { + primitive = null; + prev = null; + next = poolTop; + poolTop = this; + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/Contact.as b/0.0.9.1/src/alternativa/physics/Contact.as new file mode 100644 index 0000000..3f010ec --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/Contact.as @@ -0,0 +1,69 @@ +package alternativa.physics { + + + import alternativa.math.Vector3; + + /** + * Описывает контакт одного или двух тел. + */ + public class Contact { + /** + * Первое тело контакта, не может быть равно null. + */ + public var body1:Body; + /** + * Второе тело контакта, может быть равно null. + */ + public var body2:Body; + /** + * Предрассчитанный взаимный коэффициент отскока. + */ + public var restitution:Number; + /** + * Предрассчитанный взаимный коэффициент трения. + */ + public var friction:Number; + /** + * Нормаль контакта. Направлена к первому телу. + */ + public var normal:Vector3 = new Vector3(); + /** + * Список точек контакта. + */ + public var points:Vector. = new Vector.(MAX_POINTS, true); + /** + * Количество точек контакта. + */ + public var pcount:int; + /** + * Максимальная глубина пересечения. + */ + public var maxPenetration:Number = 0; + /** + * Флаг показывает, разрешён контакт или нет. + */ + public var satisfied:Boolean; + /** + * Следующий контакт в списке. + */ + public var next:Contact; + /** + * Индекс контакта. Первый контакт в списке имеет индекс 0. + */ + public var index:int; + + // Максимальное количество точек контакта + private const MAX_POINTS:int = 8; + + /** + * + */ + public function Contact(index:int) { + this.index = index; + for (var i:int = 0; i < MAX_POINTS; i++) { + points[i] = new ContactPoint(); + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/ContactPoint.as b/0.0.9.1/src/alternativa/physics/ContactPoint.as new file mode 100644 index 0000000..395d2dc --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/ContactPoint.as @@ -0,0 +1,55 @@ +package alternativa.physics { + import alternativa.math.Vector3; + + /** + * + */ + public class ContactPoint { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + + // Величины, расчитываемые перед началом фазы решения контактов + + // Требуемая проекция конечной скорости на нормаль для упругого контакта + public var normalVel:Number; + // Минимальная скорость разделения неупругого контакта + public var minSepVel:Number; + // Изменение проекции скорости на единицу нормального импульса + public var velByUnitImpulseN:Number; + + public var angularInertia1:Number; + public var angularInertia2:Number; + + // Радиус-вектор точки контакта относительно центра первого тела + public var r1:Vector3 = new Vector3(); + // Радиус-вектор точки контакта относительно центра второго тела + public var r2:Vector3 = new Vector3(); + + // Величины, накапливаемые во время фазы решения контактов + + // Накопленный импульс, применяемый для получения требуемой относительной скорости в точке контакта + public var accumImpulseN:Number; + // Накопленный импульс, применяемый для разделения тел в точке контакта. Не создаёт момента. +// public var accumSepImpulse:Number; + + public var satisfied:Boolean; + + /** + * + * @param cp + */ + public function copyFrom(cp:ContactPoint):void { + pos.vCopy(cp.pos); + penetration = cp.penetration; + feature1 = cp.feature1; + feature2 = cp.feature2; + r1.vCopy(cp.r1); + r2.vCopy(cp.r2); + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/PhysicsScene.as b/0.0.9.1/src/alternativa/physics/PhysicsScene.as new file mode 100644 index 0000000..abdc762 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/PhysicsScene.as @@ -0,0 +1,743 @@ +package alternativa.physics { + + + import alternativa.physics.collision.ICollisionDetector; + import alternativa.physics.collision.KdTreeCollisionDetector; + import alternativa.physics.constraints.Constraint; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * Класс реализует физическую симуляцию поведения твёрдых тел. + */ + public class PhysicsScene { + + private static var lastBodyId:int; + + // Максимальное количество контактов + public const MAX_CONTACTS:int = 1000; + + ////////////////////////////////////////////////////////////////// + // Настроечные параметры симуляции. Могут быть изменены в любой + // момент времени без нарушения корректной работы симуляции. + ////////////////////////////////////////////////////////////////// + // Количество шагов, за которое пересекающиеся тела должны разделиться + public var penResolutionSteps:int = 10; + // Величина допустимой глубины пересечения + public var allowedPenetration:Number = 0.1; + // Максимальная скорость, добавляемая с целью разделения тел + public var maxPenResolutionSpeed:Number = 0.5; + // Количество итераций для обработки упругих контактов + public var collisionIterations:int = 5; + // Количество итераций для обработки неупругих контактов + public var contactIterations:int = 5; + // Флаг использования предсказания состояний + public var usePrediction:Boolean = false; + + public var freezeSteps:int = 10; + public var linSpeedFreezeLimit:Number = 1; + public var angSpeedFreezeLimit:Number = 0.01; + + // Переменные для процедуры разделения тел путём непосредственного их перемещенеия. + // !!!!!!!!!!!!!!!!!!!!!!!! + // !!! Экспериментально !!! + // !!!!!!!!!!!!!!!!!!!!!!!! + public var staticSeparationIterations:int = 10; + public var staticSeparationSteps:int = 10; + public var maxAngleMove:Number = 10; + public var useStaticSeparation:Boolean = false; + + // Вектор гравитации + public var _gravity:Vector3 = new Vector3(0, 0, -9.8); + // Модуль вектора гравитации + public var _gravityMagnitude:Number = 9.8; + + // Использующийся детектор столкновений + public var collisionDetector:ICollisionDetector; + + // Список тел, участвующих в симуляции + public var bodies:BodyList = new BodyList(); + + // Список контактов на текущем шаге симуляции + altphysics var contacts:Contact; + // Количество контактов на текущем шаге симуляции +// altphysics var contactsNum:int; + // Список ограничений + altphysics var constraints:Vector. = new Vector.(); + // Количество ограничений + altphysics var constraintsNum:int; + // Временная метка. Число прошедших шагов с начала симуляции. + public var timeStamp:int; + // Время с начала симуляции, мс + public var time:int; + // Первый неиспользованный контакт на текущем шаге симуляции + private var borderContact:Contact; + + // Временные переменные для избежания создания экземпляров + private var _r:Vector3 = new Vector3(); + private var _t:Vector3 = new Vector3(); + private var _v:Vector3 = new Vector3(); + private var _v1:Vector3 = new Vector3(); + private var _v2:Vector3 = new Vector3(); + + /** + * + */ + public function PhysicsScene() { + contacts = new Contact(0); + var contact:Contact = contacts; + for (var i:int = 1; i < MAX_CONTACTS; i++) { + contact.next = new Contact(i); + contact = contact.next; + } + collisionDetector = new KdTreeCollisionDetector(); + } + + /** + * Вектор гравитации. + */ + public function get gravity():Vector3 { + return _gravity.vClone(); + } + + /** + * @private + */ + public function set gravity(value:Vector3):void { + _gravity.vCopy(value); + _gravityMagnitude = _gravity.vLength(); + } + + /** + * Добавляет тело в симуляцию. + * + * @param body + */ + public function addBody(body:Body):void { + body.id = lastBodyId++; + body.world = this; + bodies.append(body); + } + + /** + * Удаляет тело из симуляции. + * @param body + * @return + */ + public function removeBody(body:Body):void { + if (bodies.remove(body)) { + body.world = null; + } + } + + /** + * Добавляет ограничение. + * @param c + */ + public function addConstraint(c:Constraint):void { + constraints[constraintsNum++] = c; + c.world = this; + } + + /** + * Удаляет ограничение. + * @param c + */ + public function removeConstraint(c:Constraint):Boolean { + var idx:int = constraints.indexOf(c); + if (idx < 0) return false; + constraints.splice(idx, 1) + constraintsNum--; + c.world = null; + return true; + } + + /** + * Применяет к телам действующте на них силы. Аккумуляторы сил и моментов тел очищаются после завершения шага + * симуляции, поэтому на момент вызова метода могут уже содержать некоторые значения. + * + * @param dt промежуток времени, в течении которого действуют силы + */ + private function applyForces(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + body.beforePhysicsStep(dt); + body.calcAccelerations(); + // Ускорение свободного падения применяется только к подвижным телам во избежание некорректного изменения + // фиктивной скорости неподвижных тел. + if (body.movable && !body.frozen) { + body.accel.x += _gravity.x; + body.accel.y += _gravity.y; + body.accel.z += _gravity.z; + } + item = item.next; + } + } + + /** + * Определяет все столкновения на текущем шаге симуляции и заполняет список получившихся контактов. + * + * @param dt длительность шага симуляции + */ + private function detectCollisions(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + if (!body.frozen) { + body.contactsNum = 0; + body.saveState(); + // При включённом режиме предсказания состояние тел интегрируется на один шаг вперёд + if (usePrediction) { + body.integrateVelocity(dt); + body.integratePosition(dt); + } + body.calcDerivedData(); + } + item = item.next; + } + + borderContact = collisionDetector.getAllContacts(contacts); + + // Расчёт относительных векторов точки контакта вынесен сюда из-за необходимости учитывать + // положение тел в предсказанном состоянии + var contact:Contact = contacts; + while (contact != borderContact) { + var b1:Body = contact.body1; + var b2:Body = contact.body2; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + var bPos:Vector3 = b1.state.pos; + cp.r1.x = cp.pos.x - bPos.x; + cp.r1.y = cp.pos.y - bPos.y; + cp.r1.z = cp.pos.z - bPos.z; + if (b2 != null) { + bPos = b2.state.pos; + cp.r2.x = cp.pos.x - bPos.x; + cp.r2.y = cp.pos.y - bPos.y; + cp.r2.z = cp.pos.z - bPos.z; + } + } + contact = contact.next; + } + + // Восстановление состояния тел + if (usePrediction) { + item = bodies.head; + while (item != null) { + body = item.body; + if (!body.frozen) { + body.restoreState(); + body.calcDerivedData(); + } + item = item.next; + } + } + } + + /** + * Подготваливает полученные из детектора столкновений контакты, расчитывая значения, не меняющиеся + * в ходе шага симуляции. + */ + private function preProcessContacts(dt:Number):void { + var contact:Contact = contacts; + while (contact != borderContact) { + var b1:Body = contact.body1; + var b2:Body = contact.body2; + // Столкнувшиеся тела размораживаются + if (b1.frozen) { + b1.frozen = false; + b1.freezeCounter = 0; + } + if (b2 != null && b2.frozen) { + b2.frozen = false; + b2.freezeCounter = 0; + } + contact.restitution = b1.material.restitution; + if (b2 != null && b2.material.restitution < contact.restitution) contact.restitution = b2.material.restitution; + contact.friction = b1.material.friction; + if (b2 != null && b2.material.friction < contact.friction) contact.friction = b2.material.friction; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.accumImpulseN = 0; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + cp.velByUnitImpulseN = 0; + if (b1.movable) { + cp.angularInertia1 = _v.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1).vDot(contact.normal); + cp.velByUnitImpulseN += b1.invMass + cp.angularInertia1; + } + if (b2 != null && b2.movable) { + cp.angularInertia2 = _v.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2).vDot(contact.normal); + cp.velByUnitImpulseN += b2.invMass + cp.angularInertia2; + } + // Расчёт требуемой конечной скорости для упругого контакта + calcSepVelocity(b1, b2, cp, _v); + cp.normalVel = _v.vDot(contact.normal); + if (cp.normalVel < 0) cp.normalVel = - contact.restitution*cp.normalVel; + // Скорость разделения неупругого контакта + cp.minSepVel = cp.penetration > allowedPenetration ? (cp.penetration - allowedPenetration)/(penResolutionSteps*dt) : 0; + if (cp.minSepVel > maxPenResolutionSpeed) cp.minSepVel = maxPenResolutionSpeed; + } + contact = contact.next; + } + for (var i:int = 0; i < constraintsNum; i++) { + var constraint:Constraint = constraints[i]; + constraint.preProcess(dt); + } + } + + /** + * + * @param dt + * @param forceInelastic + */ + private function processContacts(dt:Number, forceInelastic:Boolean):void { + var iterNum:int = forceInelastic ? contactIterations : collisionIterations; + var i:int; + var forwardLoop:Boolean = false; + for (var iter:int = 0; iter < iterNum; iter++) { + forwardLoop = !forwardLoop; + var contact:Contact = contacts; + while (contact != borderContact) { + resolveContact(contact, forceInelastic, forwardLoop); + contact = contact.next; + } + // Ограничения + for (i = 0; i < constraintsNum; i++) { + var constraint:Constraint = constraints[i]; + constraint.apply(dt); + } + } + + // Разделение контактов путём непосредственного изменения координат и ориентации. Экспериментально. +// if (forceInelastic && useStaticSeparation) performStaticSeparation(); + } + + /** + * + */ + private function resolveContact(contactInfo:Contact, forceInelastic:Boolean, forwardLoop:Boolean):void { + var b1:Body = contactInfo.body1; + var b2:Body = contactInfo.body2; + var normal:Vector3 = contactInfo.normal; + var i:int; + if (forwardLoop) { + for (i = 0; i < contactInfo.pcount; i++) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } else { + for (i = contactInfo.pcount - 1; i >= 0; i--) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } + } + + /** + * + * @param idx + * @param b1 + * @param b2 + * @param colInfo + * @param normal + * @param forceInelastic + */ + private function resolveContactPoint(idx:int, b1:Body, b2:Body, contact:Contact, normal:Vector3, forceInelastic:Boolean):void { + var cp:ContactPoint = contact.points[idx]; + if (!forceInelastic) { + cp.satisfied = true; + } + + var newVel:Number = 0; + calcSepVelocity(b1, b2, cp, _v); + var cnormal:Vector3 = contact.normal; + var sepVel:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + if (forceInelastic) { + var minSpeVel:Number = useStaticSeparation ? 0 : cp.minSepVel; + if (sepVel < minSpeVel) { + cp.satisfied = false; + } else if (cp.satisfied) return; + newVel = minSpeVel; + } else { + newVel = cp.normalVel; + } + var deltaVel:Number = newVel - sepVel; + var impulse:Number = deltaVel/cp.velByUnitImpulseN; + var accumImpulse:Number = cp.accumImpulseN + impulse; + if (accumImpulse < 0) { + accumImpulse = 0; + } + var deltaImpulse:Number = accumImpulse - cp.accumImpulseN; + cp.accumImpulseN = accumImpulse; + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, normal, deltaImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, normal, -deltaImpulse); + } + + // Учёт силы трения + calcSepVelocity(b1, b2, cp, _v); + // Расчитываем изменение касательной скорости на единицу касательного импульса + var tanSpeedByUnitImpulse:Number = 0; + +// _v.vAddScaled(-_v.vDot(contact.normal), contact.normal); + var dot:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + _v.x -= dot*cnormal.x; + _v.y -= dot*cnormal.y; + _v.z -= dot*cnormal.z; + + var tanSpeed:Number = _v.vLength(); + if (tanSpeed < 0.001) return; + +// _t.vCopy(_v).vNormalize().vReverse(); + _t.x = -_v.x; + _t.y = -_v.y; + _t.z = -_v.z; + _t.vNormalize(); + + var r:Vector3; + var m:Matrix3; + var xx:Number; + var yy:Number; + var zz:Number; + // dV = b.invMass + ((invI * (r % t)) % r) * t + if (b1.movable) { +// _v.vCross2(cp.r1, _t).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1); + r = cp.r1; + m = b1.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b1.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + if (b2 != null && b2.movable) { +// _v.vCross2(cp.r2, _t).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2); + + r = cp.r2; + m = b2.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b2.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + + var tanImpulse:Number = tanSpeed/tanSpeedByUnitImpulse; + var max:Number = contact.friction*cp.accumImpulseN; + if (max < 0) { + if (tanImpulse < max) tanImpulse = max; + } else { + if (tanImpulse > max) tanImpulse = max; + } + + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, _t, tanImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, _t, -tanImpulse); + } + } + + /** + * + * @param cp + * @param normal + * @return + */ + private function calcSepVelocity(body1:Body, body2:Body, cp:ContactPoint, result:Vector3):void { + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 +// result.vCopy(body1.state.velocity).vAdd(_v1.vCross2(body1.state.rotation, cp.r1)); + var rot:Vector3 = body1.state.rotation; + var v:Vector3 = cp.r1; + var x:Number = rot.y*v.z - rot.z*v.y; + var y:Number = rot.z*v.x - rot.x*v.z; + var z:Number = rot.x*v.y - rot.y*v.x; + v = body1.state.velocity; + result.x = v.x + x; + result.y = v.y + y; + result.z = v.z + z; + // V2 = V2_c + w2%r2 + if (body2 != null) { +// result.vSubtract(body2.state.velocity).vSubtract(_v2.vCross2(body2.state.rotation, cp.r2)); + rot = body2.state.rotation; + v = cp.r2; + x = rot.y*v.z - rot.z*v.y; + y = rot.z*v.x - rot.x*v.z; + z = rot.x*v.y - rot.y*v.x; + v = body2.state.velocity; + result.x -= v.x + x; + result.y -= v.y + y; + result.z -= v.z + z; + } + } + + /** + * + * @param dt + */ + private function intergateVelocities(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + item.body.integrateVelocity(dt); + item = item.next; + } + } + + /** + * + * @param dt + */ + private function integratePositions(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + if (body.movable && !body.frozen) { + body.integratePosition(dt); + } + item = item.next; + } + } + + /** + * + */ + private function performStaticSeparation():void { + var iterNum:int = staticSeparationIterations; +// iterNum = 100; + + // 1. В начале каждой итерации для всех контактов сбрасывается флаг satisfied с одновременным поиском наихудшего контакта, т.е. имеющего наибольшую величину пересечения тел. + // 2. Если найденный контакт имеет величину пересечения меньше предельно допустимой процедура прерывается, т.к. разделение контактов не требуется. + // 3. Если процедура продолжается, то выполняется разделение найденного контакта и установка его флага satisfied в true, после чего запускается внутренний цикл, состоящий + // из contactsNum - 1 итераций. На каждой итерации ищется наихудший контакт среди оставшихся (satisfied == false). Если такой контакт найден, то выполняется его разделение + // и переход к следующей итерации внутреннего цикла, иначе внутренний цикл прерывается и выполняется переход к следующей итерации внешнего цикла (шаг 1.). +// for (var iter:int = 0; iter < iterNum; iter++) { +// // Ищем контакт с максимальной величиной пересечения тел, одновременно сбрасывая флаги satisfied в false +// var worstContact:Contact = contacts[0]; +// var i:int; +// for (i = 1; i < contactsNum; i++) { +// var contact:Contact = contacts[i]; +// contact.satisfied = false; +// if (contact.maxPenetration > worstContact.maxPenetration) worstContact = contact; +// } +// if (worstContact.maxPenetration <= allowedPenetration) return; +// +// resolveInterpenetration(worstContact); +// // Внутренний цикл по оставшимся контактам +// for (i = 1; i < contactsNum; i++) { +// worstContact = getWorstContact(); +// if (worstContact == null) break; +// resolveInterpenetration(worstContact); +// } +// } + } + + /** + * + */ +// private function getWorstContact():Contact { +// var maxPen:Number = 0; +// var worst:Contact = null; +// for (var i:int = 0; i < contactsNum; i++) { +// var c:Contact = contacts[i]; +// if (!c.satisfied && c.maxPenetration > maxPen) { +// worst = c; +// maxPen = c.maxPenetration; +// } +// } +// return maxPen > allowedPenetration ? worst : null; +// } + + /** + * Разделяет указанный контакт, выполняя staticSeparationSteps итераций по списку точек контакта. + * 1. В начале каждой итерации ищется точка с наибольшим пересечением, одновременно сбрасываются флаги satisfied у точек. + * 2. Для найденной точки выполняется процедура разделения. + * 3. После выполняется pcount - 1 итерация по списку точек, каждый раз ищется наихудшая среди имеющих satisfied == false, для которой выполняется процедура разделения. + * Если наихудшая точка не найдена, то выполнется переход к следующей итерации внешнего цикла (шаг 1). + * + * В конце процедуры разделения обновляются значения пересечений для остальных точек контакта, а также для всех прочих контактов, относящихся к телам текущего. + * + * @param contact контакт для разделения + */ + private function resolveInterpenetration(contact:Contact):void { + contact.satisfied = true; + + for (var step:int = 0; step < staticSeparationSteps; step++) { + var worstCp:ContactPoint = contact.points[0]; + var cp:ContactPoint; + var i:int; + for (i = 1; i < contact.pcount; i++) { + cp = contact.points[i]; + cp.satisfied = false; + if (cp.penetration > worstCp.penetration) worstCp = cp; + } + if (worstCp.penetration <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + // Разделяем оставшиеся точки + var maxPen:Number = 0; + for (i = 1; i < contact.pcount; i++) { + // Поиск наихудшей точки + for (var j:int = 0; j < contact.pcount; j++) { + cp = contact.points[j]; + if (cp.satisfied) continue; + if (cp.penetration > maxPen) { + maxPen = cp.penetration; + worstCp = cp; + } + } + if (maxPen <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + } + } + } + + /** + * + * @param cp + * @param contact + */ + private function separateContactPoint(cp:ContactPoint, contact:Contact):void { +// cp.satisfied = true; +// +// var b1:Body = contact.body1; +// var b2:Body = contact.body2; +// var totalMove:Number = cp.penetration - allowedPenetration; +// var moveCoeff:Number = totalMove/cp.velByUnitImpulseN; +// var linMove1:Number; +// var angleMove1:Number; +// if (b1.movable) { +// linMove1 = b1.invMass*moveCoeff; +// angleMove1 = cp.angularInertia1*moveCoeff; +// if (angleMove1 > maxAngleMove) { +// linMove1 += angleMove1 - maxAngleMove; +// angleMove1 -= maxAngleMove; +// } +// b1.state.pos.vAddScaled(linMove1, contact.normal); +// _v1.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vScale(angleMove1); +// b1.state.orientation.addScaledVector(_v1, 1); +// } +// var linMove2:Number; +// var angleMove2:Number; +// if (b2 != null && b2.movable) { +// linMove2 = b2.invMass*moveCoeff; +// angleMove2 = cp.angularInertia2*moveCoeff; +// if (angleMove2 > maxAngleMove) { +// linMove2 += angleMove2 - maxAngleMove; +// angleMove2 -= maxAngleMove; +// } +// b2.state.pos.vAddScaled(-linMove2, contact.normal); +// _v2.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vScale(angleMove2); +// _v2.vReverse(); +// b2.state.orientation.addScaledVector(_v2, 1); +// } +// cp.penetration = allowedPenetration; +// // Обновляем пересечения в других точках +// var i:int; +// for (i = 0; i < contact.pcount; i++) { +// var cp1:ContactPoint = contact.points[i]; +// if (cp1 == cp) continue; +// var angularMove:Number; +// if (b1.movable) { +// angularMove = _v.vCross2(_v1, cp1.r1).vDot(contact.normal); +// cp1.penetration -= linMove1 + angularMove; +// } +// if (b2 != null && b2.movable) { +// angularMove = _v.vCross2(_v2, cp1.r2).vDot(contact.normal); +// cp1.penetration -= linMove2 - angularMove; +// } +// // Обновление максимального значения для контакта +// if (cp1.penetration > contact.maxPenetration) contact.maxPenetration = cp1.penetration; +// } +// // Обновляем пересечения для других контактов +// var c:Contact; +// var j:int; +// if (b1.movable) { +// for (i = 0; i < b1.contactsNum; i++) { +// c = b1.contacts[i]; +// if (c == contact) continue; +// for (j = 0; j < c.pcount; j++) { +// cp1 = c.points[j]; +// if (b1 == c.body1) cp1.penetration -= linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r1).vDot(c.normal); +// else cp1.penetration += linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r2).vDot(c.normal); +// if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; +// } +// } +// } +// if (b2 != null && b2.movable) { +// for (i = 0; i < b2.contactsNum; i++) { +// c = b2.contacts[i]; +// if (c == contact) continue; +// for (j = 0; j < c.pcount; j++) { +// cp1 = c.points[j]; +// if (b2 == c.body1) cp1.penetration -= linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r1).vDot(c.normal); +// else cp1.penetration += linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r2).vDot(c.normal); +// if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; +// } +// } +// } + } + + /** + * + */ + private function postPhysics():void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + body.clearAccumulators(); + body.calcDerivedData(); + if (body.canFreeze) { + if (body.state.velocity.vLength() < linSpeedFreezeLimit && body.state.rotation.vLength() < angSpeedFreezeLimit) { + if (!body.frozen) { + body.freezeCounter++; + if (body.freezeCounter >= freezeSteps) body.frozen = true; + } + } else { + body.freezeCounter = 0; + body.frozen = false; + } + } + item = item.next; + } + } + + /** + * + * @param delta + */ + public function update(delta:int):void { + timeStamp++; + time += delta; + var dt:Number = 0.001*delta; + applyForces(dt); + detectCollisions(dt); + preProcessContacts(dt); + processContacts(dt, false); + intergateVelocities(dt); + processContacts(dt, true); + integratePositions(dt); + postPhysics(); + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/PhysicsUtils.as b/0.0.9.1/src/alternativa/physics/PhysicsUtils.as new file mode 100644 index 0000000..5a2e46f --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/PhysicsUtils.as @@ -0,0 +1,49 @@ +package alternativa.physics { + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + public class PhysicsUtils { + + /** + * @param mass + * @param halfSize + * @param result + */ + public static function getBoxInvInertia(mass:Number, halfSize:Vector3, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + result.a = 3/(mass*(yy + zz)); + result.f = 3/(mass*(zz + xx)); + result.k = 3/(mass*(xx + yy)); + } + + /** + * @param mass + * @param r + * @param h + * @param result + */ + public static function getCylinderInvInertia(mass:Number, r:Number, h:Number, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + + result.a = result.f = 1/(mass*(h*h/12 + r*r/4)); + result.k = 2/(mass*r*r); + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/altphysics.as b/0.0.9.1/src/alternativa/physics/altphysics.as new file mode 100644 index 0000000..2715307 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/altphysics.as @@ -0,0 +1,3 @@ +package alternativa.physics { + public namespace altphysics = "http://alternativaplatform.com/en/altphysics"; +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/collision/.svn/all-wcprops b/0.0.9.1/src/alternativa/physics/collision/.svn/all-wcprops new file mode 100644 index 0000000..5b44b83 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/.svn/all-wcprops @@ -0,0 +1,65 @@ +K 25 +svn:wc:ra_dav:version-url +V 113 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/collision +END +ICollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 135 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/collision/ICollisionDetector.as +END +CollisionKdNode.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/collision/CollisionKdNode.as +END +CollisionKdTree2D.as +K 25 +svn:wc:ra_dav:version-url +V 134 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/collision/CollisionKdTree2D.as +END +IRayCollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 139 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/collision/IRayCollisionPredicate.as +END +IBodyCollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/collision/IBodyCollisionPredicate.as +END +CollisionPrimitive.as +K 25 +svn:wc:ra_dav:version-url +V 135 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/collision/CollisionPrimitive.as +END +KdTreeCollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/collision/KdTreeCollisionDetector.as +END +ICollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 136 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/collision/ICollisionPredicate.as +END +ICollider.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/collision/ICollider.as +END +CollisionKdTree.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/collision/CollisionKdTree.as +END diff --git a/0.0.9.1/src/alternativa/physics/collision/.svn/entries b/0.0.9.1/src/alternativa/physics/collision/.svn/entries new file mode 100644 index 0000000..dce1699 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/.svn/entries @@ -0,0 +1,157 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/collision +http://svndev.alternativaplatform.com + + + +2010-03-06T16:51:19.781174Z +29404 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +colliders +dir + +ICollisionDetector.as +file + + + + +2010-10-28T04:32:44.000000Z +9f76503bdaf92b4170b05b6e66c45de1 +2010-02-12T12:34:25.192477Z +28424 +mike + +CollisionKdNode.as +file + + + + +2010-10-28T04:32:44.000000Z +a6fab491cd9bd45a5472bf4ffc321e08 +2010-02-12T12:34:25.192477Z +28424 +mike + +CollisionKdTree2D.as +file + + + + +2010-10-28T04:32:44.000000Z +82bb636d8b49cdfbc22694dd8b01e71d +2010-02-12T12:34:25.192477Z +28424 +mike + +IRayCollisionPredicate.as +file + + + + +2010-10-28T04:32:44.000000Z +32f9b1eaeacfcb40e7ae1927f52b8ab8 +2009-10-19T07:17:33.113306Z +22253 +mike + +types +dir + +IBodyCollisionPredicate.as +file + + + + +2010-10-28T04:32:44.000000Z +618f907a588979a8d06d4f4bcf6618a0 +2009-10-19T07:17:33.113306Z +22253 +mike + +CollisionPrimitive.as +file + + + + +2010-10-28T04:32:44.000000Z +8501ce34a3effa2380c472cc9b823afa +2009-11-24T08:44:15.941480Z +23758 +mike + +KdTreeCollisionDetector.as +file + + + + +2010-10-28T04:32:44.000000Z +ea670ad5d0f72f6a6df5163d65f204f9 +2010-02-12T12:34:25.192477Z +28424 +mike + +ICollisionPredicate.as +file + + + + +2010-10-28T04:32:44.000000Z +65f3250d36556fffc457c740fb7d38b3 +2009-10-19T07:17:33.113306Z +22253 +mike + +ICollider.as +file + + + + +2010-10-28T04:32:44.000000Z +128a7d7479e13caeccecb45161b27b22 +2009-10-19T07:17:33.113306Z +22253 +mike + +primitives +dir + +CollisionKdTree.as +file + + + + +2010-10-28T04:32:44.000000Z +8e815c1f1e527b0884b3f0124257a603 +2010-02-12T12:34:25.192477Z +28424 +mike + diff --git a/0.0.9.1/src/alternativa/physics/collision/.svn/format b/0.0.9.1/src/alternativa/physics/collision/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.1/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base b/0.0.9.1/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base new file mode 100644 index 0000000..f450f1e --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base @@ -0,0 +1,18 @@ +package alternativa.physics.collision { + + + import alternativa.physics.collision.types.BoundBox; + + public class CollisionKdNode { + public var indices:Vector.; + public var splitIndices:Vector.; + public var boundBox:BoundBox; + public var parent:CollisionKdNode; + public var splitTree:CollisionKdTree2D; + + public var axis:int = -1; // 0 - x, 1 - y, 2 - z + public var coord:Number; + public var positiveNode:CollisionKdNode; + public var negativeNode:CollisionKdNode; + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base b/0.0.9.1/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base new file mode 100644 index 0000000..d3dc307 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base @@ -0,0 +1,261 @@ +package alternativa.physics.collision { + + + + import alternativa.physics.collision.types.BoundBox; + + /** + * @author mike + */ + public class CollisionKdTree { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + public var rootNode:CollisionKdNode; + public var staticChildren:Vector.; + public var numStaticChildren:int; + public var staticBoundBoxes:Vector. = new Vector.(); + + private var splitAxis:int; + private var splitCoord:Number; + private var splitCost:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + + /** + * @param boundBox + */ + public function createTree(collisionPrimitives:Vector., boundBox:BoundBox = null):void { + staticChildren = collisionPrimitives.concat(); + numStaticChildren = staticChildren.length; + // Создаём корневую ноду + rootNode = new CollisionKdNode(); + rootNode.indices = new Vector.(); + // Расчитываем баунды объектов и рутовой ноды + var rootNodeBoundBox:BoundBox = rootNode.boundBox = (boundBox != null) ? boundBox : new BoundBox(); + for (var i:int = 0; i < numStaticChildren; ++i) { + var child:CollisionPrimitive = staticChildren[i]; + var childBoundBox:BoundBox = staticBoundBoxes[i] = child.calculateAABB(); + rootNodeBoundBox.addBoundBox(childBoundBox); + rootNode.indices[i] = i; + } + staticBoundBoxes.length = numStaticChildren; + // Разделяем рутовую ноду + splitNode(rootNode); + + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + var indices:Vector. = node.indices; + var numPrimitives:int = indices.length; + if (numPrimitives <= minPrimitivesPerNode) return; + + // Подготовка баунда с погрешностями + var nodeBoundBox:BoundBox = node.boundBox; + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + // Собираем опорные координаты + var i:int; + var j:int; + var numSplitCoordsX:int = 0 + var numSplitCoordsY:int = 0; + var numSplitCoordsZ:int = 0; + for (i = 0; i < numPrimitives; ++i) { + var boundBox:BoundBox = staticBoundBoxes[indices[i]]; + + if (boundBox.maxX - boundBox.minX <= doubleThreshold) { + if (boundBox.minX <= nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.minX; + else if (boundBox.maxX >= nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.maxX; + else splitCoordsX[numSplitCoordsX++] = (boundBox.minX + boundBox.maxX)*0.5; + } else { + if (boundBox.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = boundBox.minX; + if (boundBox.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = boundBox.maxX; + } + + if (boundBox.maxY - boundBox.minY <= doubleThreshold) { + if (boundBox.minY <= nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.minY; + else if (boundBox.maxY >= nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.maxY; + else splitCoordsY[numSplitCoordsY++] = (boundBox.minY + boundBox.maxY)*0.5; + } else { + if (boundBox.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = boundBox.minY; + if (boundBox.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = boundBox.maxY; + } + + if (boundBox.maxZ - boundBox.minZ <= doubleThreshold) { + if (boundBox.minZ <= nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.minZ; + else if (boundBox.maxZ >= nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.maxZ; + else splitCoordsZ[numSplitCoordsZ++] = (boundBox.minZ + boundBox.maxZ)*0.5; + } else { + if (boundBox.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.minZ; + if (boundBox.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numPrimitives; ++i) { + boundBox = staticBoundBoxes[indices[i]]; + var min:Number = axisX ? boundBox.minX : (axisY ? boundBox.minY : boundBox.minZ); + var max:Number = axisX ? boundBox.maxX : (axisY ? boundBox.maxY : boundBox.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(indices[i]); + indices[i] = -1; + } else { + if (node.splitIndices == null) node.splitIndices = new Vector.(); + node.splitIndices.push(indices[i]); + indices[i] = -1; + } + } else { + if (min >= coordMin) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(indices[i]); + indices[i] = -1; + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numPrimitives; ++i) { + if (indices[i] >= 0) indices[j++] = indices[i]; + } + if (j > 0) indices.length = j; + else node.indices = null; + + if (node.splitIndices != null) { + node.splitTree = new CollisionKdTree2D(this, node); + node.splitTree.createTree(); + } + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + /** + * + */ + public function traceTree():void { + traceNode("", rootNode); + } + + /** + * @param str + * @param node + */ + private function traceNode(str:String, node:CollisionKdNode):void { + if (node == null) return; + trace(str, node.axis == -1 ? "end" : ((node.axis == 0) ? "X" : ((node.axis == 1) ? "Y" : "Z")), "splitCoord=" + splitCoord, "bound", node.boundBox, "objs:", node.indices); + traceNode(str + "-", node.negativeNode); + traceNode(str + "+", node.positiveNode); + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base b/0.0.9.1/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base new file mode 100644 index 0000000..2621517 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base @@ -0,0 +1,226 @@ +package alternativa.physics.collision { + + + import alternativa.physics.collision.types.BoundBox; + + /** + * + */ + public class CollisionKdTree2D { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + + public var parentTree:CollisionKdTree; + public var parentNode:CollisionKdNode; + public var rootNode:CollisionKdNode; + + private var splitAxis:int; + private var splitCost:Number; + private var splitCoord:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + /** + * + * @param parentTree + * @param parentNode + */ + public function CollisionKdTree2D(parentTree:CollisionKdTree, parentNode:CollisionKdNode) { + this.parentTree = parentTree; + this.parentNode = parentNode; + } + + /** + * + */ + public function createTree():void { + rootNode = new CollisionKdNode(); + rootNode.boundBox = parentNode.boundBox.clone(); + rootNode.indices = new Vector.(); + var numObjects:int = parentNode.splitIndices.length; + for (var i:int = 0; i < numObjects; ++i) rootNode.indices[i] = parentNode.splitIndices[i]; + + splitNode(rootNode); + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + if (node.indices.length <= minPrimitivesPerNode) return; + + var objects:Vector. = node.indices; + var i:int; + var j:int; + + var nodeBoundBox:BoundBox = node.boundBox; + + // Подготовка баунда с погрешностями + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + // Собираем опорные координаты + var numSplitCoordsX:int; + var numSplitCoordsY:int; + var numSplitCoordsZ:int; + var numObjects:int = objects.length; + for (i = 0; i < numObjects; ++i) { + var bb:BoundBox = staticBoundBoxes[objects[i]]; + + if (parentNode.axis != 0) { + if (bb.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = bb.minX; + if (bb.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = bb.maxX; + } + + if (parentNode.axis != 1) { + if (bb.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = bb.minY; + if (bb.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = bb.maxY; + } + + if (parentNode.axis != 2) { + if (bb.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = bb.minZ; + if (bb.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = bb.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + if (parentNode.axis != 0) checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + if (parentNode.axis != 1) checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + if (parentNode.axis != 2) checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numObjects; ++i) { + bb = staticBoundBoxes[objects[i]]; + var min:Number = axisX ? bb.minX : (axisY ? bb.minY : bb.minZ); + var max:Number = axisX ? bb.maxX : (axisY ? bb.maxY : bb.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(objects[i]); + objects[i] = -1; + } + } else { + if (min >= coordMin) { + if (max > coordMax) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(objects[i]); + objects[i] = -1; + } + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numObjects; ++i) { + if (objects[i] >= 0) objects[j++] = objects[i]; + } + if (j > 0) objects.length = j; + else node.indices = null; + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/collision/.svn/text-base/CollisionPrimitive.as.svn-base b/0.0.9.1/src/alternativa/physics/collision/.svn/text-base/CollisionPrimitive.as.svn-base new file mode 100644 index 0000000..e03cfee --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/.svn/text-base/CollisionPrimitive.as.svn-base @@ -0,0 +1,140 @@ +package alternativa.physics.collision { + + import alternativa.physics.Body; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * Базовый класс для примитивов, использующихся детектором столкновений. + */ + public class CollisionPrimitive { + // Константы типов примитива + public static const BOX:int = 1; + public static const PLANE:int = 2; + public static const SPHERE:int = 3; + public static const RECT:int = 4; + public static const TRIANGLE:int = 5; + + // Тип примитива + public var type:int; + // Группы примитива. Каждая группа определяется установленным битом. Столкновения проверяются только для примитивов, + // имеющих хотя бы одну общую группу. + public var collisionGroup:int; + // Предикат примитива, вызывающийся детектором при нахождении столкновения. В зависимости от возвращённого + // предикатом результата столкновение либо регистрируется, либо игнорируется. + // Nullable + public var postCollisionPredicate:ICollisionPredicate; + // Тело, владеющее примитивом. Поле сделано открытым для быстрого доступа на чтение. Установка значения должна выполняться вызовом метода setBody(). + // Nullable + public var body:Body; + // Трансформация примитива в системе координат тела, если оно указано. Не допускается масштабирование матрицы. + public var localTransform:Matrix4; + // Полная трансформация примитива. Не допускается масштабирование матрицы. + public var transform:Matrix4 = new Matrix4(); + // AABB в мировой системе координат. Расчитывается системой вызовом функции calculateBoundBox(). + public var aabb:BoundBox = new BoundBox(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param type тип примитива + * @param collisionGroup группа примитива + */ + public function CollisionPrimitive(type:int, collisionGroup:int) { + this.type = type; + this.collisionGroup = collisionGroup; + } + + /** + * Устанавливает тело, владеющее примитивом. + * + * @param body тело, которое владеет примитивом + * @param localTransform трансформация примитива в системе координат тела. Указание значения null равносильно + * заданию единичной матрицы, однако в первом случае не будет дополнительного умножения матриц при вычислении полной трансформации примитива. + */ + public function setBody(body:Body, localTransform:Matrix4 = null):void { + if (this.body == body) return; + this.body = body; + if (body != null) { + if (localTransform != null) { + if (this.localTransform == null) { + this.localTransform = new Matrix4(); + } + this.localTransform.copy(localTransform); + } else { + this.localTransform = null; + } + } + } + + /** + * Рассчитывает AABB примитива. Наследники должны переопределять этот метод, реализуя в нём корректный рассчёт. + * + * @return ссылка на свой AABB + */ + public function calculateAABB():BoundBox { + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + return -1; + } + + /** + * Клонирует примитив. Переопределять не рекомендуется. Вместо этого переопределяются методы createPrimitive() и copyFrom(). + * + * @return клон примитива + */ + public function clone():CollisionPrimitive { + var p:CollisionPrimitive = createPrimitive(); + return p.copyFrom(this); + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + if (source == null) { + throw new ArgumentError("Parameter source cannot be null"); + } + type = source.type; + transform.copy(source.transform); + collisionGroup = source.collisionGroup; + setBody(source.body, source.localTransform); + aabb.copyFrom(source.aabb); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + public function toString():String { + return "[CollisionPrimitive type=" + type + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + protected function createPrimitive():CollisionPrimitive { + return new CollisionPrimitive(type, collisionGroup); + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base b/0.0.9.1/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base new file mode 100644 index 0000000..4ace681 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.Body; + + public interface IBodyCollisionPredicate { + function considerBodies(body1:Body, body2:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base b/0.0.9.1/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base new file mode 100644 index 0000000..e6aac2a --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base @@ -0,0 +1,29 @@ +package alternativa.physics.collision { + + import alternativa.physics.Contact; + + /** + * Интерфейс определителя столкновений между двумя примитивами. + */ + public interface ICollider { + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base b/0.0.9.1/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base new file mode 100644 index 0000000..916f67b --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base @@ -0,0 +1,64 @@ +package alternativa.physics.collision { + import alternativa.physics.Contact; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.math.Vector3; + + /** + * Интерфейс детектора столкновений. + */ + public interface ICollisionDetector { + + /** + * Получает все столкновения в текущей конфигурации физической геометрии. + * + * @param contacts список контактов, в кторые будет записана информация о столкновении + * @return количество найденных столкновений + */ + function getAllContacts(contacts:Contact):Contact; + + /** + * Тестирует луч на пересечение с физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param direction направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRay(origin:Vector3, direction:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * + * @param origin + * @param direction + * @param collisionGroup + * @param maxTime + * @param predicate + * @param result + * @return + */ + function intersectRayWithStatic(origin:Vector3, direction:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @return + */ + function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base b/0.0.9.1/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base new file mode 100644 index 0000000..33d30ca --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base @@ -0,0 +1,8 @@ +package alternativa.physics.collision { + + public interface ICollisionPredicate { + + function considerCollision(primitive:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base b/0.0.9.1/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base new file mode 100644 index 0000000..b319f62 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.Body; + + public interface IRayCollisionPredicate { + function considerBody(body:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base b/0.0.9.1/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base new file mode 100644 index 0000000..9ca4c2a --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base @@ -0,0 +1,486 @@ +package alternativa.physics.collision { + + + 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.math.Vector3; + + use namespace altphysics; + + /** + * Детектор, хранящий статическую геометрию в kD-дереве и использующий дерево для ускорения тестов на пересечения. + */ + public class KdTreeCollisionDetector implements ICollisionDetector { + + altphysics var tree:CollisionKdTree; + altphysics var dynamicPrimitives:Vector.; + 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.(); + + 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.):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. = tree.staticChildren; +// var indices:Vector. = 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., indices:Vector., 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; + } \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/collision/CollisionKdNode.as b/0.0.9.1/src/alternativa/physics/collision/CollisionKdNode.as new file mode 100644 index 0000000..f450f1e --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/CollisionKdNode.as @@ -0,0 +1,18 @@ +package alternativa.physics.collision { + + + import alternativa.physics.collision.types.BoundBox; + + public class CollisionKdNode { + public var indices:Vector.; + public var splitIndices:Vector.; + public var boundBox:BoundBox; + public var parent:CollisionKdNode; + public var splitTree:CollisionKdTree2D; + + public var axis:int = -1; // 0 - x, 1 - y, 2 - z + public var coord:Number; + public var positiveNode:CollisionKdNode; + public var negativeNode:CollisionKdNode; + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/collision/CollisionKdTree.as b/0.0.9.1/src/alternativa/physics/collision/CollisionKdTree.as new file mode 100644 index 0000000..d3dc307 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/CollisionKdTree.as @@ -0,0 +1,261 @@ +package alternativa.physics.collision { + + + + import alternativa.physics.collision.types.BoundBox; + + /** + * @author mike + */ + public class CollisionKdTree { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + public var rootNode:CollisionKdNode; + public var staticChildren:Vector.; + public var numStaticChildren:int; + public var staticBoundBoxes:Vector. = new Vector.(); + + private var splitAxis:int; + private var splitCoord:Number; + private var splitCost:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + + /** + * @param boundBox + */ + public function createTree(collisionPrimitives:Vector., boundBox:BoundBox = null):void { + staticChildren = collisionPrimitives.concat(); + numStaticChildren = staticChildren.length; + // Создаём корневую ноду + rootNode = new CollisionKdNode(); + rootNode.indices = new Vector.(); + // Расчитываем баунды объектов и рутовой ноды + var rootNodeBoundBox:BoundBox = rootNode.boundBox = (boundBox != null) ? boundBox : new BoundBox(); + for (var i:int = 0; i < numStaticChildren; ++i) { + var child:CollisionPrimitive = staticChildren[i]; + var childBoundBox:BoundBox = staticBoundBoxes[i] = child.calculateAABB(); + rootNodeBoundBox.addBoundBox(childBoundBox); + rootNode.indices[i] = i; + } + staticBoundBoxes.length = numStaticChildren; + // Разделяем рутовую ноду + splitNode(rootNode); + + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + var indices:Vector. = node.indices; + var numPrimitives:int = indices.length; + if (numPrimitives <= minPrimitivesPerNode) return; + + // Подготовка баунда с погрешностями + var nodeBoundBox:BoundBox = node.boundBox; + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + // Собираем опорные координаты + var i:int; + var j:int; + var numSplitCoordsX:int = 0 + var numSplitCoordsY:int = 0; + var numSplitCoordsZ:int = 0; + for (i = 0; i < numPrimitives; ++i) { + var boundBox:BoundBox = staticBoundBoxes[indices[i]]; + + if (boundBox.maxX - boundBox.minX <= doubleThreshold) { + if (boundBox.minX <= nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.minX; + else if (boundBox.maxX >= nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.maxX; + else splitCoordsX[numSplitCoordsX++] = (boundBox.minX + boundBox.maxX)*0.5; + } else { + if (boundBox.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = boundBox.minX; + if (boundBox.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = boundBox.maxX; + } + + if (boundBox.maxY - boundBox.minY <= doubleThreshold) { + if (boundBox.minY <= nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.minY; + else if (boundBox.maxY >= nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.maxY; + else splitCoordsY[numSplitCoordsY++] = (boundBox.minY + boundBox.maxY)*0.5; + } else { + if (boundBox.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = boundBox.minY; + if (boundBox.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = boundBox.maxY; + } + + if (boundBox.maxZ - boundBox.minZ <= doubleThreshold) { + if (boundBox.minZ <= nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.minZ; + else if (boundBox.maxZ >= nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.maxZ; + else splitCoordsZ[numSplitCoordsZ++] = (boundBox.minZ + boundBox.maxZ)*0.5; + } else { + if (boundBox.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.minZ; + if (boundBox.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numPrimitives; ++i) { + boundBox = staticBoundBoxes[indices[i]]; + var min:Number = axisX ? boundBox.minX : (axisY ? boundBox.minY : boundBox.minZ); + var max:Number = axisX ? boundBox.maxX : (axisY ? boundBox.maxY : boundBox.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(indices[i]); + indices[i] = -1; + } else { + if (node.splitIndices == null) node.splitIndices = new Vector.(); + node.splitIndices.push(indices[i]); + indices[i] = -1; + } + } else { + if (min >= coordMin) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(indices[i]); + indices[i] = -1; + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numPrimitives; ++i) { + if (indices[i] >= 0) indices[j++] = indices[i]; + } + if (j > 0) indices.length = j; + else node.indices = null; + + if (node.splitIndices != null) { + node.splitTree = new CollisionKdTree2D(this, node); + node.splitTree.createTree(); + } + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + /** + * + */ + public function traceTree():void { + traceNode("", rootNode); + } + + /** + * @param str + * @param node + */ + private function traceNode(str:String, node:CollisionKdNode):void { + if (node == null) return; + trace(str, node.axis == -1 ? "end" : ((node.axis == 0) ? "X" : ((node.axis == 1) ? "Y" : "Z")), "splitCoord=" + splitCoord, "bound", node.boundBox, "objs:", node.indices); + traceNode(str + "-", node.negativeNode); + traceNode(str + "+", node.positiveNode); + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/collision/CollisionKdTree2D.as b/0.0.9.1/src/alternativa/physics/collision/CollisionKdTree2D.as new file mode 100644 index 0000000..2621517 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/CollisionKdTree2D.as @@ -0,0 +1,226 @@ +package alternativa.physics.collision { + + + import alternativa.physics.collision.types.BoundBox; + + /** + * + */ + public class CollisionKdTree2D { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + + public var parentTree:CollisionKdTree; + public var parentNode:CollisionKdNode; + public var rootNode:CollisionKdNode; + + private var splitAxis:int; + private var splitCost:Number; + private var splitCoord:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + /** + * + * @param parentTree + * @param parentNode + */ + public function CollisionKdTree2D(parentTree:CollisionKdTree, parentNode:CollisionKdNode) { + this.parentTree = parentTree; + this.parentNode = parentNode; + } + + /** + * + */ + public function createTree():void { + rootNode = new CollisionKdNode(); + rootNode.boundBox = parentNode.boundBox.clone(); + rootNode.indices = new Vector.(); + var numObjects:int = parentNode.splitIndices.length; + for (var i:int = 0; i < numObjects; ++i) rootNode.indices[i] = parentNode.splitIndices[i]; + + splitNode(rootNode); + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + if (node.indices.length <= minPrimitivesPerNode) return; + + var objects:Vector. = node.indices; + var i:int; + var j:int; + + var nodeBoundBox:BoundBox = node.boundBox; + + // Подготовка баунда с погрешностями + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + // Собираем опорные координаты + var numSplitCoordsX:int; + var numSplitCoordsY:int; + var numSplitCoordsZ:int; + var numObjects:int = objects.length; + for (i = 0; i < numObjects; ++i) { + var bb:BoundBox = staticBoundBoxes[objects[i]]; + + if (parentNode.axis != 0) { + if (bb.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = bb.minX; + if (bb.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = bb.maxX; + } + + if (parentNode.axis != 1) { + if (bb.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = bb.minY; + if (bb.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = bb.maxY; + } + + if (parentNode.axis != 2) { + if (bb.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = bb.minZ; + if (bb.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = bb.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + if (parentNode.axis != 0) checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + if (parentNode.axis != 1) checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + if (parentNode.axis != 2) checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numObjects; ++i) { + bb = staticBoundBoxes[objects[i]]; + var min:Number = axisX ? bb.minX : (axisY ? bb.minY : bb.minZ); + var max:Number = axisX ? bb.maxX : (axisY ? bb.maxY : bb.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(objects[i]); + objects[i] = -1; + } + } else { + if (min >= coordMin) { + if (max > coordMax) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(objects[i]); + objects[i] = -1; + } + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numObjects; ++i) { + if (objects[i] >= 0) objects[j++] = objects[i]; + } + if (j > 0) objects.length = j; + else node.indices = null; + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/collision/CollisionPrimitive.as b/0.0.9.1/src/alternativa/physics/collision/CollisionPrimitive.as new file mode 100644 index 0000000..e03cfee --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/CollisionPrimitive.as @@ -0,0 +1,140 @@ +package alternativa.physics.collision { + + import alternativa.physics.Body; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * Базовый класс для примитивов, использующихся детектором столкновений. + */ + public class CollisionPrimitive { + // Константы типов примитива + public static const BOX:int = 1; + public static const PLANE:int = 2; + public static const SPHERE:int = 3; + public static const RECT:int = 4; + public static const TRIANGLE:int = 5; + + // Тип примитива + public var type:int; + // Группы примитива. Каждая группа определяется установленным битом. Столкновения проверяются только для примитивов, + // имеющих хотя бы одну общую группу. + public var collisionGroup:int; + // Предикат примитива, вызывающийся детектором при нахождении столкновения. В зависимости от возвращённого + // предикатом результата столкновение либо регистрируется, либо игнорируется. + // Nullable + public var postCollisionPredicate:ICollisionPredicate; + // Тело, владеющее примитивом. Поле сделано открытым для быстрого доступа на чтение. Установка значения должна выполняться вызовом метода setBody(). + // Nullable + public var body:Body; + // Трансформация примитива в системе координат тела, если оно указано. Не допускается масштабирование матрицы. + public var localTransform:Matrix4; + // Полная трансформация примитива. Не допускается масштабирование матрицы. + public var transform:Matrix4 = new Matrix4(); + // AABB в мировой системе координат. Расчитывается системой вызовом функции calculateBoundBox(). + public var aabb:BoundBox = new BoundBox(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param type тип примитива + * @param collisionGroup группа примитива + */ + public function CollisionPrimitive(type:int, collisionGroup:int) { + this.type = type; + this.collisionGroup = collisionGroup; + } + + /** + * Устанавливает тело, владеющее примитивом. + * + * @param body тело, которое владеет примитивом + * @param localTransform трансформация примитива в системе координат тела. Указание значения null равносильно + * заданию единичной матрицы, однако в первом случае не будет дополнительного умножения матриц при вычислении полной трансформации примитива. + */ + public function setBody(body:Body, localTransform:Matrix4 = null):void { + if (this.body == body) return; + this.body = body; + if (body != null) { + if (localTransform != null) { + if (this.localTransform == null) { + this.localTransform = new Matrix4(); + } + this.localTransform.copy(localTransform); + } else { + this.localTransform = null; + } + } + } + + /** + * Рассчитывает AABB примитива. Наследники должны переопределять этот метод, реализуя в нём корректный рассчёт. + * + * @return ссылка на свой AABB + */ + public function calculateAABB():BoundBox { + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + return -1; + } + + /** + * Клонирует примитив. Переопределять не рекомендуется. Вместо этого переопределяются методы createPrimitive() и copyFrom(). + * + * @return клон примитива + */ + public function clone():CollisionPrimitive { + var p:CollisionPrimitive = createPrimitive(); + return p.copyFrom(this); + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + if (source == null) { + throw new ArgumentError("Parameter source cannot be null"); + } + type = source.type; + transform.copy(source.transform); + collisionGroup = source.collisionGroup; + setBody(source.body, source.localTransform); + aabb.copyFrom(source.aabb); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + public function toString():String { + return "[CollisionPrimitive type=" + type + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + protected function createPrimitive():CollisionPrimitive { + return new CollisionPrimitive(type, collisionGroup); + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/collision/IBodyCollisionPredicate.as b/0.0.9.1/src/alternativa/physics/collision/IBodyCollisionPredicate.as new file mode 100644 index 0000000..4ace681 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/IBodyCollisionPredicate.as @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.Body; + + public interface IBodyCollisionPredicate { + function considerBodies(body1:Body, body2:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/collision/ICollider.as b/0.0.9.1/src/alternativa/physics/collision/ICollider.as new file mode 100644 index 0000000..e6aac2a --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/ICollider.as @@ -0,0 +1,29 @@ +package alternativa.physics.collision { + + import alternativa.physics.Contact; + + /** + * Интерфейс определителя столкновений между двумя примитивами. + */ + public interface ICollider { + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/collision/ICollisionDetector.as b/0.0.9.1/src/alternativa/physics/collision/ICollisionDetector.as new file mode 100644 index 0000000..916f67b --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/ICollisionDetector.as @@ -0,0 +1,64 @@ +package alternativa.physics.collision { + import alternativa.physics.Contact; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.math.Vector3; + + /** + * Интерфейс детектора столкновений. + */ + public interface ICollisionDetector { + + /** + * Получает все столкновения в текущей конфигурации физической геометрии. + * + * @param contacts список контактов, в кторые будет записана информация о столкновении + * @return количество найденных столкновений + */ + function getAllContacts(contacts:Contact):Contact; + + /** + * Тестирует луч на пересечение с физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param direction направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRay(origin:Vector3, direction:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * + * @param origin + * @param direction + * @param collisionGroup + * @param maxTime + * @param predicate + * @param result + * @return + */ + function intersectRayWithStatic(origin:Vector3, direction:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @return + */ + function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/collision/ICollisionPredicate.as b/0.0.9.1/src/alternativa/physics/collision/ICollisionPredicate.as new file mode 100644 index 0000000..33d30ca --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/ICollisionPredicate.as @@ -0,0 +1,8 @@ +package alternativa.physics.collision { + + public interface ICollisionPredicate { + + function considerCollision(primitive:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/collision/IRayCollisionPredicate.as b/0.0.9.1/src/alternativa/physics/collision/IRayCollisionPredicate.as new file mode 100644 index 0000000..b319f62 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/IRayCollisionPredicate.as @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.Body; + + public interface IRayCollisionPredicate { + function considerBody(body:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/collision/KdTreeCollisionDetector.as b/0.0.9.1/src/alternativa/physics/collision/KdTreeCollisionDetector.as new file mode 100644 index 0000000..9ca4c2a --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/KdTreeCollisionDetector.as @@ -0,0 +1,486 @@ +package alternativa.physics.collision { + + + 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.math.Vector3; + + use namespace altphysics; + + /** + * Детектор, хранящий статическую геометрию в kD-дереве и использующий дерево для ускорения тестов на пересечения. + */ + public class KdTreeCollisionDetector implements ICollisionDetector { + + altphysics var tree:CollisionKdTree; + altphysics var dynamicPrimitives:Vector.; + 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.(); + + 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.):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. = tree.staticChildren; +// var indices:Vector. = 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., indices:Vector., 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; + } \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/collision/colliders/.svn/all-wcprops b/0.0.9.1/src/alternativa/physics/collision/colliders/.svn/all-wcprops new file mode 100644 index 0000000..a2c4309 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/colliders/.svn/all-wcprops @@ -0,0 +1,53 @@ +K 25 +svn:wc:ra_dav:version-url +V 123 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/collision/colliders +END +BoxSphereCollider.as +K 25 +svn:wc:ra_dav:version-url +V 144 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/collision/colliders/BoxSphereCollider.as +END +BoxBoxCollider.as +K 25 +svn:wc:ra_dav:version-url +V 141 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/collision/colliders/BoxBoxCollider.as +END +BoxRectCollider.as +K 25 +svn:wc:ra_dav:version-url +V 142 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/collision/colliders/BoxRectCollider.as +END +SpherePlaneCollider.as +K 25 +svn:wc:ra_dav:version-url +V 146 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/collision/colliders/SpherePlaneCollider.as +END +BoxCollider.as +K 25 +svn:wc:ra_dav:version-url +V 138 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/collision/colliders/BoxCollider.as +END +BoxPlaneCollider.as +K 25 +svn:wc:ra_dav:version-url +V 143 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/collision/colliders/BoxPlaneCollider.as +END +SphereSphereCollider.as +K 25 +svn:wc:ra_dav:version-url +V 147 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/collision/colliders/SphereSphereCollider.as +END +BoxTriangleCollider.as +K 25 +svn:wc:ra_dav:version-url +V 146 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/collision/colliders/BoxTriangleCollider.as +END diff --git a/0.0.9.1/src/alternativa/physics/collision/colliders/.svn/entries b/0.0.9.1/src/alternativa/physics/collision/colliders/.svn/entries new file mode 100644 index 0000000..71f91c5 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/colliders/.svn/entries @@ -0,0 +1,124 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/collision/colliders +http://svndev.alternativaplatform.com + + + +2010-03-06T16:51:19.781174Z +29404 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +BoxSphereCollider.as +file + + + + +2010-10-28T04:32:43.000000Z +25170bcb9a1eec2a0941f4b50a06a554 +2009-12-08T15:53:11.094448Z +24506 +mike + +BoxBoxCollider.as +file + + + + +2010-10-28T04:32:43.000000Z +15da13b1383e9c1892edc205c8c055b0 +2010-02-12T12:34:25.192477Z +28424 +mike + +BoxRectCollider.as +file + + + + +2010-10-28T04:32:43.000000Z +4a35f94d3ef1af0a5b42e1a100580f6d +2010-03-06T16:51:19.781174Z +29404 +mike + +SpherePlaneCollider.as +file + + + + +2010-10-28T04:32:43.000000Z +67621ed47d7e90b72498acf2802c4e2d +2009-11-24T08:44:15.941480Z +23758 +mike + +BoxCollider.as +file + + + + +2010-10-28T04:32:43.000000Z +52e72ed30a24a3a43ce16a19930546b5 +2010-02-12T12:34:25.192477Z +28424 +mike + +BoxPlaneCollider.as +file + + + + +2010-10-28T04:32:43.000000Z +204c7b02a9a1913269c558658bad4262 +2010-02-12T12:34:25.192477Z +28424 +mike + +SphereSphereCollider.as +file + + + + +2010-10-28T04:32:43.000000Z +301c7102f91a7f3ede83e35ca202db0b +2009-11-24T08:44:15.941480Z +23758 +mike + +BoxTriangleCollider.as +file + + + + +2010-10-28T04:32:43.000000Z +448502271cac4f09491ca128b4bb485a +2010-03-06T16:51:19.781174Z +29404 +mike + diff --git a/0.0.9.1/src/alternativa/physics/collision/colliders/.svn/format b/0.0.9.1/src/alternativa/physics/collision/colliders/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/colliders/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.1/src/alternativa/physics/collision/colliders/.svn/text-base/BoxBoxCollider.as.svn-base b/0.0.9.1/src/alternativa/physics/collision/colliders/.svn/text-base/BoxBoxCollider.as.svn-base new file mode 100644 index 0000000..2d3f04c --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/colliders/.svn/text-base/BoxBoxCollider.as.svn-base @@ -0,0 +1,589 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * Расчитывает точки контакта двух боксов. Нормаль контакта направляется в сторону бокса с меньшим ID. + */ + public class BoxBoxCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + private var vectorToBox1:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var bestAxisIndex:int; + private var minOverlap:Number; + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + private var tmpPoints:Vector. = new Vector.(8, true); + private var pcount:int; + + /** + * + */ + public function BoxBoxCollider() { + for (var i:int = 0; i < 8; i++) { + tmpPoints[i] = new ContactPoint(); + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + + if (bestAxisIndex < 6) { + // Контакт грань-(грань|ребро|вершина) + if (!findFaceContactPoints(box1, box2, vectorToBox1, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 6; + findEdgesIntersection(box1, box2, vectorToBox1, int(bestAxisIndex/3), bestAxisIndex%3, contact); + } + contact.body1 = box1.body; + contact.body2 = box2.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + // Вектор из центра второго бокса в центр первого + vectorToBox1.x = transform1.d - transform2.d; + vectorToBox1.y = transform1.h - transform2.h; + vectorToBox1.z = transform1.l - transform2.l; + + // Проверка пересечения по основным осям + axis10.x = transform1.a; + axis10.y = transform1.e; + axis10.z = transform1.i; + if (!testMainAxis(box1, box2, axis10, 0, vectorToBox1)) return false; + axis11.x = transform1.b; + axis11.y = transform1.f; + axis11.z = transform1.j; + if (!testMainAxis(box1, box2, axis11, 1, vectorToBox1)) return false; + axis12.x = transform1.c; + axis12.y = transform1.g; + axis12.z = transform1.k; + if (!testMainAxis(box1, box2, axis12, 2, vectorToBox1)) return false; + + axis20.x = transform2.a; + axis20.y = transform2.e; + axis20.z = transform2.i; + if (!testMainAxis(box1, box2, axis20, 3, vectorToBox1)) return false; + axis21.x = transform2.b; + axis21.y = transform2.f; + axis21.z = transform2.j; + if (!testMainAxis(box1, box2, axis21, 4, vectorToBox1)) return false; + axis22.x = transform2.c; + axis22.y = transform2.g; + axis22.z = transform2.k; + if (!testMainAxis(box1, box2, axis22, 5, vectorToBox1)) return false; + + // Проверка производных осей + if (!testDerivedAxis(box1, box2, axis10, axis20, 6, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis21, 7, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis22, 8, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis11, axis20, 9, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis21, 10, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis22, 11, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis12, axis20, 12, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis21, 13, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis22, 14, vectorToBox1)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта грани одного бокса с гранью/ребром/вершиной другого. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param faceAxisIdx индекс оси первого бокса, перпендикулярной грани, с которой произошло столкновение + * @param contactInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var swapNormal:Boolean = false; + if (faceAxisIdx > 2) { + // Столкновение с гранью второго бокса. Для дальнейших расчётов боксы меняются местами, + // но нормаль контакта всё равно должна быть направлена в сторону первоначального box1 + var tmpBox:CollisionBox = box1; + box1 = box2; + box2 = tmpBox; + vectorToBox1.x = -vectorToBox1.x; + vectorToBox1.y = -vectorToBox1.y; + vectorToBox1.z = -vectorToBox1.z; + faceAxisIdx -= 3; + swapNormal = true; + } + + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + var colAxis:Vector3 = contact.normal; + + transform1.getAxis(faceAxisIdx, colAxis); + var negativeFace:Boolean = colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z > 0; + + var code:int = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box1.excludedFaces) != 0) return false; + + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + // Ищем ось второго бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + transform2.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisDot = dot; + incidentAxisIdx = axisIdx; + } + } + // Получаем список вершин грани второго бокса, переводим их в систему координат первого бокса и выполняем обрезку + // по грани первого бокса. Таким образом получается список потенциальных точек контакта. + transform2.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box2.hs, incidentAxisIdx, incidentAxisDot < 0, points1); + + // TODO: Вычислить результирующую матрицу, затем преобразовать векторы за один заход + transform2.transformVectorsN(points1, points2, 4); + transform1.transformVectorsInverseN(points2, points1, 4); + + var pnum:int = clip(box1.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points1[i]; + if ((pen = getPointBoxPenetration(box1.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + var cp:ContactPoint = tmpPoints[pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = transform1.a*v.x + transform1.b*v.y + transform1.c*v.z + transform1.d; + cpPos.y = transform1.e*v.x + transform1.f*v.y + transform1.g*v.z + transform1.h; + cpPos.z = transform1.i*v.x + transform1.j*v.y + transform1.k*v.z + transform1.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + + cp.penetration = pen; + } + } + if (swapNormal) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (pcount > 4) { + reducePoints(); + } + for (i = 0; i < pcount; i++) { + cp = contact.points[i]; + cp.copyFrom(tmpPoints[i]); + } + contact.pcount = pcount; + return true; + } + + /** + * + * @param contactInfo + */ + private function reducePoints():void { + var i:int; + var minIdx:int; + var cp1:ContactPoint; + var cp2:ContactPoint; + while (pcount > 4) { + var minLen:Number = 1e10; + var p1:Vector3 = (tmpPoints[int(pcount - 1)] as ContactPoint).pos; + var p2:Vector3; + for (i = 0; i < pcount; i++) { + p2 = (tmpPoints[i] as ContactPoint).pos; + var dx:Number = p2.x - p1.x; + var dy:Number = p2.y - p1.y; + var dz:Number = p2.z - p1.z; + var len:Number = dx*dx + dy*dy + dz*dz; + if (len < minLen) { + minLen = len; + minIdx = i; + } + p1 = p2; + } + var ii:int = minIdx == 0 ? (pcount - 1) : (minIdx - 1); + cp1 = tmpPoints[ii]; + cp2 = tmpPoints[minIdx]; + p1 = cp1.pos; + p2 = cp2.pos; + p2.x = 0.5*(p1.x + p2.x); + p2.y = 0.5*(p1.y + p2.y); + p2.z = 0.5*(p1.z + p2.z); + cp2.penetration = 0.5*(cp1.penetration + cp2.penetration); + if (minIdx > 0) { + for (i = minIdx; i < pcount; i++) { + tmpPoints[int(i - 1)] = tmpPoints[i]; + } + tmpPoints[int(pcount - 1)] = cp1; + } + pcount--; + } + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clip(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param contactInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):void { + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + transform1.getAxis(axisIdx1, axis10); + transform2.getAxis(axisIdx2, axis20); + + var colAxis:Vector3 = contact.normal; + + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону первого бокса + if (colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + + var tx:Number = box1.hs.x; + var ty:Number = box1.hs.y; + var tz:Number = box1.hs.z; + + var x2:Number = box2.hs.x; + var y2:Number = box2.hs.y; + var z2:Number = box2.hs.z; + // x + if (axisIdx1 == 0) { + tx = 0; + halfLen1 = box1.hs.x; + } else { + if (colAxis.x*transform1.a + colAxis.y*transform1.e + colAxis.z*transform1.i > 0) { + tx = -tx; + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = box2.hs.x; + } else { + if (colAxis.x*transform2.a + colAxis.y*transform2.e + colAxis.z*transform2.i < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + ty = 0; + halfLen1 = box1.hs.y; + } else { + if (colAxis.x*transform1.b + colAxis.y*transform1.f + colAxis.z*transform1.j > 0) { + ty = -ty; + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = box2.hs.y; + } else { + if (colAxis.x*transform2.b + colAxis.y*transform2.f + colAxis.z*transform2.j < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + tz = 0; + halfLen1 = box1.hs.z; + } else { + if (colAxis.x*transform1.c + colAxis.y*transform1.g + colAxis.z*transform1.k > 0) { + tz = -tz; + } + } + if (axisIdx2 == 2) { + z2 = 0; + halfLen2 = box2.hs.z; + } else { + if (colAxis.x*transform2.c + colAxis.y*transform2.g + colAxis.z*transform2.k < 0) { + z2 = -z2; + } + } + // Получаем глобальные координаты средних точек рёбер + var x1:Number = transform1.a*tx + transform1.b*ty + transform1.c*tz + transform1.d; + var y1:Number = transform1.e*tx + transform1.f*ty + transform1.g*tz + transform1.h; + var z1:Number = transform1.i*tx + transform1.j*ty + transform1.k*tz + transform1.l; + tx = x2; + ty = y2; + tz = z2; + x2 = transform2.a*tx + transform2.b*ty + transform2.c*tz + transform2.d; + y2 = transform2.e*tx + transform2.f*ty + transform2.g*tz + transform2.h; + z2 = transform2.i*tx + transform2.j*ty + transform2.k*tz + transform2.l; + + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + tx = x2 - x1; + ty = y2 - y1; + tz = z2 - z1; + var c1:Number = axis10.x*tx + axis10.y*ty + axis10.z*tz; + var c2:Number = axis20.x*tx + axis20.y*ty + axis20.z*tz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cp.pos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cp.pos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение боксов вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box1 + * @param box2 + * @param axis + * @param axisIndex + * @param toBox1 + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box1 + * @param box2 + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox1 + * @return + */ + private function testDerivedAxis(box1:CollisionBox, box2:CollisionBox, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия двух боксов вдоль заданной оси. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param axis ось + * @param vectorToBox1 вектор, соединяющий центр второго бокса с центром первого + * @return величина перекрытия боксов вдоль оси + */ + public function overlapOnAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, vectorToBox1:Vector3):Number { + var m:Matrix4 = box1.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box1.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box1.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box1.hs.z; + if (d < 0) d = -d; + projection += d; + + m = box2.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box2.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box2.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box2.hs.z; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox1.x*axis.x + vectorToBox1.y*axis.y + vectorToBox1.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} + import alternativa.math.Vector3; + + class CollisionPointTmp { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + } diff --git a/0.0.9.1/src/alternativa/physics/collision/colliders/.svn/text-base/BoxCollider.as.svn-base b/0.0.9.1/src/alternativa/physics/collision/colliders/.svn/text-base/BoxCollider.as.svn-base new file mode 100644 index 0000000..2429bb1 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/colliders/.svn/text-base/BoxCollider.as.svn-base @@ -0,0 +1,427 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + /** + * + */ + public class BoxCollider implements ICollider { + + /** + * + */ + public function BoxCollider() { + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + return false; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + protected function getFaceVertsByAxis(hs:Vector3, axisIdx:int, negativeFace:Boolean, result:Vector.):void { + var v:Vector3; + switch (axisIdx) { + case 0: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + } + break; + case 1: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + } + break; + case 2: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + } else { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } + break; + } + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/collision/colliders/.svn/text-base/BoxPlaneCollider.as.svn-base b/0.0.9.1/src/alternativa/physics/collision/colliders/.svn/text-base/BoxPlaneCollider.as.svn-base new file mode 100644 index 0000000..3bebdc3 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/colliders/.svn/text-base/BoxPlaneCollider.as.svn-base @@ -0,0 +1,88 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + public class BoxPlaneCollider implements ICollider { + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var normal:Vector3 = new Vector3(); + + public function BoxPlaneCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var box:RigidBox = body1 as RigidBox; +// var plane:RigidPlane; +// if (box == null) { +// box = body2 as RigidBox; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные координаты вершин бокса +// var sx:Number = box.halfSize.x; +// var sy:Number = box.halfSize.y; +// var sz:Number = box.halfSize.z; +// (verts1[0] as Vector3).reset(-sx, -sy, -sz); +// (verts1[1] as Vector3).reset(sx, -sy, -sz); +// (verts1[2] as Vector3).reset(sx, sy, -sz); +// (verts1[3] as Vector3).reset(-sx, sy, -sz); +// (verts1[4] as Vector3).reset(-sx, -sy, sz); +// (verts1[5] as Vector3).reset(sx, -sy, sz); +// (verts1[6] as Vector3).reset(sx, sy, sz); +// (verts1[7] as Vector3).reset(-sx, sy, sz); +// +// box.transform.transformVectors(verts1, verts2); +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// // Проверяем наличие столкновений с каждой вершиной +// collisionInfo.pcount = 0; +// for (var i:int = 0; i < 8; i++) { +// // Вершина добавляется в список точек столкновения, если лежит под плоскостью +// var dist:Number = (verts2[i] as Vector3).dot(normal); +// if (dist < offset) { +// var cp:ContactPoint; +// if (collisionInfo.pcount == collisionInfo.points.length) { +// cp = new ContactPoint(); +// collisionInfo.points[collisionInfo.pcount] = cp; +// } else { +// cp = collisionInfo.points[collisionInfo.pcount]; +// } +// cp.pos.copy(verts2[i]); +// cp.r1.diff(cp.pos, box.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); +// cp.penetration = offset - dist; +// collisionInfo.pcount++; +// } +// } +// if (collisionInfo.pcount > 0) { +// collisionInfo.body1 = box; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// return true; +// } + return false; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/collision/colliders/.svn/text-base/BoxRectCollider.as.svn-base b/0.0.9.1/src/alternativa/physics/collision/colliders/.svn/text-base/BoxRectCollider.as.svn-base new file mode 100644 index 0000000..d6ba302 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/colliders/.svn/text-base/BoxRectCollider.as.svn-base @@ -0,0 +1,575 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * + */ + public class BoxRectCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + + private var vectorToBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxRectCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, rect, vectorToBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + if (!findEdgesIntersection(box, rect, vectorToBox, int(bestAxisIndex/2), bestAxisIndex%2, contact)) { + return false; + } + } + contact.body1 = box.body; + contact.body2 = rect.body; + + // Хак для танков, чтобы исключить утыкания танков в стыки статических примитивов + if (rect.transform.k > 0.99999) { + contact.normal.x = 0; + contact.normal.y = 0; + contact.normal.z = 1; + } + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + // Вектор из центра прямоугольника в центр бокса + vectorToBox.x = boxTransform.d - rectTransform.d; + vectorToBox.y = boxTransform.h - rectTransform.h; + vectorToBox.z = boxTransform.l - rectTransform.l; + + // Проверка пересечения по нормали прямоугольника + rectTransform.getAxis(2, axis22); + if (!testMainAxis(box, rect, axis22, 0, vectorToBox)) return false; + + // Проверка пересечения по основным осям бокса + boxTransform.getAxis(0, axis10); + if (!testMainAxis(box, rect, axis10, 1, vectorToBox)) return false; + boxTransform.getAxis(1, axis11); + if (!testMainAxis(box, rect, axis11, 2, vectorToBox)) return false; + boxTransform.getAxis(2, axis12); + if (!testMainAxis(box, rect, axis12, 3, vectorToBox)) return false; + + // Получаем направляющие рёбер прямоугольника + rectTransform.getAxis(0, axis20); + rectTransform.getAxis(1, axis21); + + // Проверка производных осей + if (!testDerivedAxis(box, rect, axis10, axis20, 4, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis10, axis21, 5, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis11, axis20, 6, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis11, axis21, 7, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis12, axis20, 8, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis12, axis21, 9, vectorToBox)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта бокса с прямоугольником. + * + * @param box бокс + * @param rect прямоугольник + * @param vectorToBox вектор, направленный из центра прямоугольника в центр бокса + * @param faceAxisIdx индекс оси, идентифицирующей полскость столкновения (грань бокса или полскость прямоугольника) + * @param colInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var pnum:int; + var i:int; + var v:Vector3; + var cp:ContactPoint; + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + var colAxis:Vector3 = contact.normal; + + var negativeFace:Boolean; + var code:int; + + if (faceAxisIdx == 0) { + // Столкновение с плоскостью прямоугольника + + // Проверим положение бокса относительно плоскости прямоугольника + colAxis.x = rectTransform.c; + colAxis.y = rectTransform.g; + colAxis.z = rectTransform.k; + +// var offset:Number = colAxis.x*rectTransform.d + colAxis.y*rectTransform.h + colAxis.z*rectTransform.l; +// if (bbPos.vDot(colAxis) < offset) return false; + + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisIdx = axisIdx; + incidentAxisDot = dot; + } + } + negativeFace = incidentAxisDot > 0; + + code = 1 << (incidentAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + + // Получаем список вершин грани бокса, переводим их в систему координат прямоугольника и выполняем обрезку + // по прямоугольнику. Таким образом получается список потенциальных точек контакта. + boxTransform.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box.hs, incidentAxisIdx, negativeFace, points1); + boxTransform.transformVectorsN(points1, points2, 4); + rectTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByRect(rect.hs); + // Проверяем каждую потенциальную точку на принадлежность нижней полуплоскости прямоугольника и добавляем такие точки в список контактов + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if (v.z < epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = -v.z; + var cpPos:Vector3 = cp.pos; + cpPos.x = rectTransform.a*v.x + rectTransform.b*v.y + rectTransform.c*v.z + rectTransform.d; + cpPos.y = rectTransform.e*v.x + rectTransform.f*v.y + rectTransform.g*v.z + rectTransform.h; + cpPos.z = rectTransform.i*v.x + rectTransform.j*v.y + rectTransform.k*v.z + rectTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } else { + // Столкновение с гранью бокса + faceAxisIdx--; + boxTransform.getAxis(faceAxisIdx, colAxis); + negativeFace = colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z > 0; + + code = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) { + return false; + } + + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (rectTransform.c*colAxis.x + rectTransform.g*colAxis.y + rectTransform.k*colAxis.z < 0) return false; + + getFaceVertsByAxis(rect.hs, 2, false, points1); + rectTransform.transformVectorsN(points1, points2, 4); + boxTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if ((pen = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = pen; + cpPos = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } + return true; + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * + * @param hs + * @return + */ + private function clipByRect(hs:Vector3):int { + var pnum:int = 4; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box первый бокс + * @param rect второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param colInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + boxTransform.getAxis(axisIdx1, axis10); + rectTransform.getAxis(axisIdx2, axis20); + var colAxis:Vector3 = contact.normal; + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону бокса + if (colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + var vx:Number = box.hs.x; + var vy:Number = box.hs.y; + var vz:Number = box.hs.z; + var x2:Number = rect.hs.x; + var y2:Number = rect.hs.y; + var z2:Number = rect.hs.z; + // x + if (axisIdx1 == 0) { + vx = 0; + halfLen1 = box.hs.x; + } else { + if (boxTransform.a*colAxis.x + boxTransform.e*colAxis.y + boxTransform.i*colAxis.z > 0) { + vx = -vx; + if ((box.excludedFaces & 2) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 1) != 0) { + return false; + } + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = rect.hs.x; + } else { + if (rectTransform.a*colAxis.x + rectTransform.e*colAxis.y + rectTransform.i*colAxis.z < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + vy = 0; + halfLen1 = box.hs.y; + } else { + if (boxTransform.b*colAxis.x + boxTransform.f*colAxis.y + boxTransform.j*colAxis.z > 0) { + vy = -vy; + if ((box.excludedFaces & 8) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 4) != 0) { + return false; + } + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = rect.hs.y; + } else { + if (rectTransform.b*colAxis.x + rectTransform.f*colAxis.y + rectTransform.j*colAxis.z < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + vz = 0; + halfLen1 = box.hs.z; + } else { + if (boxTransform.c*colAxis.x + boxTransform.g*colAxis.y + boxTransform.k*colAxis.z > 0) { + vz = -vz; + if ((box.excludedFaces & 32) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 16) != 0) { + return false; + } + } + } + // Получаем глобальные координаты средних точек рёбер + + var x1:Number = boxTransform.a*vx + boxTransform.b*vy + boxTransform.c*vz + boxTransform.d; + var y1:Number = boxTransform.e*vx + boxTransform.f*vy + boxTransform.g*vz + boxTransform.h; + var z1:Number = boxTransform.i*vx + boxTransform.j*vy + boxTransform.k*vz + boxTransform.l; + vx = x2; + vy = y2; + vz = z2; + x2 = rectTransform.a*vx + rectTransform.b*vy + rectTransform.c*vz + rectTransform.d; + y2 = rectTransform.e*vx + rectTransform.f*vy + rectTransform.g*vz + rectTransform.h; + z2 = rectTransform.i*vx + rectTransform.j*vy + rectTransform.k*vz + rectTransform.l; + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + vx = x2 - x1; + vy = y2 - y1; + vz = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cpPos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cpPos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cpPos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var v:Vector3 = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + return true; + } + + /** + * Проверяет пересечение вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box + * @param rect + * @param axis + * @param axisIndex + * @param vectorToBox + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param rect + * @param axis1 + * @param axis2 + * @param axisIndex + * @param vectorToBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, rect:CollisionRect, axis1:Vector3, axis2:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия вдоль заданной оси. + * + * @param box бокс + * @param rect прямоугольник + * @param axis ось + * @param vectorToBox вектор, соединяющий центр прямоугольника с центром бокса + * @return величина перекрытия вдоль оси + */ + public function overlapOnAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, vectorToBox:Vector3):Number { + var m:Matrix4 = box.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box.hs.z; + if (d < 0) d = -d; + projection += d; + + m = rect.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*rect.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*rect.hs.y; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox.x*axis.x + vectorToBox.y*axis.y + vectorToBox.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} diff --git a/0.0.9.1/src/alternativa/physics/collision/colliders/.svn/text-base/BoxSphereCollider.as.svn-base b/0.0.9.1/src/alternativa/physics/collision/colliders/.svn/text-base/BoxSphereCollider.as.svn-base new file mode 100644 index 0000000..da5e36d --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/colliders/.svn/text-base/BoxSphereCollider.as.svn-base @@ -0,0 +1,168 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.math.Vector3; + use namespace altphysics; + /** + * + */ + public class BoxSphereCollider implements ICollider { + + private var center:Vector3 = new Vector3(); + private var closestPt:Vector3 = new Vector3(); + + private var bPos:Vector3 = new Vector3(); + private var sPos:Vector3 = new Vector3(); + + /** + * + */ + public function BoxSphereCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + // Зафиксированно столкновение + contact.body1 = sphere.body; + contact.body2 = box.body; + contact.normal.vCopy(closestPt).vTransformBy4(box.transform).vSubtract(sPos).vNormalize().vReverse(); + contact.pcount = 1; + + var cp:ContactPoint = contact.points[0]; + cp.penetration = sphere.r - Math.sqrt(distSqr); + cp.pos.vCopy(contact.normal).vScale(-sphere.r).vAdd(sPos); + cp.r1.vDiff(cp.pos, sPos); + cp.r2.vDiff(cp.pos, bPos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + + return true; + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/collision/colliders/.svn/text-base/BoxTriangleCollider.as.svn-base b/0.0.9.1/src/alternativa/physics/collision/colliders/.svn/text-base/BoxTriangleCollider.as.svn-base new file mode 100644 index 0000000..935525d --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/colliders/.svn/text-base/BoxTriangleCollider.as.svn-base @@ -0,0 +1,725 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionTriangle; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * + */ + public class BoxTriangleCollider extends BoxCollider { + + public var epsilon:Number = 0.001; + + private var bestAxisIndex:int; + private var minOverlap:Number; + private var toBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var colNormal:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxTriangleCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, tri, toBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + if (!findEdgesIntersection(box, tri, toBox, bestAxisIndex%3, int(bestAxisIndex/3), contact)) return false; + } + + contact.body1 = box.body; + contact.body2 = tri.body; + + // Хак для танков, чтобы исключить утыкания танков в стыки статических примитивов + if (tri.transform.k > 0.99999) { + contact.normal.x = 0; + contact.normal.y = 0; + contact.normal.z = 1; + } + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + toBox.x = boxTransform.d - triTransform.d; + toBox.y = boxTransform.h - triTransform.h; + toBox.z = boxTransform.l - triTransform.l; + + minOverlap = 1e308; + + // Сначала проверяется нормаль треугольника + axis.x = triTransform.c; + axis.y = triTransform.g; + axis.z = triTransform.k; + if (!testMainAxis(box, tri, axis, 0, toBox)) return false; + + // Проверка основных осей бокса + axis10.x = boxTransform.a; + axis10.y = boxTransform.e; + axis10.z = boxTransform.i; + if (!testMainAxis(box, tri, axis10, 1, toBox)) return false; + axis11.x = boxTransform.b; + axis11.y = boxTransform.f; + axis11.z = boxTransform.j; + if (!testMainAxis(box, tri, axis11, 2, toBox)) return false; + axis12.x = boxTransform.c; + axis12.y = boxTransform.g; + axis12.z = boxTransform.k; + if (!testMainAxis(box, tri, axis12, 3, toBox)) return false; + + // Проверка производных осей + // TODO: заменить вычисления векторных произведений инлайнами + var v:Vector3 = tri.e0; + axis20.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis20.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis20.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis20, 4, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis20, 5, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis20, 6, toBox)) return false; + + v = tri.e1; + axis21.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis21.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis21.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis21, 7, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis21, 8, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis21, 9, toBox)) return false; + + v = tri.e2; + axis22.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis22.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis22.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis22, 10, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis22, 11, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis22, 12, toBox)) return false; + + return true; + } + + /** + * Тестирует пересечение примитивов вдоль заданной оси. + * + * @param box бокс + * @param tri треугольник + * @param axis ось + * @param axisIndex индекс оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return true, если примитивы имеют перекрытие проекций вдоль указанной оси, иначе false + */ + private function testMainAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, axisIndex:int, toBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param tri + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, tri:CollisionTriangle, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Рассчитывает величину перекрытия проекций бокса и треугольника на заданную ось. + * + * @param box бокс + * @param tri треугольник + * @param axis единичный направляющий вектор оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return величина перекрытия. Положительное значение указывает на наличие перекрытия, нулевое или отрицательное значение указывает на отсутствие перекрытия. + */ + private function overlapOnAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, toBox:Vector3):Number { + var t:Matrix4 = box.transform; + var projection:Number = (t.a*axis.x + t.e*axis.y + t.i*axis.z)*box.hs.x; + if (projection < 0) projection = -projection; + var d:Number = (t.b*axis.x + t.f*axis.y + t.j*axis.z)*box.hs.y; + projection += d < 0 ? -d : d; + d = (t.c*axis.x + t.g*axis.y + t.k*axis.z)*box.hs.z; + projection += d < 0 ? -d : d; + + var vectorProjection:Number = toBox.x*axis.x + toBox.y*axis.y + toBox.z*axis.z; + // Для оптимизации ось преобразуется в систему треугольника, а не его вершины в мировую систему + t = tri.transform; + var ax:Number = t.a*axis.x + t.e*axis.y + t.i*axis.z; + var ay:Number = t.b*axis.x + t.f*axis.y + t.j*axis.z; + var az:Number = t.c*axis.x + t.g*axis.y + t.k*axis.z; + var max:Number = 0; + if (vectorProjection < 0) { + vectorProjection = -vectorProjection; + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d < max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d < max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d < max) max = d; + max = -max; + } else { + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d > max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d > max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d > max) max = d; + } + + return projection + max - vectorProjection; + } + + /** + * Определяет точки контакта бокса и треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function findFaceContactPoints(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIndex:int, contact:Contact):Boolean { + if (faceAxisIndex == 0) { + // Столкновение с плоскостью треугольника + return getBoxToTriContact(box, tri, toBox, contact); + } else { + // Столкновение с гранью бокса + return getTriToBoxContact(box, tri, toBox, faceAxisIndex, contact); + } + } + + /** + * Определяет точки контакта бокса с плоскостью треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getBoxToTriContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + colNormal.x = triTransform.c; + colNormal.y = triTransform.g; + colNormal.z = triTransform.k; + + var over:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + if (!over) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var incAxisDot:Number = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colNormal.x + axis.y*colNormal.y + axis.z*colNormal.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incAxisDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Обрезка грани + var negativeFace:Boolean = incAxisDot > 0; + + var code:int = 1 << (incFaceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + getFaceVertsByAxis(box.hs, incFaceAxisIdx, negativeFace, points1); + boxTransform.transformVectorsN(points1, points2, 4); + triTransform.transformVectorsInverseN(points2, points1, 4); + var pnum:int = clipByTriangle(tri); + // Среди конечного списка точек определяются лежащие под плоскостью треугольника + var cp:ContactPoint; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points2[i]; + if ((over && v.z < 0) || (!over && v.z > 0)) { + cp = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z + triTransform.d; + cpPos.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z + triTransform.h; + cpPos.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z + triTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = over ? -v.z : v.z; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Оперделяет точки контакта треугольника с гранью бокса. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getTriToBoxContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + faceAxisIdx--; + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + boxTransform.getAxis(faceAxisIdx, colNormal); + var negativeFace:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + + var code:int = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + if (!negativeFace) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + var v:Vector3 = points1[0]; + v.x = tri.v0.x; + v.y = tri.v0.y; + v.z = tri.v0.z; + + v = points1[1]; + v.x = tri.v1.x; + v.y = tri.v1.y; + v.z = tri.v1.z; + + v = points1[2]; + v.x = tri.v2.x; + v.y = tri.v2.y; + v.z = tri.v2.z; + + triTransform.transformVectorsN(points1, points2, 3); + boxTransform.transformVectorsInverseN(points2, points1, 3); + + var pnum:int = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность боксу и добавляем такие точки в список контактов + var penetration:Number; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + v = points1[i]; + penetration = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace); + if (penetration > -epsilon) { + var cp:ContactPoint = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = penetration; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Вычисляет величину проникновения точки в бокс. + * + * @param hs вектор половинных размеров бокса + * @param p точка в системе координат бокса + * @param axisIndex индекс оси + * @param negativeFace если true, проверяется нижняя грань + * @return величина проникновения точки в бокс + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, negativeFace:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (negativeFace) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (negativeFace) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (negativeFace) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, вершины которой заданы в списке points1. Результат сохраняется в этом же списке. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 3; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Обрезает грань прямоугольника сторонами треугольника. Обрезка выполняется в системе координат и в проекции на + * плоскость треугольника. Входные вершины грани в количестве четырёх штук должны находиться в списке points1. + * Конечные вершины сохраняются в списке points2. + * + * @param tri треугольник + * @return количество врешин в конечном списке + */ + private function clipByTriangle(tri:CollisionTriangle):int { + var vnum:int = 4; + vnum = clipByLine(tri.v0, tri.e0, points1, vnum, points2); + if (vnum == 0) return 0; + vnum = clipByLine(tri.v1, tri.e1, points2, vnum, points1); + if (vnum == 0) return 0; + return clipByLine(tri.v2, tri.e2, points1, vnum, points2);; + } + + /** + * Обрезает полигон указанной прямой. + * + * @param linePoint точка на прямой + * @param lineDir единичный направляющий вектор прямой + * @param verticesIn список вершин исходного полигона + * @param vnum количество вершин исходного полигона + * @param verticesOut список, куда будут записаны вершины конечного полигона + * @return количество вершин конечного полигона + */ + private function clipByLine(linePoint:Vector3, lineDir:Vector3, verticesIn:Vector., vnum:int, verticesOut:Vector.):int { + var nx:Number = -lineDir.y; + var ny:Number = lineDir.x; + var offset:Number = linePoint.x*nx + linePoint.y*ny; + var v1:Vector3 = verticesIn[int(vnum - 1)]; + var offset1:Number = v1.x*nx + v1.y*ny; + var t:Number; + var v:Vector3; + var num:int = 0; + for (var i:int = 0; i < vnum; i++) { + var v2:Vector3 = verticesIn[i]; + var offset2:Number = v2.x*nx + v2.y*ny; + if (offset1 < offset) { + // Первая точка ребра во внешней полуплоскости + if (offset2 > offset) { + // Вторая точка ребра во внутренней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } else { + // Первая точка ребра во внутренней полуплоскости. Добавляем её в конечный список. + v = verticesOut[num]; + v.x = v1.x; + v.y = v1.y; + v.z = v1.z; + num++; + if (offset2 < offset) { + // Вторая точка ребра во внешней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } + v1 = v2; + offset1 = offset2; + } + return num; + } + + /** + * + * @param box + * @param tri + * @param toBox + * @param boxAxisIdx + * @param triAxisIdx + * @param contact + */ + private function findEdgesIntersection(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, boxAxisIdx:int, triAxisIdx:int, contact:Contact):Boolean { + // Определение точки и направляющего вектора ребра треугольника + var tmpx1:Number; + var tmpy1:Number; + var tmpz1:Number; + var tmpx2:Number; + var tmpy2:Number; + var tmpz2:Number; + switch (triAxisIdx) { + case 0: + tmpx1 = tri.e0.x; + tmpy1 = tri.e0.y; + tmpz1 = tri.e0.z; + tmpx2 = tri.v0.x; + tmpy2 = tri.v0.y; + tmpz2 = tri.v0.z; + break; + case 1: + tmpx1 = tri.e1.x; + tmpy1 = tri.e1.y; + tmpz1 = tri.e1.z; + tmpx2 = tri.v1.x; + tmpy2 = tri.v1.y; + tmpz2 = tri.v1.z; + break; + case 2: + tmpx1 = tri.e2.x; + tmpy1 = tri.e2.y; + tmpz1 = tri.e2.z; + tmpx2 = tri.v2.x; + tmpy2 = tri.v2.y; + tmpz2 = tri.v2.z; + break; + } + var triTransform:Matrix4 = tri.transform; + axis20.x = triTransform.a*tmpx1 + triTransform.b*tmpy1 + triTransform.c*tmpz1; + axis20.y = triTransform.e*tmpx1 + triTransform.f*tmpy1 + triTransform.g*tmpz1; + axis20.z = triTransform.i*tmpx1 + triTransform.j*tmpy1 + triTransform.k*tmpz1; + var x2:Number = triTransform.a*tmpx2 + triTransform.b*tmpy2 + triTransform.c*tmpz2 + triTransform.d; + var y2:Number = triTransform.e*tmpx2 + triTransform.f*tmpy2 + triTransform.g*tmpz2 + triTransform.h; + var z2:Number = triTransform.i*tmpx2 + triTransform.j*tmpy2 + triTransform.k*tmpz2 + triTransform.l; + + // Определение нормали контакта, точки и направляющего вектора ребра бокса + var boxTransform:Matrix4 = box.transform; + boxTransform.getAxis(boxAxisIdx, axis10); + + // Нормаль контакта + var v:Vector3 = contact.normal; + v.x = axis10.y*axis20.z - axis10.z*axis20.y; + v.y = axis10.z*axis20.x - axis10.x*axis20.z; + v.z = axis10.x*axis20.y - axis10.y*axis20.x; + k = 1/Math.sqrt(v.x*v.x + v.y*v.y + v.z*v.z); + v.x *= k; + v.y *= k; + v.z *= k; + // Разворот нормали в сторону бокса + if (v.x*toBox.x + v.y*toBox.y + v.z*toBox.z < 0) { + v.x = -v.x; + v.y = -v.y; + v.z = -v.z; + } + + var boxHalfLen:Number; + tmpx1 = box.hs.x; + tmpy1 = box.hs.y; + tmpz1 = box.hs.z; + // X + if (boxAxisIdx == 0) { + tmpx1 = 0; + boxHalfLen = box.hs.x; + } else { + if (boxTransform.a*v.x + boxTransform.e*v.y + boxTransform.i*v.z > 0) { + tmpx1 = -tmpx1; + if ((box.excludedFaces & 2) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 1) != 0) { + return false; + } + } + } + // Y + if (boxAxisIdx == 1) { + tmpy1 = 0; + boxHalfLen = box.hs.y; + } else { + if (boxTransform.b*v.x + boxTransform.f*v.y + boxTransform.j*v.z > 0) { + tmpy1 = -tmpy1; + if ((box.excludedFaces & 8) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 4) != 0) { + return false; + } + } + } + // Z + if (boxAxisIdx == 2) { + tmpz1 = 0; + boxHalfLen = box.hs.z; + } else { + if (boxTransform.c*v.x + boxTransform.g*v.y + boxTransform.k*v.z > 0) { + tmpz1 = -tmpz1; + if ((box.excludedFaces & 32) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 16) != 0) { + return false; + } + } + } + var x1:Number = boxTransform.a*tmpx1 + boxTransform.b*tmpy1 + boxTransform.c*tmpz1 + boxTransform.d; + var y1:Number = boxTransform.e*tmpx1 + boxTransform.f*tmpy1 + boxTransform.g*tmpz1 + boxTransform.h; + var z1:Number = boxTransform.i*tmpx1 + boxTransform.j*tmpy1 + boxTransform.k*tmpz1 + boxTransform.l; + + // Находим точку пересечения рёбер, решая систему уравнений + // axis10 - направляющий вектор ребра бокса + // x1, y1, z1 - средняя точка ребра бокса + // axis20 - направляющий вектор ребра треугольника + // x2, y2, z2 - начальная точка ребра треугольника + var k:Number = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + var vx:Number = x2 - x1; + var vy:Number = y2 - y1; + var vz:Number = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + + // Запись данных о контакте + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + v = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + v.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + v.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + v.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = v.x - boxTransform.d; + r.y = v.y - boxTransform.h; + r.z = v.z - boxTransform.l; + r = cp.r2; + r.x = v.x - triTransform.d; + r.y = v.y - triTransform.h; + r.z = v.z - triTransform.l; + return true; + } + + } +} diff --git a/0.0.9.1/src/alternativa/physics/collision/colliders/.svn/text-base/SpherePlaneCollider.as.svn-base b/0.0.9.1/src/alternativa/physics/collision/colliders/.svn/text-base/SpherePlaneCollider.as.svn-base new file mode 100644 index 0000000..30cd55f --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/colliders/.svn/text-base/SpherePlaneCollider.as.svn-base @@ -0,0 +1,68 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + /** + * + */ + public class SpherePlaneCollider implements ICollider { + + private var normal:Vector3 = new Vector3(); + + /** + * + */ + public function SpherePlaneCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var sphere:RigidSphere = body1 as RigidSphere; +// var plane:RigidPlane; +// if (sphere == null) { +// sphere = body2 as RigidSphere; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// +// var dist:Number = sphere.state.pos.dot(normal) - offset; +// if (dist > sphere.r) return false; +// +// collisionInfo.body1 = sphere; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// collisionInfo.pcount = 1; +// +// var cp:ContactPoint = collisionInfo.points[0]; +// cp.penetration = sphere.r - dist; +// cp.pos.copy(normal).reverse().scale(sphere.r).add(sphere.state.pos); +// cp.r1.diff(cp.pos, sphere.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/collision/colliders/.svn/text-base/SphereSphereCollider.as.svn-base b/0.0.9.1/src/alternativa/physics/collision/colliders/.svn/text-base/SphereSphereCollider.as.svn-base new file mode 100644 index 0000000..3332f3c --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/colliders/.svn/text-base/SphereSphereCollider.as.svn-base @@ -0,0 +1,70 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.math.Vector3; + use namespace altphysics; + + public class SphereSphereCollider implements ICollider { + + private var p1:Vector3 = new Vector3(); + private var p2:Vector3 = new Vector3(); + + public function SphereSphereCollider() { + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var s1:CollisionSphere; + var s2:CollisionSphere; + if (prim1.body != null) { + s1 = prim1 as CollisionSphere; + s2 = prim2 as CollisionSphere; + } else { + s1 = prim2 as CollisionSphere; + s2 = prim1 as CollisionSphere; + } + + s1.transform.getAxis(3, p1); + s2.transform.getAxis(3, p2); + var dx:Number = p1.x - p2.x; + var dy:Number = p1.y - p2.y; + var dz:Number = p1.z - p2.z; + var len:Number = dx*dx + dy*dy + dz*dz; + var sum:Number = s1.r + s2.r; + if (len > sum*sum) return false; + len = Math.sqrt(len); + dx /= len; + dy /= len; + dz /= len; + + contact.body1 = s1.body; + contact.body2 = s2.body; + contact.normal.x = dx; + contact.normal.y = dy; + contact.normal.z = dz; + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = sum - len; + cp.pos.x = p1.x - dx*s1.r; + cp.pos.y = p1.y - dy*s1.r; + cp.pos.z = p1.z - dz*s1.r; + cp.r1.vDiff(cp.pos, p1); + cp.r2.vDiff(cp.pos, p2); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/collision/colliders/BoxBoxCollider.as b/0.0.9.1/src/alternativa/physics/collision/colliders/BoxBoxCollider.as new file mode 100644 index 0000000..2d3f04c --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/colliders/BoxBoxCollider.as @@ -0,0 +1,589 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * Расчитывает точки контакта двух боксов. Нормаль контакта направляется в сторону бокса с меньшим ID. + */ + public class BoxBoxCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + private var vectorToBox1:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var bestAxisIndex:int; + private var minOverlap:Number; + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + private var tmpPoints:Vector. = new Vector.(8, true); + private var pcount:int; + + /** + * + */ + public function BoxBoxCollider() { + for (var i:int = 0; i < 8; i++) { + tmpPoints[i] = new ContactPoint(); + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + + if (bestAxisIndex < 6) { + // Контакт грань-(грань|ребро|вершина) + if (!findFaceContactPoints(box1, box2, vectorToBox1, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 6; + findEdgesIntersection(box1, box2, vectorToBox1, int(bestAxisIndex/3), bestAxisIndex%3, contact); + } + contact.body1 = box1.body; + contact.body2 = box2.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + // Вектор из центра второго бокса в центр первого + vectorToBox1.x = transform1.d - transform2.d; + vectorToBox1.y = transform1.h - transform2.h; + vectorToBox1.z = transform1.l - transform2.l; + + // Проверка пересечения по основным осям + axis10.x = transform1.a; + axis10.y = transform1.e; + axis10.z = transform1.i; + if (!testMainAxis(box1, box2, axis10, 0, vectorToBox1)) return false; + axis11.x = transform1.b; + axis11.y = transform1.f; + axis11.z = transform1.j; + if (!testMainAxis(box1, box2, axis11, 1, vectorToBox1)) return false; + axis12.x = transform1.c; + axis12.y = transform1.g; + axis12.z = transform1.k; + if (!testMainAxis(box1, box2, axis12, 2, vectorToBox1)) return false; + + axis20.x = transform2.a; + axis20.y = transform2.e; + axis20.z = transform2.i; + if (!testMainAxis(box1, box2, axis20, 3, vectorToBox1)) return false; + axis21.x = transform2.b; + axis21.y = transform2.f; + axis21.z = transform2.j; + if (!testMainAxis(box1, box2, axis21, 4, vectorToBox1)) return false; + axis22.x = transform2.c; + axis22.y = transform2.g; + axis22.z = transform2.k; + if (!testMainAxis(box1, box2, axis22, 5, vectorToBox1)) return false; + + // Проверка производных осей + if (!testDerivedAxis(box1, box2, axis10, axis20, 6, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis21, 7, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis22, 8, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis11, axis20, 9, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis21, 10, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis22, 11, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis12, axis20, 12, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis21, 13, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis22, 14, vectorToBox1)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта грани одного бокса с гранью/ребром/вершиной другого. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param faceAxisIdx индекс оси первого бокса, перпендикулярной грани, с которой произошло столкновение + * @param contactInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var swapNormal:Boolean = false; + if (faceAxisIdx > 2) { + // Столкновение с гранью второго бокса. Для дальнейших расчётов боксы меняются местами, + // но нормаль контакта всё равно должна быть направлена в сторону первоначального box1 + var tmpBox:CollisionBox = box1; + box1 = box2; + box2 = tmpBox; + vectorToBox1.x = -vectorToBox1.x; + vectorToBox1.y = -vectorToBox1.y; + vectorToBox1.z = -vectorToBox1.z; + faceAxisIdx -= 3; + swapNormal = true; + } + + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + var colAxis:Vector3 = contact.normal; + + transform1.getAxis(faceAxisIdx, colAxis); + var negativeFace:Boolean = colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z > 0; + + var code:int = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box1.excludedFaces) != 0) return false; + + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + // Ищем ось второго бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + transform2.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisDot = dot; + incidentAxisIdx = axisIdx; + } + } + // Получаем список вершин грани второго бокса, переводим их в систему координат первого бокса и выполняем обрезку + // по грани первого бокса. Таким образом получается список потенциальных точек контакта. + transform2.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box2.hs, incidentAxisIdx, incidentAxisDot < 0, points1); + + // TODO: Вычислить результирующую матрицу, затем преобразовать векторы за один заход + transform2.transformVectorsN(points1, points2, 4); + transform1.transformVectorsInverseN(points2, points1, 4); + + var pnum:int = clip(box1.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points1[i]; + if ((pen = getPointBoxPenetration(box1.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + var cp:ContactPoint = tmpPoints[pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = transform1.a*v.x + transform1.b*v.y + transform1.c*v.z + transform1.d; + cpPos.y = transform1.e*v.x + transform1.f*v.y + transform1.g*v.z + transform1.h; + cpPos.z = transform1.i*v.x + transform1.j*v.y + transform1.k*v.z + transform1.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + + cp.penetration = pen; + } + } + if (swapNormal) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (pcount > 4) { + reducePoints(); + } + for (i = 0; i < pcount; i++) { + cp = contact.points[i]; + cp.copyFrom(tmpPoints[i]); + } + contact.pcount = pcount; + return true; + } + + /** + * + * @param contactInfo + */ + private function reducePoints():void { + var i:int; + var minIdx:int; + var cp1:ContactPoint; + var cp2:ContactPoint; + while (pcount > 4) { + var minLen:Number = 1e10; + var p1:Vector3 = (tmpPoints[int(pcount - 1)] as ContactPoint).pos; + var p2:Vector3; + for (i = 0; i < pcount; i++) { + p2 = (tmpPoints[i] as ContactPoint).pos; + var dx:Number = p2.x - p1.x; + var dy:Number = p2.y - p1.y; + var dz:Number = p2.z - p1.z; + var len:Number = dx*dx + dy*dy + dz*dz; + if (len < minLen) { + minLen = len; + minIdx = i; + } + p1 = p2; + } + var ii:int = minIdx == 0 ? (pcount - 1) : (minIdx - 1); + cp1 = tmpPoints[ii]; + cp2 = tmpPoints[minIdx]; + p1 = cp1.pos; + p2 = cp2.pos; + p2.x = 0.5*(p1.x + p2.x); + p2.y = 0.5*(p1.y + p2.y); + p2.z = 0.5*(p1.z + p2.z); + cp2.penetration = 0.5*(cp1.penetration + cp2.penetration); + if (minIdx > 0) { + for (i = minIdx; i < pcount; i++) { + tmpPoints[int(i - 1)] = tmpPoints[i]; + } + tmpPoints[int(pcount - 1)] = cp1; + } + pcount--; + } + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clip(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param contactInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):void { + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + transform1.getAxis(axisIdx1, axis10); + transform2.getAxis(axisIdx2, axis20); + + var colAxis:Vector3 = contact.normal; + + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону первого бокса + if (colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + + var tx:Number = box1.hs.x; + var ty:Number = box1.hs.y; + var tz:Number = box1.hs.z; + + var x2:Number = box2.hs.x; + var y2:Number = box2.hs.y; + var z2:Number = box2.hs.z; + // x + if (axisIdx1 == 0) { + tx = 0; + halfLen1 = box1.hs.x; + } else { + if (colAxis.x*transform1.a + colAxis.y*transform1.e + colAxis.z*transform1.i > 0) { + tx = -tx; + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = box2.hs.x; + } else { + if (colAxis.x*transform2.a + colAxis.y*transform2.e + colAxis.z*transform2.i < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + ty = 0; + halfLen1 = box1.hs.y; + } else { + if (colAxis.x*transform1.b + colAxis.y*transform1.f + colAxis.z*transform1.j > 0) { + ty = -ty; + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = box2.hs.y; + } else { + if (colAxis.x*transform2.b + colAxis.y*transform2.f + colAxis.z*transform2.j < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + tz = 0; + halfLen1 = box1.hs.z; + } else { + if (colAxis.x*transform1.c + colAxis.y*transform1.g + colAxis.z*transform1.k > 0) { + tz = -tz; + } + } + if (axisIdx2 == 2) { + z2 = 0; + halfLen2 = box2.hs.z; + } else { + if (colAxis.x*transform2.c + colAxis.y*transform2.g + colAxis.z*transform2.k < 0) { + z2 = -z2; + } + } + // Получаем глобальные координаты средних точек рёбер + var x1:Number = transform1.a*tx + transform1.b*ty + transform1.c*tz + transform1.d; + var y1:Number = transform1.e*tx + transform1.f*ty + transform1.g*tz + transform1.h; + var z1:Number = transform1.i*tx + transform1.j*ty + transform1.k*tz + transform1.l; + tx = x2; + ty = y2; + tz = z2; + x2 = transform2.a*tx + transform2.b*ty + transform2.c*tz + transform2.d; + y2 = transform2.e*tx + transform2.f*ty + transform2.g*tz + transform2.h; + z2 = transform2.i*tx + transform2.j*ty + transform2.k*tz + transform2.l; + + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + tx = x2 - x1; + ty = y2 - y1; + tz = z2 - z1; + var c1:Number = axis10.x*tx + axis10.y*ty + axis10.z*tz; + var c2:Number = axis20.x*tx + axis20.y*ty + axis20.z*tz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cp.pos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cp.pos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение боксов вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box1 + * @param box2 + * @param axis + * @param axisIndex + * @param toBox1 + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box1 + * @param box2 + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox1 + * @return + */ + private function testDerivedAxis(box1:CollisionBox, box2:CollisionBox, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия двух боксов вдоль заданной оси. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param axis ось + * @param vectorToBox1 вектор, соединяющий центр второго бокса с центром первого + * @return величина перекрытия боксов вдоль оси + */ + public function overlapOnAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, vectorToBox1:Vector3):Number { + var m:Matrix4 = box1.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box1.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box1.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box1.hs.z; + if (d < 0) d = -d; + projection += d; + + m = box2.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box2.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box2.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box2.hs.z; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox1.x*axis.x + vectorToBox1.y*axis.y + vectorToBox1.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} + import alternativa.math.Vector3; + + class CollisionPointTmp { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + } diff --git a/0.0.9.1/src/alternativa/physics/collision/colliders/BoxCollider.as b/0.0.9.1/src/alternativa/physics/collision/colliders/BoxCollider.as new file mode 100644 index 0000000..2429bb1 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/colliders/BoxCollider.as @@ -0,0 +1,427 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + /** + * + */ + public class BoxCollider implements ICollider { + + /** + * + */ + public function BoxCollider() { + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + return false; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + protected function getFaceVertsByAxis(hs:Vector3, axisIdx:int, negativeFace:Boolean, result:Vector.):void { + var v:Vector3; + switch (axisIdx) { + case 0: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + } + break; + case 1: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + } + break; + case 2: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + } else { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } + break; + } + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/collision/colliders/BoxPlaneCollider.as b/0.0.9.1/src/alternativa/physics/collision/colliders/BoxPlaneCollider.as new file mode 100644 index 0000000..3bebdc3 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/colliders/BoxPlaneCollider.as @@ -0,0 +1,88 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + public class BoxPlaneCollider implements ICollider { + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var normal:Vector3 = new Vector3(); + + public function BoxPlaneCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var box:RigidBox = body1 as RigidBox; +// var plane:RigidPlane; +// if (box == null) { +// box = body2 as RigidBox; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные координаты вершин бокса +// var sx:Number = box.halfSize.x; +// var sy:Number = box.halfSize.y; +// var sz:Number = box.halfSize.z; +// (verts1[0] as Vector3).reset(-sx, -sy, -sz); +// (verts1[1] as Vector3).reset(sx, -sy, -sz); +// (verts1[2] as Vector3).reset(sx, sy, -sz); +// (verts1[3] as Vector3).reset(-sx, sy, -sz); +// (verts1[4] as Vector3).reset(-sx, -sy, sz); +// (verts1[5] as Vector3).reset(sx, -sy, sz); +// (verts1[6] as Vector3).reset(sx, sy, sz); +// (verts1[7] as Vector3).reset(-sx, sy, sz); +// +// box.transform.transformVectors(verts1, verts2); +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// // Проверяем наличие столкновений с каждой вершиной +// collisionInfo.pcount = 0; +// for (var i:int = 0; i < 8; i++) { +// // Вершина добавляется в список точек столкновения, если лежит под плоскостью +// var dist:Number = (verts2[i] as Vector3).dot(normal); +// if (dist < offset) { +// var cp:ContactPoint; +// if (collisionInfo.pcount == collisionInfo.points.length) { +// cp = new ContactPoint(); +// collisionInfo.points[collisionInfo.pcount] = cp; +// } else { +// cp = collisionInfo.points[collisionInfo.pcount]; +// } +// cp.pos.copy(verts2[i]); +// cp.r1.diff(cp.pos, box.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); +// cp.penetration = offset - dist; +// collisionInfo.pcount++; +// } +// } +// if (collisionInfo.pcount > 0) { +// collisionInfo.body1 = box; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// return true; +// } + return false; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/collision/colliders/BoxRectCollider.as b/0.0.9.1/src/alternativa/physics/collision/colliders/BoxRectCollider.as new file mode 100644 index 0000000..d6ba302 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/colliders/BoxRectCollider.as @@ -0,0 +1,575 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * + */ + public class BoxRectCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + + private var vectorToBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxRectCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, rect, vectorToBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + if (!findEdgesIntersection(box, rect, vectorToBox, int(bestAxisIndex/2), bestAxisIndex%2, contact)) { + return false; + } + } + contact.body1 = box.body; + contact.body2 = rect.body; + + // Хак для танков, чтобы исключить утыкания танков в стыки статических примитивов + if (rect.transform.k > 0.99999) { + contact.normal.x = 0; + contact.normal.y = 0; + contact.normal.z = 1; + } + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + // Вектор из центра прямоугольника в центр бокса + vectorToBox.x = boxTransform.d - rectTransform.d; + vectorToBox.y = boxTransform.h - rectTransform.h; + vectorToBox.z = boxTransform.l - rectTransform.l; + + // Проверка пересечения по нормали прямоугольника + rectTransform.getAxis(2, axis22); + if (!testMainAxis(box, rect, axis22, 0, vectorToBox)) return false; + + // Проверка пересечения по основным осям бокса + boxTransform.getAxis(0, axis10); + if (!testMainAxis(box, rect, axis10, 1, vectorToBox)) return false; + boxTransform.getAxis(1, axis11); + if (!testMainAxis(box, rect, axis11, 2, vectorToBox)) return false; + boxTransform.getAxis(2, axis12); + if (!testMainAxis(box, rect, axis12, 3, vectorToBox)) return false; + + // Получаем направляющие рёбер прямоугольника + rectTransform.getAxis(0, axis20); + rectTransform.getAxis(1, axis21); + + // Проверка производных осей + if (!testDerivedAxis(box, rect, axis10, axis20, 4, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis10, axis21, 5, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis11, axis20, 6, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis11, axis21, 7, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis12, axis20, 8, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis12, axis21, 9, vectorToBox)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта бокса с прямоугольником. + * + * @param box бокс + * @param rect прямоугольник + * @param vectorToBox вектор, направленный из центра прямоугольника в центр бокса + * @param faceAxisIdx индекс оси, идентифицирующей полскость столкновения (грань бокса или полскость прямоугольника) + * @param colInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var pnum:int; + var i:int; + var v:Vector3; + var cp:ContactPoint; + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + var colAxis:Vector3 = contact.normal; + + var negativeFace:Boolean; + var code:int; + + if (faceAxisIdx == 0) { + // Столкновение с плоскостью прямоугольника + + // Проверим положение бокса относительно плоскости прямоугольника + colAxis.x = rectTransform.c; + colAxis.y = rectTransform.g; + colAxis.z = rectTransform.k; + +// var offset:Number = colAxis.x*rectTransform.d + colAxis.y*rectTransform.h + colAxis.z*rectTransform.l; +// if (bbPos.vDot(colAxis) < offset) return false; + + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisIdx = axisIdx; + incidentAxisDot = dot; + } + } + negativeFace = incidentAxisDot > 0; + + code = 1 << (incidentAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + + // Получаем список вершин грани бокса, переводим их в систему координат прямоугольника и выполняем обрезку + // по прямоугольнику. Таким образом получается список потенциальных точек контакта. + boxTransform.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box.hs, incidentAxisIdx, negativeFace, points1); + boxTransform.transformVectorsN(points1, points2, 4); + rectTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByRect(rect.hs); + // Проверяем каждую потенциальную точку на принадлежность нижней полуплоскости прямоугольника и добавляем такие точки в список контактов + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if (v.z < epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = -v.z; + var cpPos:Vector3 = cp.pos; + cpPos.x = rectTransform.a*v.x + rectTransform.b*v.y + rectTransform.c*v.z + rectTransform.d; + cpPos.y = rectTransform.e*v.x + rectTransform.f*v.y + rectTransform.g*v.z + rectTransform.h; + cpPos.z = rectTransform.i*v.x + rectTransform.j*v.y + rectTransform.k*v.z + rectTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } else { + // Столкновение с гранью бокса + faceAxisIdx--; + boxTransform.getAxis(faceAxisIdx, colAxis); + negativeFace = colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z > 0; + + code = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) { + return false; + } + + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (rectTransform.c*colAxis.x + rectTransform.g*colAxis.y + rectTransform.k*colAxis.z < 0) return false; + + getFaceVertsByAxis(rect.hs, 2, false, points1); + rectTransform.transformVectorsN(points1, points2, 4); + boxTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if ((pen = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = pen; + cpPos = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } + return true; + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * + * @param hs + * @return + */ + private function clipByRect(hs:Vector3):int { + var pnum:int = 4; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box первый бокс + * @param rect второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param colInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + boxTransform.getAxis(axisIdx1, axis10); + rectTransform.getAxis(axisIdx2, axis20); + var colAxis:Vector3 = contact.normal; + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону бокса + if (colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + var vx:Number = box.hs.x; + var vy:Number = box.hs.y; + var vz:Number = box.hs.z; + var x2:Number = rect.hs.x; + var y2:Number = rect.hs.y; + var z2:Number = rect.hs.z; + // x + if (axisIdx1 == 0) { + vx = 0; + halfLen1 = box.hs.x; + } else { + if (boxTransform.a*colAxis.x + boxTransform.e*colAxis.y + boxTransform.i*colAxis.z > 0) { + vx = -vx; + if ((box.excludedFaces & 2) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 1) != 0) { + return false; + } + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = rect.hs.x; + } else { + if (rectTransform.a*colAxis.x + rectTransform.e*colAxis.y + rectTransform.i*colAxis.z < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + vy = 0; + halfLen1 = box.hs.y; + } else { + if (boxTransform.b*colAxis.x + boxTransform.f*colAxis.y + boxTransform.j*colAxis.z > 0) { + vy = -vy; + if ((box.excludedFaces & 8) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 4) != 0) { + return false; + } + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = rect.hs.y; + } else { + if (rectTransform.b*colAxis.x + rectTransform.f*colAxis.y + rectTransform.j*colAxis.z < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + vz = 0; + halfLen1 = box.hs.z; + } else { + if (boxTransform.c*colAxis.x + boxTransform.g*colAxis.y + boxTransform.k*colAxis.z > 0) { + vz = -vz; + if ((box.excludedFaces & 32) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 16) != 0) { + return false; + } + } + } + // Получаем глобальные координаты средних точек рёбер + + var x1:Number = boxTransform.a*vx + boxTransform.b*vy + boxTransform.c*vz + boxTransform.d; + var y1:Number = boxTransform.e*vx + boxTransform.f*vy + boxTransform.g*vz + boxTransform.h; + var z1:Number = boxTransform.i*vx + boxTransform.j*vy + boxTransform.k*vz + boxTransform.l; + vx = x2; + vy = y2; + vz = z2; + x2 = rectTransform.a*vx + rectTransform.b*vy + rectTransform.c*vz + rectTransform.d; + y2 = rectTransform.e*vx + rectTransform.f*vy + rectTransform.g*vz + rectTransform.h; + z2 = rectTransform.i*vx + rectTransform.j*vy + rectTransform.k*vz + rectTransform.l; + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + vx = x2 - x1; + vy = y2 - y1; + vz = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cpPos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cpPos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cpPos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var v:Vector3 = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + return true; + } + + /** + * Проверяет пересечение вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box + * @param rect + * @param axis + * @param axisIndex + * @param vectorToBox + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param rect + * @param axis1 + * @param axis2 + * @param axisIndex + * @param vectorToBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, rect:CollisionRect, axis1:Vector3, axis2:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия вдоль заданной оси. + * + * @param box бокс + * @param rect прямоугольник + * @param axis ось + * @param vectorToBox вектор, соединяющий центр прямоугольника с центром бокса + * @return величина перекрытия вдоль оси + */ + public function overlapOnAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, vectorToBox:Vector3):Number { + var m:Matrix4 = box.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box.hs.z; + if (d < 0) d = -d; + projection += d; + + m = rect.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*rect.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*rect.hs.y; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox.x*axis.x + vectorToBox.y*axis.y + vectorToBox.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} diff --git a/0.0.9.1/src/alternativa/physics/collision/colliders/BoxSphereCollider.as b/0.0.9.1/src/alternativa/physics/collision/colliders/BoxSphereCollider.as new file mode 100644 index 0000000..da5e36d --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/colliders/BoxSphereCollider.as @@ -0,0 +1,168 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.math.Vector3; + use namespace altphysics; + /** + * + */ + public class BoxSphereCollider implements ICollider { + + private var center:Vector3 = new Vector3(); + private var closestPt:Vector3 = new Vector3(); + + private var bPos:Vector3 = new Vector3(); + private var sPos:Vector3 = new Vector3(); + + /** + * + */ + public function BoxSphereCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + // Зафиксированно столкновение + contact.body1 = sphere.body; + contact.body2 = box.body; + contact.normal.vCopy(closestPt).vTransformBy4(box.transform).vSubtract(sPos).vNormalize().vReverse(); + contact.pcount = 1; + + var cp:ContactPoint = contact.points[0]; + cp.penetration = sphere.r - Math.sqrt(distSqr); + cp.pos.vCopy(contact.normal).vScale(-sphere.r).vAdd(sPos); + cp.r1.vDiff(cp.pos, sPos); + cp.r2.vDiff(cp.pos, bPos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + + return true; + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/collision/colliders/BoxTriangleCollider.as b/0.0.9.1/src/alternativa/physics/collision/colliders/BoxTriangleCollider.as new file mode 100644 index 0000000..935525d --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/colliders/BoxTriangleCollider.as @@ -0,0 +1,725 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionTriangle; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * + */ + public class BoxTriangleCollider extends BoxCollider { + + public var epsilon:Number = 0.001; + + private var bestAxisIndex:int; + private var minOverlap:Number; + private var toBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var colNormal:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxTriangleCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, tri, toBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + if (!findEdgesIntersection(box, tri, toBox, bestAxisIndex%3, int(bestAxisIndex/3), contact)) return false; + } + + contact.body1 = box.body; + contact.body2 = tri.body; + + // Хак для танков, чтобы исключить утыкания танков в стыки статических примитивов + if (tri.transform.k > 0.99999) { + contact.normal.x = 0; + contact.normal.y = 0; + contact.normal.z = 1; + } + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + toBox.x = boxTransform.d - triTransform.d; + toBox.y = boxTransform.h - triTransform.h; + toBox.z = boxTransform.l - triTransform.l; + + minOverlap = 1e308; + + // Сначала проверяется нормаль треугольника + axis.x = triTransform.c; + axis.y = triTransform.g; + axis.z = triTransform.k; + if (!testMainAxis(box, tri, axis, 0, toBox)) return false; + + // Проверка основных осей бокса + axis10.x = boxTransform.a; + axis10.y = boxTransform.e; + axis10.z = boxTransform.i; + if (!testMainAxis(box, tri, axis10, 1, toBox)) return false; + axis11.x = boxTransform.b; + axis11.y = boxTransform.f; + axis11.z = boxTransform.j; + if (!testMainAxis(box, tri, axis11, 2, toBox)) return false; + axis12.x = boxTransform.c; + axis12.y = boxTransform.g; + axis12.z = boxTransform.k; + if (!testMainAxis(box, tri, axis12, 3, toBox)) return false; + + // Проверка производных осей + // TODO: заменить вычисления векторных произведений инлайнами + var v:Vector3 = tri.e0; + axis20.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis20.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis20.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis20, 4, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis20, 5, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis20, 6, toBox)) return false; + + v = tri.e1; + axis21.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis21.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis21.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis21, 7, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis21, 8, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis21, 9, toBox)) return false; + + v = tri.e2; + axis22.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis22.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis22.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis22, 10, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis22, 11, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis22, 12, toBox)) return false; + + return true; + } + + /** + * Тестирует пересечение примитивов вдоль заданной оси. + * + * @param box бокс + * @param tri треугольник + * @param axis ось + * @param axisIndex индекс оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return true, если примитивы имеют перекрытие проекций вдоль указанной оси, иначе false + */ + private function testMainAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, axisIndex:int, toBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param tri + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, tri:CollisionTriangle, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Рассчитывает величину перекрытия проекций бокса и треугольника на заданную ось. + * + * @param box бокс + * @param tri треугольник + * @param axis единичный направляющий вектор оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return величина перекрытия. Положительное значение указывает на наличие перекрытия, нулевое или отрицательное значение указывает на отсутствие перекрытия. + */ + private function overlapOnAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, toBox:Vector3):Number { + var t:Matrix4 = box.transform; + var projection:Number = (t.a*axis.x + t.e*axis.y + t.i*axis.z)*box.hs.x; + if (projection < 0) projection = -projection; + var d:Number = (t.b*axis.x + t.f*axis.y + t.j*axis.z)*box.hs.y; + projection += d < 0 ? -d : d; + d = (t.c*axis.x + t.g*axis.y + t.k*axis.z)*box.hs.z; + projection += d < 0 ? -d : d; + + var vectorProjection:Number = toBox.x*axis.x + toBox.y*axis.y + toBox.z*axis.z; + // Для оптимизации ось преобразуется в систему треугольника, а не его вершины в мировую систему + t = tri.transform; + var ax:Number = t.a*axis.x + t.e*axis.y + t.i*axis.z; + var ay:Number = t.b*axis.x + t.f*axis.y + t.j*axis.z; + var az:Number = t.c*axis.x + t.g*axis.y + t.k*axis.z; + var max:Number = 0; + if (vectorProjection < 0) { + vectorProjection = -vectorProjection; + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d < max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d < max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d < max) max = d; + max = -max; + } else { + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d > max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d > max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d > max) max = d; + } + + return projection + max - vectorProjection; + } + + /** + * Определяет точки контакта бокса и треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function findFaceContactPoints(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIndex:int, contact:Contact):Boolean { + if (faceAxisIndex == 0) { + // Столкновение с плоскостью треугольника + return getBoxToTriContact(box, tri, toBox, contact); + } else { + // Столкновение с гранью бокса + return getTriToBoxContact(box, tri, toBox, faceAxisIndex, contact); + } + } + + /** + * Определяет точки контакта бокса с плоскостью треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getBoxToTriContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + colNormal.x = triTransform.c; + colNormal.y = triTransform.g; + colNormal.z = triTransform.k; + + var over:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + if (!over) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var incAxisDot:Number = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colNormal.x + axis.y*colNormal.y + axis.z*colNormal.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incAxisDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Обрезка грани + var negativeFace:Boolean = incAxisDot > 0; + + var code:int = 1 << (incFaceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + getFaceVertsByAxis(box.hs, incFaceAxisIdx, negativeFace, points1); + boxTransform.transformVectorsN(points1, points2, 4); + triTransform.transformVectorsInverseN(points2, points1, 4); + var pnum:int = clipByTriangle(tri); + // Среди конечного списка точек определяются лежащие под плоскостью треугольника + var cp:ContactPoint; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points2[i]; + if ((over && v.z < 0) || (!over && v.z > 0)) { + cp = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z + triTransform.d; + cpPos.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z + triTransform.h; + cpPos.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z + triTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = over ? -v.z : v.z; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Оперделяет точки контакта треугольника с гранью бокса. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getTriToBoxContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + faceAxisIdx--; + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + boxTransform.getAxis(faceAxisIdx, colNormal); + var negativeFace:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + + var code:int = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + if (!negativeFace) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + var v:Vector3 = points1[0]; + v.x = tri.v0.x; + v.y = tri.v0.y; + v.z = tri.v0.z; + + v = points1[1]; + v.x = tri.v1.x; + v.y = tri.v1.y; + v.z = tri.v1.z; + + v = points1[2]; + v.x = tri.v2.x; + v.y = tri.v2.y; + v.z = tri.v2.z; + + triTransform.transformVectorsN(points1, points2, 3); + boxTransform.transformVectorsInverseN(points2, points1, 3); + + var pnum:int = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность боксу и добавляем такие точки в список контактов + var penetration:Number; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + v = points1[i]; + penetration = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace); + if (penetration > -epsilon) { + var cp:ContactPoint = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = penetration; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Вычисляет величину проникновения точки в бокс. + * + * @param hs вектор половинных размеров бокса + * @param p точка в системе координат бокса + * @param axisIndex индекс оси + * @param negativeFace если true, проверяется нижняя грань + * @return величина проникновения точки в бокс + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, negativeFace:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (negativeFace) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (negativeFace) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (negativeFace) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, вершины которой заданы в списке points1. Результат сохраняется в этом же списке. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 3; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Обрезает грань прямоугольника сторонами треугольника. Обрезка выполняется в системе координат и в проекции на + * плоскость треугольника. Входные вершины грани в количестве четырёх штук должны находиться в списке points1. + * Конечные вершины сохраняются в списке points2. + * + * @param tri треугольник + * @return количество врешин в конечном списке + */ + private function clipByTriangle(tri:CollisionTriangle):int { + var vnum:int = 4; + vnum = clipByLine(tri.v0, tri.e0, points1, vnum, points2); + if (vnum == 0) return 0; + vnum = clipByLine(tri.v1, tri.e1, points2, vnum, points1); + if (vnum == 0) return 0; + return clipByLine(tri.v2, tri.e2, points1, vnum, points2);; + } + + /** + * Обрезает полигон указанной прямой. + * + * @param linePoint точка на прямой + * @param lineDir единичный направляющий вектор прямой + * @param verticesIn список вершин исходного полигона + * @param vnum количество вершин исходного полигона + * @param verticesOut список, куда будут записаны вершины конечного полигона + * @return количество вершин конечного полигона + */ + private function clipByLine(linePoint:Vector3, lineDir:Vector3, verticesIn:Vector., vnum:int, verticesOut:Vector.):int { + var nx:Number = -lineDir.y; + var ny:Number = lineDir.x; + var offset:Number = linePoint.x*nx + linePoint.y*ny; + var v1:Vector3 = verticesIn[int(vnum - 1)]; + var offset1:Number = v1.x*nx + v1.y*ny; + var t:Number; + var v:Vector3; + var num:int = 0; + for (var i:int = 0; i < vnum; i++) { + var v2:Vector3 = verticesIn[i]; + var offset2:Number = v2.x*nx + v2.y*ny; + if (offset1 < offset) { + // Первая точка ребра во внешней полуплоскости + if (offset2 > offset) { + // Вторая точка ребра во внутренней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } else { + // Первая точка ребра во внутренней полуплоскости. Добавляем её в конечный список. + v = verticesOut[num]; + v.x = v1.x; + v.y = v1.y; + v.z = v1.z; + num++; + if (offset2 < offset) { + // Вторая точка ребра во внешней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } + v1 = v2; + offset1 = offset2; + } + return num; + } + + /** + * + * @param box + * @param tri + * @param toBox + * @param boxAxisIdx + * @param triAxisIdx + * @param contact + */ + private function findEdgesIntersection(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, boxAxisIdx:int, triAxisIdx:int, contact:Contact):Boolean { + // Определение точки и направляющего вектора ребра треугольника + var tmpx1:Number; + var tmpy1:Number; + var tmpz1:Number; + var tmpx2:Number; + var tmpy2:Number; + var tmpz2:Number; + switch (triAxisIdx) { + case 0: + tmpx1 = tri.e0.x; + tmpy1 = tri.e0.y; + tmpz1 = tri.e0.z; + tmpx2 = tri.v0.x; + tmpy2 = tri.v0.y; + tmpz2 = tri.v0.z; + break; + case 1: + tmpx1 = tri.e1.x; + tmpy1 = tri.e1.y; + tmpz1 = tri.e1.z; + tmpx2 = tri.v1.x; + tmpy2 = tri.v1.y; + tmpz2 = tri.v1.z; + break; + case 2: + tmpx1 = tri.e2.x; + tmpy1 = tri.e2.y; + tmpz1 = tri.e2.z; + tmpx2 = tri.v2.x; + tmpy2 = tri.v2.y; + tmpz2 = tri.v2.z; + break; + } + var triTransform:Matrix4 = tri.transform; + axis20.x = triTransform.a*tmpx1 + triTransform.b*tmpy1 + triTransform.c*tmpz1; + axis20.y = triTransform.e*tmpx1 + triTransform.f*tmpy1 + triTransform.g*tmpz1; + axis20.z = triTransform.i*tmpx1 + triTransform.j*tmpy1 + triTransform.k*tmpz1; + var x2:Number = triTransform.a*tmpx2 + triTransform.b*tmpy2 + triTransform.c*tmpz2 + triTransform.d; + var y2:Number = triTransform.e*tmpx2 + triTransform.f*tmpy2 + triTransform.g*tmpz2 + triTransform.h; + var z2:Number = triTransform.i*tmpx2 + triTransform.j*tmpy2 + triTransform.k*tmpz2 + triTransform.l; + + // Определение нормали контакта, точки и направляющего вектора ребра бокса + var boxTransform:Matrix4 = box.transform; + boxTransform.getAxis(boxAxisIdx, axis10); + + // Нормаль контакта + var v:Vector3 = contact.normal; + v.x = axis10.y*axis20.z - axis10.z*axis20.y; + v.y = axis10.z*axis20.x - axis10.x*axis20.z; + v.z = axis10.x*axis20.y - axis10.y*axis20.x; + k = 1/Math.sqrt(v.x*v.x + v.y*v.y + v.z*v.z); + v.x *= k; + v.y *= k; + v.z *= k; + // Разворот нормали в сторону бокса + if (v.x*toBox.x + v.y*toBox.y + v.z*toBox.z < 0) { + v.x = -v.x; + v.y = -v.y; + v.z = -v.z; + } + + var boxHalfLen:Number; + tmpx1 = box.hs.x; + tmpy1 = box.hs.y; + tmpz1 = box.hs.z; + // X + if (boxAxisIdx == 0) { + tmpx1 = 0; + boxHalfLen = box.hs.x; + } else { + if (boxTransform.a*v.x + boxTransform.e*v.y + boxTransform.i*v.z > 0) { + tmpx1 = -tmpx1; + if ((box.excludedFaces & 2) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 1) != 0) { + return false; + } + } + } + // Y + if (boxAxisIdx == 1) { + tmpy1 = 0; + boxHalfLen = box.hs.y; + } else { + if (boxTransform.b*v.x + boxTransform.f*v.y + boxTransform.j*v.z > 0) { + tmpy1 = -tmpy1; + if ((box.excludedFaces & 8) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 4) != 0) { + return false; + } + } + } + // Z + if (boxAxisIdx == 2) { + tmpz1 = 0; + boxHalfLen = box.hs.z; + } else { + if (boxTransform.c*v.x + boxTransform.g*v.y + boxTransform.k*v.z > 0) { + tmpz1 = -tmpz1; + if ((box.excludedFaces & 32) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 16) != 0) { + return false; + } + } + } + var x1:Number = boxTransform.a*tmpx1 + boxTransform.b*tmpy1 + boxTransform.c*tmpz1 + boxTransform.d; + var y1:Number = boxTransform.e*tmpx1 + boxTransform.f*tmpy1 + boxTransform.g*tmpz1 + boxTransform.h; + var z1:Number = boxTransform.i*tmpx1 + boxTransform.j*tmpy1 + boxTransform.k*tmpz1 + boxTransform.l; + + // Находим точку пересечения рёбер, решая систему уравнений + // axis10 - направляющий вектор ребра бокса + // x1, y1, z1 - средняя точка ребра бокса + // axis20 - направляющий вектор ребра треугольника + // x2, y2, z2 - начальная точка ребра треугольника + var k:Number = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + var vx:Number = x2 - x1; + var vy:Number = y2 - y1; + var vz:Number = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + + // Запись данных о контакте + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + v = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + v.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + v.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + v.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = v.x - boxTransform.d; + r.y = v.y - boxTransform.h; + r.z = v.z - boxTransform.l; + r = cp.r2; + r.x = v.x - triTransform.d; + r.y = v.y - triTransform.h; + r.z = v.z - triTransform.l; + return true; + } + + } +} diff --git a/0.0.9.1/src/alternativa/physics/collision/colliders/SpherePlaneCollider.as b/0.0.9.1/src/alternativa/physics/collision/colliders/SpherePlaneCollider.as new file mode 100644 index 0000000..30cd55f --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/colliders/SpherePlaneCollider.as @@ -0,0 +1,68 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + /** + * + */ + public class SpherePlaneCollider implements ICollider { + + private var normal:Vector3 = new Vector3(); + + /** + * + */ + public function SpherePlaneCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var sphere:RigidSphere = body1 as RigidSphere; +// var plane:RigidPlane; +// if (sphere == null) { +// sphere = body2 as RigidSphere; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// +// var dist:Number = sphere.state.pos.dot(normal) - offset; +// if (dist > sphere.r) return false; +// +// collisionInfo.body1 = sphere; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// collisionInfo.pcount = 1; +// +// var cp:ContactPoint = collisionInfo.points[0]; +// cp.penetration = sphere.r - dist; +// cp.pos.copy(normal).reverse().scale(sphere.r).add(sphere.state.pos); +// cp.r1.diff(cp.pos, sphere.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/collision/colliders/SphereSphereCollider.as b/0.0.9.1/src/alternativa/physics/collision/colliders/SphereSphereCollider.as new file mode 100644 index 0000000..3332f3c --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/colliders/SphereSphereCollider.as @@ -0,0 +1,70 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.math.Vector3; + use namespace altphysics; + + public class SphereSphereCollider implements ICollider { + + private var p1:Vector3 = new Vector3(); + private var p2:Vector3 = new Vector3(); + + public function SphereSphereCollider() { + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var s1:CollisionSphere; + var s2:CollisionSphere; + if (prim1.body != null) { + s1 = prim1 as CollisionSphere; + s2 = prim2 as CollisionSphere; + } else { + s1 = prim2 as CollisionSphere; + s2 = prim1 as CollisionSphere; + } + + s1.transform.getAxis(3, p1); + s2.transform.getAxis(3, p2); + var dx:Number = p1.x - p2.x; + var dy:Number = p1.y - p2.y; + var dz:Number = p1.z - p2.z; + var len:Number = dx*dx + dy*dy + dz*dz; + var sum:Number = s1.r + s2.r; + if (len > sum*sum) return false; + len = Math.sqrt(len); + dx /= len; + dy /= len; + dz /= len; + + contact.body1 = s1.body; + contact.body2 = s2.body; + contact.normal.x = dx; + contact.normal.y = dy; + contact.normal.z = dz; + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = sum - len; + cp.pos.x = p1.x - dx*s1.r; + cp.pos.y = p1.y - dy*s1.r; + cp.pos.z = p1.z - dz*s1.r; + cp.r1.vDiff(cp.pos, p1); + cp.r2.vDiff(cp.pos, p2); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/collision/primitives/.svn/all-wcprops b/0.0.9.1/src/alternativa/physics/collision/primitives/.svn/all-wcprops new file mode 100644 index 0000000..a6c88bd --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/primitives/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 124 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/collision/primitives +END +CollisionTriangle.as +K 25 +svn:wc:ra_dav:version-url +V 145 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/collision/primitives/CollisionTriangle.as +END +CollisionSphere.as +K 25 +svn:wc:ra_dav:version-url +V 143 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/collision/primitives/CollisionSphere.as +END +CollisionBox.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/collision/primitives/CollisionBox.as +END +CollisionRect.as +K 25 +svn:wc:ra_dav:version-url +V 141 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/collision/primitives/CollisionRect.as +END diff --git a/0.0.9.1/src/alternativa/physics/collision/primitives/.svn/entries b/0.0.9.1/src/alternativa/physics/collision/primitives/.svn/entries new file mode 100644 index 0000000..83a6793 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/primitives/.svn/entries @@ -0,0 +1,76 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/collision/primitives +http://svndev.alternativaplatform.com + + + +2009-11-24T08:44:15.941480Z +23758 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +CollisionTriangle.as +file + + + + +2010-10-28T04:32:44.000000Z +23ebb96cda309378ce15ead4cf865446 +2009-11-24T08:44:15.941480Z +23758 +mike + +CollisionSphere.as +file + + + + +2010-10-28T04:32:44.000000Z +335b6b72039123d6d54fee4d6b17f74e +2009-11-24T08:44:15.941480Z +23758 +mike + +CollisionBox.as +file + + + + +2010-10-28T04:32:44.000000Z +65e81a3d0034265e568d549306df1406 +2009-11-24T08:44:15.941480Z +23758 +mike + +CollisionRect.as +file + + + + +2010-10-28T04:32:44.000000Z +91b913679dca5ae4baabafb09c5aa051 +2009-11-24T08:44:15.941480Z +23758 +mike + diff --git a/0.0.9.1/src/alternativa/physics/collision/primitives/.svn/format b/0.0.9.1/src/alternativa/physics/collision/primitives/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/primitives/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.1/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base b/0.0.9.1/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base new file mode 100644 index 0000000..1febfb4 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base @@ -0,0 +1,204 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * Ориентированный бокс. + */ + public class CollisionBox extends CollisionPrimitive { + + // Половинные размеры вдоль каждой из осей + public var hs:Vector3 = new Vector3(); + + public var excludedFaces:int; + + /** + * @param hs + * @param collisionGroup + */ + public function CollisionBox(hs:Vector3, collisionGroup:int) { + super(BOX, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + var t:Matrix4 = transform; + + var xx:Number; + var yy:Number; + var zz:Number; + + xx = t.a < 0 ? -t.a : t.a; + yy = t.b < 0 ? -t.b : t.b; + zz = t.c < 0 ? -t.c : t.c; + aabb.maxX = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minX = -aabb.maxX; + + xx = t.e < 0 ? -t.e : t.e; + yy = t.f < 0 ? -t.f : t.f; + zz = t.g < 0 ? -t.g : t.g; + aabb.maxY = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minY = -aabb.maxY; + + xx = t.i < 0 ? -t.i : t.i; + yy = t.j < 0 ? -t.j : t.j; + zz = t.k < 0 ? -t.k : t.k; + aabb.maxZ = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var box:CollisionBox = source as CollisionBox; + if (box == null) return this; + super.copyFrom(box); + hs.vCopy(box.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionBox(hs, collisionGroup); + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + var tMin:Number = -1; + var tMax:Number = 1e308; + var t1:Number; + var t2:Number; + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + // X + if (vx < epsilon && vx > -epsilon) { + if (ox < -hs.x || ox > hs.x) return -1; + } else { + t1 = (-hs.x - ox)/vx; + t2 = (hs.x - ox)/vx; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.x = -1; + normal.y = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.x = 1; + normal.y = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Y + if (vy < epsilon && vy > -epsilon) { + if (oy < -hs.y || oy > hs.y) return -1; + } else { + t1 = (-hs.y - oy)/vy; + t2 = (hs.y - oy)/vy; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.y = -1; + normal.x = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.y = 1; + normal.x = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Z + if (vz < epsilon && vz > -epsilon) { + if (oz < -hs.z || oz > hs.z) return -1; + } else { + t1 = (-hs.z - oz)/vz; + t2 = (hs.z - oz)/vz; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.z = -1; + normal.x = normal.y = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.z = 1; + normal.x = normal.y = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Перевод нормали в мировую систему координат + // Inlined normal.vDeltaTransformBy4(transform); + vx = normal.x; + vy = normal.y; + vz = normal.z; + normal.x = transform.a*vx + transform.b*vy + transform.c*vz; + normal.y = transform.e*vx + transform.f*vy + transform.g*vz; + normal.z = transform.i*vx + transform.j*vy + transform.k*vz; + + return tMin; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionBox hs=" + hs + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base b/0.0.9.1/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base new file mode 100644 index 0000000..7a33b79 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base @@ -0,0 +1,143 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * Ориентированный прямоугольник. Задаётся половинами размеров вдоль осей X (ширина) и Y (длина) локальной системы + * координат примитива. Таким образом, прямоугольник лежит в плоскости XY, его стороны параллельны осям этой + * плоскости, а нормаль направлена вдоль локальной оси Z. + * + * Прямоугольник может быть одно- или двусторонним. В случае одностороннего прямоугольника, + * столкновения с внутренней стороны не регистрируются. + */ + public class CollisionRect extends CollisionPrimitive { + // Половинные размеры прямоугольника вдоль осей X и Y. Нормаль направлена вдоль оси Z. + public var hs:Vector3 = new Vector3(); + // Флаг указывает, является примитив одно- или двусторонним + public var twoSided:Boolean = true; + + // Малое значение. Используется для указания фиктивной высоты примитива и в функции определения пересечения с сегментом. + private static const EPSILON:Number = 0.005; + + /** + * Создаёт новый экземпляр примитива. + * + * @param hs половинные размерв прямоугольника вдоль осей X и Y. Значение z игнорируется. + * @param collisionGroup группа примитива + */ + public function CollisionRect(hs:Vector3, collisionGroup:int) { + super(RECT, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * Расчитывает ограничивающий бокс прямоугольника. Для избежания проблем высота примитива принимается равной + * не нулю, а малому значению. + * + * @return + */ + override public function calculateAABB():BoundBox { + // Баунд бокс прямоугольника имеет минимальную высоту, отличную от нуля во избежание проблем с построением kd-дерева + var t:Matrix4 = transform; + var xx:Number = t.a < 0 ? -t.a : t.a; + var yy:Number = t.b < 0 ? -t.b : t.b; + var zz:Number = t.c < 0 ? -t.c : t.c; + aabb.maxX = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minX = -aabb.maxX; + + xx = t.e < 0 ? -t.e : t.e; + yy = t.f < 0 ? -t.f : t.f; + zz = t.g < 0 ? -t.g : t.g; + aabb.maxY = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minY = -aabb.maxY; + + xx = t.i < 0 ? -t.i : t.i; + yy = t.j < 0 ? -t.j : t.j; + zz = t.k < 0 ? -t.k : t.k; + aabb.maxZ = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var rect:CollisionRect = source as CollisionRect; + if (rect == null) return this; + super.copyFrom(rect); + hs.vCopy(rect.hs); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionRect hs=" + hs + "]"; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionRect(hs, collisionGroup); + } + + /** + * @param origin + * @param vector + * @param collisionGroup + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + + // Проверка параллельности сегмента и плоскости примитива + if (vz > -threshold && vz < threshold) return -1; + var t:Number = -oz/vz; + if (t < 0) return -1; + // Проверка вхождения точки пересечения в прямоугольник + ox += vx*t; + oy += vy*t; + oz = 0; + if (ox < (-hs.x - threshold) || ox > (hs.x + threshold) || oy < (-hs.y - threshold) || oy > (hs.y + threshold)) return -1; + + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + return t; + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base b/0.0.9.1/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base new file mode 100644 index 0000000..b912a5b --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base @@ -0,0 +1,81 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Vector3; + use namespace altphysics; + + + /** + * Сфера. + */ + public class CollisionSphere extends CollisionPrimitive { + + // Радиус сферы + public var r:Number = 0; + + /** + * + * @param r + * @param collisionGroup + */ + public function CollisionSphere(r:Number, collisionGroup:int) { + super(SPHERE, collisionGroup); + this.r = r; + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + aabb.maxX = transform.d + r; + aabb.minX = transform.d - r; + + aabb.maxY = transform.h + r; + aabb.minY = transform.h - r; + + aabb.maxZ = transform.l + r; + aabb.minZ = transform.l - r; + + return aabb; + } + + /** + * @param origin + * @param vector + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var px:Number = origin.x - transform.d; + var py:Number = origin.y - transform.h; + var pz:Number = origin.z - transform.l; + var k:Number = vector.x*px + vector.y*py + vector.z*pz; + if (k > 0) return -1; + var a:Number = vector.x*vector.x + vector.y*vector.y + vector.z*vector.z; + var D:Number = k*k - a*(px*px + py*py + pz*pz - r*r); + if (D < 0) return -1; + return -(k + Math.sqrt(D))/a; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var sphere:CollisionSphere = source as CollisionSphere; + if (sphere == null) return this; + super.copyFrom(sphere); + r = sphere.r; + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionSphere(r, collisionGroup); + } + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base b/0.0.9.1/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base new file mode 100644 index 0000000..bdc3166 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base @@ -0,0 +1,244 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * Примитив треугольник. + */ + public class CollisionTriangle extends CollisionPrimitive { + + public var v0:Vector3 = new Vector3(); + public var v1:Vector3 = new Vector3(); + public var v2:Vector3 = new Vector3(); + + public var e0:Vector3 = new Vector3(); + public var e1:Vector3 = new Vector3(); + public var e2:Vector3 = new Vector3(); + +// public var len0:Number; +// public var len1:Number; +// public var len2:Number; +// + /** + * + * @param v0 + * @param v1 + * @param v2 + * @param collisionGroup + */ + public function CollisionTriangle(v0:Vector3, v1:Vector3, v2:Vector3, collisionGroup:int) { + super(TRIANGLE, collisionGroup); + initVertices(v0, v1, v2); + } + + /** + * Рассчитывает AABB примитива. + * + * @return ссылка на свой AABB + */ + override public function calculateAABB():BoundBox { + var epsilon:Number = 0.005; + var a:Number; + var b:Number; + var eps_c:Number = epsilon*transform.c; + var eps_g:Number = epsilon*transform.g; + var eps_k:Number = epsilon*transform.k; + + // Вершина 0 + // Ось X + a = v0.x*transform.a + v0.y*transform.b; + aabb.minX = aabb.maxX = a + eps_c; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v0.x*transform.e + v0.y*transform.f; + aabb.minY = aabb.maxY = a + eps_g; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v0.x*transform.i + v0.y*transform.j; + aabb.minZ = aabb.maxZ = a + eps_k; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 1 + // Ось X + a = v1.x*transform.a + v1.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v1.x*transform.e + v1.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v1.x*transform.i + v1.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 2 + // Ось X + a = v2.x*transform.a + v2.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v2.x*transform.e + v2.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v2.x*transform.i + v2.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + aabb.minX += transform.d; + aabb.maxX += transform.d; + + aabb.minY += transform.h; + aabb.maxY += transform.h; + + aabb.minZ += transform.l; + aabb.maxZ += transform.l; + + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + // Луч трансформируется в систему координат примитива, затем проверяется пересечение + + var vz:Number = vector.x*transform.c + vector.y*transform.g + vector.z*transform.k; + // Если луч параллелен плоскости птимитива, то пересечения нет + if (vz < epsilon && vz > -epsilon) return -1; + + var tx:Number = origin.x - transform.d; + var ty:Number = origin.y - transform.h; + var tz:Number = origin.z - transform.l; + + var oz:Number = tx*transform.c + ty*transform.g + tz*transform.k; + var t:Number = -oz/vz; + if (t < 0) return -1; + + var ox:Number = tx*transform.a + ty*transform.e + tz*transform.i; + var oy:Number = tx*transform.b + ty*transform.f + tz*transform.j; + + tx = ox + t*(vector.x*transform.a + vector.y*transform.e + vector.z*transform.i); + ty = oy + t*(vector.x*transform.b + vector.y*transform.f + vector.z*transform.j); + tz = oz + t*vz; + + // Проверка вхождения точки в треугольник + if ((e0.x*(ty - v0.y) - e0.y*(tx - v0.x) < 0) || (e1.x*(ty - v1.y) - e1.y*(tx - v1.x) < 0) || (e2.x*(ty - v2.y) - e2.y*(tx - v2.x) < 0)) return -1; + + // Запись нормали + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + + return t; + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + super.copyFrom(source); + var tri:CollisionTriangle = source as CollisionTriangle; + if (tri != null) { + v0.vCopy(tri.v0); + v1.vCopy(tri.v1); + v2.vCopy(tri.v2); + + e0.vCopy(tri.e0); + e1.vCopy(tri.e1); + e2.vCopy(tri.e2); + +// len0 = tri.len0; +// len1 = tri.len1; +// len2 = tri.len2; + } + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionTriangle v0=" + v0 + ", v1=" + v1 + ", v2=" + v2 + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionTriangle(v0, v1, v2, collisionGroup); + } + + /** + * + * @param v0 + * @param v1 + * @param v2 + */ + private function initVertices(v0:Vector3, v1:Vector3, v2:Vector3):void { + this.v0.vCopy(v0); + this.v1.vCopy(v1); + this.v2.vCopy(v2); + + e0.vDiff(v1, v0); +// len0 = e0.vLength(); + e0.vNormalize(); + + e1.vDiff(v2, v1); +// len1 = e1.vLength(); + e1.vNormalize(); + + e2.vDiff(v0, v2); +// len2 = e2.vLength(); + e2.vNormalize(); + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/collision/primitives/CollisionBox.as b/0.0.9.1/src/alternativa/physics/collision/primitives/CollisionBox.as new file mode 100644 index 0000000..1febfb4 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/primitives/CollisionBox.as @@ -0,0 +1,204 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * Ориентированный бокс. + */ + public class CollisionBox extends CollisionPrimitive { + + // Половинные размеры вдоль каждой из осей + public var hs:Vector3 = new Vector3(); + + public var excludedFaces:int; + + /** + * @param hs + * @param collisionGroup + */ + public function CollisionBox(hs:Vector3, collisionGroup:int) { + super(BOX, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + var t:Matrix4 = transform; + + var xx:Number; + var yy:Number; + var zz:Number; + + xx = t.a < 0 ? -t.a : t.a; + yy = t.b < 0 ? -t.b : t.b; + zz = t.c < 0 ? -t.c : t.c; + aabb.maxX = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minX = -aabb.maxX; + + xx = t.e < 0 ? -t.e : t.e; + yy = t.f < 0 ? -t.f : t.f; + zz = t.g < 0 ? -t.g : t.g; + aabb.maxY = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minY = -aabb.maxY; + + xx = t.i < 0 ? -t.i : t.i; + yy = t.j < 0 ? -t.j : t.j; + zz = t.k < 0 ? -t.k : t.k; + aabb.maxZ = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var box:CollisionBox = source as CollisionBox; + if (box == null) return this; + super.copyFrom(box); + hs.vCopy(box.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionBox(hs, collisionGroup); + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + var tMin:Number = -1; + var tMax:Number = 1e308; + var t1:Number; + var t2:Number; + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + // X + if (vx < epsilon && vx > -epsilon) { + if (ox < -hs.x || ox > hs.x) return -1; + } else { + t1 = (-hs.x - ox)/vx; + t2 = (hs.x - ox)/vx; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.x = -1; + normal.y = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.x = 1; + normal.y = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Y + if (vy < epsilon && vy > -epsilon) { + if (oy < -hs.y || oy > hs.y) return -1; + } else { + t1 = (-hs.y - oy)/vy; + t2 = (hs.y - oy)/vy; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.y = -1; + normal.x = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.y = 1; + normal.x = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Z + if (vz < epsilon && vz > -epsilon) { + if (oz < -hs.z || oz > hs.z) return -1; + } else { + t1 = (-hs.z - oz)/vz; + t2 = (hs.z - oz)/vz; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.z = -1; + normal.x = normal.y = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.z = 1; + normal.x = normal.y = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Перевод нормали в мировую систему координат + // Inlined normal.vDeltaTransformBy4(transform); + vx = normal.x; + vy = normal.y; + vz = normal.z; + normal.x = transform.a*vx + transform.b*vy + transform.c*vz; + normal.y = transform.e*vx + transform.f*vy + transform.g*vz; + normal.z = transform.i*vx + transform.j*vy + transform.k*vz; + + return tMin; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionBox hs=" + hs + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/collision/primitives/CollisionRect.as b/0.0.9.1/src/alternativa/physics/collision/primitives/CollisionRect.as new file mode 100644 index 0000000..7a33b79 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/primitives/CollisionRect.as @@ -0,0 +1,143 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * Ориентированный прямоугольник. Задаётся половинами размеров вдоль осей X (ширина) и Y (длина) локальной системы + * координат примитива. Таким образом, прямоугольник лежит в плоскости XY, его стороны параллельны осям этой + * плоскости, а нормаль направлена вдоль локальной оси Z. + * + * Прямоугольник может быть одно- или двусторонним. В случае одностороннего прямоугольника, + * столкновения с внутренней стороны не регистрируются. + */ + public class CollisionRect extends CollisionPrimitive { + // Половинные размеры прямоугольника вдоль осей X и Y. Нормаль направлена вдоль оси Z. + public var hs:Vector3 = new Vector3(); + // Флаг указывает, является примитив одно- или двусторонним + public var twoSided:Boolean = true; + + // Малое значение. Используется для указания фиктивной высоты примитива и в функции определения пересечения с сегментом. + private static const EPSILON:Number = 0.005; + + /** + * Создаёт новый экземпляр примитива. + * + * @param hs половинные размерв прямоугольника вдоль осей X и Y. Значение z игнорируется. + * @param collisionGroup группа примитива + */ + public function CollisionRect(hs:Vector3, collisionGroup:int) { + super(RECT, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * Расчитывает ограничивающий бокс прямоугольника. Для избежания проблем высота примитива принимается равной + * не нулю, а малому значению. + * + * @return + */ + override public function calculateAABB():BoundBox { + // Баунд бокс прямоугольника имеет минимальную высоту, отличную от нуля во избежание проблем с построением kd-дерева + var t:Matrix4 = transform; + var xx:Number = t.a < 0 ? -t.a : t.a; + var yy:Number = t.b < 0 ? -t.b : t.b; + var zz:Number = t.c < 0 ? -t.c : t.c; + aabb.maxX = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minX = -aabb.maxX; + + xx = t.e < 0 ? -t.e : t.e; + yy = t.f < 0 ? -t.f : t.f; + zz = t.g < 0 ? -t.g : t.g; + aabb.maxY = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minY = -aabb.maxY; + + xx = t.i < 0 ? -t.i : t.i; + yy = t.j < 0 ? -t.j : t.j; + zz = t.k < 0 ? -t.k : t.k; + aabb.maxZ = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var rect:CollisionRect = source as CollisionRect; + if (rect == null) return this; + super.copyFrom(rect); + hs.vCopy(rect.hs); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionRect hs=" + hs + "]"; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionRect(hs, collisionGroup); + } + + /** + * @param origin + * @param vector + * @param collisionGroup + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + + // Проверка параллельности сегмента и плоскости примитива + if (vz > -threshold && vz < threshold) return -1; + var t:Number = -oz/vz; + if (t < 0) return -1; + // Проверка вхождения точки пересечения в прямоугольник + ox += vx*t; + oy += vy*t; + oz = 0; + if (ox < (-hs.x - threshold) || ox > (hs.x + threshold) || oy < (-hs.y - threshold) || oy > (hs.y + threshold)) return -1; + + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + return t; + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/collision/primitives/CollisionSphere.as b/0.0.9.1/src/alternativa/physics/collision/primitives/CollisionSphere.as new file mode 100644 index 0000000..b912a5b --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/primitives/CollisionSphere.as @@ -0,0 +1,81 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Vector3; + use namespace altphysics; + + + /** + * Сфера. + */ + public class CollisionSphere extends CollisionPrimitive { + + // Радиус сферы + public var r:Number = 0; + + /** + * + * @param r + * @param collisionGroup + */ + public function CollisionSphere(r:Number, collisionGroup:int) { + super(SPHERE, collisionGroup); + this.r = r; + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + aabb.maxX = transform.d + r; + aabb.minX = transform.d - r; + + aabb.maxY = transform.h + r; + aabb.minY = transform.h - r; + + aabb.maxZ = transform.l + r; + aabb.minZ = transform.l - r; + + return aabb; + } + + /** + * @param origin + * @param vector + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var px:Number = origin.x - transform.d; + var py:Number = origin.y - transform.h; + var pz:Number = origin.z - transform.l; + var k:Number = vector.x*px + vector.y*py + vector.z*pz; + if (k > 0) return -1; + var a:Number = vector.x*vector.x + vector.y*vector.y + vector.z*vector.z; + var D:Number = k*k - a*(px*px + py*py + pz*pz - r*r); + if (D < 0) return -1; + return -(k + Math.sqrt(D))/a; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var sphere:CollisionSphere = source as CollisionSphere; + if (sphere == null) return this; + super.copyFrom(sphere); + r = sphere.r; + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionSphere(r, collisionGroup); + } + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/collision/primitives/CollisionTriangle.as b/0.0.9.1/src/alternativa/physics/collision/primitives/CollisionTriangle.as new file mode 100644 index 0000000..bdc3166 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/primitives/CollisionTriangle.as @@ -0,0 +1,244 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * Примитив треугольник. + */ + public class CollisionTriangle extends CollisionPrimitive { + + public var v0:Vector3 = new Vector3(); + public var v1:Vector3 = new Vector3(); + public var v2:Vector3 = new Vector3(); + + public var e0:Vector3 = new Vector3(); + public var e1:Vector3 = new Vector3(); + public var e2:Vector3 = new Vector3(); + +// public var len0:Number; +// public var len1:Number; +// public var len2:Number; +// + /** + * + * @param v0 + * @param v1 + * @param v2 + * @param collisionGroup + */ + public function CollisionTriangle(v0:Vector3, v1:Vector3, v2:Vector3, collisionGroup:int) { + super(TRIANGLE, collisionGroup); + initVertices(v0, v1, v2); + } + + /** + * Рассчитывает AABB примитива. + * + * @return ссылка на свой AABB + */ + override public function calculateAABB():BoundBox { + var epsilon:Number = 0.005; + var a:Number; + var b:Number; + var eps_c:Number = epsilon*transform.c; + var eps_g:Number = epsilon*transform.g; + var eps_k:Number = epsilon*transform.k; + + // Вершина 0 + // Ось X + a = v0.x*transform.a + v0.y*transform.b; + aabb.minX = aabb.maxX = a + eps_c; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v0.x*transform.e + v0.y*transform.f; + aabb.minY = aabb.maxY = a + eps_g; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v0.x*transform.i + v0.y*transform.j; + aabb.minZ = aabb.maxZ = a + eps_k; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 1 + // Ось X + a = v1.x*transform.a + v1.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v1.x*transform.e + v1.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v1.x*transform.i + v1.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 2 + // Ось X + a = v2.x*transform.a + v2.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v2.x*transform.e + v2.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v2.x*transform.i + v2.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + aabb.minX += transform.d; + aabb.maxX += transform.d; + + aabb.minY += transform.h; + aabb.maxY += transform.h; + + aabb.minZ += transform.l; + aabb.maxZ += transform.l; + + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + // Луч трансформируется в систему координат примитива, затем проверяется пересечение + + var vz:Number = vector.x*transform.c + vector.y*transform.g + vector.z*transform.k; + // Если луч параллелен плоскости птимитива, то пересечения нет + if (vz < epsilon && vz > -epsilon) return -1; + + var tx:Number = origin.x - transform.d; + var ty:Number = origin.y - transform.h; + var tz:Number = origin.z - transform.l; + + var oz:Number = tx*transform.c + ty*transform.g + tz*transform.k; + var t:Number = -oz/vz; + if (t < 0) return -1; + + var ox:Number = tx*transform.a + ty*transform.e + tz*transform.i; + var oy:Number = tx*transform.b + ty*transform.f + tz*transform.j; + + tx = ox + t*(vector.x*transform.a + vector.y*transform.e + vector.z*transform.i); + ty = oy + t*(vector.x*transform.b + vector.y*transform.f + vector.z*transform.j); + tz = oz + t*vz; + + // Проверка вхождения точки в треугольник + if ((e0.x*(ty - v0.y) - e0.y*(tx - v0.x) < 0) || (e1.x*(ty - v1.y) - e1.y*(tx - v1.x) < 0) || (e2.x*(ty - v2.y) - e2.y*(tx - v2.x) < 0)) return -1; + + // Запись нормали + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + + return t; + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + super.copyFrom(source); + var tri:CollisionTriangle = source as CollisionTriangle; + if (tri != null) { + v0.vCopy(tri.v0); + v1.vCopy(tri.v1); + v2.vCopy(tri.v2); + + e0.vCopy(tri.e0); + e1.vCopy(tri.e1); + e2.vCopy(tri.e2); + +// len0 = tri.len0; +// len1 = tri.len1; +// len2 = tri.len2; + } + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionTriangle v0=" + v0 + ", v1=" + v1 + ", v2=" + v2 + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionTriangle(v0, v1, v2, collisionGroup); + } + + /** + * + * @param v0 + * @param v1 + * @param v2 + */ + private function initVertices(v0:Vector3, v1:Vector3, v2:Vector3):void { + this.v0.vCopy(v0); + this.v1.vCopy(v1); + this.v2.vCopy(v2); + + e0.vDiff(v1, v0); +// len0 = e0.vLength(); + e0.vNormalize(); + + e1.vDiff(v2, v1); +// len1 = e1.vLength(); + e1.vNormalize(); + + e2.vDiff(v0, v2); +// len2 = e2.vLength(); + e2.vNormalize(); + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/collision/types/.svn/all-wcprops b/0.0.9.1/src/alternativa/physics/collision/types/.svn/all-wcprops new file mode 100644 index 0000000..f1457b5 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/types/.svn/all-wcprops @@ -0,0 +1,23 @@ +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/collision/types +END +BoundBox.as +K 25 +svn:wc:ra_dav:version-url +V 131 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/collision/types/BoundBox.as +END +RayIntersection.as +K 25 +svn:wc:ra_dav:version-url +V 138 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/collision/types/RayIntersection.as +END +Ray.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/collision/types/Ray.as +END diff --git a/0.0.9.1/src/alternativa/physics/collision/types/.svn/entries b/0.0.9.1/src/alternativa/physics/collision/types/.svn/entries new file mode 100644 index 0000000..69c8b07 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/types/.svn/entries @@ -0,0 +1,64 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/collision/types +http://svndev.alternativaplatform.com + + + +2009-11-24T08:44:15.941480Z +23758 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +BoundBox.as +file + + + + +2010-10-28T04:32:44.000000Z +d92d4ab3dde698feb674fb9488f59678 +2009-09-09T14:02:48.134744Z +19633 +mike + +RayIntersection.as +file + + + + +2010-10-28T04:32:44.000000Z +88c0836ead77b8eb0841f11159dfb836 +2009-11-24T08:44:15.941480Z +23758 +mike + +Ray.as +file + + + + +2010-10-28T04:32:44.000000Z +c65cc8bfae377f8f0520b5c79c8b3ab9 +2009-11-24T08:44:15.941480Z +23758 +mike + diff --git a/0.0.9.1/src/alternativa/physics/collision/types/.svn/format b/0.0.9.1/src/alternativa/physics/collision/types/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/types/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.1/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base b/0.0.9.1/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base new file mode 100644 index 0000000..b4d5278 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base @@ -0,0 +1,75 @@ +package alternativa.physics.collision.types { + + /** + * Ограничивающий параллелепипед. + */ + public class BoundBox { + + public var minX:Number = 1e308; + public var minY:Number = 1e308; + public var minZ:Number = 1e308; + public var maxX:Number = -1e308; + public var maxY:Number = -1e308; + public var maxZ:Number = -1e308; + + public function setSize(minX:Number, minY:Number, minZ:Number, maxX:Number, maxY:Number, maxZ:Number):void { + this.minX = minX; + this.minY = minY; + this.minZ = minZ; + this.maxX = maxX; + this.maxY = maxY; + this.maxZ = maxZ; + } + + public function addBoundBox(boundBox:BoundBox):void { + minX = (boundBox.minX < minX) ? boundBox.minX : minX; + minY = (boundBox.minY < minY) ? boundBox.minY : minY; + minZ = (boundBox.minZ < minZ) ? boundBox.minZ : minZ; + maxX = (boundBox.maxX > maxX) ? boundBox.maxX : maxX; + maxY = (boundBox.maxY > maxY) ? boundBox.maxY : maxY; + maxZ = (boundBox.maxZ > maxZ) ? boundBox.maxZ : maxZ; + } + + public function addPoint(x:Number, y:Number, z:Number):void { + if (x < minX) minX = x; + if (x > maxX) maxX = x; + if (y < minY) minY = y; + if (y > maxY) maxY = y; + if (z < minZ) minZ = z; + if (z > maxZ) maxZ = z; + } + + public function infinity():void { + minX = 1e308; + minY = 1e308; + minZ = 1e308; + maxX = -1e308; + maxY = -1e308; + maxZ = -1e308; + } + + public function intersects(bb:BoundBox, epsilon:Number):Boolean { + return !(minX > bb.maxX + epsilon || maxX < bb.minX - epsilon || minY > bb.maxY + epsilon || maxY < bb.minY - epsilon || minZ > bb.maxZ + epsilon || maxZ < bb.minZ - epsilon); + } + + public function copyFrom(boundBox:BoundBox):void { + minX = boundBox.minX; + minY = boundBox.minY; + minZ = boundBox.minZ; + maxX = boundBox.maxX; + maxY = boundBox.maxY; + maxZ = boundBox.maxZ; + } + + public function clone():BoundBox { + var clone:BoundBox = new BoundBox(); + clone.copyFrom(this); + return clone; + } + + public function toString():String { + return "BoundBox [" + minX + ", " + minY + ", " + minZ + " : " + maxX + ", " + maxY + ", " + maxZ + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base b/0.0.9.1/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base new file mode 100644 index 0000000..fb2ffbc --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base @@ -0,0 +1,13 @@ +package alternativa.physics.collision.types { + import alternativa.math.Vector3; + + /** + * Структура, описывающая луч и сегмент прямой в пространстве. + */ + public class Ray { + // Начало луча. Начальная точка сегмента на прямой луча. + public var origin:Vector3 = new Vector3(); + // Направляющий вектор луча. Длина вектора задаёт длину сегмента на луче. Конечная точка сегмента вычисляется как origin + dir. + public var dir:Vector3 = new Vector3(); + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base b/0.0.9.1/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base new file mode 100644 index 0000000..21e9290 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base @@ -0,0 +1,25 @@ +package alternativa.physics.collision.types { + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.math.Vector3; + + /** + * Структура описывает точку пересечения луча с физической геометрией. + */ + public class RayIntersection { + // Физический примитив, с которым пересекается луч + public var primitive:CollisionPrimitive; + // Положенеи точки пересечения + public var pos:Vector3 = new Vector3(); + // Нормаль поверхности физического примитива в точке пересечения + public var normal:Vector3 = new Vector3(); + // Время пересечения (|pos - ray.origin|/|ray.dir|) + public var t:Number = 0; + + public function copy(source:RayIntersection):void { + primitive = source.primitive; + pos.vCopy(source.pos); + normal.vCopy(source.normal); + t = source.t; + } + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/collision/types/BoundBox.as b/0.0.9.1/src/alternativa/physics/collision/types/BoundBox.as new file mode 100644 index 0000000..b4d5278 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/types/BoundBox.as @@ -0,0 +1,75 @@ +package alternativa.physics.collision.types { + + /** + * Ограничивающий параллелепипед. + */ + public class BoundBox { + + public var minX:Number = 1e308; + public var minY:Number = 1e308; + public var minZ:Number = 1e308; + public var maxX:Number = -1e308; + public var maxY:Number = -1e308; + public var maxZ:Number = -1e308; + + public function setSize(minX:Number, minY:Number, minZ:Number, maxX:Number, maxY:Number, maxZ:Number):void { + this.minX = minX; + this.minY = minY; + this.minZ = minZ; + this.maxX = maxX; + this.maxY = maxY; + this.maxZ = maxZ; + } + + public function addBoundBox(boundBox:BoundBox):void { + minX = (boundBox.minX < minX) ? boundBox.minX : minX; + minY = (boundBox.minY < minY) ? boundBox.minY : minY; + minZ = (boundBox.minZ < minZ) ? boundBox.minZ : minZ; + maxX = (boundBox.maxX > maxX) ? boundBox.maxX : maxX; + maxY = (boundBox.maxY > maxY) ? boundBox.maxY : maxY; + maxZ = (boundBox.maxZ > maxZ) ? boundBox.maxZ : maxZ; + } + + public function addPoint(x:Number, y:Number, z:Number):void { + if (x < minX) minX = x; + if (x > maxX) maxX = x; + if (y < minY) minY = y; + if (y > maxY) maxY = y; + if (z < minZ) minZ = z; + if (z > maxZ) maxZ = z; + } + + public function infinity():void { + minX = 1e308; + minY = 1e308; + minZ = 1e308; + maxX = -1e308; + maxY = -1e308; + maxZ = -1e308; + } + + public function intersects(bb:BoundBox, epsilon:Number):Boolean { + return !(minX > bb.maxX + epsilon || maxX < bb.minX - epsilon || minY > bb.maxY + epsilon || maxY < bb.minY - epsilon || minZ > bb.maxZ + epsilon || maxZ < bb.minZ - epsilon); + } + + public function copyFrom(boundBox:BoundBox):void { + minX = boundBox.minX; + minY = boundBox.minY; + minZ = boundBox.minZ; + maxX = boundBox.maxX; + maxY = boundBox.maxY; + maxZ = boundBox.maxZ; + } + + public function clone():BoundBox { + var clone:BoundBox = new BoundBox(); + clone.copyFrom(this); + return clone; + } + + public function toString():String { + return "BoundBox [" + minX + ", " + minY + ", " + minZ + " : " + maxX + ", " + maxY + ", " + maxZ + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/collision/types/Ray.as b/0.0.9.1/src/alternativa/physics/collision/types/Ray.as new file mode 100644 index 0000000..fb2ffbc --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/types/Ray.as @@ -0,0 +1,13 @@ +package alternativa.physics.collision.types { + import alternativa.math.Vector3; + + /** + * Структура, описывающая луч и сегмент прямой в пространстве. + */ + public class Ray { + // Начало луча. Начальная точка сегмента на прямой луча. + public var origin:Vector3 = new Vector3(); + // Направляющий вектор луча. Длина вектора задаёт длину сегмента на луче. Конечная точка сегмента вычисляется как origin + dir. + public var dir:Vector3 = new Vector3(); + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/collision/types/RayIntersection.as b/0.0.9.1/src/alternativa/physics/collision/types/RayIntersection.as new file mode 100644 index 0000000..21e9290 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/collision/types/RayIntersection.as @@ -0,0 +1,25 @@ +package alternativa.physics.collision.types { + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.math.Vector3; + + /** + * Структура описывает точку пересечения луча с физической геометрией. + */ + public class RayIntersection { + // Физический примитив, с которым пересекается луч + public var primitive:CollisionPrimitive; + // Положенеи точки пересечения + public var pos:Vector3 = new Vector3(); + // Нормаль поверхности физического примитива в точке пересечения + public var normal:Vector3 = new Vector3(); + // Время пересечения (|pos - ray.origin|/|ray.dir|) + public var t:Number = 0; + + public function copy(source:RayIntersection):void { + primitive = source.primitive; + pos.vCopy(source.pos); + normal.vCopy(source.normal); + t = source.t; + } + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/constraints/.svn/all-wcprops b/0.0.9.1/src/alternativa/physics/constraints/.svn/all-wcprops new file mode 100644 index 0000000..ecea754 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/constraints/.svn/all-wcprops @@ -0,0 +1,17 @@ +K 25 +svn:wc:ra_dav:version-url +V 115 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/constraints +END +Constraint.as +K 25 +svn:wc:ra_dav:version-url +V 129 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/constraints/Constraint.as +END +MaxDistanceConstraint.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/constraints/MaxDistanceConstraint.as +END diff --git a/0.0.9.1/src/alternativa/physics/constraints/.svn/entries b/0.0.9.1/src/alternativa/physics/constraints/.svn/entries new file mode 100644 index 0000000..62bfb7a --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/constraints/.svn/entries @@ -0,0 +1,52 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/constraints +http://svndev.alternativaplatform.com + + + +2010-01-20T17:53:18.555251Z +26849 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Constraint.as +file + + + + +2010-10-28T04:32:43.000000Z +02bdbd40eda515549956d6393551a63b +2010-01-20T17:53:18.555251Z +26849 +mike + +MaxDistanceConstraint.as +file + + + + +2010-10-28T04:32:43.000000Z +dbf792986b9dd7e2a1c720cf4a578f42 +2009-11-24T08:44:15.941480Z +23758 +mike + diff --git a/0.0.9.1/src/alternativa/physics/constraints/.svn/format b/0.0.9.1/src/alternativa/physics/constraints/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/constraints/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.1/src/alternativa/physics/constraints/.svn/text-base/Constraint.as.svn-base b/0.0.9.1/src/alternativa/physics/constraints/.svn/text-base/Constraint.as.svn-base new file mode 100644 index 0000000..8607442 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/constraints/.svn/text-base/Constraint.as.svn-base @@ -0,0 +1,35 @@ +package alternativa.physics.constraints { + + import alternativa.physics.PhysicsScene; + import alternativa.physics.altphysics; + + use namespace altphysics; + + /** + * + */ + public class Constraint { + + altphysics var satisfied:Boolean; + altphysics var world:PhysicsScene; + + /** + * + */ + public function Constraint() { + } + + /** + * @param dt + */ + public function preProcess(dt:Number):void { + } + + /** + * @param dt + */ + public function apply(dt:Number):void { + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base b/0.0.9.1/src/alternativa/physics/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base new file mode 100644 index 0000000..d29f518 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base @@ -0,0 +1,183 @@ +package alternativa.physics.constraints { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * + */ + public class MaxDistanceConstraint extends Constraint { + // Первое тело ограничения + altphysics var body1:Body; + // Второе тело ограничения. Может быть null. + altphysics var body2:Body; + // Первая точка крепления в системе координат первого тела + altphysics var r1:Vector3; + // Вторая точка привязки в системе координат второго тела. В случае отсутствия второго тела, координаты задаются в мировой системе. + altphysics var r2:Vector3; + // Максимально допустимое расстояние между точками крепления + altphysics var maxDistance:Number; + // Радиус-вектор первой точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr1:Vector3 = new Vector3(); + // Радиус-вектор второй точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr2:Vector3 = new Vector3(); + // Минимальная скорость сближения для удовлетворения ограничения + private var minClosingVel:Number; + // Изменение относительной скорости на единицу импульса + private var velByUnitImpulseN:Number; + // Направление корректирующего импульса + private var impulseDirection:Vector3 = new Vector3(); + + // Вспомогательные переменные +// private static var _v:Vector3 = new Vector3(); +// private static var _v1:Vector3 = new Vector3(); +// private static var _v2:Vector3 = new Vector3(); + + /** + * @param body1 + * @param body2 + * @param r1 + * @param r2 + * @param maxDistance + */ + public function MaxDistanceConstraint(body1:Body, body2:Body, r1:Vector3, r2:Vector3, maxDistance:Number) { + super(); + this.body1 = body1; + this.body2 = body2; + this.r1 = r1.vClone(); + this.r2 = r2.vClone(); + this.maxDistance = maxDistance; + } + + /** + * @param dt + */ + override public function preProcess(dt:Number):void { + // Вычислим расстояние между точками + var m:Matrix3 = body1.baseMatrix; + wr1.x = m.a*r1.x + m.b*r1.y + m.c*r1.z; + wr1.y = m.e*r1.x + m.f*r1.y + m.g*r1.z; + wr1.z = m.i*r1.x + m.j*r1.y + m.k*r1.z; + if (body2 != null) { + m = body2.baseMatrix; + wr2.x = m.a*r2.x + m.b*r2.y + m.c*r2.z; + wr2.y = m.e*r2.x + m.f*r2.y + m.g*r2.z; + wr2.z = m.i*r2.x + m.j*r2.y + m.k*r2.z; + } else { + wr2.x = r2.x; + wr2.y = r2.y; + wr2.z = r2.z; + } + var p1:Vector3 = body1.state.pos; + impulseDirection.x = wr2.x - wr1.x - p1.x; + impulseDirection.y = wr2.y - wr1.y - p1.y; + impulseDirection.z = wr2.z - wr1.z - p1.z; + if (body2 != null) { + var p2:Vector3 = body2.state.pos; + impulseDirection.x += p2.x; + impulseDirection.y += p2.y; + impulseDirection.z += p2.z; + } + + var len:Number = Math.sqrt(impulseDirection.x*impulseDirection.x + impulseDirection.y*impulseDirection.y + impulseDirection.z*impulseDirection.z); + var delta:Number = len - maxDistance; + if (delta > 0) { + satisfied = false; + if (len < 0.001) { + impulseDirection.x = 1; + } else { + len = 1/len; + impulseDirection.x *= len; + impulseDirection.y *= len; + impulseDirection.z *= len; + } + minClosingVel = delta/(world.penResolutionSteps*dt); + if (minClosingVel > world.maxPenResolutionSpeed) { + minClosingVel = world.maxPenResolutionSpeed; + } + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + var x:Number; + var y:Number; + var z:Number; + var vx:Number; + var vy:Number; + var vz:Number; + velByUnitImpulseN = 0; + if (body1.movable) { +// velByUnitImpulseN += body1.invMass + _v.vCross2(wr1, impulseDirection).vTransformBy3(body1.invInertiaWorld).vCross(wr1).vDot(impulseDirection); + vx = wr1.y*impulseDirection.z - wr1.z*impulseDirection.y; + vy = wr1.z*impulseDirection.x - wr1.x*impulseDirection.z; + vz = wr1.x*impulseDirection.y - wr1.y*impulseDirection.x; + + m = body1.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr1.z - z*wr1.y; + vy = z*wr1.x - x*wr1.z; + vz = x*wr1.y - y*wr1.x; + velByUnitImpulseN += body1.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + if (body2 != null && body2.movable) { +// velByUnitImpulseN += body2.invMass + _v.vCross2(wr2, impulseDirection).vTransformBy3(body2.invInertiaWorld).vCross(wr2).vDot(impulseDirection); + vx = wr2.y*impulseDirection.z - wr2.z*impulseDirection.y; + vy = wr2.z*impulseDirection.x - wr2.x*impulseDirection.z; + vz = wr2.x*impulseDirection.y - wr2.y*impulseDirection.x; + + m = body2.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr2.z - z*wr2.y; + vy = z*wr2.x - x*wr2.z; + vz = x*wr2.y - y*wr2.x; + velByUnitImpulseN += body2.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + } else { + satisfied = true; + } + } + + /** + * @param dt + */ + override public function apply(dt:Number):void { + if (satisfied) return; + // Расчитываем проекцию скорости относительной скорости + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 + var vel:Vector3 = body1.state.velocity; + var rot:Vector3 = body1.state.rotation; +// v1Cross2(state.rotation, wr1); + var vx:Number = vel.x + rot.y*wr1.z - rot.z*wr1.y; + var vy:Number = vel.y + rot.z*wr1.x - rot.x*wr1.z; + var vz:Number = vel.z + rot.x*wr1.y - rot.y*wr1.x; + // V2 = V2_c + w2%r2 + if (body2 != null) { + vel = body2.state.velocity; + rot = body2.state.rotation; +// _v2.vCross2(state.rotation, wr2); + vx -= vel.x + rot.y*wr2.z - rot.z*wr2.y; + vy -= vel.y + rot.z*wr2.x - rot.x*wr2.z; + vz -= vel.z + rot.x*wr2.y - rot.y*wr2.x; + } + var closingVel:Number = vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + if (closingVel > minClosingVel) return; + var impulse:Number = (minClosingVel - closingVel)/velByUnitImpulseN; + if (body1.movable) { + body1.applyRelPosWorldImpulse(wr1, impulseDirection, impulse); + } + if (body2 != null && body2.movable) { + body2.applyRelPosWorldImpulse(wr2, impulseDirection, -impulse); + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/constraints/Constraint.as b/0.0.9.1/src/alternativa/physics/constraints/Constraint.as new file mode 100644 index 0000000..8607442 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/constraints/Constraint.as @@ -0,0 +1,35 @@ +package alternativa.physics.constraints { + + import alternativa.physics.PhysicsScene; + import alternativa.physics.altphysics; + + use namespace altphysics; + + /** + * + */ + public class Constraint { + + altphysics var satisfied:Boolean; + altphysics var world:PhysicsScene; + + /** + * + */ + public function Constraint() { + } + + /** + * @param dt + */ + public function preProcess(dt:Number):void { + } + + /** + * @param dt + */ + public function apply(dt:Number):void { + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/constraints/MaxDistanceConstraint.as b/0.0.9.1/src/alternativa/physics/constraints/MaxDistanceConstraint.as new file mode 100644 index 0000000..d29f518 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/constraints/MaxDistanceConstraint.as @@ -0,0 +1,183 @@ +package alternativa.physics.constraints { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * + */ + public class MaxDistanceConstraint extends Constraint { + // Первое тело ограничения + altphysics var body1:Body; + // Второе тело ограничения. Может быть null. + altphysics var body2:Body; + // Первая точка крепления в системе координат первого тела + altphysics var r1:Vector3; + // Вторая точка привязки в системе координат второго тела. В случае отсутствия второго тела, координаты задаются в мировой системе. + altphysics var r2:Vector3; + // Максимально допустимое расстояние между точками крепления + altphysics var maxDistance:Number; + // Радиус-вектор первой точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr1:Vector3 = new Vector3(); + // Радиус-вектор второй точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr2:Vector3 = new Vector3(); + // Минимальная скорость сближения для удовлетворения ограничения + private var minClosingVel:Number; + // Изменение относительной скорости на единицу импульса + private var velByUnitImpulseN:Number; + // Направление корректирующего импульса + private var impulseDirection:Vector3 = new Vector3(); + + // Вспомогательные переменные +// private static var _v:Vector3 = new Vector3(); +// private static var _v1:Vector3 = new Vector3(); +// private static var _v2:Vector3 = new Vector3(); + + /** + * @param body1 + * @param body2 + * @param r1 + * @param r2 + * @param maxDistance + */ + public function MaxDistanceConstraint(body1:Body, body2:Body, r1:Vector3, r2:Vector3, maxDistance:Number) { + super(); + this.body1 = body1; + this.body2 = body2; + this.r1 = r1.vClone(); + this.r2 = r2.vClone(); + this.maxDistance = maxDistance; + } + + /** + * @param dt + */ + override public function preProcess(dt:Number):void { + // Вычислим расстояние между точками + var m:Matrix3 = body1.baseMatrix; + wr1.x = m.a*r1.x + m.b*r1.y + m.c*r1.z; + wr1.y = m.e*r1.x + m.f*r1.y + m.g*r1.z; + wr1.z = m.i*r1.x + m.j*r1.y + m.k*r1.z; + if (body2 != null) { + m = body2.baseMatrix; + wr2.x = m.a*r2.x + m.b*r2.y + m.c*r2.z; + wr2.y = m.e*r2.x + m.f*r2.y + m.g*r2.z; + wr2.z = m.i*r2.x + m.j*r2.y + m.k*r2.z; + } else { + wr2.x = r2.x; + wr2.y = r2.y; + wr2.z = r2.z; + } + var p1:Vector3 = body1.state.pos; + impulseDirection.x = wr2.x - wr1.x - p1.x; + impulseDirection.y = wr2.y - wr1.y - p1.y; + impulseDirection.z = wr2.z - wr1.z - p1.z; + if (body2 != null) { + var p2:Vector3 = body2.state.pos; + impulseDirection.x += p2.x; + impulseDirection.y += p2.y; + impulseDirection.z += p2.z; + } + + var len:Number = Math.sqrt(impulseDirection.x*impulseDirection.x + impulseDirection.y*impulseDirection.y + impulseDirection.z*impulseDirection.z); + var delta:Number = len - maxDistance; + if (delta > 0) { + satisfied = false; + if (len < 0.001) { + impulseDirection.x = 1; + } else { + len = 1/len; + impulseDirection.x *= len; + impulseDirection.y *= len; + impulseDirection.z *= len; + } + minClosingVel = delta/(world.penResolutionSteps*dt); + if (minClosingVel > world.maxPenResolutionSpeed) { + minClosingVel = world.maxPenResolutionSpeed; + } + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + var x:Number; + var y:Number; + var z:Number; + var vx:Number; + var vy:Number; + var vz:Number; + velByUnitImpulseN = 0; + if (body1.movable) { +// velByUnitImpulseN += body1.invMass + _v.vCross2(wr1, impulseDirection).vTransformBy3(body1.invInertiaWorld).vCross(wr1).vDot(impulseDirection); + vx = wr1.y*impulseDirection.z - wr1.z*impulseDirection.y; + vy = wr1.z*impulseDirection.x - wr1.x*impulseDirection.z; + vz = wr1.x*impulseDirection.y - wr1.y*impulseDirection.x; + + m = body1.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr1.z - z*wr1.y; + vy = z*wr1.x - x*wr1.z; + vz = x*wr1.y - y*wr1.x; + velByUnitImpulseN += body1.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + if (body2 != null && body2.movable) { +// velByUnitImpulseN += body2.invMass + _v.vCross2(wr2, impulseDirection).vTransformBy3(body2.invInertiaWorld).vCross(wr2).vDot(impulseDirection); + vx = wr2.y*impulseDirection.z - wr2.z*impulseDirection.y; + vy = wr2.z*impulseDirection.x - wr2.x*impulseDirection.z; + vz = wr2.x*impulseDirection.y - wr2.y*impulseDirection.x; + + m = body2.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr2.z - z*wr2.y; + vy = z*wr2.x - x*wr2.z; + vz = x*wr2.y - y*wr2.x; + velByUnitImpulseN += body2.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + } else { + satisfied = true; + } + } + + /** + * @param dt + */ + override public function apply(dt:Number):void { + if (satisfied) return; + // Расчитываем проекцию скорости относительной скорости + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 + var vel:Vector3 = body1.state.velocity; + var rot:Vector3 = body1.state.rotation; +// v1Cross2(state.rotation, wr1); + var vx:Number = vel.x + rot.y*wr1.z - rot.z*wr1.y; + var vy:Number = vel.y + rot.z*wr1.x - rot.x*wr1.z; + var vz:Number = vel.z + rot.x*wr1.y - rot.y*wr1.x; + // V2 = V2_c + w2%r2 + if (body2 != null) { + vel = body2.state.velocity; + rot = body2.state.rotation; +// _v2.vCross2(state.rotation, wr2); + vx -= vel.x + rot.y*wr2.z - rot.z*wr2.y; + vy -= vel.y + rot.z*wr2.x - rot.x*wr2.z; + vz -= vel.z + rot.x*wr2.y - rot.y*wr2.x; + } + var closingVel:Number = vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + if (closingVel > minClosingVel) return; + var impulse:Number = (minClosingVel - closingVel)/velByUnitImpulseN; + if (body1.movable) { + body1.applyRelPosWorldImpulse(wr1, impulseDirection, impulse); + } + if (body2 != null && body2.movable) { + body2.applyRelPosWorldImpulse(wr2, impulseDirection, -impulse); + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/primitives/.svn/all-wcprops b/0.0.9.1/src/alternativa/physics/primitives/.svn/all-wcprops new file mode 100644 index 0000000..440db5b --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/primitives/.svn/all-wcprops @@ -0,0 +1,35 @@ +K 25 +svn:wc:ra_dav:version-url +V 114 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/primitives +END +RigidPlane.as +K 25 +svn:wc:ra_dav:version-url +V 128 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/primitives/RigidPlane.as +END +RigidSphere.as +K 25 +svn:wc:ra_dav:version-url +V 129 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/primitives/RigidSphere.as +END +RigidBox.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/primitives/RigidBox.as +END +RigidCylinder.as +K 25 +svn:wc:ra_dav:version-url +V 131 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/primitives/RigidCylinder.as +END +RigidRect.as +K 25 +svn:wc:ra_dav:version-url +V 127 +/!svn/ver/29407/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/primitives/RigidRect.as +END diff --git a/0.0.9.1/src/alternativa/physics/primitives/.svn/entries b/0.0.9.1/src/alternativa/physics/primitives/.svn/entries new file mode 100644 index 0000000..b8312e2 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/primitives/.svn/entries @@ -0,0 +1,88 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.1/src/alternativa/physics/primitives +http://svndev.alternativaplatform.com + + + +2009-11-24T08:44:15.941480Z +23758 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +RigidPlane.as +file + + + + +2010-10-28T04:32:44.000000Z +8faf607cf544d7388852668f5cd5672f +2009-11-24T08:44:15.941480Z +23758 +mike + +RigidSphere.as +file + + + + +2010-10-28T04:32:44.000000Z +774b0c23b084bfffb3381eb40e616deb +2009-11-24T08:44:15.941480Z +23758 +mike + +RigidBox.as +file + + + + +2010-10-28T04:32:44.000000Z +7b93f349a2818f22b76d9590cb05f84a +2009-11-24T08:44:15.941480Z +23758 +mike + +RigidCylinder.as +file + + + + +2010-10-28T04:32:44.000000Z +e9ce0eeb457428727cc10d2e53a0f164 +2009-11-24T08:44:15.941480Z +23758 +mike + +RigidRect.as +file + + + + +2010-10-28T04:32:44.000000Z +2eb6d0ebde8c2b46c4b0f810b1d64e1e +2009-11-24T08:44:15.941480Z +23758 +mike + diff --git a/0.0.9.1/src/alternativa/physics/primitives/.svn/format b/0.0.9.1/src/alternativa/physics/primitives/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/primitives/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.1/src/alternativa/physics/primitives/.svn/text-base/RigidBox.as.svn-base b/0.0.9.1/src/alternativa/physics/primitives/.svn/text-base/RigidBox.as.svn-base new file mode 100644 index 0000000..24e9b9f --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/primitives/.svn/text-base/RigidBox.as.svn-base @@ -0,0 +1,56 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * + */ + public class RigidBox extends Body { + + /** + * + * @param halfSize + * @param mass + */ + public function RigidBox(halfSize:Vector3, mass:Number) { + super(0, Matrix3.ZERO); + addCollisionPrimitive(new CollisionBox(halfSize, 1)); + setParams(halfSize, mass); + } + + /** + * @param halfSize + * @param mass + */ + public function setParams(halfSize:Vector3, mass:Number):void { + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + invInertia.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + invInertia.a = 3*invMass/(yy + zz); + invInertia.f = 3*invMass/(zz + xx); + invInertia.k = 3*invMass/(xx + yy); + } + var prim:CollisionBox = CollisionBox(collisionPrimitives.head.primitive); + prim.hs.vCopy(halfSize); + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/primitives/.svn/text-base/RigidCylinder.as.svn-base b/0.0.9.1/src/alternativa/physics/primitives/.svn/text-base/RigidCylinder.as.svn-base new file mode 100644 index 0000000..4ff65dd --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/primitives/.svn/text-base/RigidCylinder.as.svn-base @@ -0,0 +1,32 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.math.Matrix3; + + use namespace altphysics; + + public class RigidCylinder extends Body { + + altphysics var r:Number; + altphysics var h:Number; + + public function RigidCylinder(mass:Number, radius:Number, height:Number) { + super(0, Matrix3.ZERO); + setParams(mass, radius, height); + } + + public function setParams(mass:Number, radius:Number, height:Number):void { + r = radius; + h = height; + invInertiaWorld.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + invInertia.a = invInertia.f = mass*(h*h/12 + r*r/4); + invInertia.k = 0.5*mass*r*r; + invInertia.invert(); + } + } + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/primitives/.svn/text-base/RigidPlane.as.svn-base b/0.0.9.1/src/alternativa/physics/primitives/.svn/text-base/RigidPlane.as.svn-base new file mode 100644 index 0000000..d9ebf9b --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/primitives/.svn/text-base/RigidPlane.as.svn-base @@ -0,0 +1,23 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + public class RigidPlane extends Body { + + public var normal:Vector3 = new Vector3(); + public var offset:Number; + + public function RigidPlane() { + super(0, Matrix3.ZERO); + this.normal.vCopy(Vector3.Z_AXIS); + this.offset = 0; + movable = false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/primitives/.svn/text-base/RigidRect.as.svn-base b/0.0.9.1/src/alternativa/physics/primitives/.svn/text-base/RigidRect.as.svn-base new file mode 100644 index 0000000..4591931 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/primitives/.svn/text-base/RigidRect.as.svn-base @@ -0,0 +1,20 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + public class RigidRect extends Body { + + public function RigidRect(halfWidth:Number, halfLength:Number) { + super(0, Matrix3.ZERO); + movable = false; + addCollisionPrimitive(new CollisionRect(new Vector3(halfWidth, halfLength, 0), 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/primitives/.svn/text-base/RigidSphere.as.svn-base b/0.0.9.1/src/alternativa/physics/primitives/.svn/text-base/RigidSphere.as.svn-base new file mode 100644 index 0000000..8ad180c --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/primitives/.svn/text-base/RigidSphere.as.svn-base @@ -0,0 +1,29 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.math.Matrix3; + + use namespace altphysics; + + public class RigidSphere extends Body { + + public function RigidSphere(radius:Number, mass:Number) { + var m:Matrix3; + if (mass == Infinity) { + mass = 0; + m = Matrix3.ZERO; + } else { + mass = 1/mass; + var r2:Number = radius*radius; + m = new Matrix3(); + m.a = m.f = m.k = 2.5*mass/r2; + } + super(mass, m); + if (mass == 0) movable = false; + addCollisionPrimitive(new CollisionSphere(radius, 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/primitives/RigidBox.as b/0.0.9.1/src/alternativa/physics/primitives/RigidBox.as new file mode 100644 index 0000000..24e9b9f --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/primitives/RigidBox.as @@ -0,0 +1,56 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * + */ + public class RigidBox extends Body { + + /** + * + * @param halfSize + * @param mass + */ + public function RigidBox(halfSize:Vector3, mass:Number) { + super(0, Matrix3.ZERO); + addCollisionPrimitive(new CollisionBox(halfSize, 1)); + setParams(halfSize, mass); + } + + /** + * @param halfSize + * @param mass + */ + public function setParams(halfSize:Vector3, mass:Number):void { + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + invInertia.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + invInertia.a = 3*invMass/(yy + zz); + invInertia.f = 3*invMass/(zz + xx); + invInertia.k = 3*invMass/(xx + yy); + } + var prim:CollisionBox = CollisionBox(collisionPrimitives.head.primitive); + prim.hs.vCopy(halfSize); + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/primitives/RigidCylinder.as b/0.0.9.1/src/alternativa/physics/primitives/RigidCylinder.as new file mode 100644 index 0000000..4ff65dd --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/primitives/RigidCylinder.as @@ -0,0 +1,32 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.math.Matrix3; + + use namespace altphysics; + + public class RigidCylinder extends Body { + + altphysics var r:Number; + altphysics var h:Number; + + public function RigidCylinder(mass:Number, radius:Number, height:Number) { + super(0, Matrix3.ZERO); + setParams(mass, radius, height); + } + + public function setParams(mass:Number, radius:Number, height:Number):void { + r = radius; + h = height; + invInertiaWorld.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + invInertia.a = invInertia.f = mass*(h*h/12 + r*r/4); + invInertia.k = 0.5*mass*r*r; + invInertia.invert(); + } + } + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/primitives/RigidPlane.as b/0.0.9.1/src/alternativa/physics/primitives/RigidPlane.as new file mode 100644 index 0000000..d9ebf9b --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/primitives/RigidPlane.as @@ -0,0 +1,23 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + public class RigidPlane extends Body { + + public var normal:Vector3 = new Vector3(); + public var offset:Number; + + public function RigidPlane() { + super(0, Matrix3.ZERO); + this.normal.vCopy(Vector3.Z_AXIS); + this.offset = 0; + movable = false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/primitives/RigidRect.as b/0.0.9.1/src/alternativa/physics/primitives/RigidRect.as new file mode 100644 index 0000000..4591931 --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/primitives/RigidRect.as @@ -0,0 +1,20 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + public class RigidRect extends Body { + + public function RigidRect(halfWidth:Number, halfLength:Number) { + super(0, Matrix3.ZERO); + movable = false; + addCollisionPrimitive(new CollisionRect(new Vector3(halfWidth, halfLength, 0), 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.9.1/src/alternativa/physics/primitives/RigidSphere.as b/0.0.9.1/src/alternativa/physics/primitives/RigidSphere.as new file mode 100644 index 0000000..8ad180c --- /dev/null +++ b/0.0.9.1/src/alternativa/physics/primitives/RigidSphere.as @@ -0,0 +1,29 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.math.Matrix3; + + use namespace altphysics; + + public class RigidSphere extends Body { + + public function RigidSphere(radius:Number, mass:Number) { + var m:Matrix3; + if (mass == Infinity) { + mass = 0; + m = Matrix3.ZERO; + } else { + mass = 1/mass; + var r2:Number = radius*radius; + m = new Matrix3(); + m.a = m.f = m.k = 2.5*mass/r2; + } + super(mass, m); + if (mass == 0) movable = false; + addCollisionPrimitive(new CollisionSphere(radius, 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/.actionScriptProperties b/0.0.9.2/.actionScriptProperties new file mode 100644 index 0000000..f73adc8 --- /dev/null +++ b/0.0.9.2/.actionScriptProperties @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/0.0.9.2/.flexLibProperties b/0.0.9.2/.flexLibProperties new file mode 100644 index 0000000..0b65396 --- /dev/null +++ b/0.0.9.2/.flexLibProperties @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/0.0.9.2/.project b/0.0.9.2/.project new file mode 100644 index 0000000..1f1425f --- /dev/null +++ b/0.0.9.2/.project @@ -0,0 +1,19 @@ + + + AlternativaPhysics + + + + + + com.adobe.flexbuilder.project.flexbuilder + + + + + + org.maven.ide.eclipse.maven2Nature + com.adobe.flexbuilder.project.flexlibnature + com.adobe.flexbuilder.project.actionscriptnature + + diff --git a/0.0.9.2/.settings/.svn/all-wcprops b/0.0.9.2/.settings/.svn/all-wcprops new file mode 100644 index 0000000..2810333 --- /dev/null +++ b/0.0.9.2/.settings/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 89 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/.settings +END +org.eclipse.core.resources.prefs +K 25 +svn:wc:ra_dav:version-url +V 122 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/.settings/org.eclipse.core.resources.prefs +END diff --git a/0.0.9.2/.settings/.svn/entries b/0.0.9.2/.settings/.svn/entries new file mode 100644 index 0000000..2795f26 --- /dev/null +++ b/0.0.9.2/.settings/.svn/entries @@ -0,0 +1,40 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/.settings +http://svndev.alternativaplatform.com + + + +2009-04-01T12:25:29.638016Z +10301 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +org.eclipse.core.resources.prefs +file + + + + +2010-10-28T04:32:44.000000Z +4f70b476f3e5075e399505021df2f89b +2009-04-01T12:25:29.638016Z +10301 +mike + diff --git a/0.0.9.2/.settings/.svn/format b/0.0.9.2/.settings/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.2/.settings/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.2/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base b/0.0.9.2/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base new file mode 100644 index 0000000..b3d0d49 --- /dev/null +++ b/0.0.9.2/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base @@ -0,0 +1,3 @@ +#Fri Mar 20 00:19:33 YEKT 2009 +eclipse.preferences.version=1 +encoding/=UTF-8 diff --git a/0.0.9.2/.settings/org.eclipse.core.resources.prefs b/0.0.9.2/.settings/org.eclipse.core.resources.prefs new file mode 100644 index 0000000..b3d0d49 --- /dev/null +++ b/0.0.9.2/.settings/org.eclipse.core.resources.prefs @@ -0,0 +1,3 @@ +#Fri Mar 20 00:19:33 YEKT 2009 +eclipse.preferences.version=1 +encoding/=UTF-8 diff --git a/0.0.9.2/.svn/all-wcprops b/0.0.9.2/.svn/all-wcprops new file mode 100644 index 0000000..f04f5e9 --- /dev/null +++ b/0.0.9.2/.svn/all-wcprops @@ -0,0 +1,35 @@ +K 25 +svn:wc:ra_dav:version-url +V 79 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2 +END +.flexLibProperties +K 25 +svn:wc:ra_dav:version-url +V 98 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/.flexLibProperties +END +.project +K 25 +svn:wc:ra_dav:version-url +V 88 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/.project +END +pom.xml +K 25 +svn:wc:ra_dav:version-url +V 87 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/pom.xml +END +.actionScriptProperties +K 25 +svn:wc:ra_dav:version-url +V 103 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/.actionScriptProperties +END +AlternativaPhysics.iml +K 25 +svn:wc:ra_dav:version-url +V 102 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/AlternativaPhysics.iml +END diff --git a/0.0.9.2/.svn/dir-prop-base b/0.0.9.2/.svn/dir-prop-base new file mode 100644 index 0000000..c31ca28 --- /dev/null +++ b/0.0.9.2/.svn/dir-prop-base @@ -0,0 +1,7 @@ +K 13 +svn:mergeinfo +V 278 +/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized:19806-19860 +/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.7.0.LinkedList:20360-22217,22226-22252 +/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D:13892-14098 +END diff --git a/0.0.9.2/.svn/entries b/0.0.9.2/.svn/entries new file mode 100644 index 0000000..4215566 --- /dev/null +++ b/0.0.9.2/.svn/entries @@ -0,0 +1,97 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2 +http://svndev.alternativaplatform.com + + + +2010-03-06T18:35:38.097601Z +29411 +mike +has-props + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +META-INF +dir + +.flexLibProperties +file + + + + +2010-10-28T04:32:44.000000Z +b170dccb732dcb12462ac421cdca9723 +2010-02-12T12:34:25.192477Z +28424 +mike + +.project +file + + + + +2010-10-28T04:32:44.000000Z +1e90cc68b52b93173b2b5de88eb5c3a3 +2009-04-20T17:15:55.260110Z +11541 +mike + +src +dir + +pom.xml +file + + + + +2010-10-28T04:32:44.000000Z +dc7965d6172d265e83100b9fdee16d5f +2010-03-06T18:35:38.097601Z +29411 +mike + +.actionScriptProperties +file + + + + +2010-10-28T04:32:44.000000Z +b8656f882fff2baedd57a92db86fcf8c +2010-01-14T07:05:07.823716Z +26358 +mike + +AlternativaPhysics.iml +file + + + + +2010-10-28T04:32:44.000000Z +5162d434bd39d50f59a8057c1fa2c961 +2010-02-19T13:52:11.808507Z +28758 +mike + +.settings +dir + diff --git a/0.0.9.2/.svn/format b/0.0.9.2/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.2/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.2/.svn/text-base/.actionScriptProperties.svn-base b/0.0.9.2/.svn/text-base/.actionScriptProperties.svn-base new file mode 100644 index 0000000..f73adc8 --- /dev/null +++ b/0.0.9.2/.svn/text-base/.actionScriptProperties.svn-base @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/0.0.9.2/.svn/text-base/.flexLibProperties.svn-base b/0.0.9.2/.svn/text-base/.flexLibProperties.svn-base new file mode 100644 index 0000000..0b65396 --- /dev/null +++ b/0.0.9.2/.svn/text-base/.flexLibProperties.svn-base @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/0.0.9.2/.svn/text-base/.project.svn-base b/0.0.9.2/.svn/text-base/.project.svn-base new file mode 100644 index 0000000..1f1425f --- /dev/null +++ b/0.0.9.2/.svn/text-base/.project.svn-base @@ -0,0 +1,19 @@ + + + AlternativaPhysics + + + + + + com.adobe.flexbuilder.project.flexbuilder + + + + + + org.maven.ide.eclipse.maven2Nature + com.adobe.flexbuilder.project.flexlibnature + com.adobe.flexbuilder.project.actionscriptnature + + diff --git a/0.0.9.2/.svn/text-base/AlternativaPhysics.iml.svn-base b/0.0.9.2/.svn/text-base/AlternativaPhysics.iml.svn-base new file mode 100644 index 0000000..7783af3 --- /dev/null +++ b/0.0.9.2/.svn/text-base/AlternativaPhysics.iml.svn-base @@ -0,0 +1,65 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/0.0.9.2/.svn/text-base/pom.xml.svn-base b/0.0.9.2/.svn/text-base/pom.xml.svn-base new file mode 100644 index 0000000..095262f --- /dev/null +++ b/0.0.9.2/.svn/text-base/pom.xml.svn-base @@ -0,0 +1,17 @@ + + 4.0.0 + platform.clients.fp10.libraries + AlternativaPhysics + swc + 0.0.9.2 + + platform.tools.maven + FlashBasePom + 2.0.0.0 + + + scm:svn:http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2 + + + + diff --git a/0.0.9.2/AlternativaPhysics.iml b/0.0.9.2/AlternativaPhysics.iml new file mode 100644 index 0000000..7783af3 --- /dev/null +++ b/0.0.9.2/AlternativaPhysics.iml @@ -0,0 +1,65 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/0.0.9.2/META-INF/.svn/all-wcprops b/0.0.9.2/META-INF/.svn/all-wcprops new file mode 100644 index 0000000..78d657e --- /dev/null +++ b/0.0.9.2/META-INF/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 88 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/META-INF +END +MANIFEST.MF +K 25 +svn:wc:ra_dav:version-url +V 100 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/META-INF/MANIFEST.MF +END diff --git a/0.0.9.2/META-INF/.svn/entries b/0.0.9.2/META-INF/.svn/entries new file mode 100644 index 0000000..1a1e57b --- /dev/null +++ b/0.0.9.2/META-INF/.svn/entries @@ -0,0 +1,40 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/META-INF +http://svndev.alternativaplatform.com + + + +2010-03-06T18:33:59.318892Z +29409 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +MANIFEST.MF +file + + + + +2010-10-28T04:32:44.000000Z +7b64dceb50a6905850ff00a0bfbe77eb +2009-04-20T17:15:55.260110Z +11541 +mike + diff --git a/0.0.9.2/META-INF/.svn/format b/0.0.9.2/META-INF/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.2/META-INF/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.2/META-INF/.svn/text-base/MANIFEST.MF.svn-base b/0.0.9.2/META-INF/.svn/text-base/MANIFEST.MF.svn-base new file mode 100644 index 0000000..5066a55 --- /dev/null +++ b/0.0.9.2/META-INF/.svn/text-base/MANIFEST.MF.svn-base @@ -0,0 +1 @@ +Bundle-Name: platform.clients.fp10.libraries.AlternativaPhysics diff --git a/0.0.9.2/META-INF/MANIFEST.MF b/0.0.9.2/META-INF/MANIFEST.MF new file mode 100644 index 0000000..5066a55 --- /dev/null +++ b/0.0.9.2/META-INF/MANIFEST.MF @@ -0,0 +1 @@ +Bundle-Name: platform.clients.fp10.libraries.AlternativaPhysics diff --git a/0.0.9.2/pom.xml b/0.0.9.2/pom.xml new file mode 100644 index 0000000..095262f --- /dev/null +++ b/0.0.9.2/pom.xml @@ -0,0 +1,17 @@ + + 4.0.0 + platform.clients.fp10.libraries + AlternativaPhysics + swc + 0.0.9.2 + + platform.tools.maven + FlashBasePom + 2.0.0.0 + + + scm:svn:http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2 + + + + diff --git a/0.0.9.2/src/.svn/all-wcprops b/0.0.9.2/src/.svn/all-wcprops new file mode 100644 index 0000000..daa77b2 --- /dev/null +++ b/0.0.9.2/src/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 83 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src +END diff --git a/0.0.9.2/src/.svn/entries b/0.0.9.2/src/.svn/entries new file mode 100644 index 0000000..cd12b79 --- /dev/null +++ b/0.0.9.2/src/.svn/entries @@ -0,0 +1,31 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src +http://svndev.alternativaplatform.com + + + +2010-03-06T16:51:19.781174Z +29404 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +alternativa +dir + diff --git a/0.0.9.2/src/.svn/format b/0.0.9.2/src/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.2/src/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.2/src/alternativa/.svn/all-wcprops b/0.0.9.2/src/alternativa/.svn/all-wcprops new file mode 100644 index 0000000..442ce4a --- /dev/null +++ b/0.0.9.2/src/alternativa/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 95 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa +END diff --git a/0.0.9.2/src/alternativa/.svn/entries b/0.0.9.2/src/alternativa/.svn/entries new file mode 100644 index 0000000..95c1f8f --- /dev/null +++ b/0.0.9.2/src/alternativa/.svn/entries @@ -0,0 +1,34 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa +http://svndev.alternativaplatform.com + + + +2010-03-06T16:51:19.781174Z +29404 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +physics +dir + +math +dir + diff --git a/0.0.9.2/src/alternativa/.svn/format b/0.0.9.2/src/alternativa/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.2/src/alternativa/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.2/src/alternativa/math/.svn/all-wcprops b/0.0.9.2/src/alternativa/math/.svn/all-wcprops new file mode 100644 index 0000000..97d3069 --- /dev/null +++ b/0.0.9.2/src/alternativa/math/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 100 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/math +END +Quaternion.as +K 25 +svn:wc:ra_dav:version-url +V 114 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/math/Quaternion.as +END +Vector3.as +K 25 +svn:wc:ra_dav:version-url +V 111 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/math/Vector3.as +END +Matrix3.as +K 25 +svn:wc:ra_dav:version-url +V 111 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/math/Matrix3.as +END +Matrix4.as +K 25 +svn:wc:ra_dav:version-url +V 111 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/math/Matrix4.as +END diff --git a/0.0.9.2/src/alternativa/math/.svn/entries b/0.0.9.2/src/alternativa/math/.svn/entries new file mode 100644 index 0000000..7bad352 --- /dev/null +++ b/0.0.9.2/src/alternativa/math/.svn/entries @@ -0,0 +1,76 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/math +http://svndev.alternativaplatform.com + + + +2010-02-12T12:34:25.192477Z +28424 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Quaternion.as +file + + + + +2010-10-28T04:32:44.000000Z +7332c9620420bd10062d913decfb9501 +2010-02-12T12:34:25.192477Z +28424 +mike + +Vector3.as +file + + + + +2010-10-28T04:32:44.000000Z +a1571b527786783cef1a9f56db3f2af6 +2010-02-12T12:34:25.192477Z +28424 +mike + +Matrix3.as +file + + + + +2010-10-28T04:32:44.000000Z +0054bd91fe694474a28c8ec17697292b +2010-02-12T12:34:25.192477Z +28424 +mike + +Matrix4.as +file + + + + +2010-10-28T04:32:44.000000Z +47b31d4350499ebe77a5973a938e4b44 +2010-02-12T12:34:25.192477Z +28424 +mike + diff --git a/0.0.9.2/src/alternativa/math/.svn/format b/0.0.9.2/src/alternativa/math/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.2/src/alternativa/math/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.2/src/alternativa/math/.svn/text-base/Matrix3.as.svn-base b/0.0.9.2/src/alternativa/math/.svn/text-base/Matrix3.as.svn-base new file mode 100644 index 0000000..244cc98 --- /dev/null +++ b/0.0.9.2/src/alternativa/math/.svn/text-base/Matrix3.as.svn-base @@ -0,0 +1,390 @@ +package alternativa.math { + import flash.geom.Vector3D; + + /** + * + */ + public class Matrix3 { + + public static const ZERO:Matrix3 = new Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0); + public static const IDENTITY:Matrix3 = new Matrix3(); + + public var a:Number; + public var b:Number; + public var c:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix3(a:Number = 1, b:Number = 0, c:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1) { + this.a = a; + this.b = b; + this.c = c; + + this.e = e; + this.f = f; + this.g = g; + + this.i = i; + this.j = j; + this.k = k; + } + + /** + * + * @return + */ + public function toIdentity():Matrix3 { + a = f = k = 1; + b = c = e = g = i = j = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + var det:Number = 1/(-cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk); + + a = (ff*kk - gg*jj)*det; + b = (cc*jj - bb*kk)*det; + c = (bb*gg - cc*ff)*det; + e = (gg*ii - ee*kk)*det; + f = (aa*kk - cc*ii)*det; + g = (cc*ee - aa*gg)*det; + i = (ee*jj - ff*ii)*det; + j = (bb*ii - aa*jj)*det; + k = (aa*ff - bb*ee)*det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prependTransposed(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.b + cc*m.c; + b = aa*m.e + bb*m.f + cc*m.g; + c = aa*m.i + bb*m.j + cc*m.k; + e = ee*m.a + ff*m.b + gg*m.c; + f = ee*m.e + ff*m.f + gg*m.g; + g = ee*m.i + ff*m.j + gg*m.k; + i = ii*m.a + jj*m.b + kk*m.c; + j = ii*m.e + jj*m.f + kk*m.g; + k = ii*m.i + jj*m.j + kk*m.k; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix3):Matrix3 { + a += m.a; + b += m.b; + c += m.c; + e += m.e; + f += m.f; + g += m.g; + i += m.i; + j += m.j; + k += m.k; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix3):Matrix3 { + a -= m.a; + b -= m.b; + c -= m.c; + e -= m.e; + f -= m.f; + g -= m.g; + i -= m.i; + j -= m.j; + k -= m.k; + + return this; + } + + /** + * + * @return + */ + public function transpose():Matrix3 { + var tmp:Number = b; + b = e; + e = tmp; + tmp = c; + c = i; + i = tmp; + tmp = g; + g = j; + j = tmp; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector3To3D(vin:Vector3, vout:Vector3D):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * + * @param matrix + */ + public function createSkewSymmetric(v:Vector3):Matrix3 { + a = f = k = 0; + b = -v.z; + c = v.y; + e = v.z; + g = -v.x; + i = -v.y; + j = v.x; + return this; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix3):Matrix3 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @return + */ + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):Matrix3 { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + + return this; + } + + /** + * Метод формирует матрицу поворота на заданный угол относительно заданной оси. + * + * @param axis нормализованный вектор, задающий ось, относительно которой выполняется поворот + * @param angle угол поворота в радианах + */ + public function fromAxisAngle(axis:Vector3, angle:Number):void { + var c1:Number = Math.cos(angle); + var s:Number = Math.sin(angle); + var t:Number = 1 - c1; + var x:Number = axis.x; + var y:Number = axis.y; + var z:Number = axis.z; + + a = t*x*x + c1; + b = t*x*y - z*s; + c = t*x*z + y*s; + + e = t*x*y + z*s; + f = t*y*y + c1; + g = t*y*z - x*s; + + i = t*x*z - y*s; + j = t*y*z + x*s; + k = t*z*z + c1; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix3 { + return new Matrix3(a, b, c, e, f, g, i, j, k); + } + + /** + * + * @return + */ + public function toString():String { + return "[Matrix3 (" + a + ", " + b + ", " + c + "), (" + e + ", " + f + ", " + g + "), (" + i + ", " + j + ", " + k + ")]"; + } + + /** + * @param angles + */ + public function getEulerAngles(angles:Vector3):void { + if (-1 < i && i < 1) { + angles.x = Math.atan2(j, k); + angles.y = -Math.asin(i); + angles.z = Math.atan2(e, a); + } else { + angles.x = 0; + angles.y = (i <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-b, f); + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/math/.svn/text-base/Matrix4.as.svn-base b/0.0.9.2/src/alternativa/math/.svn/text-base/Matrix4.as.svn-base new file mode 100644 index 0000000..d06eb5f --- /dev/null +++ b/0.0.9.2/src/alternativa/math/.svn/text-base/Matrix4.as.svn-base @@ -0,0 +1,475 @@ +package alternativa.math { + + /** + * + */ + public class Matrix4 { + + public static const IDENTITY:Matrix4 = new Matrix4(); + + public var a:Number; + public var b:Number; + public var c:Number; + public var d:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + public var h:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + public var l:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix4(a:Number = 1, b:Number = 0, c:Number = 0, d:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, h:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1, l:Number = 0) { + this.a = a; + this.b = b; + this.c = c; + this.d = d; + + this.e = e; + this.f = f; + this.g = g; + this.h = h; + + this.i = i; + this.j = j; + this.k = k; + this.l = l; + } + + /** + * + * @return + */ + public function toIdentity():Matrix4 { + a = f = k = 1; + b = c = e = g = i = j = d = h = l = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + var det:Number = -cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk; + + a = (-gg*jj + ff*kk)/det; + b = (cc*jj - bb*kk)/det; + c = (-cc*ff + bb*gg)/det; + d = (dd*gg*jj - cc*hh*jj - dd*ff*kk + bb*hh*kk + cc*ff*ll - bb*gg*ll)/det; + e = (gg*ii - ee*kk)/det; + f = (-cc*ii + aa*kk)/det; + g = (cc*ee - aa*gg)/det; + h = (cc*hh*ii - dd*gg*ii + dd*ee*kk - aa*hh*kk - cc*ee*ll + aa*gg*ll)/det; + i = (-ff*ii + ee*jj)/det; + j = (bb*ii - aa*jj)/det; + k = (-bb*ee + aa*ff)/det; + l = (dd*ff*ii - bb*hh*ii - dd*ee*jj + aa*hh*jj + bb*ee*ll - aa*ff*ll)/det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + d = m.a*dd + m.b*hh + m.c*ll + m.d; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + h = m.e*dd + m.f*hh + m.g*ll + m.h; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + l = m.i*dd + m.j*hh + m.k*ll + m.l; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + d = aa*m.d + bb*m.h + cc*m.l + dd; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + h = ee*m.d + ff*m.h + gg*m.l + hh; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + l = ii*m.d + jj*m.h + kk*m.l + ll; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix4):Matrix4 { + a += m.a; + b += m.b; + c += m.c; + d += m.d; + e += m.e; + f += m.f; + g += m.g; + h += m.h; + i += m.i; + j += m.j; + k += m.k; + l += m.l; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix4):Matrix4 { + a -= m.a; + b -= m.b; + c -= m.c; + d -= m.d; + e -= m.e; + f -= m.f; + g -= m.g; + h -= m.h; + i -= m.i; + j -= m.j; + k -= m.k; + l -= m.l; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectors(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + * @param len + */ + public function transformVectorsN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverse(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverseN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(idx:int, axis:Vector3):void { + switch (idx) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + case 3: + axis.x = d; + axis.y = h; + axis.z = l; + return; + } + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix4):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = m.d; + e = m.e; + f = m.f; + g = m.g; + h = m.h; + i = m.i; + j = m.j; + k = m.k; + l = m.l; + + return this; + } + + public function setFromMatrix3(m:Matrix3, offset:Vector3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = offset.x; + e = m.e; + f = m.f; + g = m.g; + h = offset.y; + i = m.i; + j = m.j; + k = m.k; + l = offset.z; + + return this; + } + + public function setOrientationFromMatrix3(m:Matrix3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @param angles + */ + public function getEulerAngles(angles:Vector3):void { + if (-1 < i && i < 1) { + angles.x = Math.atan2(j, k); + angles.y = -Math.asin(i); + angles.z = Math.atan2(e, a); + } else { + angles.x = 0; + angles.y = (i <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-b, f); + } + } + + /** + * Устанавливает координаты матрицы. + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + d = pos.x; + h = pos.y; + l = pos.z; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix4 { + return new Matrix4(a, b, c, d, e, f, g, h, i, j, k, l); + } + + /** + * + * @return + */ + public function toString():String { + return "[Matrix4 [" + a.toFixed(3) + " " + b.toFixed(3) + " " + c.toFixed(3) + " " + d.toFixed(3) + "] [" + e.toFixed(3) + " " + f.toFixed(3) + " " + g.toFixed(3) + " " + h.toFixed(3) + "] [" + i.toFixed(3) + " " + j.toFixed(3) + " " + k.toFixed(3) + " " + l.toFixed(3) + "]]"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/math/.svn/text-base/Quaternion.as.svn-base b/0.0.9.2/src/alternativa/math/.svn/text-base/Quaternion.as.svn-base new file mode 100644 index 0000000..ddc85f5 --- /dev/null +++ b/0.0.9.2/src/alternativa/math/.svn/text-base/Quaternion.as.svn-base @@ -0,0 +1,458 @@ +package alternativa.math { + import flash.geom.Vector3D; + + public class Quaternion { + + public var w:Number; + public var x:Number; + public var y:Number; + public var z:Number; + + public static function multiply(q1:Quaternion, q2:Quaternion, result:Quaternion):void { + result.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z; + result.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y; + result.y = q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z; + result.z = q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x; + } + + public static function createFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngle(axis, angle); + return q; + } + + public static function createFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngleComponents(x, y, z, angle); + return q; + } + + private static var _q:Quaternion = new Quaternion(); + + public function Quaternion(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0) { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + } + + public function reset(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0):Quaternion { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + return this; + } + + public function normalize():Quaternion { + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * Умножает на указанный кватернион слева: this * q + * + * @param q множитель + */ + public function prepend(q:Quaternion):Quaternion { + var ww:Number = w*q.w - x*q.x - y*q.y - z*q.z; + var xx:Number = w*q.x + x*q.w + y*q.z - z*q.y; + var yy:Number = w*q.y + y*q.w + z*q.x - x*q.z; + var zz:Number = w*q.z + z*q.w + x*q.y - y*q.x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Умножает на указанный кватернион справа: q * this + * + * @param q множитель + */ + public function append(q:Quaternion):Quaternion { + var ww:Number = q.w*w - q.x*x - q.y*y - q.z*z; + var xx:Number = q.w*x + q.x*w + q.y*z - q.z*y; + var yy:Number = q.w*y + q.y*w + q.z*x - q.x*z; + var zz:Number = q.w*z + q.z*w + q.x*y - q.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * + * @param vector + */ + public function rotateByVector(v:Vector3):Quaternion { + var ww:Number = -v.x*x - v.y*y - v.z*z; + var xx:Number = v.x*w + v.y*z - v.z*y; + var yy:Number = v.y*w + v.z*x - v.x*z; + var zz:Number = v.z*w + v.x*y - v.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Добавляет вращение, приданное вектором угловой скорости за указанное время. + * + * @param v + * @param scale + */ + public function addScaledVector(v:Vector3, scale:Number):Quaternion { + var vx:Number = v.x*scale; + var vy:Number = v.y*scale; + var vz:Number = v.z*scale; + var ww:Number = -x*vx - y*vy - z*vz; + var xx:Number = vx*w + vy*z - vz*y; + var yy:Number = vy*w + vz*x - vx*z; + var zz:Number = vz*w + vx*y - vy*x; + w += 0.5*ww; + x += 0.5*xx; + y += 0.5*yy; + z += 0.5*zz; + // inlined normalize + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param m + * @return + */ + public function toMatrix3(m:Matrix3):Quaternion { + var xx2:Number = 2.0*x*x; + var yy2:Number = 2.0*y*y; + var zz2:Number = 2.0*z*z; + var xy2:Number = 2.0*x*y; + var yz2:Number = 2.0*y*z; + var zx2:Number = 2.0*z*x; + var wx2:Number = 2.0*w*x; + var wy2:Number = 2.0*w*y; + var wz2:Number = 2.0*w*z; + + m.a = 1.0 - yy2 - zz2; + m.b = xy2 - wz2; + m.c = zx2 + wy2; + + m.e = xy2 + wz2; + m.f = 1.0 - xx2 - zz2; + m.g = yz2 - wx2; + + m.i = zx2 - wy2; + m.j = yz2 + wx2; + m.k = 1.0 - xx2 - yy2; + return this; + } + + /** + * + * @return + */ + public function length():Number { + return Math.sqrt(w*w + x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function lengthSqr():Number { + return w*w + x*x + y*y + z*z; + } + + /** + * + * @param axis + * @param angle + * @return + */ + public function setFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z); + x = axis.x*k; + y = axis.y*k; + z = axis.z*k; + return this; + } + + /** + * + * @param x + * @param y + * @param z + * @param angle + */ + public function setFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(x*x + y*y + z*z); + this.x = x*k; + this.y = y*k; + this.z = z*k; + return this; + } + + /** + * + * @param vector + */ + public function toAxisVector(v:Vector3 = null):Vector3 { + if (w < -1 || w > 1) { + normalize(); + } + if (v == null) { + v = new Vector3(); + } + if (w > -1 && w < 1) { + if (w == 0) { + v.x = x; + v.y = y; + v.z = z; + } else { + var angle:Number = 2*Math.acos(w); + var coeff:Number = 1/Math.sqrt(1 - w*w); + v.x = x*coeff*angle; + v.y = y*coeff*angle; + v.z = z*coeff*angle; + } + } else { + v.x = 0; + v.y = 0; + v.z = 0; + } + return v; + } + + /** + * + * @param rotations + */ + public function getEulerAngles(angles:Vector3):Vector3 { + var qi2:Number = 2*x*x; + var qj2:Number = 2*y*y; + var qk2:Number = 2*z*z; + var qij:Number = 2*x*y; + var qjk:Number = 2*y*z; + var qki:Number = 2*z*x; + var qri:Number = 2*w*x; + var qrj:Number = 2*w*y; + var qrk:Number = 2*w*z; + + var aa:Number = 1 - qj2 - qk2; + var bb:Number = qij - qrk; + var ee:Number = qij + qrk; + var ff:Number = 1 - qi2 - qk2; + var ii:Number = qki - qrj; + var jj:Number = qjk + qri; + var kk:Number = 1 - qi2 - qj2; + + if (-1 < ii && ii < 1) { + if (angles == null) { + angles = new Vector3(Math.atan2(jj, kk), -Math.asin(ii), Math.atan2(ee, aa)); + } else { + angles.x = Math.atan2(jj, kk); + angles.y = -Math.asin(ii); + angles.z = Math.atan2(ee, aa); + } + } else { + if (angles == null) { + angles = new Vector3(0, 0.5*((ii <= -1) ? Math.PI : -Math.PI), Math.atan2(-bb, ff)); + } else { + angles.x = 0; + angles.y = (ii <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-bb, ff); + } + } + return angles; + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setFromEulerAnglesXYZ(x:Number, y:Number, z:Number):void { + setFromAxisAngleComponents(1, 0, 0, x); + + _q.setFromAxisAngleComponents(0, 1, 0, y); + append(_q); + normalize(); + + _q.setFromAxisAngleComponents(0, 0, 1, z); + append(_q); + normalize(); + } + + /** + * + */ + public function conjugate():void { + x = -x; + y = -y; + z = -z; + } + + /** + * Выполняет линейную интерполяцию. + * + * @param q1 начало отрезка + * @param q2 конец отрезка + * @param t время, обычно задаётся в интервале [0, 1] + */ + public function nlerp(q1:Quaternion, q2:Quaternion, t:Number):Quaternion { + var d:Number = 1 - t; + w = q1.w*d + q2.w*t; + x = q1.x*d + q2.x*t; + y = q1.y*d + q2.y*t; + z = q1.z*d + q2.z*t; + // inlined normalize + d = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param q + * @return + */ + public function subtract(q:Quaternion):Quaternion { + w -= q.w; + x -= q.x; + y -= q.y; + z -= q.z; + return this; + } + + /** + * + * @param q1 + * @param q2 + * @return + */ + public function diff(q1:Quaternion, q2:Quaternion):Quaternion { + w = q2.w - q1.w; + x = q2.x - q1.x; + y = q2.y - q1.y; + z = q2.z - q1.z; + return this; + } + + /** + * + * @param q + * @return + */ + public function copy(q:Quaternion):Quaternion { + w = q.w; + x = q.x; + y = q.y; + z = q.z; + return this; + } + + /** + * @param reslult + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + result.w = w; + return result; + } + + /** + * + * @return + */ + public function clone():Quaternion { + return new Quaternion(w, x, y, z); + } + + /** + * + * @return + */ + public function toString():String { + return "[" + w + ", " + x + ", " + y + ", " + z + "]"; + } + + /** + * Выполняет сферическую интерполяцию между двумя заданными кватернионами по наименьшему расстоянию. + * + * @param a первый кватерион + * @param b второй кватернион + * @param t параметр интерполяции, обычно принадлежит отрезку [0, 1] + * @return this + */ + public function slerp(a:Quaternion, b:Quaternion, t:Number):Quaternion { + var flip:Number = 1; + // Так как одна и та же ориентация представляется двумя значениями q и -q, нужно сменить знак одного из кватернионов + // если скалярное произведение отрицательно. Иначе будет получено интерполированное значение по наибольшему расстоянию. + var cosine:Number = a.w*b.w + a.x*b.x + a.y*b.y + a.z*b.z; + if (cosine < 0) { + cosine = -cosine; + flip = -1; + } + + if ((1 - cosine) < 0.001) { + // Вблизи нуля используется линейная интерполяция + var k1:Number = 1 - t; + var k2:Number = t*flip; + w = a.w*k1 + b.w*k2; + x = a.x*k1 + b.x*k2; + y = a.y*k1 + b.y*k2; + z = a.z*k1 + b.z*k2; + normalize(); + } else { + var theta:Number = Math.acos(cosine); + var sine:Number = Math.sin(theta); + var beta:Number = Math.sin((1 - t)*theta)/sine; + var alpha:Number = Math.sin(t*theta)/sine*flip; + w = a.w*beta + b.w*alpha; + x = a.x*beta + b.x*alpha; + y = a.y*beta + b.y*alpha; + z = a.z*beta + b.z*alpha; + } + + return this; + } + + } +} diff --git a/0.0.9.2/src/alternativa/math/.svn/text-base/Vector3.as.svn-base b/0.0.9.2/src/alternativa/math/.svn/text-base/Vector3.as.svn-base new file mode 100644 index 0000000..89010a8 --- /dev/null +++ b/0.0.9.2/src/alternativa/math/.svn/text-base/Vector3.as.svn-base @@ -0,0 +1,334 @@ +package alternativa.math { + import flash.geom.Vector3D; + + /** + * + */ + public class Vector3 extends Vector3D { + + public static const ZERO:Vector3 = new Vector3(0, 0, 0); + public static const X_AXIS:Vector3 = new Vector3(1, 0, 0); + public static const Y_AXIS:Vector3 = new Vector3(0, 1, 0); + public static const Z_AXIS:Vector3 = new Vector3(0, 0, 1); + + /** + * + * @param x + * @param y + * @param z + */ + public function Vector3(x:Number = 0, y:Number = 0, z:Number = 0) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * + * @return + */ + public function vLength():Number { + return Math.sqrt(x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function vLengthSqr():Number { + return x*x + y*y + z*z; + } + + /** + * + * @param length + * @return + */ + public function vSetLength(length:Number):Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = length; + } else { + var k:Number = length/Math.sqrt(x*x + y*y + z*z); + x *= k; + y *= k; + z *= k; + } + return this; + } + + /** + * + * @return + */ + public function vNormalize():Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = 1; + } else { + d = Math.sqrt(d); + x /= d; + y /= d; + z /= d; + } + return this; + } + + /** + * Сложение координат. + * + * @param v точка, координаты которой прибавляются к собственным + */ + public function vAdd(v:Vector3):Vector3 { + x += v.x; + y += v.y; + z += v.z; + return this; + } + + /** + * + * @param k + * @param v + * @return + */ + public function vAddScaled(k:Number, v:Vector3):Vector3 { + x += k*v.x; + y += k*v.y; + z += k*v.z; + return this; + } + + /** + * Вычитание координат. + * + * @param v точка, координаты которой вычитаются из собственных + */ + public function vSubtract(v:Vector3):Vector3 { + x -= v.x; + y -= v.y; + z -= v.z; + return this; + } + + /** + * Вычисляет сумму векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + * @return this + */ + public function vSum(a:Vector3, b:Vector3):Vector3 { + x = a.x + b.x; + y = a.y + b.y; + z = a.z + b.z; + return this; + } + + /** + * Вычисление разности векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + */ + public function vDiff(a:Vector3, b:Vector3):Vector3 { + x = a.x - b.x; + y = a.y - b.y; + z = a.z - b.z; + return this; + } + + /** + * Умножение на скаляр. + * + * @param k число, на которое умножаются координаты + */ + public function vScale(k:Number):Vector3 { + x *= k; + y *= k; + z *= k; + return this; + } + + /** + * Инвертирование вектора. + */ + public function vReverse():Vector3 { + x = -x; + y = -y; + z = -z; + return this; + } + + /** + * + * @param v + * @return + */ + public function vDot(v:Vector3):Number { + return x*v.x + y*v.y + z*v.z; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross(v:Vector3):Vector3 { + var xx:Number = y*v.z - z*v.y; + var yy:Number = z*v.x - x*v.z; + var zz:Number = x*v.y - y*v.x; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross2(a:Vector3, b:Vector3):Vector3 { + x = a.y*b.z - a.z*b.y; + y = a.z*b.x - a.x*b.z; + z = a.x*b.y - a.y*b.x; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy3(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * + * @param matrix + */ + public function vTransformBy3Tr(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz + m.d; + y = m.e*xx + m.f*yy + m.g*zz + m.h; + z = m.i*xx + m.j*yy + m.k*zz + m.l; + return this; + } + + /** + * + * @param m + * @return + */ + public function vTransformInverseBy4(m:Matrix4):Vector3 { + var xx:Number = x - m.d; + var yy:Number = y - m.h; + var zz:Number = z - m.l; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * + * @param m + */ + public function vDeltaTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * Установка координат. + */ + public function vReset(x:Number = 0, y:Number = 0, z:Number = 0):Vector3 { + this.x = x; + this.y = y; + this.z = z; + return this; + } + + /** + * Копирование координат точки. + * + * @param v точка, координаты которой копируются + */ + public function vCopy(v:Vector3):Vector3 { + x = v.x; + y = v.y; + z = v.z; + return this; + } + + /** + * Клонирование точки. + * + * @return клонированная точка + */ + public function vClone():Vector3 { + return new Vector3(x, y, z); + } + + /** + * Копирует координаты вектора в экземпляр класса Vector3D. + * + * @param result экземпляр, в который копируются координаты + * @return переданный параметр + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + return result; + } + + /** + * @param source + * @return + */ + public function copyFromVector3D(source:Vector3D):Vector3 { + x = source.x; + y = source.y; + z = source.z; + return this; + } + + override public function toString():String { + return "Vector3(" + x + "," + y + ", " + z + ")"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/math/Matrix3.as b/0.0.9.2/src/alternativa/math/Matrix3.as new file mode 100644 index 0000000..244cc98 --- /dev/null +++ b/0.0.9.2/src/alternativa/math/Matrix3.as @@ -0,0 +1,390 @@ +package alternativa.math { + import flash.geom.Vector3D; + + /** + * + */ + public class Matrix3 { + + public static const ZERO:Matrix3 = new Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0); + public static const IDENTITY:Matrix3 = new Matrix3(); + + public var a:Number; + public var b:Number; + public var c:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix3(a:Number = 1, b:Number = 0, c:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1) { + this.a = a; + this.b = b; + this.c = c; + + this.e = e; + this.f = f; + this.g = g; + + this.i = i; + this.j = j; + this.k = k; + } + + /** + * + * @return + */ + public function toIdentity():Matrix3 { + a = f = k = 1; + b = c = e = g = i = j = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + var det:Number = 1/(-cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk); + + a = (ff*kk - gg*jj)*det; + b = (cc*jj - bb*kk)*det; + c = (bb*gg - cc*ff)*det; + e = (gg*ii - ee*kk)*det; + f = (aa*kk - cc*ii)*det; + g = (cc*ee - aa*gg)*det; + i = (ee*jj - ff*ii)*det; + j = (bb*ii - aa*jj)*det; + k = (aa*ff - bb*ee)*det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prependTransposed(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.b + cc*m.c; + b = aa*m.e + bb*m.f + cc*m.g; + c = aa*m.i + bb*m.j + cc*m.k; + e = ee*m.a + ff*m.b + gg*m.c; + f = ee*m.e + ff*m.f + gg*m.g; + g = ee*m.i + ff*m.j + gg*m.k; + i = ii*m.a + jj*m.b + kk*m.c; + j = ii*m.e + jj*m.f + kk*m.g; + k = ii*m.i + jj*m.j + kk*m.k; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix3):Matrix3 { + a += m.a; + b += m.b; + c += m.c; + e += m.e; + f += m.f; + g += m.g; + i += m.i; + j += m.j; + k += m.k; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix3):Matrix3 { + a -= m.a; + b -= m.b; + c -= m.c; + e -= m.e; + f -= m.f; + g -= m.g; + i -= m.i; + j -= m.j; + k -= m.k; + + return this; + } + + /** + * + * @return + */ + public function transpose():Matrix3 { + var tmp:Number = b; + b = e; + e = tmp; + tmp = c; + c = i; + i = tmp; + tmp = g; + g = j; + j = tmp; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector3To3D(vin:Vector3, vout:Vector3D):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * + * @param matrix + */ + public function createSkewSymmetric(v:Vector3):Matrix3 { + a = f = k = 0; + b = -v.z; + c = v.y; + e = v.z; + g = -v.x; + i = -v.y; + j = v.x; + return this; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix3):Matrix3 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @return + */ + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):Matrix3 { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + + return this; + } + + /** + * Метод формирует матрицу поворота на заданный угол относительно заданной оси. + * + * @param axis нормализованный вектор, задающий ось, относительно которой выполняется поворот + * @param angle угол поворота в радианах + */ + public function fromAxisAngle(axis:Vector3, angle:Number):void { + var c1:Number = Math.cos(angle); + var s:Number = Math.sin(angle); + var t:Number = 1 - c1; + var x:Number = axis.x; + var y:Number = axis.y; + var z:Number = axis.z; + + a = t*x*x + c1; + b = t*x*y - z*s; + c = t*x*z + y*s; + + e = t*x*y + z*s; + f = t*y*y + c1; + g = t*y*z - x*s; + + i = t*x*z - y*s; + j = t*y*z + x*s; + k = t*z*z + c1; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix3 { + return new Matrix3(a, b, c, e, f, g, i, j, k); + } + + /** + * + * @return + */ + public function toString():String { + return "[Matrix3 (" + a + ", " + b + ", " + c + "), (" + e + ", " + f + ", " + g + "), (" + i + ", " + j + ", " + k + ")]"; + } + + /** + * @param angles + */ + public function getEulerAngles(angles:Vector3):void { + if (-1 < i && i < 1) { + angles.x = Math.atan2(j, k); + angles.y = -Math.asin(i); + angles.z = Math.atan2(e, a); + } else { + angles.x = 0; + angles.y = (i <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-b, f); + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/math/Matrix4.as b/0.0.9.2/src/alternativa/math/Matrix4.as new file mode 100644 index 0000000..d06eb5f --- /dev/null +++ b/0.0.9.2/src/alternativa/math/Matrix4.as @@ -0,0 +1,475 @@ +package alternativa.math { + + /** + * + */ + public class Matrix4 { + + public static const IDENTITY:Matrix4 = new Matrix4(); + + public var a:Number; + public var b:Number; + public var c:Number; + public var d:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + public var h:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + public var l:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix4(a:Number = 1, b:Number = 0, c:Number = 0, d:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, h:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1, l:Number = 0) { + this.a = a; + this.b = b; + this.c = c; + this.d = d; + + this.e = e; + this.f = f; + this.g = g; + this.h = h; + + this.i = i; + this.j = j; + this.k = k; + this.l = l; + } + + /** + * + * @return + */ + public function toIdentity():Matrix4 { + a = f = k = 1; + b = c = e = g = i = j = d = h = l = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + var det:Number = -cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk; + + a = (-gg*jj + ff*kk)/det; + b = (cc*jj - bb*kk)/det; + c = (-cc*ff + bb*gg)/det; + d = (dd*gg*jj - cc*hh*jj - dd*ff*kk + bb*hh*kk + cc*ff*ll - bb*gg*ll)/det; + e = (gg*ii - ee*kk)/det; + f = (-cc*ii + aa*kk)/det; + g = (cc*ee - aa*gg)/det; + h = (cc*hh*ii - dd*gg*ii + dd*ee*kk - aa*hh*kk - cc*ee*ll + aa*gg*ll)/det; + i = (-ff*ii + ee*jj)/det; + j = (bb*ii - aa*jj)/det; + k = (-bb*ee + aa*ff)/det; + l = (dd*ff*ii - bb*hh*ii - dd*ee*jj + aa*hh*jj + bb*ee*ll - aa*ff*ll)/det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + d = m.a*dd + m.b*hh + m.c*ll + m.d; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + h = m.e*dd + m.f*hh + m.g*ll + m.h; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + l = m.i*dd + m.j*hh + m.k*ll + m.l; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + d = aa*m.d + bb*m.h + cc*m.l + dd; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + h = ee*m.d + ff*m.h + gg*m.l + hh; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + l = ii*m.d + jj*m.h + kk*m.l + ll; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix4):Matrix4 { + a += m.a; + b += m.b; + c += m.c; + d += m.d; + e += m.e; + f += m.f; + g += m.g; + h += m.h; + i += m.i; + j += m.j; + k += m.k; + l += m.l; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix4):Matrix4 { + a -= m.a; + b -= m.b; + c -= m.c; + d -= m.d; + e -= m.e; + f -= m.f; + g -= m.g; + h -= m.h; + i -= m.i; + j -= m.j; + k -= m.k; + l -= m.l; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectors(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + * @param len + */ + public function transformVectorsN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverse(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverseN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(idx:int, axis:Vector3):void { + switch (idx) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + case 3: + axis.x = d; + axis.y = h; + axis.z = l; + return; + } + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix4):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = m.d; + e = m.e; + f = m.f; + g = m.g; + h = m.h; + i = m.i; + j = m.j; + k = m.k; + l = m.l; + + return this; + } + + public function setFromMatrix3(m:Matrix3, offset:Vector3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = offset.x; + e = m.e; + f = m.f; + g = m.g; + h = offset.y; + i = m.i; + j = m.j; + k = m.k; + l = offset.z; + + return this; + } + + public function setOrientationFromMatrix3(m:Matrix3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @param angles + */ + public function getEulerAngles(angles:Vector3):void { + if (-1 < i && i < 1) { + angles.x = Math.atan2(j, k); + angles.y = -Math.asin(i); + angles.z = Math.atan2(e, a); + } else { + angles.x = 0; + angles.y = (i <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-b, f); + } + } + + /** + * Устанавливает координаты матрицы. + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + d = pos.x; + h = pos.y; + l = pos.z; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix4 { + return new Matrix4(a, b, c, d, e, f, g, h, i, j, k, l); + } + + /** + * + * @return + */ + public function toString():String { + return "[Matrix4 [" + a.toFixed(3) + " " + b.toFixed(3) + " " + c.toFixed(3) + " " + d.toFixed(3) + "] [" + e.toFixed(3) + " " + f.toFixed(3) + " " + g.toFixed(3) + " " + h.toFixed(3) + "] [" + i.toFixed(3) + " " + j.toFixed(3) + " " + k.toFixed(3) + " " + l.toFixed(3) + "]]"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/math/Quaternion.as b/0.0.9.2/src/alternativa/math/Quaternion.as new file mode 100644 index 0000000..ddc85f5 --- /dev/null +++ b/0.0.9.2/src/alternativa/math/Quaternion.as @@ -0,0 +1,458 @@ +package alternativa.math { + import flash.geom.Vector3D; + + public class Quaternion { + + public var w:Number; + public var x:Number; + public var y:Number; + public var z:Number; + + public static function multiply(q1:Quaternion, q2:Quaternion, result:Quaternion):void { + result.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z; + result.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y; + result.y = q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z; + result.z = q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x; + } + + public static function createFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngle(axis, angle); + return q; + } + + public static function createFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngleComponents(x, y, z, angle); + return q; + } + + private static var _q:Quaternion = new Quaternion(); + + public function Quaternion(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0) { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + } + + public function reset(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0):Quaternion { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + return this; + } + + public function normalize():Quaternion { + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * Умножает на указанный кватернион слева: this * q + * + * @param q множитель + */ + public function prepend(q:Quaternion):Quaternion { + var ww:Number = w*q.w - x*q.x - y*q.y - z*q.z; + var xx:Number = w*q.x + x*q.w + y*q.z - z*q.y; + var yy:Number = w*q.y + y*q.w + z*q.x - x*q.z; + var zz:Number = w*q.z + z*q.w + x*q.y - y*q.x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Умножает на указанный кватернион справа: q * this + * + * @param q множитель + */ + public function append(q:Quaternion):Quaternion { + var ww:Number = q.w*w - q.x*x - q.y*y - q.z*z; + var xx:Number = q.w*x + q.x*w + q.y*z - q.z*y; + var yy:Number = q.w*y + q.y*w + q.z*x - q.x*z; + var zz:Number = q.w*z + q.z*w + q.x*y - q.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * + * @param vector + */ + public function rotateByVector(v:Vector3):Quaternion { + var ww:Number = -v.x*x - v.y*y - v.z*z; + var xx:Number = v.x*w + v.y*z - v.z*y; + var yy:Number = v.y*w + v.z*x - v.x*z; + var zz:Number = v.z*w + v.x*y - v.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Добавляет вращение, приданное вектором угловой скорости за указанное время. + * + * @param v + * @param scale + */ + public function addScaledVector(v:Vector3, scale:Number):Quaternion { + var vx:Number = v.x*scale; + var vy:Number = v.y*scale; + var vz:Number = v.z*scale; + var ww:Number = -x*vx - y*vy - z*vz; + var xx:Number = vx*w + vy*z - vz*y; + var yy:Number = vy*w + vz*x - vx*z; + var zz:Number = vz*w + vx*y - vy*x; + w += 0.5*ww; + x += 0.5*xx; + y += 0.5*yy; + z += 0.5*zz; + // inlined normalize + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param m + * @return + */ + public function toMatrix3(m:Matrix3):Quaternion { + var xx2:Number = 2.0*x*x; + var yy2:Number = 2.0*y*y; + var zz2:Number = 2.0*z*z; + var xy2:Number = 2.0*x*y; + var yz2:Number = 2.0*y*z; + var zx2:Number = 2.0*z*x; + var wx2:Number = 2.0*w*x; + var wy2:Number = 2.0*w*y; + var wz2:Number = 2.0*w*z; + + m.a = 1.0 - yy2 - zz2; + m.b = xy2 - wz2; + m.c = zx2 + wy2; + + m.e = xy2 + wz2; + m.f = 1.0 - xx2 - zz2; + m.g = yz2 - wx2; + + m.i = zx2 - wy2; + m.j = yz2 + wx2; + m.k = 1.0 - xx2 - yy2; + return this; + } + + /** + * + * @return + */ + public function length():Number { + return Math.sqrt(w*w + x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function lengthSqr():Number { + return w*w + x*x + y*y + z*z; + } + + /** + * + * @param axis + * @param angle + * @return + */ + public function setFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z); + x = axis.x*k; + y = axis.y*k; + z = axis.z*k; + return this; + } + + /** + * + * @param x + * @param y + * @param z + * @param angle + */ + public function setFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(x*x + y*y + z*z); + this.x = x*k; + this.y = y*k; + this.z = z*k; + return this; + } + + /** + * + * @param vector + */ + public function toAxisVector(v:Vector3 = null):Vector3 { + if (w < -1 || w > 1) { + normalize(); + } + if (v == null) { + v = new Vector3(); + } + if (w > -1 && w < 1) { + if (w == 0) { + v.x = x; + v.y = y; + v.z = z; + } else { + var angle:Number = 2*Math.acos(w); + var coeff:Number = 1/Math.sqrt(1 - w*w); + v.x = x*coeff*angle; + v.y = y*coeff*angle; + v.z = z*coeff*angle; + } + } else { + v.x = 0; + v.y = 0; + v.z = 0; + } + return v; + } + + /** + * + * @param rotations + */ + public function getEulerAngles(angles:Vector3):Vector3 { + var qi2:Number = 2*x*x; + var qj2:Number = 2*y*y; + var qk2:Number = 2*z*z; + var qij:Number = 2*x*y; + var qjk:Number = 2*y*z; + var qki:Number = 2*z*x; + var qri:Number = 2*w*x; + var qrj:Number = 2*w*y; + var qrk:Number = 2*w*z; + + var aa:Number = 1 - qj2 - qk2; + var bb:Number = qij - qrk; + var ee:Number = qij + qrk; + var ff:Number = 1 - qi2 - qk2; + var ii:Number = qki - qrj; + var jj:Number = qjk + qri; + var kk:Number = 1 - qi2 - qj2; + + if (-1 < ii && ii < 1) { + if (angles == null) { + angles = new Vector3(Math.atan2(jj, kk), -Math.asin(ii), Math.atan2(ee, aa)); + } else { + angles.x = Math.atan2(jj, kk); + angles.y = -Math.asin(ii); + angles.z = Math.atan2(ee, aa); + } + } else { + if (angles == null) { + angles = new Vector3(0, 0.5*((ii <= -1) ? Math.PI : -Math.PI), Math.atan2(-bb, ff)); + } else { + angles.x = 0; + angles.y = (ii <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-bb, ff); + } + } + return angles; + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setFromEulerAnglesXYZ(x:Number, y:Number, z:Number):void { + setFromAxisAngleComponents(1, 0, 0, x); + + _q.setFromAxisAngleComponents(0, 1, 0, y); + append(_q); + normalize(); + + _q.setFromAxisAngleComponents(0, 0, 1, z); + append(_q); + normalize(); + } + + /** + * + */ + public function conjugate():void { + x = -x; + y = -y; + z = -z; + } + + /** + * Выполняет линейную интерполяцию. + * + * @param q1 начало отрезка + * @param q2 конец отрезка + * @param t время, обычно задаётся в интервале [0, 1] + */ + public function nlerp(q1:Quaternion, q2:Quaternion, t:Number):Quaternion { + var d:Number = 1 - t; + w = q1.w*d + q2.w*t; + x = q1.x*d + q2.x*t; + y = q1.y*d + q2.y*t; + z = q1.z*d + q2.z*t; + // inlined normalize + d = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param q + * @return + */ + public function subtract(q:Quaternion):Quaternion { + w -= q.w; + x -= q.x; + y -= q.y; + z -= q.z; + return this; + } + + /** + * + * @param q1 + * @param q2 + * @return + */ + public function diff(q1:Quaternion, q2:Quaternion):Quaternion { + w = q2.w - q1.w; + x = q2.x - q1.x; + y = q2.y - q1.y; + z = q2.z - q1.z; + return this; + } + + /** + * + * @param q + * @return + */ + public function copy(q:Quaternion):Quaternion { + w = q.w; + x = q.x; + y = q.y; + z = q.z; + return this; + } + + /** + * @param reslult + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + result.w = w; + return result; + } + + /** + * + * @return + */ + public function clone():Quaternion { + return new Quaternion(w, x, y, z); + } + + /** + * + * @return + */ + public function toString():String { + return "[" + w + ", " + x + ", " + y + ", " + z + "]"; + } + + /** + * Выполняет сферическую интерполяцию между двумя заданными кватернионами по наименьшему расстоянию. + * + * @param a первый кватерион + * @param b второй кватернион + * @param t параметр интерполяции, обычно принадлежит отрезку [0, 1] + * @return this + */ + public function slerp(a:Quaternion, b:Quaternion, t:Number):Quaternion { + var flip:Number = 1; + // Так как одна и та же ориентация представляется двумя значениями q и -q, нужно сменить знак одного из кватернионов + // если скалярное произведение отрицательно. Иначе будет получено интерполированное значение по наибольшему расстоянию. + var cosine:Number = a.w*b.w + a.x*b.x + a.y*b.y + a.z*b.z; + if (cosine < 0) { + cosine = -cosine; + flip = -1; + } + + if ((1 - cosine) < 0.001) { + // Вблизи нуля используется линейная интерполяция + var k1:Number = 1 - t; + var k2:Number = t*flip; + w = a.w*k1 + b.w*k2; + x = a.x*k1 + b.x*k2; + y = a.y*k1 + b.y*k2; + z = a.z*k1 + b.z*k2; + normalize(); + } else { + var theta:Number = Math.acos(cosine); + var sine:Number = Math.sin(theta); + var beta:Number = Math.sin((1 - t)*theta)/sine; + var alpha:Number = Math.sin(t*theta)/sine*flip; + w = a.w*beta + b.w*alpha; + x = a.x*beta + b.x*alpha; + y = a.y*beta + b.y*alpha; + z = a.z*beta + b.z*alpha; + } + + return this; + } + + } +} diff --git a/0.0.9.2/src/alternativa/math/Vector3.as b/0.0.9.2/src/alternativa/math/Vector3.as new file mode 100644 index 0000000..89010a8 --- /dev/null +++ b/0.0.9.2/src/alternativa/math/Vector3.as @@ -0,0 +1,334 @@ +package alternativa.math { + import flash.geom.Vector3D; + + /** + * + */ + public class Vector3 extends Vector3D { + + public static const ZERO:Vector3 = new Vector3(0, 0, 0); + public static const X_AXIS:Vector3 = new Vector3(1, 0, 0); + public static const Y_AXIS:Vector3 = new Vector3(0, 1, 0); + public static const Z_AXIS:Vector3 = new Vector3(0, 0, 1); + + /** + * + * @param x + * @param y + * @param z + */ + public function Vector3(x:Number = 0, y:Number = 0, z:Number = 0) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * + * @return + */ + public function vLength():Number { + return Math.sqrt(x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function vLengthSqr():Number { + return x*x + y*y + z*z; + } + + /** + * + * @param length + * @return + */ + public function vSetLength(length:Number):Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = length; + } else { + var k:Number = length/Math.sqrt(x*x + y*y + z*z); + x *= k; + y *= k; + z *= k; + } + return this; + } + + /** + * + * @return + */ + public function vNormalize():Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = 1; + } else { + d = Math.sqrt(d); + x /= d; + y /= d; + z /= d; + } + return this; + } + + /** + * Сложение координат. + * + * @param v точка, координаты которой прибавляются к собственным + */ + public function vAdd(v:Vector3):Vector3 { + x += v.x; + y += v.y; + z += v.z; + return this; + } + + /** + * + * @param k + * @param v + * @return + */ + public function vAddScaled(k:Number, v:Vector3):Vector3 { + x += k*v.x; + y += k*v.y; + z += k*v.z; + return this; + } + + /** + * Вычитание координат. + * + * @param v точка, координаты которой вычитаются из собственных + */ + public function vSubtract(v:Vector3):Vector3 { + x -= v.x; + y -= v.y; + z -= v.z; + return this; + } + + /** + * Вычисляет сумму векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + * @return this + */ + public function vSum(a:Vector3, b:Vector3):Vector3 { + x = a.x + b.x; + y = a.y + b.y; + z = a.z + b.z; + return this; + } + + /** + * Вычисление разности векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + */ + public function vDiff(a:Vector3, b:Vector3):Vector3 { + x = a.x - b.x; + y = a.y - b.y; + z = a.z - b.z; + return this; + } + + /** + * Умножение на скаляр. + * + * @param k число, на которое умножаются координаты + */ + public function vScale(k:Number):Vector3 { + x *= k; + y *= k; + z *= k; + return this; + } + + /** + * Инвертирование вектора. + */ + public function vReverse():Vector3 { + x = -x; + y = -y; + z = -z; + return this; + } + + /** + * + * @param v + * @return + */ + public function vDot(v:Vector3):Number { + return x*v.x + y*v.y + z*v.z; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross(v:Vector3):Vector3 { + var xx:Number = y*v.z - z*v.y; + var yy:Number = z*v.x - x*v.z; + var zz:Number = x*v.y - y*v.x; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross2(a:Vector3, b:Vector3):Vector3 { + x = a.y*b.z - a.z*b.y; + y = a.z*b.x - a.x*b.z; + z = a.x*b.y - a.y*b.x; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy3(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * + * @param matrix + */ + public function vTransformBy3Tr(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz + m.d; + y = m.e*xx + m.f*yy + m.g*zz + m.h; + z = m.i*xx + m.j*yy + m.k*zz + m.l; + return this; + } + + /** + * + * @param m + * @return + */ + public function vTransformInverseBy4(m:Matrix4):Vector3 { + var xx:Number = x - m.d; + var yy:Number = y - m.h; + var zz:Number = z - m.l; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * + * @param m + */ + public function vDeltaTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * Установка координат. + */ + public function vReset(x:Number = 0, y:Number = 0, z:Number = 0):Vector3 { + this.x = x; + this.y = y; + this.z = z; + return this; + } + + /** + * Копирование координат точки. + * + * @param v точка, координаты которой копируются + */ + public function vCopy(v:Vector3):Vector3 { + x = v.x; + y = v.y; + z = v.z; + return this; + } + + /** + * Клонирование точки. + * + * @return клонированная точка + */ + public function vClone():Vector3 { + return new Vector3(x, y, z); + } + + /** + * Копирует координаты вектора в экземпляр класса Vector3D. + * + * @param result экземпляр, в который копируются координаты + * @return переданный параметр + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + return result; + } + + /** + * @param source + * @return + */ + public function copyFromVector3D(source:Vector3D):Vector3 { + x = source.x; + y = source.y; + z = source.z; + return this; + } + + override public function toString():String { + return "Vector3(" + x + "," + y + ", " + z + ")"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/.svn/all-wcprops b/0.0.9.2/src/alternativa/physics/.svn/all-wcprops new file mode 100644 index 0000000..87aa623 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/.svn/all-wcprops @@ -0,0 +1,77 @@ +K 25 +svn:wc:ra_dav:version-url +V 103 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics +END +Body.as +K 25 +svn:wc:ra_dav:version-url +V 111 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/Body.as +END +BodyState.as +K 25 +svn:wc:ra_dav:version-url +V 116 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/BodyState.as +END +CollisionPrimitiveListItem.as +K 25 +svn:wc:ra_dav:version-url +V 133 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/CollisionPrimitiveListItem.as +END +CollisionPrimitiveList.as +K 25 +svn:wc:ra_dav:version-url +V 129 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/CollisionPrimitiveList.as +END +PhysicsScene.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/PhysicsScene.as +END +PhysicsUtils.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/PhysicsUtils.as +END +altphysics.as +K 25 +svn:wc:ra_dav:version-url +V 117 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/altphysics.as +END +ContactPoint.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/ContactPoint.as +END +BodyListItem.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/BodyListItem.as +END +BodyList.as +K 25 +svn:wc:ra_dav:version-url +V 115 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/BodyList.as +END +Contact.as +K 25 +svn:wc:ra_dav:version-url +V 114 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/Contact.as +END +BodyMaterial.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/BodyMaterial.as +END diff --git a/0.0.9.2/src/alternativa/physics/.svn/entries b/0.0.9.2/src/alternativa/physics/.svn/entries new file mode 100644 index 0000000..67e3c5e --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/.svn/entries @@ -0,0 +1,181 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics +http://svndev.alternativaplatform.com + + + +2010-03-06T16:51:19.781174Z +29404 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +CollisionPrimitiveList.as +file + + + + +2010-10-28T04:32:44.000000Z +5752b1f4909c5146720b58fbfffed468 +2009-11-01T12:35:52.270571Z +22955 +mike + +altphysics.as +file + + + + +2010-10-28T04:32:44.000000Z +04fbe40a27502dbbd0dba02a76b2df50 +2009-04-20T20:09:14.715403Z +11579 +mike + +collision +dir + +primitives +dir + +BodyList.as +file + + + + +2010-10-28T04:32:44.000000Z +08a90332660bd0273728006d1402dd25 +2009-11-01T12:35:52.270571Z +22955 +mike + +Contact.as +file + + + + +2010-10-28T04:32:44.000000Z +eb89f4f37bcf2d21eadc733f244fcf64 +2010-02-12T12:34:25.192477Z +28424 +mike + +Body.as +file + + + + +2010-10-28T04:32:44.000000Z +355d0fb70dbb4946d4090a7c94c0d58a +2010-02-12T12:34:25.192477Z +28424 +mike + +BodyState.as +file + + + + +2010-10-28T04:32:44.000000Z +5be68820e6f6185d62b3928c39dde285 +2009-11-24T08:44:15.941480Z +23758 +mike + +CollisionPrimitiveListItem.as +file + + + + +2010-10-28T04:32:44.000000Z +cec5dbfefef812d87481478455f367b1 +2009-10-19T07:17:33.113306Z +22253 +mike + +PhysicsScene.as +file + + + + +2010-10-28T04:32:44.000000Z +96c74784bb71c06141cc469eec57ccb8 +2010-02-19T13:09:00.412754Z +28731 +mike + +PhysicsUtils.as +file + + + + +2010-10-28T04:32:44.000000Z +dce0a94f8a861291a2a8942ed0ba54a5 +2009-11-24T08:44:15.941480Z +23758 +mike + +constraints +dir + +ContactPoint.as +file + + + + +2010-10-28T04:32:44.000000Z +14f23db94810328dedd7da2992812ced +2009-11-24T08:44:15.941480Z +23758 +mike + +BodyListItem.as +file + + + + +2010-10-28T04:32:44.000000Z +2fc2b598310b24aef3f8b90e14a8ba4f +2009-10-19T07:17:33.113306Z +22253 +mike + +BodyMaterial.as +file + + + + +2010-10-28T04:32:44.000000Z +9b700692ebb8017882f3e8cec095e1a9 +2009-10-19T07:17:33.113306Z +22253 +mike + diff --git a/0.0.9.2/src/alternativa/physics/.svn/format b/0.0.9.2/src/alternativa/physics/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.2/src/alternativa/physics/.svn/text-base/Body.as.svn-base b/0.0.9.2/src/alternativa/physics/.svn/text-base/Body.as.svn-base new file mode 100644 index 0000000..cd22c5b --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/.svn/text-base/Body.as.svn-base @@ -0,0 +1,405 @@ +package alternativa.physics { + + import alternativa.math.Matrix3; + import alternativa.math.Matrix4; + import alternativa.math.Quaternion; + import alternativa.math.Vector3; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.IBodyCollisionPredicate; + import alternativa.physics.collision.types.BoundBox; + + use namespace altphysics; + + /** + * + */ + public class Body { + + public static var linDamping:Number = 0.997; + public static var rotDamping:Number = 0.997; + + // Идентификатор тела, уникальный в пределах мира + public var id:int; + // Имя тела + public var name:String; + // Мир, в котором находится тело + public var world:PhysicsScene; + // Флаг подвижности тела + public var movable:Boolean = true; + // Флаг указывает, может ли тело быть заморожено + public var canFreeze:Boolean = false; + + public var freezeCounter:int; + public var frozen:Boolean = false; + // Ограничивающий бокс тела + public var aabb:BoundBox = new BoundBox(); + // Предикат, через который тело получает сообщения о столкновениях + public var postCollisionPredicate:IBodyCollisionPredicate; + + // Текущее состояние тела + public var state:BodyState = new BodyState(); + // Предыдущее состояние тела + public var prevState:BodyState = new BodyState(); + // Линейное ускорение тела на текущем шаге симуляции + public var accel:Vector3 = new Vector3(); + // Угловое ускорение тела на текущем шаге симуляции + public var angleAccel:Vector3 = new Vector3(); + // Физический материал тела + public var material:BodyMaterial = new BodyMaterial(); + // Обратная масса тела + public var invMass:Number = 1; + // Обратная матрица тензора инерции в локальных координатах + public var invInertia:Matrix3 = new Matrix3(); + // Обратная матрица тензора инерции в мировых координатах + public var invInertiaWorld:Matrix3 = new Matrix3(); + // Базисная матрица тела в мировых координатах + public var baseMatrix:Matrix3 = new Matrix3(); + + public const MAX_CONTACTS:int = 20; + public var contacts:Vector. = new Vector.(MAX_CONTACTS); + public var contactsNum:int; + + public var collisionPrimitives:CollisionPrimitiveList; + + // Аккумулятор сил + public var forceAccum:Vector3 = new Vector3(); + // Аккумулятор моментов + public var torqueAccum:Vector3 = new Vector3(); + + // Внутренние переменные для избежания создания экземпляров + private static var _r:Vector3 = new Vector3(); + private static var _f:Vector3 = new Vector3(); + + /** + * + * @param invMass + * @param invInertia + */ + public function Body(invMass:Number, invInertia:Matrix3) { + this.invMass = invMass; + this.invInertia.copy(invInertia); + } + + /** + * @param primitive + * @param localTransform + */ + public function addCollisionPrimitive(primitive:CollisionPrimitive, localTransform:Matrix4 = null):void { + if (primitive == null) { + throw new ArgumentError("Primitive cannot be null"); + } + if (collisionPrimitives == null) { + collisionPrimitives = new CollisionPrimitiveList(); + } + collisionPrimitives.append(primitive); + primitive.setBody(this, localTransform); + } + + /** + * + * @param primitive + */ + public function removeCollisionPrimitive(primitive:CollisionPrimitive):void { + if (collisionPrimitives == null) return; + primitive.setBody(null); + collisionPrimitives.remove(primitive); + if (collisionPrimitives.size == 0) { + collisionPrimitives = null; + } + } + + /** + * @param t + * @param result + */ + public function interpolate(t:Number, pos:Vector3, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.w = prevState.orientation.w*t1 + state.orientation.w*t; + orientation.x = prevState.orientation.x*t1 + state.orientation.x*t; + orientation.y = prevState.orientation.y*t1 + state.orientation.y*t; + orientation.z = prevState.orientation.z*t1 + state.orientation.z*t; + } + + /** + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + state.pos.vCopy(pos); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setPositionXYZ(x:Number, y:Number, z:Number):void { + state.pos.vReset(x, y, z); + } + + /** + * + * @param vel + */ + public function setVelocity(vel:Vector3):void { + state.velocity.vCopy(vel); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setVelocityXYZ(x:Number, y:Number, z:Number):void { + state.velocity.vReset(x, y, z); + } + + /** + * + * @param rot + */ + public function setRotation(rot:Vector3):void { + state.rotation.vCopy(rot); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setRotationXYZ(x:Number, y:Number, z:Number):void { + state.rotation.vReset(x, y, z); + } + + /** + * + * @param q + */ + public function setOrientation(q:Quaternion):void { + state.orientation.copy(q); + } + + /** + * @param r + * @param dir + * @param magnitude + */ + public function applyRelPosWorldImpulse(r:Vector3, dir:Vector3, magnitude:Number):void { + var d:Number = magnitude*invMass; + // Линейная часть + state.velocity.x += d*dir.x; + state.velocity.y += d*dir.y; + state.velocity.z += d*dir.z; + + // Вращательная часть + var x:Number = (r.y*dir.z - r.z*dir.y)*magnitude; + var y:Number = (r.z*dir.x - r.x*dir.z)*magnitude; + var z:Number = (r.x*dir.y - r.y*dir.x)*magnitude; + + state.rotation.x += invInertiaWorld.a*x + invInertiaWorld.b*y + invInertiaWorld.c*z; + state.rotation.y += invInertiaWorld.e*x + invInertiaWorld.f*y + invInertiaWorld.g*z; + state.rotation.z += invInertiaWorld.i*x + invInertiaWorld.j*y + invInertiaWorld.k*z; + } + + /** + * @param f + */ + public function addForce(f:Vector3):void { + forceAccum.vAdd(f); + } + + /** + * + * @param fx + * @param fy + * @param fz + */ + public function addForceXYZ(fx:Number, fy:Number, fz:Number):void { + forceAccum.x += fx; + forceAccum.y += fy; + forceAccum.z += fz; + } + + /** + * @param pos + * @param f + */ + public function addWorldForceXYZ(px:Number, py:Number, pz:Number, fx:Number, fy:Number, fz:Number):void { + forceAccum.x += fx; + forceAccum.y += fy; + forceAccum.z += fz; + + var pos:Vector3 = state.pos; + var rx:Number = px - pos.x; + var ry:Number = py - pos.y; + var rz:Number = pz - pos.z; + +// var x:Number = ry*fz - rz*fy; +// var y:Number = rz*fx - rx*fz; +// var z:Number = rx*fy - ry*fx; + + torqueAccum.x += ry*fz - rz*fy; + torqueAccum.y += rz*fx - rx*fz; + torqueAccum.z += rx*fy - ry*fx; + } + + /** + * @param pos + * @param f + */ + public function addWorldForce(pos:Vector3, force:Vector3):void { + forceAccum.vAdd(force); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(force)); + } + + /** + * @param pos + * @param f + */ + public function addWorldForceScaled(pos:Vector3, force:Vector3, scale:Number):void { + _f.x = scale*force.x; + _f.y = scale*force.y; + _f.z = scale*force.z; + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(_f)); + } + + /** + * @param pos + * @param f + */ + public function addLocalForce(pos:Vector3, force:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(pos, _r); + // Трансформируем вектор силы в мировую систему + baseMatrix.transformVector(force, _f); + // Добавляем силу и момент + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vCross(_f)); + } + + /** + * + * @param localPos + * @param worldForce + */ + public function addWorldForceAtLocalPoint(localPos:Vector3, worldForce:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(localPos, _r); + // Добавляем силу и момент + forceAccum.vAdd(worldForce); + torqueAccum.vAdd(_r.vCross(worldForce)); + } + + /** + * @param dt + */ + public function beforePhysicsStep(dt:Number):void { + } + + /** + * @param t + */ + public function addTorque(t:Vector3):void { + torqueAccum.vAdd(t); + } + + /** + * + */ + altphysics function clearAccumulators():void { + forceAccum.x = forceAccum.y = forceAccum.z = 0; + torqueAccum.x = torqueAccum.y = torqueAccum.z = 0; + } + + /** + * + */ + altphysics function calcAccelerations():void { + accel.x = forceAccum.x*invMass; + accel.y = forceAccum.y*invMass; + accel.z = forceAccum.z*invMass; + angleAccel.x = invInertiaWorld.a*torqueAccum.x + invInertiaWorld.b*torqueAccum.y + invInertiaWorld.c*torqueAccum.z; + angleAccel.y = invInertiaWorld.e*torqueAccum.x + invInertiaWorld.f*torqueAccum.y + invInertiaWorld.g*torqueAccum.z; + angleAccel.z = invInertiaWorld.i*torqueAccum.x + invInertiaWorld.j*torqueAccum.y + invInertiaWorld.k*torqueAccum.z; + } + + /** + * Вычисляет производные данные. + */ + public function calcDerivedData():void { + // Вычисление базисной матрицы и обратного тензора инерции в мировых координатах + state.orientation.toMatrix3(baseMatrix); + invInertiaWorld.copy(invInertia).append(baseMatrix).prependTransposed(baseMatrix); + if (collisionPrimitives != null) { + aabb.infinity(); + var item:CollisionPrimitiveListItem = collisionPrimitives.head; + while (item != null) { + var primitive:CollisionPrimitive = item.primitive; + primitive.transform.setFromMatrix3(baseMatrix, state.pos); + if (primitive.localTransform != null) { + primitive.transform.prepend(primitive.localTransform); + } + primitive.calculateAABB(); + aabb.addBoundBox(primitive.aabb); + item = item.next; + } + } + } + + /** + * + */ + altphysics function saveState():void { + prevState.copy(state); + } + + /** + * + */ + altphysics function restoreState():void { + state.copy(prevState); + } + + /** + * @param dt + */ + altphysics function integrateVelocity(dt:Number):void { + // v = v + a*t + state.velocity.x += accel.x*dt; + state.velocity.y += accel.y*dt; + state.velocity.z += accel.z*dt; + // rot = rot + eps*t + state.rotation.x += angleAccel.x*dt; + state.rotation.y += angleAccel.y*dt; + state.rotation.z += angleAccel.z*dt; + + state.velocity.x *= linDamping; + state.velocity.y *= linDamping; + state.velocity.z *= linDamping; + + state.rotation.x *= rotDamping; + state.rotation.y *= rotDamping; + state.rotation.z *= rotDamping; + } + + /** + * + */ + altphysics function integratePosition(dt:Number):void { + // pos = pos + v*t + state.pos.x += state.velocity.x*dt; + state.pos.y += state.velocity.y*dt; + state.pos.z += state.velocity.z*dt; + // q = q + 0.5*rot*q + state.orientation.addScaledVector(state.rotation, dt); + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/.svn/text-base/BodyList.as.svn-base b/0.0.9.2/src/alternativa/physics/.svn/text-base/BodyList.as.svn-base new file mode 100644 index 0000000..aa8f79f --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/.svn/text-base/BodyList.as.svn-base @@ -0,0 +1,76 @@ +package alternativa.physics { + + /** + * + */ + public class BodyList { + + public var head:BodyListItem; + public var tail:BodyListItem; + public var size:int; + + /** + * + */ + public function BodyList() { + } + + /** + * + * @param body + */ + public function append(body:Body):void { + var item:BodyListItem = BodyListItem.create(body); + if (head == null) { + head = tail = item; + } else { + tail.next = item; + item.prev = tail; + tail = item; + } + size++; + } + + /** + * + * @param body + */ + public function remove(body:Body):Boolean { + var item:BodyListItem = findItem(body); + if (item == null) return false; + if (item == head) { + if (size == 1) { + head = tail = null; + } else { + head = item.next; + head.prev = null; + } + } else { + if (item == tail) { + tail = item.prev; + tail.next = null; + } else { + item.prev.next = item.next; + item.next.prev = item.prev; + } + } + item.dispose(); + size--; + return true; + } + + /** + * + * @param body + * @return + */ + public function findItem(body:Body):BodyListItem { + var item:BodyListItem = head; + while (item != null && item.body != body) { + item = item.next; + } + return item; + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/.svn/text-base/BodyListItem.as.svn-base b/0.0.9.2/src/alternativa/physics/.svn/text-base/BodyListItem.as.svn-base new file mode 100644 index 0000000..382e11c --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/.svn/text-base/BodyListItem.as.svn-base @@ -0,0 +1,74 @@ +package alternativa.physics { + + /** + * + */ + public class BodyListItem { + + // Верхний элемент хранилища + private static var poolTop:BodyListItem; + + /** + * Создаёт новый элемент списка. + * + * @param primitive примитив, содержащийся в элементе + * @return новый элемент списка + */ + public static function create(body:Body):BodyListItem { + var item:BodyListItem; + if (poolTop == null) { + item = new BodyListItem(body); + } else { + item = poolTop; + poolTop = item.next; + item.next = null; + item.body = body; + } + return item; + } + + /** + * Очищает хранилище. + */ + public static function clearPool():void { + var item:BodyListItem = poolTop; + while (item != null) { + poolTop = item.next; + item.next = null; + item = poolTop; + } + } + + /** + * + */ + public var body:Body; + /** + * + */ + public var next:BodyListItem; + /** + * + */ + public var prev:BodyListItem; + + /** + * + * @param body + */ + public function BodyListItem(body:Body) { + this.body = body; + } + + /** + * + */ + public function dispose():void { + body = null; + prev = null; + next = poolTop; + poolTop = this; + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/.svn/text-base/BodyMaterial.as.svn-base b/0.0.9.2/src/alternativa/physics/.svn/text-base/BodyMaterial.as.svn-base new file mode 100644 index 0000000..7a7c47d --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/.svn/text-base/BodyMaterial.as.svn-base @@ -0,0 +1,11 @@ +package alternativa.physics { + + public class BodyMaterial { + + public var restitution:Number = 0; + public var friction:Number = 0.3; +// public var dynamicFriction:Number = 0.2; +// public var dynamicFriction:Number = 0.2; + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/.svn/text-base/BodyState.as.svn-base b/0.0.9.2/src/alternativa/physics/.svn/text-base/BodyState.as.svn-base new file mode 100644 index 0000000..3987388 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/.svn/text-base/BodyState.as.svn-base @@ -0,0 +1,39 @@ +package alternativa.physics { + import alternativa.math.Quaternion; + import alternativa.math.Vector3; + + /** + * Класс описывает состояние твёрдого тела. + */ + public class BodyState { + /** + * Положение тела. + */ + public var pos:Vector3 = new Vector3(); + /** + * Ориентация тела. + */ + public var orientation:Quaternion = new Quaternion(); + /** + * Скорость тела. + */ + public var velocity:Vector3 = new Vector3(); + /** + * Угловая скорость тела. + */ + public var rotation:Vector3 = new Vector3(); + + /** + * Копирует значение указанного объекта. + * + * @param state + */ + public function copy(state:BodyState):void { + pos.vCopy(state.pos); + orientation.copy(state.orientation); + velocity.vCopy(state.velocity); + rotation.vCopy(state.rotation); + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/.svn/text-base/CollisionPrimitiveList.as.svn-base b/0.0.9.2/src/alternativa/physics/.svn/text-base/CollisionPrimitiveList.as.svn-base new file mode 100644 index 0000000..5d5304f --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/.svn/text-base/CollisionPrimitiveList.as.svn-base @@ -0,0 +1,89 @@ +package alternativa.physics { + import alternativa.physics.collision.CollisionPrimitive; + + /** + * + */ + public class CollisionPrimitiveList { + + public var head:CollisionPrimitiveListItem; + public var tail:CollisionPrimitiveListItem; + public var size:int; + + /** + * + */ + public function CollisionPrimitiveList() { + } + + /** + * + * @param primitive + */ + public function append(primitive:CollisionPrimitive):void { + var item:CollisionPrimitiveListItem = CollisionPrimitiveListItem.create(primitive); + if (head == null) { + head = tail = item; + } else { + tail.next = item; + item.prev = tail; + tail = item; + } + size++; + } + + /** + * + * @param primitve + */ + public function remove(primitve:CollisionPrimitive):void { + var item:CollisionPrimitiveListItem = findItem(primitve); + if (item == null) return; + if (item == head) { + if (size == 1) { + head = tail = null; + } else { + head = item.next; + head.prev = null; + } + } else { + if (item == tail) { + tail = tail.prev; + tail.next = null; + } else { + item.prev.next = item.next; + item.next.prev = item.prev; + } + } + item.dispose(); + size--; + } + + /** + * + * @param primitive + * @return + */ + public function findItem(primitive:CollisionPrimitive):CollisionPrimitiveListItem { + var item:CollisionPrimitiveListItem = head; + while (item != null && item.primitive != primitive) { + item = item.next; + } + return item; + } + + /** + * + */ + public function clear():void { + while (head != null) { + var item:CollisionPrimitiveListItem = head; + head = head.next; + item.dispose(); + } + tail = null; + size = 0; + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/.svn/text-base/CollisionPrimitiveListItem.as.svn-base b/0.0.9.2/src/alternativa/physics/.svn/text-base/CollisionPrimitiveListItem.as.svn-base new file mode 100644 index 0000000..d204532 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/.svn/text-base/CollisionPrimitiveListItem.as.svn-base @@ -0,0 +1,76 @@ +package alternativa.physics { + import alternativa.physics.collision.CollisionPrimitive; + + /** + * Элемент списка примитивов. + */ + public class CollisionPrimitiveListItem { + + // Верхний элемент хранилища + private static var poolTop:CollisionPrimitiveListItem; + + /** + * Создаёт новый элемент списка. + * + * @param primitive примитив, содержащийся в элементе + * @return новый элемент списка + */ + public static function create(primitive:CollisionPrimitive):CollisionPrimitiveListItem { + var item:CollisionPrimitiveListItem; + if (poolTop == null) { + item = new CollisionPrimitiveListItem(primitive); + } else { + item = poolTop; + item.primitive = primitive; + poolTop = item.next; + item.next = null; + } + return item; + } + + /** + * Очищает хранилище. + */ + public static function clearPool():void { + var curr:CollisionPrimitiveListItem = poolTop; + while (curr != null) { + poolTop = curr.next; + curr.next = null; + curr = poolTop; + } + } + + /** + * Примитив, хранящийся в элементе списка. + */ + public var primitive:CollisionPrimitive; + /** + * Ссылка не следующий элемент списка. + */ + public var next:CollisionPrimitiveListItem; + /** + * Ссылка не предыдущий элемент списка. + */ + public var prev:CollisionPrimitiveListItem; + + /** + * Создаёт новый экземпляр. + * + * @param primitive примитив, хранящийся в элементе списка + */ + public function CollisionPrimitiveListItem(primitive:CollisionPrimitive) { + this.primitive = primitive; + } + + /** + * Очищает внутренние ссылки и помещает элемент в хранилище для дальнейшего использования. + */ + public function dispose():void { + primitive = null; + prev = null; + next = poolTop; + poolTop = this; + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/.svn/text-base/Contact.as.svn-base b/0.0.9.2/src/alternativa/physics/.svn/text-base/Contact.as.svn-base new file mode 100644 index 0000000..3f010ec --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/.svn/text-base/Contact.as.svn-base @@ -0,0 +1,69 @@ +package alternativa.physics { + + + import alternativa.math.Vector3; + + /** + * Описывает контакт одного или двух тел. + */ + public class Contact { + /** + * Первое тело контакта, не может быть равно null. + */ + public var body1:Body; + /** + * Второе тело контакта, может быть равно null. + */ + public var body2:Body; + /** + * Предрассчитанный взаимный коэффициент отскока. + */ + public var restitution:Number; + /** + * Предрассчитанный взаимный коэффициент трения. + */ + public var friction:Number; + /** + * Нормаль контакта. Направлена к первому телу. + */ + public var normal:Vector3 = new Vector3(); + /** + * Список точек контакта. + */ + public var points:Vector. = new Vector.(MAX_POINTS, true); + /** + * Количество точек контакта. + */ + public var pcount:int; + /** + * Максимальная глубина пересечения. + */ + public var maxPenetration:Number = 0; + /** + * Флаг показывает, разрешён контакт или нет. + */ + public var satisfied:Boolean; + /** + * Следующий контакт в списке. + */ + public var next:Contact; + /** + * Индекс контакта. Первый контакт в списке имеет индекс 0. + */ + public var index:int; + + // Максимальное количество точек контакта + private const MAX_POINTS:int = 8; + + /** + * + */ + public function Contact(index:int) { + this.index = index; + for (var i:int = 0; i < MAX_POINTS; i++) { + points[i] = new ContactPoint(); + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/.svn/text-base/ContactPoint.as.svn-base b/0.0.9.2/src/alternativa/physics/.svn/text-base/ContactPoint.as.svn-base new file mode 100644 index 0000000..395d2dc --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/.svn/text-base/ContactPoint.as.svn-base @@ -0,0 +1,55 @@ +package alternativa.physics { + import alternativa.math.Vector3; + + /** + * + */ + public class ContactPoint { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + + // Величины, расчитываемые перед началом фазы решения контактов + + // Требуемая проекция конечной скорости на нормаль для упругого контакта + public var normalVel:Number; + // Минимальная скорость разделения неупругого контакта + public var minSepVel:Number; + // Изменение проекции скорости на единицу нормального импульса + public var velByUnitImpulseN:Number; + + public var angularInertia1:Number; + public var angularInertia2:Number; + + // Радиус-вектор точки контакта относительно центра первого тела + public var r1:Vector3 = new Vector3(); + // Радиус-вектор точки контакта относительно центра второго тела + public var r2:Vector3 = new Vector3(); + + // Величины, накапливаемые во время фазы решения контактов + + // Накопленный импульс, применяемый для получения требуемой относительной скорости в точке контакта + public var accumImpulseN:Number; + // Накопленный импульс, применяемый для разделения тел в точке контакта. Не создаёт момента. +// public var accumSepImpulse:Number; + + public var satisfied:Boolean; + + /** + * + * @param cp + */ + public function copyFrom(cp:ContactPoint):void { + pos.vCopy(cp.pos); + penetration = cp.penetration; + feature1 = cp.feature1; + feature2 = cp.feature2; + r1.vCopy(cp.r1); + r2.vCopy(cp.r2); + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/.svn/text-base/PhysicsScene.as.svn-base b/0.0.9.2/src/alternativa/physics/.svn/text-base/PhysicsScene.as.svn-base new file mode 100644 index 0000000..abdc762 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/.svn/text-base/PhysicsScene.as.svn-base @@ -0,0 +1,743 @@ +package alternativa.physics { + + + import alternativa.physics.collision.ICollisionDetector; + import alternativa.physics.collision.KdTreeCollisionDetector; + import alternativa.physics.constraints.Constraint; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * Класс реализует физическую симуляцию поведения твёрдых тел. + */ + public class PhysicsScene { + + private static var lastBodyId:int; + + // Максимальное количество контактов + public const MAX_CONTACTS:int = 1000; + + ////////////////////////////////////////////////////////////////// + // Настроечные параметры симуляции. Могут быть изменены в любой + // момент времени без нарушения корректной работы симуляции. + ////////////////////////////////////////////////////////////////// + // Количество шагов, за которое пересекающиеся тела должны разделиться + public var penResolutionSteps:int = 10; + // Величина допустимой глубины пересечения + public var allowedPenetration:Number = 0.1; + // Максимальная скорость, добавляемая с целью разделения тел + public var maxPenResolutionSpeed:Number = 0.5; + // Количество итераций для обработки упругих контактов + public var collisionIterations:int = 5; + // Количество итераций для обработки неупругих контактов + public var contactIterations:int = 5; + // Флаг использования предсказания состояний + public var usePrediction:Boolean = false; + + public var freezeSteps:int = 10; + public var linSpeedFreezeLimit:Number = 1; + public var angSpeedFreezeLimit:Number = 0.01; + + // Переменные для процедуры разделения тел путём непосредственного их перемещенеия. + // !!!!!!!!!!!!!!!!!!!!!!!! + // !!! Экспериментально !!! + // !!!!!!!!!!!!!!!!!!!!!!!! + public var staticSeparationIterations:int = 10; + public var staticSeparationSteps:int = 10; + public var maxAngleMove:Number = 10; + public var useStaticSeparation:Boolean = false; + + // Вектор гравитации + public var _gravity:Vector3 = new Vector3(0, 0, -9.8); + // Модуль вектора гравитации + public var _gravityMagnitude:Number = 9.8; + + // Использующийся детектор столкновений + public var collisionDetector:ICollisionDetector; + + // Список тел, участвующих в симуляции + public var bodies:BodyList = new BodyList(); + + // Список контактов на текущем шаге симуляции + altphysics var contacts:Contact; + // Количество контактов на текущем шаге симуляции +// altphysics var contactsNum:int; + // Список ограничений + altphysics var constraints:Vector. = new Vector.(); + // Количество ограничений + altphysics var constraintsNum:int; + // Временная метка. Число прошедших шагов с начала симуляции. + public var timeStamp:int; + // Время с начала симуляции, мс + public var time:int; + // Первый неиспользованный контакт на текущем шаге симуляции + private var borderContact:Contact; + + // Временные переменные для избежания создания экземпляров + private var _r:Vector3 = new Vector3(); + private var _t:Vector3 = new Vector3(); + private var _v:Vector3 = new Vector3(); + private var _v1:Vector3 = new Vector3(); + private var _v2:Vector3 = new Vector3(); + + /** + * + */ + public function PhysicsScene() { + contacts = new Contact(0); + var contact:Contact = contacts; + for (var i:int = 1; i < MAX_CONTACTS; i++) { + contact.next = new Contact(i); + contact = contact.next; + } + collisionDetector = new KdTreeCollisionDetector(); + } + + /** + * Вектор гравитации. + */ + public function get gravity():Vector3 { + return _gravity.vClone(); + } + + /** + * @private + */ + public function set gravity(value:Vector3):void { + _gravity.vCopy(value); + _gravityMagnitude = _gravity.vLength(); + } + + /** + * Добавляет тело в симуляцию. + * + * @param body + */ + public function addBody(body:Body):void { + body.id = lastBodyId++; + body.world = this; + bodies.append(body); + } + + /** + * Удаляет тело из симуляции. + * @param body + * @return + */ + public function removeBody(body:Body):void { + if (bodies.remove(body)) { + body.world = null; + } + } + + /** + * Добавляет ограничение. + * @param c + */ + public function addConstraint(c:Constraint):void { + constraints[constraintsNum++] = c; + c.world = this; + } + + /** + * Удаляет ограничение. + * @param c + */ + public function removeConstraint(c:Constraint):Boolean { + var idx:int = constraints.indexOf(c); + if (idx < 0) return false; + constraints.splice(idx, 1) + constraintsNum--; + c.world = null; + return true; + } + + /** + * Применяет к телам действующте на них силы. Аккумуляторы сил и моментов тел очищаются после завершения шага + * симуляции, поэтому на момент вызова метода могут уже содержать некоторые значения. + * + * @param dt промежуток времени, в течении которого действуют силы + */ + private function applyForces(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + body.beforePhysicsStep(dt); + body.calcAccelerations(); + // Ускорение свободного падения применяется только к подвижным телам во избежание некорректного изменения + // фиктивной скорости неподвижных тел. + if (body.movable && !body.frozen) { + body.accel.x += _gravity.x; + body.accel.y += _gravity.y; + body.accel.z += _gravity.z; + } + item = item.next; + } + } + + /** + * Определяет все столкновения на текущем шаге симуляции и заполняет список получившихся контактов. + * + * @param dt длительность шага симуляции + */ + private function detectCollisions(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + if (!body.frozen) { + body.contactsNum = 0; + body.saveState(); + // При включённом режиме предсказания состояние тел интегрируется на один шаг вперёд + if (usePrediction) { + body.integrateVelocity(dt); + body.integratePosition(dt); + } + body.calcDerivedData(); + } + item = item.next; + } + + borderContact = collisionDetector.getAllContacts(contacts); + + // Расчёт относительных векторов точки контакта вынесен сюда из-за необходимости учитывать + // положение тел в предсказанном состоянии + var contact:Contact = contacts; + while (contact != borderContact) { + var b1:Body = contact.body1; + var b2:Body = contact.body2; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + var bPos:Vector3 = b1.state.pos; + cp.r1.x = cp.pos.x - bPos.x; + cp.r1.y = cp.pos.y - bPos.y; + cp.r1.z = cp.pos.z - bPos.z; + if (b2 != null) { + bPos = b2.state.pos; + cp.r2.x = cp.pos.x - bPos.x; + cp.r2.y = cp.pos.y - bPos.y; + cp.r2.z = cp.pos.z - bPos.z; + } + } + contact = contact.next; + } + + // Восстановление состояния тел + if (usePrediction) { + item = bodies.head; + while (item != null) { + body = item.body; + if (!body.frozen) { + body.restoreState(); + body.calcDerivedData(); + } + item = item.next; + } + } + } + + /** + * Подготваливает полученные из детектора столкновений контакты, расчитывая значения, не меняющиеся + * в ходе шага симуляции. + */ + private function preProcessContacts(dt:Number):void { + var contact:Contact = contacts; + while (contact != borderContact) { + var b1:Body = contact.body1; + var b2:Body = contact.body2; + // Столкнувшиеся тела размораживаются + if (b1.frozen) { + b1.frozen = false; + b1.freezeCounter = 0; + } + if (b2 != null && b2.frozen) { + b2.frozen = false; + b2.freezeCounter = 0; + } + contact.restitution = b1.material.restitution; + if (b2 != null && b2.material.restitution < contact.restitution) contact.restitution = b2.material.restitution; + contact.friction = b1.material.friction; + if (b2 != null && b2.material.friction < contact.friction) contact.friction = b2.material.friction; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.accumImpulseN = 0; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + cp.velByUnitImpulseN = 0; + if (b1.movable) { + cp.angularInertia1 = _v.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1).vDot(contact.normal); + cp.velByUnitImpulseN += b1.invMass + cp.angularInertia1; + } + if (b2 != null && b2.movable) { + cp.angularInertia2 = _v.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2).vDot(contact.normal); + cp.velByUnitImpulseN += b2.invMass + cp.angularInertia2; + } + // Расчёт требуемой конечной скорости для упругого контакта + calcSepVelocity(b1, b2, cp, _v); + cp.normalVel = _v.vDot(contact.normal); + if (cp.normalVel < 0) cp.normalVel = - contact.restitution*cp.normalVel; + // Скорость разделения неупругого контакта + cp.minSepVel = cp.penetration > allowedPenetration ? (cp.penetration - allowedPenetration)/(penResolutionSteps*dt) : 0; + if (cp.minSepVel > maxPenResolutionSpeed) cp.minSepVel = maxPenResolutionSpeed; + } + contact = contact.next; + } + for (var i:int = 0; i < constraintsNum; i++) { + var constraint:Constraint = constraints[i]; + constraint.preProcess(dt); + } + } + + /** + * + * @param dt + * @param forceInelastic + */ + private function processContacts(dt:Number, forceInelastic:Boolean):void { + var iterNum:int = forceInelastic ? contactIterations : collisionIterations; + var i:int; + var forwardLoop:Boolean = false; + for (var iter:int = 0; iter < iterNum; iter++) { + forwardLoop = !forwardLoop; + var contact:Contact = contacts; + while (contact != borderContact) { + resolveContact(contact, forceInelastic, forwardLoop); + contact = contact.next; + } + // Ограничения + for (i = 0; i < constraintsNum; i++) { + var constraint:Constraint = constraints[i]; + constraint.apply(dt); + } + } + + // Разделение контактов путём непосредственного изменения координат и ориентации. Экспериментально. +// if (forceInelastic && useStaticSeparation) performStaticSeparation(); + } + + /** + * + */ + private function resolveContact(contactInfo:Contact, forceInelastic:Boolean, forwardLoop:Boolean):void { + var b1:Body = contactInfo.body1; + var b2:Body = contactInfo.body2; + var normal:Vector3 = contactInfo.normal; + var i:int; + if (forwardLoop) { + for (i = 0; i < contactInfo.pcount; i++) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } else { + for (i = contactInfo.pcount - 1; i >= 0; i--) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } + } + + /** + * + * @param idx + * @param b1 + * @param b2 + * @param colInfo + * @param normal + * @param forceInelastic + */ + private function resolveContactPoint(idx:int, b1:Body, b2:Body, contact:Contact, normal:Vector3, forceInelastic:Boolean):void { + var cp:ContactPoint = contact.points[idx]; + if (!forceInelastic) { + cp.satisfied = true; + } + + var newVel:Number = 0; + calcSepVelocity(b1, b2, cp, _v); + var cnormal:Vector3 = contact.normal; + var sepVel:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + if (forceInelastic) { + var minSpeVel:Number = useStaticSeparation ? 0 : cp.minSepVel; + if (sepVel < minSpeVel) { + cp.satisfied = false; + } else if (cp.satisfied) return; + newVel = minSpeVel; + } else { + newVel = cp.normalVel; + } + var deltaVel:Number = newVel - sepVel; + var impulse:Number = deltaVel/cp.velByUnitImpulseN; + var accumImpulse:Number = cp.accumImpulseN + impulse; + if (accumImpulse < 0) { + accumImpulse = 0; + } + var deltaImpulse:Number = accumImpulse - cp.accumImpulseN; + cp.accumImpulseN = accumImpulse; + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, normal, deltaImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, normal, -deltaImpulse); + } + + // Учёт силы трения + calcSepVelocity(b1, b2, cp, _v); + // Расчитываем изменение касательной скорости на единицу касательного импульса + var tanSpeedByUnitImpulse:Number = 0; + +// _v.vAddScaled(-_v.vDot(contact.normal), contact.normal); + var dot:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + _v.x -= dot*cnormal.x; + _v.y -= dot*cnormal.y; + _v.z -= dot*cnormal.z; + + var tanSpeed:Number = _v.vLength(); + if (tanSpeed < 0.001) return; + +// _t.vCopy(_v).vNormalize().vReverse(); + _t.x = -_v.x; + _t.y = -_v.y; + _t.z = -_v.z; + _t.vNormalize(); + + var r:Vector3; + var m:Matrix3; + var xx:Number; + var yy:Number; + var zz:Number; + // dV = b.invMass + ((invI * (r % t)) % r) * t + if (b1.movable) { +// _v.vCross2(cp.r1, _t).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1); + r = cp.r1; + m = b1.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b1.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + if (b2 != null && b2.movable) { +// _v.vCross2(cp.r2, _t).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2); + + r = cp.r2; + m = b2.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b2.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + + var tanImpulse:Number = tanSpeed/tanSpeedByUnitImpulse; + var max:Number = contact.friction*cp.accumImpulseN; + if (max < 0) { + if (tanImpulse < max) tanImpulse = max; + } else { + if (tanImpulse > max) tanImpulse = max; + } + + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, _t, tanImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, _t, -tanImpulse); + } + } + + /** + * + * @param cp + * @param normal + * @return + */ + private function calcSepVelocity(body1:Body, body2:Body, cp:ContactPoint, result:Vector3):void { + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 +// result.vCopy(body1.state.velocity).vAdd(_v1.vCross2(body1.state.rotation, cp.r1)); + var rot:Vector3 = body1.state.rotation; + var v:Vector3 = cp.r1; + var x:Number = rot.y*v.z - rot.z*v.y; + var y:Number = rot.z*v.x - rot.x*v.z; + var z:Number = rot.x*v.y - rot.y*v.x; + v = body1.state.velocity; + result.x = v.x + x; + result.y = v.y + y; + result.z = v.z + z; + // V2 = V2_c + w2%r2 + if (body2 != null) { +// result.vSubtract(body2.state.velocity).vSubtract(_v2.vCross2(body2.state.rotation, cp.r2)); + rot = body2.state.rotation; + v = cp.r2; + x = rot.y*v.z - rot.z*v.y; + y = rot.z*v.x - rot.x*v.z; + z = rot.x*v.y - rot.y*v.x; + v = body2.state.velocity; + result.x -= v.x + x; + result.y -= v.y + y; + result.z -= v.z + z; + } + } + + /** + * + * @param dt + */ + private function intergateVelocities(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + item.body.integrateVelocity(dt); + item = item.next; + } + } + + /** + * + * @param dt + */ + private function integratePositions(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + if (body.movable && !body.frozen) { + body.integratePosition(dt); + } + item = item.next; + } + } + + /** + * + */ + private function performStaticSeparation():void { + var iterNum:int = staticSeparationIterations; +// iterNum = 100; + + // 1. В начале каждой итерации для всех контактов сбрасывается флаг satisfied с одновременным поиском наихудшего контакта, т.е. имеющего наибольшую величину пересечения тел. + // 2. Если найденный контакт имеет величину пересечения меньше предельно допустимой процедура прерывается, т.к. разделение контактов не требуется. + // 3. Если процедура продолжается, то выполняется разделение найденного контакта и установка его флага satisfied в true, после чего запускается внутренний цикл, состоящий + // из contactsNum - 1 итераций. На каждой итерации ищется наихудший контакт среди оставшихся (satisfied == false). Если такой контакт найден, то выполняется его разделение + // и переход к следующей итерации внутреннего цикла, иначе внутренний цикл прерывается и выполняется переход к следующей итерации внешнего цикла (шаг 1.). +// for (var iter:int = 0; iter < iterNum; iter++) { +// // Ищем контакт с максимальной величиной пересечения тел, одновременно сбрасывая флаги satisfied в false +// var worstContact:Contact = contacts[0]; +// var i:int; +// for (i = 1; i < contactsNum; i++) { +// var contact:Contact = contacts[i]; +// contact.satisfied = false; +// if (contact.maxPenetration > worstContact.maxPenetration) worstContact = contact; +// } +// if (worstContact.maxPenetration <= allowedPenetration) return; +// +// resolveInterpenetration(worstContact); +// // Внутренний цикл по оставшимся контактам +// for (i = 1; i < contactsNum; i++) { +// worstContact = getWorstContact(); +// if (worstContact == null) break; +// resolveInterpenetration(worstContact); +// } +// } + } + + /** + * + */ +// private function getWorstContact():Contact { +// var maxPen:Number = 0; +// var worst:Contact = null; +// for (var i:int = 0; i < contactsNum; i++) { +// var c:Contact = contacts[i]; +// if (!c.satisfied && c.maxPenetration > maxPen) { +// worst = c; +// maxPen = c.maxPenetration; +// } +// } +// return maxPen > allowedPenetration ? worst : null; +// } + + /** + * Разделяет указанный контакт, выполняя staticSeparationSteps итераций по списку точек контакта. + * 1. В начале каждой итерации ищется точка с наибольшим пересечением, одновременно сбрасываются флаги satisfied у точек. + * 2. Для найденной точки выполняется процедура разделения. + * 3. После выполняется pcount - 1 итерация по списку точек, каждый раз ищется наихудшая среди имеющих satisfied == false, для которой выполняется процедура разделения. + * Если наихудшая точка не найдена, то выполнется переход к следующей итерации внешнего цикла (шаг 1). + * + * В конце процедуры разделения обновляются значения пересечений для остальных точек контакта, а также для всех прочих контактов, относящихся к телам текущего. + * + * @param contact контакт для разделения + */ + private function resolveInterpenetration(contact:Contact):void { + contact.satisfied = true; + + for (var step:int = 0; step < staticSeparationSteps; step++) { + var worstCp:ContactPoint = contact.points[0]; + var cp:ContactPoint; + var i:int; + for (i = 1; i < contact.pcount; i++) { + cp = contact.points[i]; + cp.satisfied = false; + if (cp.penetration > worstCp.penetration) worstCp = cp; + } + if (worstCp.penetration <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + // Разделяем оставшиеся точки + var maxPen:Number = 0; + for (i = 1; i < contact.pcount; i++) { + // Поиск наихудшей точки + for (var j:int = 0; j < contact.pcount; j++) { + cp = contact.points[j]; + if (cp.satisfied) continue; + if (cp.penetration > maxPen) { + maxPen = cp.penetration; + worstCp = cp; + } + } + if (maxPen <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + } + } + } + + /** + * + * @param cp + * @param contact + */ + private function separateContactPoint(cp:ContactPoint, contact:Contact):void { +// cp.satisfied = true; +// +// var b1:Body = contact.body1; +// var b2:Body = contact.body2; +// var totalMove:Number = cp.penetration - allowedPenetration; +// var moveCoeff:Number = totalMove/cp.velByUnitImpulseN; +// var linMove1:Number; +// var angleMove1:Number; +// if (b1.movable) { +// linMove1 = b1.invMass*moveCoeff; +// angleMove1 = cp.angularInertia1*moveCoeff; +// if (angleMove1 > maxAngleMove) { +// linMove1 += angleMove1 - maxAngleMove; +// angleMove1 -= maxAngleMove; +// } +// b1.state.pos.vAddScaled(linMove1, contact.normal); +// _v1.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vScale(angleMove1); +// b1.state.orientation.addScaledVector(_v1, 1); +// } +// var linMove2:Number; +// var angleMove2:Number; +// if (b2 != null && b2.movable) { +// linMove2 = b2.invMass*moveCoeff; +// angleMove2 = cp.angularInertia2*moveCoeff; +// if (angleMove2 > maxAngleMove) { +// linMove2 += angleMove2 - maxAngleMove; +// angleMove2 -= maxAngleMove; +// } +// b2.state.pos.vAddScaled(-linMove2, contact.normal); +// _v2.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vScale(angleMove2); +// _v2.vReverse(); +// b2.state.orientation.addScaledVector(_v2, 1); +// } +// cp.penetration = allowedPenetration; +// // Обновляем пересечения в других точках +// var i:int; +// for (i = 0; i < contact.pcount; i++) { +// var cp1:ContactPoint = contact.points[i]; +// if (cp1 == cp) continue; +// var angularMove:Number; +// if (b1.movable) { +// angularMove = _v.vCross2(_v1, cp1.r1).vDot(contact.normal); +// cp1.penetration -= linMove1 + angularMove; +// } +// if (b2 != null && b2.movable) { +// angularMove = _v.vCross2(_v2, cp1.r2).vDot(contact.normal); +// cp1.penetration -= linMove2 - angularMove; +// } +// // Обновление максимального значения для контакта +// if (cp1.penetration > contact.maxPenetration) contact.maxPenetration = cp1.penetration; +// } +// // Обновляем пересечения для других контактов +// var c:Contact; +// var j:int; +// if (b1.movable) { +// for (i = 0; i < b1.contactsNum; i++) { +// c = b1.contacts[i]; +// if (c == contact) continue; +// for (j = 0; j < c.pcount; j++) { +// cp1 = c.points[j]; +// if (b1 == c.body1) cp1.penetration -= linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r1).vDot(c.normal); +// else cp1.penetration += linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r2).vDot(c.normal); +// if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; +// } +// } +// } +// if (b2 != null && b2.movable) { +// for (i = 0; i < b2.contactsNum; i++) { +// c = b2.contacts[i]; +// if (c == contact) continue; +// for (j = 0; j < c.pcount; j++) { +// cp1 = c.points[j]; +// if (b2 == c.body1) cp1.penetration -= linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r1).vDot(c.normal); +// else cp1.penetration += linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r2).vDot(c.normal); +// if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; +// } +// } +// } + } + + /** + * + */ + private function postPhysics():void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + body.clearAccumulators(); + body.calcDerivedData(); + if (body.canFreeze) { + if (body.state.velocity.vLength() < linSpeedFreezeLimit && body.state.rotation.vLength() < angSpeedFreezeLimit) { + if (!body.frozen) { + body.freezeCounter++; + if (body.freezeCounter >= freezeSteps) body.frozen = true; + } + } else { + body.freezeCounter = 0; + body.frozen = false; + } + } + item = item.next; + } + } + + /** + * + * @param delta + */ + public function update(delta:int):void { + timeStamp++; + time += delta; + var dt:Number = 0.001*delta; + applyForces(dt); + detectCollisions(dt); + preProcessContacts(dt); + processContacts(dt, false); + intergateVelocities(dt); + processContacts(dt, true); + integratePositions(dt); + postPhysics(); + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/.svn/text-base/PhysicsUtils.as.svn-base b/0.0.9.2/src/alternativa/physics/.svn/text-base/PhysicsUtils.as.svn-base new file mode 100644 index 0000000..5a2e46f --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/.svn/text-base/PhysicsUtils.as.svn-base @@ -0,0 +1,49 @@ +package alternativa.physics { + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + public class PhysicsUtils { + + /** + * @param mass + * @param halfSize + * @param result + */ + public static function getBoxInvInertia(mass:Number, halfSize:Vector3, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + result.a = 3/(mass*(yy + zz)); + result.f = 3/(mass*(zz + xx)); + result.k = 3/(mass*(xx + yy)); + } + + /** + * @param mass + * @param r + * @param h + * @param result + */ + public static function getCylinderInvInertia(mass:Number, r:Number, h:Number, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + + result.a = result.f = 1/(mass*(h*h/12 + r*r/4)); + result.k = 2/(mass*r*r); + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base b/0.0.9.2/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base new file mode 100644 index 0000000..2715307 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base @@ -0,0 +1,3 @@ +package alternativa.physics { + public namespace altphysics = "http://alternativaplatform.com/en/altphysics"; +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/Body.as b/0.0.9.2/src/alternativa/physics/Body.as new file mode 100644 index 0000000..cd22c5b --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/Body.as @@ -0,0 +1,405 @@ +package alternativa.physics { + + import alternativa.math.Matrix3; + import alternativa.math.Matrix4; + import alternativa.math.Quaternion; + import alternativa.math.Vector3; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.IBodyCollisionPredicate; + import alternativa.physics.collision.types.BoundBox; + + use namespace altphysics; + + /** + * + */ + public class Body { + + public static var linDamping:Number = 0.997; + public static var rotDamping:Number = 0.997; + + // Идентификатор тела, уникальный в пределах мира + public var id:int; + // Имя тела + public var name:String; + // Мир, в котором находится тело + public var world:PhysicsScene; + // Флаг подвижности тела + public var movable:Boolean = true; + // Флаг указывает, может ли тело быть заморожено + public var canFreeze:Boolean = false; + + public var freezeCounter:int; + public var frozen:Boolean = false; + // Ограничивающий бокс тела + public var aabb:BoundBox = new BoundBox(); + // Предикат, через который тело получает сообщения о столкновениях + public var postCollisionPredicate:IBodyCollisionPredicate; + + // Текущее состояние тела + public var state:BodyState = new BodyState(); + // Предыдущее состояние тела + public var prevState:BodyState = new BodyState(); + // Линейное ускорение тела на текущем шаге симуляции + public var accel:Vector3 = new Vector3(); + // Угловое ускорение тела на текущем шаге симуляции + public var angleAccel:Vector3 = new Vector3(); + // Физический материал тела + public var material:BodyMaterial = new BodyMaterial(); + // Обратная масса тела + public var invMass:Number = 1; + // Обратная матрица тензора инерции в локальных координатах + public var invInertia:Matrix3 = new Matrix3(); + // Обратная матрица тензора инерции в мировых координатах + public var invInertiaWorld:Matrix3 = new Matrix3(); + // Базисная матрица тела в мировых координатах + public var baseMatrix:Matrix3 = new Matrix3(); + + public const MAX_CONTACTS:int = 20; + public var contacts:Vector. = new Vector.(MAX_CONTACTS); + public var contactsNum:int; + + public var collisionPrimitives:CollisionPrimitiveList; + + // Аккумулятор сил + public var forceAccum:Vector3 = new Vector3(); + // Аккумулятор моментов + public var torqueAccum:Vector3 = new Vector3(); + + // Внутренние переменные для избежания создания экземпляров + private static var _r:Vector3 = new Vector3(); + private static var _f:Vector3 = new Vector3(); + + /** + * + * @param invMass + * @param invInertia + */ + public function Body(invMass:Number, invInertia:Matrix3) { + this.invMass = invMass; + this.invInertia.copy(invInertia); + } + + /** + * @param primitive + * @param localTransform + */ + public function addCollisionPrimitive(primitive:CollisionPrimitive, localTransform:Matrix4 = null):void { + if (primitive == null) { + throw new ArgumentError("Primitive cannot be null"); + } + if (collisionPrimitives == null) { + collisionPrimitives = new CollisionPrimitiveList(); + } + collisionPrimitives.append(primitive); + primitive.setBody(this, localTransform); + } + + /** + * + * @param primitive + */ + public function removeCollisionPrimitive(primitive:CollisionPrimitive):void { + if (collisionPrimitives == null) return; + primitive.setBody(null); + collisionPrimitives.remove(primitive); + if (collisionPrimitives.size == 0) { + collisionPrimitives = null; + } + } + + /** + * @param t + * @param result + */ + public function interpolate(t:Number, pos:Vector3, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.w = prevState.orientation.w*t1 + state.orientation.w*t; + orientation.x = prevState.orientation.x*t1 + state.orientation.x*t; + orientation.y = prevState.orientation.y*t1 + state.orientation.y*t; + orientation.z = prevState.orientation.z*t1 + state.orientation.z*t; + } + + /** + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + state.pos.vCopy(pos); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setPositionXYZ(x:Number, y:Number, z:Number):void { + state.pos.vReset(x, y, z); + } + + /** + * + * @param vel + */ + public function setVelocity(vel:Vector3):void { + state.velocity.vCopy(vel); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setVelocityXYZ(x:Number, y:Number, z:Number):void { + state.velocity.vReset(x, y, z); + } + + /** + * + * @param rot + */ + public function setRotation(rot:Vector3):void { + state.rotation.vCopy(rot); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setRotationXYZ(x:Number, y:Number, z:Number):void { + state.rotation.vReset(x, y, z); + } + + /** + * + * @param q + */ + public function setOrientation(q:Quaternion):void { + state.orientation.copy(q); + } + + /** + * @param r + * @param dir + * @param magnitude + */ + public function applyRelPosWorldImpulse(r:Vector3, dir:Vector3, magnitude:Number):void { + var d:Number = magnitude*invMass; + // Линейная часть + state.velocity.x += d*dir.x; + state.velocity.y += d*dir.y; + state.velocity.z += d*dir.z; + + // Вращательная часть + var x:Number = (r.y*dir.z - r.z*dir.y)*magnitude; + var y:Number = (r.z*dir.x - r.x*dir.z)*magnitude; + var z:Number = (r.x*dir.y - r.y*dir.x)*magnitude; + + state.rotation.x += invInertiaWorld.a*x + invInertiaWorld.b*y + invInertiaWorld.c*z; + state.rotation.y += invInertiaWorld.e*x + invInertiaWorld.f*y + invInertiaWorld.g*z; + state.rotation.z += invInertiaWorld.i*x + invInertiaWorld.j*y + invInertiaWorld.k*z; + } + + /** + * @param f + */ + public function addForce(f:Vector3):void { + forceAccum.vAdd(f); + } + + /** + * + * @param fx + * @param fy + * @param fz + */ + public function addForceXYZ(fx:Number, fy:Number, fz:Number):void { + forceAccum.x += fx; + forceAccum.y += fy; + forceAccum.z += fz; + } + + /** + * @param pos + * @param f + */ + public function addWorldForceXYZ(px:Number, py:Number, pz:Number, fx:Number, fy:Number, fz:Number):void { + forceAccum.x += fx; + forceAccum.y += fy; + forceAccum.z += fz; + + var pos:Vector3 = state.pos; + var rx:Number = px - pos.x; + var ry:Number = py - pos.y; + var rz:Number = pz - pos.z; + +// var x:Number = ry*fz - rz*fy; +// var y:Number = rz*fx - rx*fz; +// var z:Number = rx*fy - ry*fx; + + torqueAccum.x += ry*fz - rz*fy; + torqueAccum.y += rz*fx - rx*fz; + torqueAccum.z += rx*fy - ry*fx; + } + + /** + * @param pos + * @param f + */ + public function addWorldForce(pos:Vector3, force:Vector3):void { + forceAccum.vAdd(force); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(force)); + } + + /** + * @param pos + * @param f + */ + public function addWorldForceScaled(pos:Vector3, force:Vector3, scale:Number):void { + _f.x = scale*force.x; + _f.y = scale*force.y; + _f.z = scale*force.z; + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(_f)); + } + + /** + * @param pos + * @param f + */ + public function addLocalForce(pos:Vector3, force:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(pos, _r); + // Трансформируем вектор силы в мировую систему + baseMatrix.transformVector(force, _f); + // Добавляем силу и момент + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vCross(_f)); + } + + /** + * + * @param localPos + * @param worldForce + */ + public function addWorldForceAtLocalPoint(localPos:Vector3, worldForce:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(localPos, _r); + // Добавляем силу и момент + forceAccum.vAdd(worldForce); + torqueAccum.vAdd(_r.vCross(worldForce)); + } + + /** + * @param dt + */ + public function beforePhysicsStep(dt:Number):void { + } + + /** + * @param t + */ + public function addTorque(t:Vector3):void { + torqueAccum.vAdd(t); + } + + /** + * + */ + altphysics function clearAccumulators():void { + forceAccum.x = forceAccum.y = forceAccum.z = 0; + torqueAccum.x = torqueAccum.y = torqueAccum.z = 0; + } + + /** + * + */ + altphysics function calcAccelerations():void { + accel.x = forceAccum.x*invMass; + accel.y = forceAccum.y*invMass; + accel.z = forceAccum.z*invMass; + angleAccel.x = invInertiaWorld.a*torqueAccum.x + invInertiaWorld.b*torqueAccum.y + invInertiaWorld.c*torqueAccum.z; + angleAccel.y = invInertiaWorld.e*torqueAccum.x + invInertiaWorld.f*torqueAccum.y + invInertiaWorld.g*torqueAccum.z; + angleAccel.z = invInertiaWorld.i*torqueAccum.x + invInertiaWorld.j*torqueAccum.y + invInertiaWorld.k*torqueAccum.z; + } + + /** + * Вычисляет производные данные. + */ + public function calcDerivedData():void { + // Вычисление базисной матрицы и обратного тензора инерции в мировых координатах + state.orientation.toMatrix3(baseMatrix); + invInertiaWorld.copy(invInertia).append(baseMatrix).prependTransposed(baseMatrix); + if (collisionPrimitives != null) { + aabb.infinity(); + var item:CollisionPrimitiveListItem = collisionPrimitives.head; + while (item != null) { + var primitive:CollisionPrimitive = item.primitive; + primitive.transform.setFromMatrix3(baseMatrix, state.pos); + if (primitive.localTransform != null) { + primitive.transform.prepend(primitive.localTransform); + } + primitive.calculateAABB(); + aabb.addBoundBox(primitive.aabb); + item = item.next; + } + } + } + + /** + * + */ + altphysics function saveState():void { + prevState.copy(state); + } + + /** + * + */ + altphysics function restoreState():void { + state.copy(prevState); + } + + /** + * @param dt + */ + altphysics function integrateVelocity(dt:Number):void { + // v = v + a*t + state.velocity.x += accel.x*dt; + state.velocity.y += accel.y*dt; + state.velocity.z += accel.z*dt; + // rot = rot + eps*t + state.rotation.x += angleAccel.x*dt; + state.rotation.y += angleAccel.y*dt; + state.rotation.z += angleAccel.z*dt; + + state.velocity.x *= linDamping; + state.velocity.y *= linDamping; + state.velocity.z *= linDamping; + + state.rotation.x *= rotDamping; + state.rotation.y *= rotDamping; + state.rotation.z *= rotDamping; + } + + /** + * + */ + altphysics function integratePosition(dt:Number):void { + // pos = pos + v*t + state.pos.x += state.velocity.x*dt; + state.pos.y += state.velocity.y*dt; + state.pos.z += state.velocity.z*dt; + // q = q + 0.5*rot*q + state.orientation.addScaledVector(state.rotation, dt); + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/BodyList.as b/0.0.9.2/src/alternativa/physics/BodyList.as new file mode 100644 index 0000000..aa8f79f --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/BodyList.as @@ -0,0 +1,76 @@ +package alternativa.physics { + + /** + * + */ + public class BodyList { + + public var head:BodyListItem; + public var tail:BodyListItem; + public var size:int; + + /** + * + */ + public function BodyList() { + } + + /** + * + * @param body + */ + public function append(body:Body):void { + var item:BodyListItem = BodyListItem.create(body); + if (head == null) { + head = tail = item; + } else { + tail.next = item; + item.prev = tail; + tail = item; + } + size++; + } + + /** + * + * @param body + */ + public function remove(body:Body):Boolean { + var item:BodyListItem = findItem(body); + if (item == null) return false; + if (item == head) { + if (size == 1) { + head = tail = null; + } else { + head = item.next; + head.prev = null; + } + } else { + if (item == tail) { + tail = item.prev; + tail.next = null; + } else { + item.prev.next = item.next; + item.next.prev = item.prev; + } + } + item.dispose(); + size--; + return true; + } + + /** + * + * @param body + * @return + */ + public function findItem(body:Body):BodyListItem { + var item:BodyListItem = head; + while (item != null && item.body != body) { + item = item.next; + } + return item; + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/BodyListItem.as b/0.0.9.2/src/alternativa/physics/BodyListItem.as new file mode 100644 index 0000000..382e11c --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/BodyListItem.as @@ -0,0 +1,74 @@ +package alternativa.physics { + + /** + * + */ + public class BodyListItem { + + // Верхний элемент хранилища + private static var poolTop:BodyListItem; + + /** + * Создаёт новый элемент списка. + * + * @param primitive примитив, содержащийся в элементе + * @return новый элемент списка + */ + public static function create(body:Body):BodyListItem { + var item:BodyListItem; + if (poolTop == null) { + item = new BodyListItem(body); + } else { + item = poolTop; + poolTop = item.next; + item.next = null; + item.body = body; + } + return item; + } + + /** + * Очищает хранилище. + */ + public static function clearPool():void { + var item:BodyListItem = poolTop; + while (item != null) { + poolTop = item.next; + item.next = null; + item = poolTop; + } + } + + /** + * + */ + public var body:Body; + /** + * + */ + public var next:BodyListItem; + /** + * + */ + public var prev:BodyListItem; + + /** + * + * @param body + */ + public function BodyListItem(body:Body) { + this.body = body; + } + + /** + * + */ + public function dispose():void { + body = null; + prev = null; + next = poolTop; + poolTop = this; + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/BodyMaterial.as b/0.0.9.2/src/alternativa/physics/BodyMaterial.as new file mode 100644 index 0000000..7a7c47d --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/BodyMaterial.as @@ -0,0 +1,11 @@ +package alternativa.physics { + + public class BodyMaterial { + + public var restitution:Number = 0; + public var friction:Number = 0.3; +// public var dynamicFriction:Number = 0.2; +// public var dynamicFriction:Number = 0.2; + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/BodyState.as b/0.0.9.2/src/alternativa/physics/BodyState.as new file mode 100644 index 0000000..3987388 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/BodyState.as @@ -0,0 +1,39 @@ +package alternativa.physics { + import alternativa.math.Quaternion; + import alternativa.math.Vector3; + + /** + * Класс описывает состояние твёрдого тела. + */ + public class BodyState { + /** + * Положение тела. + */ + public var pos:Vector3 = new Vector3(); + /** + * Ориентация тела. + */ + public var orientation:Quaternion = new Quaternion(); + /** + * Скорость тела. + */ + public var velocity:Vector3 = new Vector3(); + /** + * Угловая скорость тела. + */ + public var rotation:Vector3 = new Vector3(); + + /** + * Копирует значение указанного объекта. + * + * @param state + */ + public function copy(state:BodyState):void { + pos.vCopy(state.pos); + orientation.copy(state.orientation); + velocity.vCopy(state.velocity); + rotation.vCopy(state.rotation); + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/CollisionPrimitiveList.as b/0.0.9.2/src/alternativa/physics/CollisionPrimitiveList.as new file mode 100644 index 0000000..5d5304f --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/CollisionPrimitiveList.as @@ -0,0 +1,89 @@ +package alternativa.physics { + import alternativa.physics.collision.CollisionPrimitive; + + /** + * + */ + public class CollisionPrimitiveList { + + public var head:CollisionPrimitiveListItem; + public var tail:CollisionPrimitiveListItem; + public var size:int; + + /** + * + */ + public function CollisionPrimitiveList() { + } + + /** + * + * @param primitive + */ + public function append(primitive:CollisionPrimitive):void { + var item:CollisionPrimitiveListItem = CollisionPrimitiveListItem.create(primitive); + if (head == null) { + head = tail = item; + } else { + tail.next = item; + item.prev = tail; + tail = item; + } + size++; + } + + /** + * + * @param primitve + */ + public function remove(primitve:CollisionPrimitive):void { + var item:CollisionPrimitiveListItem = findItem(primitve); + if (item == null) return; + if (item == head) { + if (size == 1) { + head = tail = null; + } else { + head = item.next; + head.prev = null; + } + } else { + if (item == tail) { + tail = tail.prev; + tail.next = null; + } else { + item.prev.next = item.next; + item.next.prev = item.prev; + } + } + item.dispose(); + size--; + } + + /** + * + * @param primitive + * @return + */ + public function findItem(primitive:CollisionPrimitive):CollisionPrimitiveListItem { + var item:CollisionPrimitiveListItem = head; + while (item != null && item.primitive != primitive) { + item = item.next; + } + return item; + } + + /** + * + */ + public function clear():void { + while (head != null) { + var item:CollisionPrimitiveListItem = head; + head = head.next; + item.dispose(); + } + tail = null; + size = 0; + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/CollisionPrimitiveListItem.as b/0.0.9.2/src/alternativa/physics/CollisionPrimitiveListItem.as new file mode 100644 index 0000000..d204532 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/CollisionPrimitiveListItem.as @@ -0,0 +1,76 @@ +package alternativa.physics { + import alternativa.physics.collision.CollisionPrimitive; + + /** + * Элемент списка примитивов. + */ + public class CollisionPrimitiveListItem { + + // Верхний элемент хранилища + private static var poolTop:CollisionPrimitiveListItem; + + /** + * Создаёт новый элемент списка. + * + * @param primitive примитив, содержащийся в элементе + * @return новый элемент списка + */ + public static function create(primitive:CollisionPrimitive):CollisionPrimitiveListItem { + var item:CollisionPrimitiveListItem; + if (poolTop == null) { + item = new CollisionPrimitiveListItem(primitive); + } else { + item = poolTop; + item.primitive = primitive; + poolTop = item.next; + item.next = null; + } + return item; + } + + /** + * Очищает хранилище. + */ + public static function clearPool():void { + var curr:CollisionPrimitiveListItem = poolTop; + while (curr != null) { + poolTop = curr.next; + curr.next = null; + curr = poolTop; + } + } + + /** + * Примитив, хранящийся в элементе списка. + */ + public var primitive:CollisionPrimitive; + /** + * Ссылка не следующий элемент списка. + */ + public var next:CollisionPrimitiveListItem; + /** + * Ссылка не предыдущий элемент списка. + */ + public var prev:CollisionPrimitiveListItem; + + /** + * Создаёт новый экземпляр. + * + * @param primitive примитив, хранящийся в элементе списка + */ + public function CollisionPrimitiveListItem(primitive:CollisionPrimitive) { + this.primitive = primitive; + } + + /** + * Очищает внутренние ссылки и помещает элемент в хранилище для дальнейшего использования. + */ + public function dispose():void { + primitive = null; + prev = null; + next = poolTop; + poolTop = this; + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/Contact.as b/0.0.9.2/src/alternativa/physics/Contact.as new file mode 100644 index 0000000..3f010ec --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/Contact.as @@ -0,0 +1,69 @@ +package alternativa.physics { + + + import alternativa.math.Vector3; + + /** + * Описывает контакт одного или двух тел. + */ + public class Contact { + /** + * Первое тело контакта, не может быть равно null. + */ + public var body1:Body; + /** + * Второе тело контакта, может быть равно null. + */ + public var body2:Body; + /** + * Предрассчитанный взаимный коэффициент отскока. + */ + public var restitution:Number; + /** + * Предрассчитанный взаимный коэффициент трения. + */ + public var friction:Number; + /** + * Нормаль контакта. Направлена к первому телу. + */ + public var normal:Vector3 = new Vector3(); + /** + * Список точек контакта. + */ + public var points:Vector. = new Vector.(MAX_POINTS, true); + /** + * Количество точек контакта. + */ + public var pcount:int; + /** + * Максимальная глубина пересечения. + */ + public var maxPenetration:Number = 0; + /** + * Флаг показывает, разрешён контакт или нет. + */ + public var satisfied:Boolean; + /** + * Следующий контакт в списке. + */ + public var next:Contact; + /** + * Индекс контакта. Первый контакт в списке имеет индекс 0. + */ + public var index:int; + + // Максимальное количество точек контакта + private const MAX_POINTS:int = 8; + + /** + * + */ + public function Contact(index:int) { + this.index = index; + for (var i:int = 0; i < MAX_POINTS; i++) { + points[i] = new ContactPoint(); + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/ContactPoint.as b/0.0.9.2/src/alternativa/physics/ContactPoint.as new file mode 100644 index 0000000..395d2dc --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/ContactPoint.as @@ -0,0 +1,55 @@ +package alternativa.physics { + import alternativa.math.Vector3; + + /** + * + */ + public class ContactPoint { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + + // Величины, расчитываемые перед началом фазы решения контактов + + // Требуемая проекция конечной скорости на нормаль для упругого контакта + public var normalVel:Number; + // Минимальная скорость разделения неупругого контакта + public var minSepVel:Number; + // Изменение проекции скорости на единицу нормального импульса + public var velByUnitImpulseN:Number; + + public var angularInertia1:Number; + public var angularInertia2:Number; + + // Радиус-вектор точки контакта относительно центра первого тела + public var r1:Vector3 = new Vector3(); + // Радиус-вектор точки контакта относительно центра второго тела + public var r2:Vector3 = new Vector3(); + + // Величины, накапливаемые во время фазы решения контактов + + // Накопленный импульс, применяемый для получения требуемой относительной скорости в точке контакта + public var accumImpulseN:Number; + // Накопленный импульс, применяемый для разделения тел в точке контакта. Не создаёт момента. +// public var accumSepImpulse:Number; + + public var satisfied:Boolean; + + /** + * + * @param cp + */ + public function copyFrom(cp:ContactPoint):void { + pos.vCopy(cp.pos); + penetration = cp.penetration; + feature1 = cp.feature1; + feature2 = cp.feature2; + r1.vCopy(cp.r1); + r2.vCopy(cp.r2); + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/PhysicsScene.as b/0.0.9.2/src/alternativa/physics/PhysicsScene.as new file mode 100644 index 0000000..abdc762 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/PhysicsScene.as @@ -0,0 +1,743 @@ +package alternativa.physics { + + + import alternativa.physics.collision.ICollisionDetector; + import alternativa.physics.collision.KdTreeCollisionDetector; + import alternativa.physics.constraints.Constraint; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * Класс реализует физическую симуляцию поведения твёрдых тел. + */ + public class PhysicsScene { + + private static var lastBodyId:int; + + // Максимальное количество контактов + public const MAX_CONTACTS:int = 1000; + + ////////////////////////////////////////////////////////////////// + // Настроечные параметры симуляции. Могут быть изменены в любой + // момент времени без нарушения корректной работы симуляции. + ////////////////////////////////////////////////////////////////// + // Количество шагов, за которое пересекающиеся тела должны разделиться + public var penResolutionSteps:int = 10; + // Величина допустимой глубины пересечения + public var allowedPenetration:Number = 0.1; + // Максимальная скорость, добавляемая с целью разделения тел + public var maxPenResolutionSpeed:Number = 0.5; + // Количество итераций для обработки упругих контактов + public var collisionIterations:int = 5; + // Количество итераций для обработки неупругих контактов + public var contactIterations:int = 5; + // Флаг использования предсказания состояний + public var usePrediction:Boolean = false; + + public var freezeSteps:int = 10; + public var linSpeedFreezeLimit:Number = 1; + public var angSpeedFreezeLimit:Number = 0.01; + + // Переменные для процедуры разделения тел путём непосредственного их перемещенеия. + // !!!!!!!!!!!!!!!!!!!!!!!! + // !!! Экспериментально !!! + // !!!!!!!!!!!!!!!!!!!!!!!! + public var staticSeparationIterations:int = 10; + public var staticSeparationSteps:int = 10; + public var maxAngleMove:Number = 10; + public var useStaticSeparation:Boolean = false; + + // Вектор гравитации + public var _gravity:Vector3 = new Vector3(0, 0, -9.8); + // Модуль вектора гравитации + public var _gravityMagnitude:Number = 9.8; + + // Использующийся детектор столкновений + public var collisionDetector:ICollisionDetector; + + // Список тел, участвующих в симуляции + public var bodies:BodyList = new BodyList(); + + // Список контактов на текущем шаге симуляции + altphysics var contacts:Contact; + // Количество контактов на текущем шаге симуляции +// altphysics var contactsNum:int; + // Список ограничений + altphysics var constraints:Vector. = new Vector.(); + // Количество ограничений + altphysics var constraintsNum:int; + // Временная метка. Число прошедших шагов с начала симуляции. + public var timeStamp:int; + // Время с начала симуляции, мс + public var time:int; + // Первый неиспользованный контакт на текущем шаге симуляции + private var borderContact:Contact; + + // Временные переменные для избежания создания экземпляров + private var _r:Vector3 = new Vector3(); + private var _t:Vector3 = new Vector3(); + private var _v:Vector3 = new Vector3(); + private var _v1:Vector3 = new Vector3(); + private var _v2:Vector3 = new Vector3(); + + /** + * + */ + public function PhysicsScene() { + contacts = new Contact(0); + var contact:Contact = contacts; + for (var i:int = 1; i < MAX_CONTACTS; i++) { + contact.next = new Contact(i); + contact = contact.next; + } + collisionDetector = new KdTreeCollisionDetector(); + } + + /** + * Вектор гравитации. + */ + public function get gravity():Vector3 { + return _gravity.vClone(); + } + + /** + * @private + */ + public function set gravity(value:Vector3):void { + _gravity.vCopy(value); + _gravityMagnitude = _gravity.vLength(); + } + + /** + * Добавляет тело в симуляцию. + * + * @param body + */ + public function addBody(body:Body):void { + body.id = lastBodyId++; + body.world = this; + bodies.append(body); + } + + /** + * Удаляет тело из симуляции. + * @param body + * @return + */ + public function removeBody(body:Body):void { + if (bodies.remove(body)) { + body.world = null; + } + } + + /** + * Добавляет ограничение. + * @param c + */ + public function addConstraint(c:Constraint):void { + constraints[constraintsNum++] = c; + c.world = this; + } + + /** + * Удаляет ограничение. + * @param c + */ + public function removeConstraint(c:Constraint):Boolean { + var idx:int = constraints.indexOf(c); + if (idx < 0) return false; + constraints.splice(idx, 1) + constraintsNum--; + c.world = null; + return true; + } + + /** + * Применяет к телам действующте на них силы. Аккумуляторы сил и моментов тел очищаются после завершения шага + * симуляции, поэтому на момент вызова метода могут уже содержать некоторые значения. + * + * @param dt промежуток времени, в течении которого действуют силы + */ + private function applyForces(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + body.beforePhysicsStep(dt); + body.calcAccelerations(); + // Ускорение свободного падения применяется только к подвижным телам во избежание некорректного изменения + // фиктивной скорости неподвижных тел. + if (body.movable && !body.frozen) { + body.accel.x += _gravity.x; + body.accel.y += _gravity.y; + body.accel.z += _gravity.z; + } + item = item.next; + } + } + + /** + * Определяет все столкновения на текущем шаге симуляции и заполняет список получившихся контактов. + * + * @param dt длительность шага симуляции + */ + private function detectCollisions(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + if (!body.frozen) { + body.contactsNum = 0; + body.saveState(); + // При включённом режиме предсказания состояние тел интегрируется на один шаг вперёд + if (usePrediction) { + body.integrateVelocity(dt); + body.integratePosition(dt); + } + body.calcDerivedData(); + } + item = item.next; + } + + borderContact = collisionDetector.getAllContacts(contacts); + + // Расчёт относительных векторов точки контакта вынесен сюда из-за необходимости учитывать + // положение тел в предсказанном состоянии + var contact:Contact = contacts; + while (contact != borderContact) { + var b1:Body = contact.body1; + var b2:Body = contact.body2; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + var bPos:Vector3 = b1.state.pos; + cp.r1.x = cp.pos.x - bPos.x; + cp.r1.y = cp.pos.y - bPos.y; + cp.r1.z = cp.pos.z - bPos.z; + if (b2 != null) { + bPos = b2.state.pos; + cp.r2.x = cp.pos.x - bPos.x; + cp.r2.y = cp.pos.y - bPos.y; + cp.r2.z = cp.pos.z - bPos.z; + } + } + contact = contact.next; + } + + // Восстановление состояния тел + if (usePrediction) { + item = bodies.head; + while (item != null) { + body = item.body; + if (!body.frozen) { + body.restoreState(); + body.calcDerivedData(); + } + item = item.next; + } + } + } + + /** + * Подготваливает полученные из детектора столкновений контакты, расчитывая значения, не меняющиеся + * в ходе шага симуляции. + */ + private function preProcessContacts(dt:Number):void { + var contact:Contact = contacts; + while (contact != borderContact) { + var b1:Body = contact.body1; + var b2:Body = contact.body2; + // Столкнувшиеся тела размораживаются + if (b1.frozen) { + b1.frozen = false; + b1.freezeCounter = 0; + } + if (b2 != null && b2.frozen) { + b2.frozen = false; + b2.freezeCounter = 0; + } + contact.restitution = b1.material.restitution; + if (b2 != null && b2.material.restitution < contact.restitution) contact.restitution = b2.material.restitution; + contact.friction = b1.material.friction; + if (b2 != null && b2.material.friction < contact.friction) contact.friction = b2.material.friction; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.accumImpulseN = 0; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + cp.velByUnitImpulseN = 0; + if (b1.movable) { + cp.angularInertia1 = _v.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1).vDot(contact.normal); + cp.velByUnitImpulseN += b1.invMass + cp.angularInertia1; + } + if (b2 != null && b2.movable) { + cp.angularInertia2 = _v.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2).vDot(contact.normal); + cp.velByUnitImpulseN += b2.invMass + cp.angularInertia2; + } + // Расчёт требуемой конечной скорости для упругого контакта + calcSepVelocity(b1, b2, cp, _v); + cp.normalVel = _v.vDot(contact.normal); + if (cp.normalVel < 0) cp.normalVel = - contact.restitution*cp.normalVel; + // Скорость разделения неупругого контакта + cp.minSepVel = cp.penetration > allowedPenetration ? (cp.penetration - allowedPenetration)/(penResolutionSteps*dt) : 0; + if (cp.minSepVel > maxPenResolutionSpeed) cp.minSepVel = maxPenResolutionSpeed; + } + contact = contact.next; + } + for (var i:int = 0; i < constraintsNum; i++) { + var constraint:Constraint = constraints[i]; + constraint.preProcess(dt); + } + } + + /** + * + * @param dt + * @param forceInelastic + */ + private function processContacts(dt:Number, forceInelastic:Boolean):void { + var iterNum:int = forceInelastic ? contactIterations : collisionIterations; + var i:int; + var forwardLoop:Boolean = false; + for (var iter:int = 0; iter < iterNum; iter++) { + forwardLoop = !forwardLoop; + var contact:Contact = contacts; + while (contact != borderContact) { + resolveContact(contact, forceInelastic, forwardLoop); + contact = contact.next; + } + // Ограничения + for (i = 0; i < constraintsNum; i++) { + var constraint:Constraint = constraints[i]; + constraint.apply(dt); + } + } + + // Разделение контактов путём непосредственного изменения координат и ориентации. Экспериментально. +// if (forceInelastic && useStaticSeparation) performStaticSeparation(); + } + + /** + * + */ + private function resolveContact(contactInfo:Contact, forceInelastic:Boolean, forwardLoop:Boolean):void { + var b1:Body = contactInfo.body1; + var b2:Body = contactInfo.body2; + var normal:Vector3 = contactInfo.normal; + var i:int; + if (forwardLoop) { + for (i = 0; i < contactInfo.pcount; i++) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } else { + for (i = contactInfo.pcount - 1; i >= 0; i--) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } + } + + /** + * + * @param idx + * @param b1 + * @param b2 + * @param colInfo + * @param normal + * @param forceInelastic + */ + private function resolveContactPoint(idx:int, b1:Body, b2:Body, contact:Contact, normal:Vector3, forceInelastic:Boolean):void { + var cp:ContactPoint = contact.points[idx]; + if (!forceInelastic) { + cp.satisfied = true; + } + + var newVel:Number = 0; + calcSepVelocity(b1, b2, cp, _v); + var cnormal:Vector3 = contact.normal; + var sepVel:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + if (forceInelastic) { + var minSpeVel:Number = useStaticSeparation ? 0 : cp.minSepVel; + if (sepVel < minSpeVel) { + cp.satisfied = false; + } else if (cp.satisfied) return; + newVel = minSpeVel; + } else { + newVel = cp.normalVel; + } + var deltaVel:Number = newVel - sepVel; + var impulse:Number = deltaVel/cp.velByUnitImpulseN; + var accumImpulse:Number = cp.accumImpulseN + impulse; + if (accumImpulse < 0) { + accumImpulse = 0; + } + var deltaImpulse:Number = accumImpulse - cp.accumImpulseN; + cp.accumImpulseN = accumImpulse; + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, normal, deltaImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, normal, -deltaImpulse); + } + + // Учёт силы трения + calcSepVelocity(b1, b2, cp, _v); + // Расчитываем изменение касательной скорости на единицу касательного импульса + var tanSpeedByUnitImpulse:Number = 0; + +// _v.vAddScaled(-_v.vDot(contact.normal), contact.normal); + var dot:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + _v.x -= dot*cnormal.x; + _v.y -= dot*cnormal.y; + _v.z -= dot*cnormal.z; + + var tanSpeed:Number = _v.vLength(); + if (tanSpeed < 0.001) return; + +// _t.vCopy(_v).vNormalize().vReverse(); + _t.x = -_v.x; + _t.y = -_v.y; + _t.z = -_v.z; + _t.vNormalize(); + + var r:Vector3; + var m:Matrix3; + var xx:Number; + var yy:Number; + var zz:Number; + // dV = b.invMass + ((invI * (r % t)) % r) * t + if (b1.movable) { +// _v.vCross2(cp.r1, _t).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1); + r = cp.r1; + m = b1.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b1.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + if (b2 != null && b2.movable) { +// _v.vCross2(cp.r2, _t).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2); + + r = cp.r2; + m = b2.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b2.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + + var tanImpulse:Number = tanSpeed/tanSpeedByUnitImpulse; + var max:Number = contact.friction*cp.accumImpulseN; + if (max < 0) { + if (tanImpulse < max) tanImpulse = max; + } else { + if (tanImpulse > max) tanImpulse = max; + } + + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, _t, tanImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, _t, -tanImpulse); + } + } + + /** + * + * @param cp + * @param normal + * @return + */ + private function calcSepVelocity(body1:Body, body2:Body, cp:ContactPoint, result:Vector3):void { + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 +// result.vCopy(body1.state.velocity).vAdd(_v1.vCross2(body1.state.rotation, cp.r1)); + var rot:Vector3 = body1.state.rotation; + var v:Vector3 = cp.r1; + var x:Number = rot.y*v.z - rot.z*v.y; + var y:Number = rot.z*v.x - rot.x*v.z; + var z:Number = rot.x*v.y - rot.y*v.x; + v = body1.state.velocity; + result.x = v.x + x; + result.y = v.y + y; + result.z = v.z + z; + // V2 = V2_c + w2%r2 + if (body2 != null) { +// result.vSubtract(body2.state.velocity).vSubtract(_v2.vCross2(body2.state.rotation, cp.r2)); + rot = body2.state.rotation; + v = cp.r2; + x = rot.y*v.z - rot.z*v.y; + y = rot.z*v.x - rot.x*v.z; + z = rot.x*v.y - rot.y*v.x; + v = body2.state.velocity; + result.x -= v.x + x; + result.y -= v.y + y; + result.z -= v.z + z; + } + } + + /** + * + * @param dt + */ + private function intergateVelocities(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + item.body.integrateVelocity(dt); + item = item.next; + } + } + + /** + * + * @param dt + */ + private function integratePositions(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + if (body.movable && !body.frozen) { + body.integratePosition(dt); + } + item = item.next; + } + } + + /** + * + */ + private function performStaticSeparation():void { + var iterNum:int = staticSeparationIterations; +// iterNum = 100; + + // 1. В начале каждой итерации для всех контактов сбрасывается флаг satisfied с одновременным поиском наихудшего контакта, т.е. имеющего наибольшую величину пересечения тел. + // 2. Если найденный контакт имеет величину пересечения меньше предельно допустимой процедура прерывается, т.к. разделение контактов не требуется. + // 3. Если процедура продолжается, то выполняется разделение найденного контакта и установка его флага satisfied в true, после чего запускается внутренний цикл, состоящий + // из contactsNum - 1 итераций. На каждой итерации ищется наихудший контакт среди оставшихся (satisfied == false). Если такой контакт найден, то выполняется его разделение + // и переход к следующей итерации внутреннего цикла, иначе внутренний цикл прерывается и выполняется переход к следующей итерации внешнего цикла (шаг 1.). +// for (var iter:int = 0; iter < iterNum; iter++) { +// // Ищем контакт с максимальной величиной пересечения тел, одновременно сбрасывая флаги satisfied в false +// var worstContact:Contact = contacts[0]; +// var i:int; +// for (i = 1; i < contactsNum; i++) { +// var contact:Contact = contacts[i]; +// contact.satisfied = false; +// if (contact.maxPenetration > worstContact.maxPenetration) worstContact = contact; +// } +// if (worstContact.maxPenetration <= allowedPenetration) return; +// +// resolveInterpenetration(worstContact); +// // Внутренний цикл по оставшимся контактам +// for (i = 1; i < contactsNum; i++) { +// worstContact = getWorstContact(); +// if (worstContact == null) break; +// resolveInterpenetration(worstContact); +// } +// } + } + + /** + * + */ +// private function getWorstContact():Contact { +// var maxPen:Number = 0; +// var worst:Contact = null; +// for (var i:int = 0; i < contactsNum; i++) { +// var c:Contact = contacts[i]; +// if (!c.satisfied && c.maxPenetration > maxPen) { +// worst = c; +// maxPen = c.maxPenetration; +// } +// } +// return maxPen > allowedPenetration ? worst : null; +// } + + /** + * Разделяет указанный контакт, выполняя staticSeparationSteps итераций по списку точек контакта. + * 1. В начале каждой итерации ищется точка с наибольшим пересечением, одновременно сбрасываются флаги satisfied у точек. + * 2. Для найденной точки выполняется процедура разделения. + * 3. После выполняется pcount - 1 итерация по списку точек, каждый раз ищется наихудшая среди имеющих satisfied == false, для которой выполняется процедура разделения. + * Если наихудшая точка не найдена, то выполнется переход к следующей итерации внешнего цикла (шаг 1). + * + * В конце процедуры разделения обновляются значения пересечений для остальных точек контакта, а также для всех прочих контактов, относящихся к телам текущего. + * + * @param contact контакт для разделения + */ + private function resolveInterpenetration(contact:Contact):void { + contact.satisfied = true; + + for (var step:int = 0; step < staticSeparationSteps; step++) { + var worstCp:ContactPoint = contact.points[0]; + var cp:ContactPoint; + var i:int; + for (i = 1; i < contact.pcount; i++) { + cp = contact.points[i]; + cp.satisfied = false; + if (cp.penetration > worstCp.penetration) worstCp = cp; + } + if (worstCp.penetration <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + // Разделяем оставшиеся точки + var maxPen:Number = 0; + for (i = 1; i < contact.pcount; i++) { + // Поиск наихудшей точки + for (var j:int = 0; j < contact.pcount; j++) { + cp = contact.points[j]; + if (cp.satisfied) continue; + if (cp.penetration > maxPen) { + maxPen = cp.penetration; + worstCp = cp; + } + } + if (maxPen <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + } + } + } + + /** + * + * @param cp + * @param contact + */ + private function separateContactPoint(cp:ContactPoint, contact:Contact):void { +// cp.satisfied = true; +// +// var b1:Body = contact.body1; +// var b2:Body = contact.body2; +// var totalMove:Number = cp.penetration - allowedPenetration; +// var moveCoeff:Number = totalMove/cp.velByUnitImpulseN; +// var linMove1:Number; +// var angleMove1:Number; +// if (b1.movable) { +// linMove1 = b1.invMass*moveCoeff; +// angleMove1 = cp.angularInertia1*moveCoeff; +// if (angleMove1 > maxAngleMove) { +// linMove1 += angleMove1 - maxAngleMove; +// angleMove1 -= maxAngleMove; +// } +// b1.state.pos.vAddScaled(linMove1, contact.normal); +// _v1.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vScale(angleMove1); +// b1.state.orientation.addScaledVector(_v1, 1); +// } +// var linMove2:Number; +// var angleMove2:Number; +// if (b2 != null && b2.movable) { +// linMove2 = b2.invMass*moveCoeff; +// angleMove2 = cp.angularInertia2*moveCoeff; +// if (angleMove2 > maxAngleMove) { +// linMove2 += angleMove2 - maxAngleMove; +// angleMove2 -= maxAngleMove; +// } +// b2.state.pos.vAddScaled(-linMove2, contact.normal); +// _v2.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vScale(angleMove2); +// _v2.vReverse(); +// b2.state.orientation.addScaledVector(_v2, 1); +// } +// cp.penetration = allowedPenetration; +// // Обновляем пересечения в других точках +// var i:int; +// for (i = 0; i < contact.pcount; i++) { +// var cp1:ContactPoint = contact.points[i]; +// if (cp1 == cp) continue; +// var angularMove:Number; +// if (b1.movable) { +// angularMove = _v.vCross2(_v1, cp1.r1).vDot(contact.normal); +// cp1.penetration -= linMove1 + angularMove; +// } +// if (b2 != null && b2.movable) { +// angularMove = _v.vCross2(_v2, cp1.r2).vDot(contact.normal); +// cp1.penetration -= linMove2 - angularMove; +// } +// // Обновление максимального значения для контакта +// if (cp1.penetration > contact.maxPenetration) contact.maxPenetration = cp1.penetration; +// } +// // Обновляем пересечения для других контактов +// var c:Contact; +// var j:int; +// if (b1.movable) { +// for (i = 0; i < b1.contactsNum; i++) { +// c = b1.contacts[i]; +// if (c == contact) continue; +// for (j = 0; j < c.pcount; j++) { +// cp1 = c.points[j]; +// if (b1 == c.body1) cp1.penetration -= linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r1).vDot(c.normal); +// else cp1.penetration += linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r2).vDot(c.normal); +// if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; +// } +// } +// } +// if (b2 != null && b2.movable) { +// for (i = 0; i < b2.contactsNum; i++) { +// c = b2.contacts[i]; +// if (c == contact) continue; +// for (j = 0; j < c.pcount; j++) { +// cp1 = c.points[j]; +// if (b2 == c.body1) cp1.penetration -= linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r1).vDot(c.normal); +// else cp1.penetration += linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r2).vDot(c.normal); +// if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; +// } +// } +// } + } + + /** + * + */ + private function postPhysics():void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + body.clearAccumulators(); + body.calcDerivedData(); + if (body.canFreeze) { + if (body.state.velocity.vLength() < linSpeedFreezeLimit && body.state.rotation.vLength() < angSpeedFreezeLimit) { + if (!body.frozen) { + body.freezeCounter++; + if (body.freezeCounter >= freezeSteps) body.frozen = true; + } + } else { + body.freezeCounter = 0; + body.frozen = false; + } + } + item = item.next; + } + } + + /** + * + * @param delta + */ + public function update(delta:int):void { + timeStamp++; + time += delta; + var dt:Number = 0.001*delta; + applyForces(dt); + detectCollisions(dt); + preProcessContacts(dt); + processContacts(dt, false); + intergateVelocities(dt); + processContacts(dt, true); + integratePositions(dt); + postPhysics(); + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/PhysicsUtils.as b/0.0.9.2/src/alternativa/physics/PhysicsUtils.as new file mode 100644 index 0000000..5a2e46f --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/PhysicsUtils.as @@ -0,0 +1,49 @@ +package alternativa.physics { + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + public class PhysicsUtils { + + /** + * @param mass + * @param halfSize + * @param result + */ + public static function getBoxInvInertia(mass:Number, halfSize:Vector3, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + result.a = 3/(mass*(yy + zz)); + result.f = 3/(mass*(zz + xx)); + result.k = 3/(mass*(xx + yy)); + } + + /** + * @param mass + * @param r + * @param h + * @param result + */ + public static function getCylinderInvInertia(mass:Number, r:Number, h:Number, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + + result.a = result.f = 1/(mass*(h*h/12 + r*r/4)); + result.k = 2/(mass*r*r); + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/altphysics.as b/0.0.9.2/src/alternativa/physics/altphysics.as new file mode 100644 index 0000000..2715307 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/altphysics.as @@ -0,0 +1,3 @@ +package alternativa.physics { + public namespace altphysics = "http://alternativaplatform.com/en/altphysics"; +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/collision/.svn/all-wcprops b/0.0.9.2/src/alternativa/physics/collision/.svn/all-wcprops new file mode 100644 index 0000000..8819a91 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/.svn/all-wcprops @@ -0,0 +1,65 @@ +K 25 +svn:wc:ra_dav:version-url +V 113 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/collision +END +ICollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 135 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/collision/ICollisionDetector.as +END +CollisionKdNode.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/collision/CollisionKdNode.as +END +CollisionKdTree2D.as +K 25 +svn:wc:ra_dav:version-url +V 134 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/collision/CollisionKdTree2D.as +END +IRayCollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 139 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/collision/IRayCollisionPredicate.as +END +IBodyCollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/collision/IBodyCollisionPredicate.as +END +CollisionPrimitive.as +K 25 +svn:wc:ra_dav:version-url +V 135 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/collision/CollisionPrimitive.as +END +KdTreeCollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/collision/KdTreeCollisionDetector.as +END +ICollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 136 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/collision/ICollisionPredicate.as +END +ICollider.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/collision/ICollider.as +END +CollisionKdTree.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/collision/CollisionKdTree.as +END diff --git a/0.0.9.2/src/alternativa/physics/collision/.svn/entries b/0.0.9.2/src/alternativa/physics/collision/.svn/entries new file mode 100644 index 0000000..f15dcec --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/.svn/entries @@ -0,0 +1,157 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/collision +http://svndev.alternativaplatform.com + + + +2010-03-06T16:51:19.781174Z +29404 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +colliders +dir + +ICollisionDetector.as +file + + + + +2010-10-28T04:32:44.000000Z +9f76503bdaf92b4170b05b6e66c45de1 +2010-02-12T12:34:25.192477Z +28424 +mike + +CollisionKdNode.as +file + + + + +2010-10-28T04:32:44.000000Z +a6fab491cd9bd45a5472bf4ffc321e08 +2010-02-12T12:34:25.192477Z +28424 +mike + +CollisionKdTree2D.as +file + + + + +2010-10-28T04:32:44.000000Z +82bb636d8b49cdfbc22694dd8b01e71d +2010-02-12T12:34:25.192477Z +28424 +mike + +IRayCollisionPredicate.as +file + + + + +2010-10-28T04:32:44.000000Z +32f9b1eaeacfcb40e7ae1927f52b8ab8 +2009-10-19T07:17:33.113306Z +22253 +mike + +types +dir + +IBodyCollisionPredicate.as +file + + + + +2010-10-28T04:32:44.000000Z +618f907a588979a8d06d4f4bcf6618a0 +2009-10-19T07:17:33.113306Z +22253 +mike + +CollisionPrimitive.as +file + + + + +2010-10-28T04:32:44.000000Z +8501ce34a3effa2380c472cc9b823afa +2009-11-24T08:44:15.941480Z +23758 +mike + +KdTreeCollisionDetector.as +file + + + + +2010-10-28T04:32:44.000000Z +ea670ad5d0f72f6a6df5163d65f204f9 +2010-02-12T12:34:25.192477Z +28424 +mike + +ICollisionPredicate.as +file + + + + +2010-10-28T04:32:44.000000Z +65f3250d36556fffc457c740fb7d38b3 +2009-10-19T07:17:33.113306Z +22253 +mike + +ICollider.as +file + + + + +2010-10-28T04:32:44.000000Z +128a7d7479e13caeccecb45161b27b22 +2009-10-19T07:17:33.113306Z +22253 +mike + +primitives +dir + +CollisionKdTree.as +file + + + + +2010-10-28T04:32:44.000000Z +8e815c1f1e527b0884b3f0124257a603 +2010-02-12T12:34:25.192477Z +28424 +mike + diff --git a/0.0.9.2/src/alternativa/physics/collision/.svn/format b/0.0.9.2/src/alternativa/physics/collision/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.2/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base b/0.0.9.2/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base new file mode 100644 index 0000000..f450f1e --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base @@ -0,0 +1,18 @@ +package alternativa.physics.collision { + + + import alternativa.physics.collision.types.BoundBox; + + public class CollisionKdNode { + public var indices:Vector.; + public var splitIndices:Vector.; + public var boundBox:BoundBox; + public var parent:CollisionKdNode; + public var splitTree:CollisionKdTree2D; + + public var axis:int = -1; // 0 - x, 1 - y, 2 - z + public var coord:Number; + public var positiveNode:CollisionKdNode; + public var negativeNode:CollisionKdNode; + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base b/0.0.9.2/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base new file mode 100644 index 0000000..d3dc307 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base @@ -0,0 +1,261 @@ +package alternativa.physics.collision { + + + + import alternativa.physics.collision.types.BoundBox; + + /** + * @author mike + */ + public class CollisionKdTree { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + public var rootNode:CollisionKdNode; + public var staticChildren:Vector.; + public var numStaticChildren:int; + public var staticBoundBoxes:Vector. = new Vector.(); + + private var splitAxis:int; + private var splitCoord:Number; + private var splitCost:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + + /** + * @param boundBox + */ + public function createTree(collisionPrimitives:Vector., boundBox:BoundBox = null):void { + staticChildren = collisionPrimitives.concat(); + numStaticChildren = staticChildren.length; + // Создаём корневую ноду + rootNode = new CollisionKdNode(); + rootNode.indices = new Vector.(); + // Расчитываем баунды объектов и рутовой ноды + var rootNodeBoundBox:BoundBox = rootNode.boundBox = (boundBox != null) ? boundBox : new BoundBox(); + for (var i:int = 0; i < numStaticChildren; ++i) { + var child:CollisionPrimitive = staticChildren[i]; + var childBoundBox:BoundBox = staticBoundBoxes[i] = child.calculateAABB(); + rootNodeBoundBox.addBoundBox(childBoundBox); + rootNode.indices[i] = i; + } + staticBoundBoxes.length = numStaticChildren; + // Разделяем рутовую ноду + splitNode(rootNode); + + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + var indices:Vector. = node.indices; + var numPrimitives:int = indices.length; + if (numPrimitives <= minPrimitivesPerNode) return; + + // Подготовка баунда с погрешностями + var nodeBoundBox:BoundBox = node.boundBox; + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + // Собираем опорные координаты + var i:int; + var j:int; + var numSplitCoordsX:int = 0 + var numSplitCoordsY:int = 0; + var numSplitCoordsZ:int = 0; + for (i = 0; i < numPrimitives; ++i) { + var boundBox:BoundBox = staticBoundBoxes[indices[i]]; + + if (boundBox.maxX - boundBox.minX <= doubleThreshold) { + if (boundBox.minX <= nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.minX; + else if (boundBox.maxX >= nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.maxX; + else splitCoordsX[numSplitCoordsX++] = (boundBox.minX + boundBox.maxX)*0.5; + } else { + if (boundBox.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = boundBox.minX; + if (boundBox.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = boundBox.maxX; + } + + if (boundBox.maxY - boundBox.minY <= doubleThreshold) { + if (boundBox.minY <= nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.minY; + else if (boundBox.maxY >= nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.maxY; + else splitCoordsY[numSplitCoordsY++] = (boundBox.minY + boundBox.maxY)*0.5; + } else { + if (boundBox.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = boundBox.minY; + if (boundBox.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = boundBox.maxY; + } + + if (boundBox.maxZ - boundBox.minZ <= doubleThreshold) { + if (boundBox.minZ <= nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.minZ; + else if (boundBox.maxZ >= nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.maxZ; + else splitCoordsZ[numSplitCoordsZ++] = (boundBox.minZ + boundBox.maxZ)*0.5; + } else { + if (boundBox.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.minZ; + if (boundBox.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numPrimitives; ++i) { + boundBox = staticBoundBoxes[indices[i]]; + var min:Number = axisX ? boundBox.minX : (axisY ? boundBox.minY : boundBox.minZ); + var max:Number = axisX ? boundBox.maxX : (axisY ? boundBox.maxY : boundBox.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(indices[i]); + indices[i] = -1; + } else { + if (node.splitIndices == null) node.splitIndices = new Vector.(); + node.splitIndices.push(indices[i]); + indices[i] = -1; + } + } else { + if (min >= coordMin) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(indices[i]); + indices[i] = -1; + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numPrimitives; ++i) { + if (indices[i] >= 0) indices[j++] = indices[i]; + } + if (j > 0) indices.length = j; + else node.indices = null; + + if (node.splitIndices != null) { + node.splitTree = new CollisionKdTree2D(this, node); + node.splitTree.createTree(); + } + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + /** + * + */ + public function traceTree():void { + traceNode("", rootNode); + } + + /** + * @param str + * @param node + */ + private function traceNode(str:String, node:CollisionKdNode):void { + if (node == null) return; + trace(str, node.axis == -1 ? "end" : ((node.axis == 0) ? "X" : ((node.axis == 1) ? "Y" : "Z")), "splitCoord=" + splitCoord, "bound", node.boundBox, "objs:", node.indices); + traceNode(str + "-", node.negativeNode); + traceNode(str + "+", node.positiveNode); + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base b/0.0.9.2/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base new file mode 100644 index 0000000..2621517 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base @@ -0,0 +1,226 @@ +package alternativa.physics.collision { + + + import alternativa.physics.collision.types.BoundBox; + + /** + * + */ + public class CollisionKdTree2D { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + + public var parentTree:CollisionKdTree; + public var parentNode:CollisionKdNode; + public var rootNode:CollisionKdNode; + + private var splitAxis:int; + private var splitCost:Number; + private var splitCoord:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + /** + * + * @param parentTree + * @param parentNode + */ + public function CollisionKdTree2D(parentTree:CollisionKdTree, parentNode:CollisionKdNode) { + this.parentTree = parentTree; + this.parentNode = parentNode; + } + + /** + * + */ + public function createTree():void { + rootNode = new CollisionKdNode(); + rootNode.boundBox = parentNode.boundBox.clone(); + rootNode.indices = new Vector.(); + var numObjects:int = parentNode.splitIndices.length; + for (var i:int = 0; i < numObjects; ++i) rootNode.indices[i] = parentNode.splitIndices[i]; + + splitNode(rootNode); + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + if (node.indices.length <= minPrimitivesPerNode) return; + + var objects:Vector. = node.indices; + var i:int; + var j:int; + + var nodeBoundBox:BoundBox = node.boundBox; + + // Подготовка баунда с погрешностями + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + // Собираем опорные координаты + var numSplitCoordsX:int; + var numSplitCoordsY:int; + var numSplitCoordsZ:int; + var numObjects:int = objects.length; + for (i = 0; i < numObjects; ++i) { + var bb:BoundBox = staticBoundBoxes[objects[i]]; + + if (parentNode.axis != 0) { + if (bb.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = bb.minX; + if (bb.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = bb.maxX; + } + + if (parentNode.axis != 1) { + if (bb.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = bb.minY; + if (bb.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = bb.maxY; + } + + if (parentNode.axis != 2) { + if (bb.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = bb.minZ; + if (bb.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = bb.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + if (parentNode.axis != 0) checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + if (parentNode.axis != 1) checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + if (parentNode.axis != 2) checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numObjects; ++i) { + bb = staticBoundBoxes[objects[i]]; + var min:Number = axisX ? bb.minX : (axisY ? bb.minY : bb.minZ); + var max:Number = axisX ? bb.maxX : (axisY ? bb.maxY : bb.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(objects[i]); + objects[i] = -1; + } + } else { + if (min >= coordMin) { + if (max > coordMax) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(objects[i]); + objects[i] = -1; + } + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numObjects; ++i) { + if (objects[i] >= 0) objects[j++] = objects[i]; + } + if (j > 0) objects.length = j; + else node.indices = null; + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/collision/.svn/text-base/CollisionPrimitive.as.svn-base b/0.0.9.2/src/alternativa/physics/collision/.svn/text-base/CollisionPrimitive.as.svn-base new file mode 100644 index 0000000..e03cfee --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/.svn/text-base/CollisionPrimitive.as.svn-base @@ -0,0 +1,140 @@ +package alternativa.physics.collision { + + import alternativa.physics.Body; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * Базовый класс для примитивов, использующихся детектором столкновений. + */ + public class CollisionPrimitive { + // Константы типов примитива + public static const BOX:int = 1; + public static const PLANE:int = 2; + public static const SPHERE:int = 3; + public static const RECT:int = 4; + public static const TRIANGLE:int = 5; + + // Тип примитива + public var type:int; + // Группы примитива. Каждая группа определяется установленным битом. Столкновения проверяются только для примитивов, + // имеющих хотя бы одну общую группу. + public var collisionGroup:int; + // Предикат примитива, вызывающийся детектором при нахождении столкновения. В зависимости от возвращённого + // предикатом результата столкновение либо регистрируется, либо игнорируется. + // Nullable + public var postCollisionPredicate:ICollisionPredicate; + // Тело, владеющее примитивом. Поле сделано открытым для быстрого доступа на чтение. Установка значения должна выполняться вызовом метода setBody(). + // Nullable + public var body:Body; + // Трансформация примитива в системе координат тела, если оно указано. Не допускается масштабирование матрицы. + public var localTransform:Matrix4; + // Полная трансформация примитива. Не допускается масштабирование матрицы. + public var transform:Matrix4 = new Matrix4(); + // AABB в мировой системе координат. Расчитывается системой вызовом функции calculateBoundBox(). + public var aabb:BoundBox = new BoundBox(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param type тип примитива + * @param collisionGroup группа примитива + */ + public function CollisionPrimitive(type:int, collisionGroup:int) { + this.type = type; + this.collisionGroup = collisionGroup; + } + + /** + * Устанавливает тело, владеющее примитивом. + * + * @param body тело, которое владеет примитивом + * @param localTransform трансформация примитива в системе координат тела. Указание значения null равносильно + * заданию единичной матрицы, однако в первом случае не будет дополнительного умножения матриц при вычислении полной трансформации примитива. + */ + public function setBody(body:Body, localTransform:Matrix4 = null):void { + if (this.body == body) return; + this.body = body; + if (body != null) { + if (localTransform != null) { + if (this.localTransform == null) { + this.localTransform = new Matrix4(); + } + this.localTransform.copy(localTransform); + } else { + this.localTransform = null; + } + } + } + + /** + * Рассчитывает AABB примитива. Наследники должны переопределять этот метод, реализуя в нём корректный рассчёт. + * + * @return ссылка на свой AABB + */ + public function calculateAABB():BoundBox { + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + return -1; + } + + /** + * Клонирует примитив. Переопределять не рекомендуется. Вместо этого переопределяются методы createPrimitive() и copyFrom(). + * + * @return клон примитива + */ + public function clone():CollisionPrimitive { + var p:CollisionPrimitive = createPrimitive(); + return p.copyFrom(this); + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + if (source == null) { + throw new ArgumentError("Parameter source cannot be null"); + } + type = source.type; + transform.copy(source.transform); + collisionGroup = source.collisionGroup; + setBody(source.body, source.localTransform); + aabb.copyFrom(source.aabb); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + public function toString():String { + return "[CollisionPrimitive type=" + type + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + protected function createPrimitive():CollisionPrimitive { + return new CollisionPrimitive(type, collisionGroup); + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base b/0.0.9.2/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base new file mode 100644 index 0000000..4ace681 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.Body; + + public interface IBodyCollisionPredicate { + function considerBodies(body1:Body, body2:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base b/0.0.9.2/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base new file mode 100644 index 0000000..e6aac2a --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base @@ -0,0 +1,29 @@ +package alternativa.physics.collision { + + import alternativa.physics.Contact; + + /** + * Интерфейс определителя столкновений между двумя примитивами. + */ + public interface ICollider { + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base b/0.0.9.2/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base new file mode 100644 index 0000000..916f67b --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base @@ -0,0 +1,64 @@ +package alternativa.physics.collision { + import alternativa.physics.Contact; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.math.Vector3; + + /** + * Интерфейс детектора столкновений. + */ + public interface ICollisionDetector { + + /** + * Получает все столкновения в текущей конфигурации физической геометрии. + * + * @param contacts список контактов, в кторые будет записана информация о столкновении + * @return количество найденных столкновений + */ + function getAllContacts(contacts:Contact):Contact; + + /** + * Тестирует луч на пересечение с физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param direction направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRay(origin:Vector3, direction:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * + * @param origin + * @param direction + * @param collisionGroup + * @param maxTime + * @param predicate + * @param result + * @return + */ + function intersectRayWithStatic(origin:Vector3, direction:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @return + */ + function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base b/0.0.9.2/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base new file mode 100644 index 0000000..33d30ca --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base @@ -0,0 +1,8 @@ +package alternativa.physics.collision { + + public interface ICollisionPredicate { + + function considerCollision(primitive:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base b/0.0.9.2/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base new file mode 100644 index 0000000..b319f62 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.Body; + + public interface IRayCollisionPredicate { + function considerBody(body:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base b/0.0.9.2/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base new file mode 100644 index 0000000..9ca4c2a --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base @@ -0,0 +1,486 @@ +package alternativa.physics.collision { + + + 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.math.Vector3; + + use namespace altphysics; + + /** + * Детектор, хранящий статическую геометрию в kD-дереве и использующий дерево для ускорения тестов на пересечения. + */ + public class KdTreeCollisionDetector implements ICollisionDetector { + + altphysics var tree:CollisionKdTree; + altphysics var dynamicPrimitives:Vector.; + 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.(); + + 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.):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. = tree.staticChildren; +// var indices:Vector. = 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., indices:Vector., 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; + } \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/collision/CollisionKdNode.as b/0.0.9.2/src/alternativa/physics/collision/CollisionKdNode.as new file mode 100644 index 0000000..f450f1e --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/CollisionKdNode.as @@ -0,0 +1,18 @@ +package alternativa.physics.collision { + + + import alternativa.physics.collision.types.BoundBox; + + public class CollisionKdNode { + public var indices:Vector.; + public var splitIndices:Vector.; + public var boundBox:BoundBox; + public var parent:CollisionKdNode; + public var splitTree:CollisionKdTree2D; + + public var axis:int = -1; // 0 - x, 1 - y, 2 - z + public var coord:Number; + public var positiveNode:CollisionKdNode; + public var negativeNode:CollisionKdNode; + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/collision/CollisionKdTree.as b/0.0.9.2/src/alternativa/physics/collision/CollisionKdTree.as new file mode 100644 index 0000000..d3dc307 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/CollisionKdTree.as @@ -0,0 +1,261 @@ +package alternativa.physics.collision { + + + + import alternativa.physics.collision.types.BoundBox; + + /** + * @author mike + */ + public class CollisionKdTree { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + public var rootNode:CollisionKdNode; + public var staticChildren:Vector.; + public var numStaticChildren:int; + public var staticBoundBoxes:Vector. = new Vector.(); + + private var splitAxis:int; + private var splitCoord:Number; + private var splitCost:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + + /** + * @param boundBox + */ + public function createTree(collisionPrimitives:Vector., boundBox:BoundBox = null):void { + staticChildren = collisionPrimitives.concat(); + numStaticChildren = staticChildren.length; + // Создаём корневую ноду + rootNode = new CollisionKdNode(); + rootNode.indices = new Vector.(); + // Расчитываем баунды объектов и рутовой ноды + var rootNodeBoundBox:BoundBox = rootNode.boundBox = (boundBox != null) ? boundBox : new BoundBox(); + for (var i:int = 0; i < numStaticChildren; ++i) { + var child:CollisionPrimitive = staticChildren[i]; + var childBoundBox:BoundBox = staticBoundBoxes[i] = child.calculateAABB(); + rootNodeBoundBox.addBoundBox(childBoundBox); + rootNode.indices[i] = i; + } + staticBoundBoxes.length = numStaticChildren; + // Разделяем рутовую ноду + splitNode(rootNode); + + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + var indices:Vector. = node.indices; + var numPrimitives:int = indices.length; + if (numPrimitives <= minPrimitivesPerNode) return; + + // Подготовка баунда с погрешностями + var nodeBoundBox:BoundBox = node.boundBox; + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + // Собираем опорные координаты + var i:int; + var j:int; + var numSplitCoordsX:int = 0 + var numSplitCoordsY:int = 0; + var numSplitCoordsZ:int = 0; + for (i = 0; i < numPrimitives; ++i) { + var boundBox:BoundBox = staticBoundBoxes[indices[i]]; + + if (boundBox.maxX - boundBox.minX <= doubleThreshold) { + if (boundBox.minX <= nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.minX; + else if (boundBox.maxX >= nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.maxX; + else splitCoordsX[numSplitCoordsX++] = (boundBox.minX + boundBox.maxX)*0.5; + } else { + if (boundBox.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = boundBox.minX; + if (boundBox.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = boundBox.maxX; + } + + if (boundBox.maxY - boundBox.minY <= doubleThreshold) { + if (boundBox.minY <= nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.minY; + else if (boundBox.maxY >= nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.maxY; + else splitCoordsY[numSplitCoordsY++] = (boundBox.minY + boundBox.maxY)*0.5; + } else { + if (boundBox.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = boundBox.minY; + if (boundBox.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = boundBox.maxY; + } + + if (boundBox.maxZ - boundBox.minZ <= doubleThreshold) { + if (boundBox.minZ <= nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.minZ; + else if (boundBox.maxZ >= nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.maxZ; + else splitCoordsZ[numSplitCoordsZ++] = (boundBox.minZ + boundBox.maxZ)*0.5; + } else { + if (boundBox.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.minZ; + if (boundBox.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numPrimitives; ++i) { + boundBox = staticBoundBoxes[indices[i]]; + var min:Number = axisX ? boundBox.minX : (axisY ? boundBox.minY : boundBox.minZ); + var max:Number = axisX ? boundBox.maxX : (axisY ? boundBox.maxY : boundBox.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(indices[i]); + indices[i] = -1; + } else { + if (node.splitIndices == null) node.splitIndices = new Vector.(); + node.splitIndices.push(indices[i]); + indices[i] = -1; + } + } else { + if (min >= coordMin) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(indices[i]); + indices[i] = -1; + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numPrimitives; ++i) { + if (indices[i] >= 0) indices[j++] = indices[i]; + } + if (j > 0) indices.length = j; + else node.indices = null; + + if (node.splitIndices != null) { + node.splitTree = new CollisionKdTree2D(this, node); + node.splitTree.createTree(); + } + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + /** + * + */ + public function traceTree():void { + traceNode("", rootNode); + } + + /** + * @param str + * @param node + */ + private function traceNode(str:String, node:CollisionKdNode):void { + if (node == null) return; + trace(str, node.axis == -1 ? "end" : ((node.axis == 0) ? "X" : ((node.axis == 1) ? "Y" : "Z")), "splitCoord=" + splitCoord, "bound", node.boundBox, "objs:", node.indices); + traceNode(str + "-", node.negativeNode); + traceNode(str + "+", node.positiveNode); + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/collision/CollisionKdTree2D.as b/0.0.9.2/src/alternativa/physics/collision/CollisionKdTree2D.as new file mode 100644 index 0000000..2621517 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/CollisionKdTree2D.as @@ -0,0 +1,226 @@ +package alternativa.physics.collision { + + + import alternativa.physics.collision.types.BoundBox; + + /** + * + */ + public class CollisionKdTree2D { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + + public var parentTree:CollisionKdTree; + public var parentNode:CollisionKdNode; + public var rootNode:CollisionKdNode; + + private var splitAxis:int; + private var splitCost:Number; + private var splitCoord:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + /** + * + * @param parentTree + * @param parentNode + */ + public function CollisionKdTree2D(parentTree:CollisionKdTree, parentNode:CollisionKdNode) { + this.parentTree = parentTree; + this.parentNode = parentNode; + } + + /** + * + */ + public function createTree():void { + rootNode = new CollisionKdNode(); + rootNode.boundBox = parentNode.boundBox.clone(); + rootNode.indices = new Vector.(); + var numObjects:int = parentNode.splitIndices.length; + for (var i:int = 0; i < numObjects; ++i) rootNode.indices[i] = parentNode.splitIndices[i]; + + splitNode(rootNode); + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + if (node.indices.length <= minPrimitivesPerNode) return; + + var objects:Vector. = node.indices; + var i:int; + var j:int; + + var nodeBoundBox:BoundBox = node.boundBox; + + // Подготовка баунда с погрешностями + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + // Собираем опорные координаты + var numSplitCoordsX:int; + var numSplitCoordsY:int; + var numSplitCoordsZ:int; + var numObjects:int = objects.length; + for (i = 0; i < numObjects; ++i) { + var bb:BoundBox = staticBoundBoxes[objects[i]]; + + if (parentNode.axis != 0) { + if (bb.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = bb.minX; + if (bb.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = bb.maxX; + } + + if (parentNode.axis != 1) { + if (bb.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = bb.minY; + if (bb.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = bb.maxY; + } + + if (parentNode.axis != 2) { + if (bb.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = bb.minZ; + if (bb.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = bb.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + if (parentNode.axis != 0) checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + if (parentNode.axis != 1) checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + if (parentNode.axis != 2) checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numObjects; ++i) { + bb = staticBoundBoxes[objects[i]]; + var min:Number = axisX ? bb.minX : (axisY ? bb.minY : bb.minZ); + var max:Number = axisX ? bb.maxX : (axisY ? bb.maxY : bb.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(objects[i]); + objects[i] = -1; + } + } else { + if (min >= coordMin) { + if (max > coordMax) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(objects[i]); + objects[i] = -1; + } + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numObjects; ++i) { + if (objects[i] >= 0) objects[j++] = objects[i]; + } + if (j > 0) objects.length = j; + else node.indices = null; + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/collision/CollisionPrimitive.as b/0.0.9.2/src/alternativa/physics/collision/CollisionPrimitive.as new file mode 100644 index 0000000..e03cfee --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/CollisionPrimitive.as @@ -0,0 +1,140 @@ +package alternativa.physics.collision { + + import alternativa.physics.Body; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * Базовый класс для примитивов, использующихся детектором столкновений. + */ + public class CollisionPrimitive { + // Константы типов примитива + public static const BOX:int = 1; + public static const PLANE:int = 2; + public static const SPHERE:int = 3; + public static const RECT:int = 4; + public static const TRIANGLE:int = 5; + + // Тип примитива + public var type:int; + // Группы примитива. Каждая группа определяется установленным битом. Столкновения проверяются только для примитивов, + // имеющих хотя бы одну общую группу. + public var collisionGroup:int; + // Предикат примитива, вызывающийся детектором при нахождении столкновения. В зависимости от возвращённого + // предикатом результата столкновение либо регистрируется, либо игнорируется. + // Nullable + public var postCollisionPredicate:ICollisionPredicate; + // Тело, владеющее примитивом. Поле сделано открытым для быстрого доступа на чтение. Установка значения должна выполняться вызовом метода setBody(). + // Nullable + public var body:Body; + // Трансформация примитива в системе координат тела, если оно указано. Не допускается масштабирование матрицы. + public var localTransform:Matrix4; + // Полная трансформация примитива. Не допускается масштабирование матрицы. + public var transform:Matrix4 = new Matrix4(); + // AABB в мировой системе координат. Расчитывается системой вызовом функции calculateBoundBox(). + public var aabb:BoundBox = new BoundBox(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param type тип примитива + * @param collisionGroup группа примитива + */ + public function CollisionPrimitive(type:int, collisionGroup:int) { + this.type = type; + this.collisionGroup = collisionGroup; + } + + /** + * Устанавливает тело, владеющее примитивом. + * + * @param body тело, которое владеет примитивом + * @param localTransform трансформация примитива в системе координат тела. Указание значения null равносильно + * заданию единичной матрицы, однако в первом случае не будет дополнительного умножения матриц при вычислении полной трансформации примитива. + */ + public function setBody(body:Body, localTransform:Matrix4 = null):void { + if (this.body == body) return; + this.body = body; + if (body != null) { + if (localTransform != null) { + if (this.localTransform == null) { + this.localTransform = new Matrix4(); + } + this.localTransform.copy(localTransform); + } else { + this.localTransform = null; + } + } + } + + /** + * Рассчитывает AABB примитива. Наследники должны переопределять этот метод, реализуя в нём корректный рассчёт. + * + * @return ссылка на свой AABB + */ + public function calculateAABB():BoundBox { + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + return -1; + } + + /** + * Клонирует примитив. Переопределять не рекомендуется. Вместо этого переопределяются методы createPrimitive() и copyFrom(). + * + * @return клон примитива + */ + public function clone():CollisionPrimitive { + var p:CollisionPrimitive = createPrimitive(); + return p.copyFrom(this); + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + if (source == null) { + throw new ArgumentError("Parameter source cannot be null"); + } + type = source.type; + transform.copy(source.transform); + collisionGroup = source.collisionGroup; + setBody(source.body, source.localTransform); + aabb.copyFrom(source.aabb); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + public function toString():String { + return "[CollisionPrimitive type=" + type + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + protected function createPrimitive():CollisionPrimitive { + return new CollisionPrimitive(type, collisionGroup); + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/collision/IBodyCollisionPredicate.as b/0.0.9.2/src/alternativa/physics/collision/IBodyCollisionPredicate.as new file mode 100644 index 0000000..4ace681 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/IBodyCollisionPredicate.as @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.Body; + + public interface IBodyCollisionPredicate { + function considerBodies(body1:Body, body2:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/collision/ICollider.as b/0.0.9.2/src/alternativa/physics/collision/ICollider.as new file mode 100644 index 0000000..e6aac2a --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/ICollider.as @@ -0,0 +1,29 @@ +package alternativa.physics.collision { + + import alternativa.physics.Contact; + + /** + * Интерфейс определителя столкновений между двумя примитивами. + */ + public interface ICollider { + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/collision/ICollisionDetector.as b/0.0.9.2/src/alternativa/physics/collision/ICollisionDetector.as new file mode 100644 index 0000000..916f67b --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/ICollisionDetector.as @@ -0,0 +1,64 @@ +package alternativa.physics.collision { + import alternativa.physics.Contact; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.math.Vector3; + + /** + * Интерфейс детектора столкновений. + */ + public interface ICollisionDetector { + + /** + * Получает все столкновения в текущей конфигурации физической геометрии. + * + * @param contacts список контактов, в кторые будет записана информация о столкновении + * @return количество найденных столкновений + */ + function getAllContacts(contacts:Contact):Contact; + + /** + * Тестирует луч на пересечение с физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param direction направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRay(origin:Vector3, direction:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * + * @param origin + * @param direction + * @param collisionGroup + * @param maxTime + * @param predicate + * @param result + * @return + */ + function intersectRayWithStatic(origin:Vector3, direction:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @return + */ + function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/collision/ICollisionPredicate.as b/0.0.9.2/src/alternativa/physics/collision/ICollisionPredicate.as new file mode 100644 index 0000000..33d30ca --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/ICollisionPredicate.as @@ -0,0 +1,8 @@ +package alternativa.physics.collision { + + public interface ICollisionPredicate { + + function considerCollision(primitive:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/collision/IRayCollisionPredicate.as b/0.0.9.2/src/alternativa/physics/collision/IRayCollisionPredicate.as new file mode 100644 index 0000000..b319f62 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/IRayCollisionPredicate.as @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.Body; + + public interface IRayCollisionPredicate { + function considerBody(body:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/collision/KdTreeCollisionDetector.as b/0.0.9.2/src/alternativa/physics/collision/KdTreeCollisionDetector.as new file mode 100644 index 0000000..9ca4c2a --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/KdTreeCollisionDetector.as @@ -0,0 +1,486 @@ +package alternativa.physics.collision { + + + 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.math.Vector3; + + use namespace altphysics; + + /** + * Детектор, хранящий статическую геометрию в kD-дереве и использующий дерево для ускорения тестов на пересечения. + */ + public class KdTreeCollisionDetector implements ICollisionDetector { + + altphysics var tree:CollisionKdTree; + altphysics var dynamicPrimitives:Vector.; + 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.(); + + 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.):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. = tree.staticChildren; +// var indices:Vector. = 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., indices:Vector., 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; + } \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/collision/colliders/.svn/all-wcprops b/0.0.9.2/src/alternativa/physics/collision/colliders/.svn/all-wcprops new file mode 100644 index 0000000..c0e96ca --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/colliders/.svn/all-wcprops @@ -0,0 +1,53 @@ +K 25 +svn:wc:ra_dav:version-url +V 123 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/collision/colliders +END +BoxSphereCollider.as +K 25 +svn:wc:ra_dav:version-url +V 144 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/collision/colliders/BoxSphereCollider.as +END +BoxBoxCollider.as +K 25 +svn:wc:ra_dav:version-url +V 141 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/collision/colliders/BoxBoxCollider.as +END +BoxRectCollider.as +K 25 +svn:wc:ra_dav:version-url +V 142 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/collision/colliders/BoxRectCollider.as +END +SpherePlaneCollider.as +K 25 +svn:wc:ra_dav:version-url +V 146 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/collision/colliders/SpherePlaneCollider.as +END +BoxCollider.as +K 25 +svn:wc:ra_dav:version-url +V 138 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/collision/colliders/BoxCollider.as +END +BoxPlaneCollider.as +K 25 +svn:wc:ra_dav:version-url +V 143 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/collision/colliders/BoxPlaneCollider.as +END +SphereSphereCollider.as +K 25 +svn:wc:ra_dav:version-url +V 147 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/collision/colliders/SphereSphereCollider.as +END +BoxTriangleCollider.as +K 25 +svn:wc:ra_dav:version-url +V 146 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/collision/colliders/BoxTriangleCollider.as +END diff --git a/0.0.9.2/src/alternativa/physics/collision/colliders/.svn/entries b/0.0.9.2/src/alternativa/physics/collision/colliders/.svn/entries new file mode 100644 index 0000000..151fdb5 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/colliders/.svn/entries @@ -0,0 +1,124 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/collision/colliders +http://svndev.alternativaplatform.com + + + +2010-03-06T16:51:19.781174Z +29404 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +BoxSphereCollider.as +file + + + + +2010-10-28T04:32:44.000000Z +25170bcb9a1eec2a0941f4b50a06a554 +2009-12-08T15:53:11.094448Z +24506 +mike + +BoxBoxCollider.as +file + + + + +2010-10-28T04:32:44.000000Z +15da13b1383e9c1892edc205c8c055b0 +2010-02-12T12:34:25.192477Z +28424 +mike + +BoxRectCollider.as +file + + + + +2010-10-28T04:32:44.000000Z +4a35f94d3ef1af0a5b42e1a100580f6d +2010-03-06T16:51:19.781174Z +29404 +mike + +SpherePlaneCollider.as +file + + + + +2010-10-28T04:32:44.000000Z +67621ed47d7e90b72498acf2802c4e2d +2009-11-24T08:44:15.941480Z +23758 +mike + +BoxCollider.as +file + + + + +2010-10-28T04:32:44.000000Z +52e72ed30a24a3a43ce16a19930546b5 +2010-02-12T12:34:25.192477Z +28424 +mike + +BoxPlaneCollider.as +file + + + + +2010-10-28T04:32:44.000000Z +204c7b02a9a1913269c558658bad4262 +2010-02-12T12:34:25.192477Z +28424 +mike + +SphereSphereCollider.as +file + + + + +2010-10-28T04:32:44.000000Z +301c7102f91a7f3ede83e35ca202db0b +2009-11-24T08:44:15.941480Z +23758 +mike + +BoxTriangleCollider.as +file + + + + +2010-10-28T04:32:44.000000Z +448502271cac4f09491ca128b4bb485a +2010-03-06T16:51:19.781174Z +29404 +mike + diff --git a/0.0.9.2/src/alternativa/physics/collision/colliders/.svn/format b/0.0.9.2/src/alternativa/physics/collision/colliders/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/colliders/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.2/src/alternativa/physics/collision/colliders/.svn/text-base/BoxBoxCollider.as.svn-base b/0.0.9.2/src/alternativa/physics/collision/colliders/.svn/text-base/BoxBoxCollider.as.svn-base new file mode 100644 index 0000000..2d3f04c --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/colliders/.svn/text-base/BoxBoxCollider.as.svn-base @@ -0,0 +1,589 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * Расчитывает точки контакта двух боксов. Нормаль контакта направляется в сторону бокса с меньшим ID. + */ + public class BoxBoxCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + private var vectorToBox1:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var bestAxisIndex:int; + private var minOverlap:Number; + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + private var tmpPoints:Vector. = new Vector.(8, true); + private var pcount:int; + + /** + * + */ + public function BoxBoxCollider() { + for (var i:int = 0; i < 8; i++) { + tmpPoints[i] = new ContactPoint(); + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + + if (bestAxisIndex < 6) { + // Контакт грань-(грань|ребро|вершина) + if (!findFaceContactPoints(box1, box2, vectorToBox1, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 6; + findEdgesIntersection(box1, box2, vectorToBox1, int(bestAxisIndex/3), bestAxisIndex%3, contact); + } + contact.body1 = box1.body; + contact.body2 = box2.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + // Вектор из центра второго бокса в центр первого + vectorToBox1.x = transform1.d - transform2.d; + vectorToBox1.y = transform1.h - transform2.h; + vectorToBox1.z = transform1.l - transform2.l; + + // Проверка пересечения по основным осям + axis10.x = transform1.a; + axis10.y = transform1.e; + axis10.z = transform1.i; + if (!testMainAxis(box1, box2, axis10, 0, vectorToBox1)) return false; + axis11.x = transform1.b; + axis11.y = transform1.f; + axis11.z = transform1.j; + if (!testMainAxis(box1, box2, axis11, 1, vectorToBox1)) return false; + axis12.x = transform1.c; + axis12.y = transform1.g; + axis12.z = transform1.k; + if (!testMainAxis(box1, box2, axis12, 2, vectorToBox1)) return false; + + axis20.x = transform2.a; + axis20.y = transform2.e; + axis20.z = transform2.i; + if (!testMainAxis(box1, box2, axis20, 3, vectorToBox1)) return false; + axis21.x = transform2.b; + axis21.y = transform2.f; + axis21.z = transform2.j; + if (!testMainAxis(box1, box2, axis21, 4, vectorToBox1)) return false; + axis22.x = transform2.c; + axis22.y = transform2.g; + axis22.z = transform2.k; + if (!testMainAxis(box1, box2, axis22, 5, vectorToBox1)) return false; + + // Проверка производных осей + if (!testDerivedAxis(box1, box2, axis10, axis20, 6, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis21, 7, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis22, 8, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis11, axis20, 9, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis21, 10, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis22, 11, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis12, axis20, 12, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis21, 13, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis22, 14, vectorToBox1)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта грани одного бокса с гранью/ребром/вершиной другого. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param faceAxisIdx индекс оси первого бокса, перпендикулярной грани, с которой произошло столкновение + * @param contactInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var swapNormal:Boolean = false; + if (faceAxisIdx > 2) { + // Столкновение с гранью второго бокса. Для дальнейших расчётов боксы меняются местами, + // но нормаль контакта всё равно должна быть направлена в сторону первоначального box1 + var tmpBox:CollisionBox = box1; + box1 = box2; + box2 = tmpBox; + vectorToBox1.x = -vectorToBox1.x; + vectorToBox1.y = -vectorToBox1.y; + vectorToBox1.z = -vectorToBox1.z; + faceAxisIdx -= 3; + swapNormal = true; + } + + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + var colAxis:Vector3 = contact.normal; + + transform1.getAxis(faceAxisIdx, colAxis); + var negativeFace:Boolean = colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z > 0; + + var code:int = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box1.excludedFaces) != 0) return false; + + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + // Ищем ось второго бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + transform2.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisDot = dot; + incidentAxisIdx = axisIdx; + } + } + // Получаем список вершин грани второго бокса, переводим их в систему координат первого бокса и выполняем обрезку + // по грани первого бокса. Таким образом получается список потенциальных точек контакта. + transform2.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box2.hs, incidentAxisIdx, incidentAxisDot < 0, points1); + + // TODO: Вычислить результирующую матрицу, затем преобразовать векторы за один заход + transform2.transformVectorsN(points1, points2, 4); + transform1.transformVectorsInverseN(points2, points1, 4); + + var pnum:int = clip(box1.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points1[i]; + if ((pen = getPointBoxPenetration(box1.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + var cp:ContactPoint = tmpPoints[pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = transform1.a*v.x + transform1.b*v.y + transform1.c*v.z + transform1.d; + cpPos.y = transform1.e*v.x + transform1.f*v.y + transform1.g*v.z + transform1.h; + cpPos.z = transform1.i*v.x + transform1.j*v.y + transform1.k*v.z + transform1.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + + cp.penetration = pen; + } + } + if (swapNormal) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (pcount > 4) { + reducePoints(); + } + for (i = 0; i < pcount; i++) { + cp = contact.points[i]; + cp.copyFrom(tmpPoints[i]); + } + contact.pcount = pcount; + return true; + } + + /** + * + * @param contactInfo + */ + private function reducePoints():void { + var i:int; + var minIdx:int; + var cp1:ContactPoint; + var cp2:ContactPoint; + while (pcount > 4) { + var minLen:Number = 1e10; + var p1:Vector3 = (tmpPoints[int(pcount - 1)] as ContactPoint).pos; + var p2:Vector3; + for (i = 0; i < pcount; i++) { + p2 = (tmpPoints[i] as ContactPoint).pos; + var dx:Number = p2.x - p1.x; + var dy:Number = p2.y - p1.y; + var dz:Number = p2.z - p1.z; + var len:Number = dx*dx + dy*dy + dz*dz; + if (len < minLen) { + minLen = len; + minIdx = i; + } + p1 = p2; + } + var ii:int = minIdx == 0 ? (pcount - 1) : (minIdx - 1); + cp1 = tmpPoints[ii]; + cp2 = tmpPoints[minIdx]; + p1 = cp1.pos; + p2 = cp2.pos; + p2.x = 0.5*(p1.x + p2.x); + p2.y = 0.5*(p1.y + p2.y); + p2.z = 0.5*(p1.z + p2.z); + cp2.penetration = 0.5*(cp1.penetration + cp2.penetration); + if (minIdx > 0) { + for (i = minIdx; i < pcount; i++) { + tmpPoints[int(i - 1)] = tmpPoints[i]; + } + tmpPoints[int(pcount - 1)] = cp1; + } + pcount--; + } + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clip(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param contactInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):void { + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + transform1.getAxis(axisIdx1, axis10); + transform2.getAxis(axisIdx2, axis20); + + var colAxis:Vector3 = contact.normal; + + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону первого бокса + if (colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + + var tx:Number = box1.hs.x; + var ty:Number = box1.hs.y; + var tz:Number = box1.hs.z; + + var x2:Number = box2.hs.x; + var y2:Number = box2.hs.y; + var z2:Number = box2.hs.z; + // x + if (axisIdx1 == 0) { + tx = 0; + halfLen1 = box1.hs.x; + } else { + if (colAxis.x*transform1.a + colAxis.y*transform1.e + colAxis.z*transform1.i > 0) { + tx = -tx; + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = box2.hs.x; + } else { + if (colAxis.x*transform2.a + colAxis.y*transform2.e + colAxis.z*transform2.i < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + ty = 0; + halfLen1 = box1.hs.y; + } else { + if (colAxis.x*transform1.b + colAxis.y*transform1.f + colAxis.z*transform1.j > 0) { + ty = -ty; + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = box2.hs.y; + } else { + if (colAxis.x*transform2.b + colAxis.y*transform2.f + colAxis.z*transform2.j < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + tz = 0; + halfLen1 = box1.hs.z; + } else { + if (colAxis.x*transform1.c + colAxis.y*transform1.g + colAxis.z*transform1.k > 0) { + tz = -tz; + } + } + if (axisIdx2 == 2) { + z2 = 0; + halfLen2 = box2.hs.z; + } else { + if (colAxis.x*transform2.c + colAxis.y*transform2.g + colAxis.z*transform2.k < 0) { + z2 = -z2; + } + } + // Получаем глобальные координаты средних точек рёбер + var x1:Number = transform1.a*tx + transform1.b*ty + transform1.c*tz + transform1.d; + var y1:Number = transform1.e*tx + transform1.f*ty + transform1.g*tz + transform1.h; + var z1:Number = transform1.i*tx + transform1.j*ty + transform1.k*tz + transform1.l; + tx = x2; + ty = y2; + tz = z2; + x2 = transform2.a*tx + transform2.b*ty + transform2.c*tz + transform2.d; + y2 = transform2.e*tx + transform2.f*ty + transform2.g*tz + transform2.h; + z2 = transform2.i*tx + transform2.j*ty + transform2.k*tz + transform2.l; + + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + tx = x2 - x1; + ty = y2 - y1; + tz = z2 - z1; + var c1:Number = axis10.x*tx + axis10.y*ty + axis10.z*tz; + var c2:Number = axis20.x*tx + axis20.y*ty + axis20.z*tz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cp.pos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cp.pos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение боксов вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box1 + * @param box2 + * @param axis + * @param axisIndex + * @param toBox1 + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box1 + * @param box2 + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox1 + * @return + */ + private function testDerivedAxis(box1:CollisionBox, box2:CollisionBox, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия двух боксов вдоль заданной оси. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param axis ось + * @param vectorToBox1 вектор, соединяющий центр второго бокса с центром первого + * @return величина перекрытия боксов вдоль оси + */ + public function overlapOnAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, vectorToBox1:Vector3):Number { + var m:Matrix4 = box1.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box1.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box1.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box1.hs.z; + if (d < 0) d = -d; + projection += d; + + m = box2.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box2.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box2.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box2.hs.z; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox1.x*axis.x + vectorToBox1.y*axis.y + vectorToBox1.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} + import alternativa.math.Vector3; + + class CollisionPointTmp { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + } diff --git a/0.0.9.2/src/alternativa/physics/collision/colliders/.svn/text-base/BoxCollider.as.svn-base b/0.0.9.2/src/alternativa/physics/collision/colliders/.svn/text-base/BoxCollider.as.svn-base new file mode 100644 index 0000000..2429bb1 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/colliders/.svn/text-base/BoxCollider.as.svn-base @@ -0,0 +1,427 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + /** + * + */ + public class BoxCollider implements ICollider { + + /** + * + */ + public function BoxCollider() { + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + return false; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + protected function getFaceVertsByAxis(hs:Vector3, axisIdx:int, negativeFace:Boolean, result:Vector.):void { + var v:Vector3; + switch (axisIdx) { + case 0: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + } + break; + case 1: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + } + break; + case 2: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + } else { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } + break; + } + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/collision/colliders/.svn/text-base/BoxPlaneCollider.as.svn-base b/0.0.9.2/src/alternativa/physics/collision/colliders/.svn/text-base/BoxPlaneCollider.as.svn-base new file mode 100644 index 0000000..3bebdc3 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/colliders/.svn/text-base/BoxPlaneCollider.as.svn-base @@ -0,0 +1,88 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + public class BoxPlaneCollider implements ICollider { + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var normal:Vector3 = new Vector3(); + + public function BoxPlaneCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var box:RigidBox = body1 as RigidBox; +// var plane:RigidPlane; +// if (box == null) { +// box = body2 as RigidBox; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные координаты вершин бокса +// var sx:Number = box.halfSize.x; +// var sy:Number = box.halfSize.y; +// var sz:Number = box.halfSize.z; +// (verts1[0] as Vector3).reset(-sx, -sy, -sz); +// (verts1[1] as Vector3).reset(sx, -sy, -sz); +// (verts1[2] as Vector3).reset(sx, sy, -sz); +// (verts1[3] as Vector3).reset(-sx, sy, -sz); +// (verts1[4] as Vector3).reset(-sx, -sy, sz); +// (verts1[5] as Vector3).reset(sx, -sy, sz); +// (verts1[6] as Vector3).reset(sx, sy, sz); +// (verts1[7] as Vector3).reset(-sx, sy, sz); +// +// box.transform.transformVectors(verts1, verts2); +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// // Проверяем наличие столкновений с каждой вершиной +// collisionInfo.pcount = 0; +// for (var i:int = 0; i < 8; i++) { +// // Вершина добавляется в список точек столкновения, если лежит под плоскостью +// var dist:Number = (verts2[i] as Vector3).dot(normal); +// if (dist < offset) { +// var cp:ContactPoint; +// if (collisionInfo.pcount == collisionInfo.points.length) { +// cp = new ContactPoint(); +// collisionInfo.points[collisionInfo.pcount] = cp; +// } else { +// cp = collisionInfo.points[collisionInfo.pcount]; +// } +// cp.pos.copy(verts2[i]); +// cp.r1.diff(cp.pos, box.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); +// cp.penetration = offset - dist; +// collisionInfo.pcount++; +// } +// } +// if (collisionInfo.pcount > 0) { +// collisionInfo.body1 = box; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// return true; +// } + return false; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/collision/colliders/.svn/text-base/BoxRectCollider.as.svn-base b/0.0.9.2/src/alternativa/physics/collision/colliders/.svn/text-base/BoxRectCollider.as.svn-base new file mode 100644 index 0000000..d6ba302 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/colliders/.svn/text-base/BoxRectCollider.as.svn-base @@ -0,0 +1,575 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * + */ + public class BoxRectCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + + private var vectorToBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxRectCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, rect, vectorToBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + if (!findEdgesIntersection(box, rect, vectorToBox, int(bestAxisIndex/2), bestAxisIndex%2, contact)) { + return false; + } + } + contact.body1 = box.body; + contact.body2 = rect.body; + + // Хак для танков, чтобы исключить утыкания танков в стыки статических примитивов + if (rect.transform.k > 0.99999) { + contact.normal.x = 0; + contact.normal.y = 0; + contact.normal.z = 1; + } + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + // Вектор из центра прямоугольника в центр бокса + vectorToBox.x = boxTransform.d - rectTransform.d; + vectorToBox.y = boxTransform.h - rectTransform.h; + vectorToBox.z = boxTransform.l - rectTransform.l; + + // Проверка пересечения по нормали прямоугольника + rectTransform.getAxis(2, axis22); + if (!testMainAxis(box, rect, axis22, 0, vectorToBox)) return false; + + // Проверка пересечения по основным осям бокса + boxTransform.getAxis(0, axis10); + if (!testMainAxis(box, rect, axis10, 1, vectorToBox)) return false; + boxTransform.getAxis(1, axis11); + if (!testMainAxis(box, rect, axis11, 2, vectorToBox)) return false; + boxTransform.getAxis(2, axis12); + if (!testMainAxis(box, rect, axis12, 3, vectorToBox)) return false; + + // Получаем направляющие рёбер прямоугольника + rectTransform.getAxis(0, axis20); + rectTransform.getAxis(1, axis21); + + // Проверка производных осей + if (!testDerivedAxis(box, rect, axis10, axis20, 4, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis10, axis21, 5, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis11, axis20, 6, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis11, axis21, 7, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis12, axis20, 8, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis12, axis21, 9, vectorToBox)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта бокса с прямоугольником. + * + * @param box бокс + * @param rect прямоугольник + * @param vectorToBox вектор, направленный из центра прямоугольника в центр бокса + * @param faceAxisIdx индекс оси, идентифицирующей полскость столкновения (грань бокса или полскость прямоугольника) + * @param colInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var pnum:int; + var i:int; + var v:Vector3; + var cp:ContactPoint; + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + var colAxis:Vector3 = contact.normal; + + var negativeFace:Boolean; + var code:int; + + if (faceAxisIdx == 0) { + // Столкновение с плоскостью прямоугольника + + // Проверим положение бокса относительно плоскости прямоугольника + colAxis.x = rectTransform.c; + colAxis.y = rectTransform.g; + colAxis.z = rectTransform.k; + +// var offset:Number = colAxis.x*rectTransform.d + colAxis.y*rectTransform.h + colAxis.z*rectTransform.l; +// if (bbPos.vDot(colAxis) < offset) return false; + + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisIdx = axisIdx; + incidentAxisDot = dot; + } + } + negativeFace = incidentAxisDot > 0; + + code = 1 << (incidentAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + + // Получаем список вершин грани бокса, переводим их в систему координат прямоугольника и выполняем обрезку + // по прямоугольнику. Таким образом получается список потенциальных точек контакта. + boxTransform.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box.hs, incidentAxisIdx, negativeFace, points1); + boxTransform.transformVectorsN(points1, points2, 4); + rectTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByRect(rect.hs); + // Проверяем каждую потенциальную точку на принадлежность нижней полуплоскости прямоугольника и добавляем такие точки в список контактов + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if (v.z < epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = -v.z; + var cpPos:Vector3 = cp.pos; + cpPos.x = rectTransform.a*v.x + rectTransform.b*v.y + rectTransform.c*v.z + rectTransform.d; + cpPos.y = rectTransform.e*v.x + rectTransform.f*v.y + rectTransform.g*v.z + rectTransform.h; + cpPos.z = rectTransform.i*v.x + rectTransform.j*v.y + rectTransform.k*v.z + rectTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } else { + // Столкновение с гранью бокса + faceAxisIdx--; + boxTransform.getAxis(faceAxisIdx, colAxis); + negativeFace = colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z > 0; + + code = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) { + return false; + } + + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (rectTransform.c*colAxis.x + rectTransform.g*colAxis.y + rectTransform.k*colAxis.z < 0) return false; + + getFaceVertsByAxis(rect.hs, 2, false, points1); + rectTransform.transformVectorsN(points1, points2, 4); + boxTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if ((pen = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = pen; + cpPos = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } + return true; + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * + * @param hs + * @return + */ + private function clipByRect(hs:Vector3):int { + var pnum:int = 4; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box первый бокс + * @param rect второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param colInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + boxTransform.getAxis(axisIdx1, axis10); + rectTransform.getAxis(axisIdx2, axis20); + var colAxis:Vector3 = contact.normal; + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону бокса + if (colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + var vx:Number = box.hs.x; + var vy:Number = box.hs.y; + var vz:Number = box.hs.z; + var x2:Number = rect.hs.x; + var y2:Number = rect.hs.y; + var z2:Number = rect.hs.z; + // x + if (axisIdx1 == 0) { + vx = 0; + halfLen1 = box.hs.x; + } else { + if (boxTransform.a*colAxis.x + boxTransform.e*colAxis.y + boxTransform.i*colAxis.z > 0) { + vx = -vx; + if ((box.excludedFaces & 2) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 1) != 0) { + return false; + } + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = rect.hs.x; + } else { + if (rectTransform.a*colAxis.x + rectTransform.e*colAxis.y + rectTransform.i*colAxis.z < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + vy = 0; + halfLen1 = box.hs.y; + } else { + if (boxTransform.b*colAxis.x + boxTransform.f*colAxis.y + boxTransform.j*colAxis.z > 0) { + vy = -vy; + if ((box.excludedFaces & 8) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 4) != 0) { + return false; + } + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = rect.hs.y; + } else { + if (rectTransform.b*colAxis.x + rectTransform.f*colAxis.y + rectTransform.j*colAxis.z < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + vz = 0; + halfLen1 = box.hs.z; + } else { + if (boxTransform.c*colAxis.x + boxTransform.g*colAxis.y + boxTransform.k*colAxis.z > 0) { + vz = -vz; + if ((box.excludedFaces & 32) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 16) != 0) { + return false; + } + } + } + // Получаем глобальные координаты средних точек рёбер + + var x1:Number = boxTransform.a*vx + boxTransform.b*vy + boxTransform.c*vz + boxTransform.d; + var y1:Number = boxTransform.e*vx + boxTransform.f*vy + boxTransform.g*vz + boxTransform.h; + var z1:Number = boxTransform.i*vx + boxTransform.j*vy + boxTransform.k*vz + boxTransform.l; + vx = x2; + vy = y2; + vz = z2; + x2 = rectTransform.a*vx + rectTransform.b*vy + rectTransform.c*vz + rectTransform.d; + y2 = rectTransform.e*vx + rectTransform.f*vy + rectTransform.g*vz + rectTransform.h; + z2 = rectTransform.i*vx + rectTransform.j*vy + rectTransform.k*vz + rectTransform.l; + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + vx = x2 - x1; + vy = y2 - y1; + vz = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cpPos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cpPos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cpPos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var v:Vector3 = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + return true; + } + + /** + * Проверяет пересечение вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box + * @param rect + * @param axis + * @param axisIndex + * @param vectorToBox + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param rect + * @param axis1 + * @param axis2 + * @param axisIndex + * @param vectorToBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, rect:CollisionRect, axis1:Vector3, axis2:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия вдоль заданной оси. + * + * @param box бокс + * @param rect прямоугольник + * @param axis ось + * @param vectorToBox вектор, соединяющий центр прямоугольника с центром бокса + * @return величина перекрытия вдоль оси + */ + public function overlapOnAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, vectorToBox:Vector3):Number { + var m:Matrix4 = box.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box.hs.z; + if (d < 0) d = -d; + projection += d; + + m = rect.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*rect.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*rect.hs.y; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox.x*axis.x + vectorToBox.y*axis.y + vectorToBox.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} diff --git a/0.0.9.2/src/alternativa/physics/collision/colliders/.svn/text-base/BoxSphereCollider.as.svn-base b/0.0.9.2/src/alternativa/physics/collision/colliders/.svn/text-base/BoxSphereCollider.as.svn-base new file mode 100644 index 0000000..da5e36d --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/colliders/.svn/text-base/BoxSphereCollider.as.svn-base @@ -0,0 +1,168 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.math.Vector3; + use namespace altphysics; + /** + * + */ + public class BoxSphereCollider implements ICollider { + + private var center:Vector3 = new Vector3(); + private var closestPt:Vector3 = new Vector3(); + + private var bPos:Vector3 = new Vector3(); + private var sPos:Vector3 = new Vector3(); + + /** + * + */ + public function BoxSphereCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + // Зафиксированно столкновение + contact.body1 = sphere.body; + contact.body2 = box.body; + contact.normal.vCopy(closestPt).vTransformBy4(box.transform).vSubtract(sPos).vNormalize().vReverse(); + contact.pcount = 1; + + var cp:ContactPoint = contact.points[0]; + cp.penetration = sphere.r - Math.sqrt(distSqr); + cp.pos.vCopy(contact.normal).vScale(-sphere.r).vAdd(sPos); + cp.r1.vDiff(cp.pos, sPos); + cp.r2.vDiff(cp.pos, bPos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + + return true; + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/collision/colliders/.svn/text-base/BoxTriangleCollider.as.svn-base b/0.0.9.2/src/alternativa/physics/collision/colliders/.svn/text-base/BoxTriangleCollider.as.svn-base new file mode 100644 index 0000000..935525d --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/colliders/.svn/text-base/BoxTriangleCollider.as.svn-base @@ -0,0 +1,725 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionTriangle; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * + */ + public class BoxTriangleCollider extends BoxCollider { + + public var epsilon:Number = 0.001; + + private var bestAxisIndex:int; + private var minOverlap:Number; + private var toBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var colNormal:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxTriangleCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, tri, toBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + if (!findEdgesIntersection(box, tri, toBox, bestAxisIndex%3, int(bestAxisIndex/3), contact)) return false; + } + + contact.body1 = box.body; + contact.body2 = tri.body; + + // Хак для танков, чтобы исключить утыкания танков в стыки статических примитивов + if (tri.transform.k > 0.99999) { + contact.normal.x = 0; + contact.normal.y = 0; + contact.normal.z = 1; + } + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + toBox.x = boxTransform.d - triTransform.d; + toBox.y = boxTransform.h - triTransform.h; + toBox.z = boxTransform.l - triTransform.l; + + minOverlap = 1e308; + + // Сначала проверяется нормаль треугольника + axis.x = triTransform.c; + axis.y = triTransform.g; + axis.z = triTransform.k; + if (!testMainAxis(box, tri, axis, 0, toBox)) return false; + + // Проверка основных осей бокса + axis10.x = boxTransform.a; + axis10.y = boxTransform.e; + axis10.z = boxTransform.i; + if (!testMainAxis(box, tri, axis10, 1, toBox)) return false; + axis11.x = boxTransform.b; + axis11.y = boxTransform.f; + axis11.z = boxTransform.j; + if (!testMainAxis(box, tri, axis11, 2, toBox)) return false; + axis12.x = boxTransform.c; + axis12.y = boxTransform.g; + axis12.z = boxTransform.k; + if (!testMainAxis(box, tri, axis12, 3, toBox)) return false; + + // Проверка производных осей + // TODO: заменить вычисления векторных произведений инлайнами + var v:Vector3 = tri.e0; + axis20.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis20.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis20.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis20, 4, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis20, 5, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis20, 6, toBox)) return false; + + v = tri.e1; + axis21.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis21.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis21.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis21, 7, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis21, 8, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis21, 9, toBox)) return false; + + v = tri.e2; + axis22.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis22.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis22.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis22, 10, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis22, 11, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis22, 12, toBox)) return false; + + return true; + } + + /** + * Тестирует пересечение примитивов вдоль заданной оси. + * + * @param box бокс + * @param tri треугольник + * @param axis ось + * @param axisIndex индекс оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return true, если примитивы имеют перекрытие проекций вдоль указанной оси, иначе false + */ + private function testMainAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, axisIndex:int, toBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param tri + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, tri:CollisionTriangle, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Рассчитывает величину перекрытия проекций бокса и треугольника на заданную ось. + * + * @param box бокс + * @param tri треугольник + * @param axis единичный направляющий вектор оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return величина перекрытия. Положительное значение указывает на наличие перекрытия, нулевое или отрицательное значение указывает на отсутствие перекрытия. + */ + private function overlapOnAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, toBox:Vector3):Number { + var t:Matrix4 = box.transform; + var projection:Number = (t.a*axis.x + t.e*axis.y + t.i*axis.z)*box.hs.x; + if (projection < 0) projection = -projection; + var d:Number = (t.b*axis.x + t.f*axis.y + t.j*axis.z)*box.hs.y; + projection += d < 0 ? -d : d; + d = (t.c*axis.x + t.g*axis.y + t.k*axis.z)*box.hs.z; + projection += d < 0 ? -d : d; + + var vectorProjection:Number = toBox.x*axis.x + toBox.y*axis.y + toBox.z*axis.z; + // Для оптимизации ось преобразуется в систему треугольника, а не его вершины в мировую систему + t = tri.transform; + var ax:Number = t.a*axis.x + t.e*axis.y + t.i*axis.z; + var ay:Number = t.b*axis.x + t.f*axis.y + t.j*axis.z; + var az:Number = t.c*axis.x + t.g*axis.y + t.k*axis.z; + var max:Number = 0; + if (vectorProjection < 0) { + vectorProjection = -vectorProjection; + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d < max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d < max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d < max) max = d; + max = -max; + } else { + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d > max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d > max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d > max) max = d; + } + + return projection + max - vectorProjection; + } + + /** + * Определяет точки контакта бокса и треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function findFaceContactPoints(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIndex:int, contact:Contact):Boolean { + if (faceAxisIndex == 0) { + // Столкновение с плоскостью треугольника + return getBoxToTriContact(box, tri, toBox, contact); + } else { + // Столкновение с гранью бокса + return getTriToBoxContact(box, tri, toBox, faceAxisIndex, contact); + } + } + + /** + * Определяет точки контакта бокса с плоскостью треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getBoxToTriContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + colNormal.x = triTransform.c; + colNormal.y = triTransform.g; + colNormal.z = triTransform.k; + + var over:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + if (!over) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var incAxisDot:Number = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colNormal.x + axis.y*colNormal.y + axis.z*colNormal.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incAxisDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Обрезка грани + var negativeFace:Boolean = incAxisDot > 0; + + var code:int = 1 << (incFaceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + getFaceVertsByAxis(box.hs, incFaceAxisIdx, negativeFace, points1); + boxTransform.transformVectorsN(points1, points2, 4); + triTransform.transformVectorsInverseN(points2, points1, 4); + var pnum:int = clipByTriangle(tri); + // Среди конечного списка точек определяются лежащие под плоскостью треугольника + var cp:ContactPoint; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points2[i]; + if ((over && v.z < 0) || (!over && v.z > 0)) { + cp = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z + triTransform.d; + cpPos.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z + triTransform.h; + cpPos.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z + triTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = over ? -v.z : v.z; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Оперделяет точки контакта треугольника с гранью бокса. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getTriToBoxContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + faceAxisIdx--; + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + boxTransform.getAxis(faceAxisIdx, colNormal); + var negativeFace:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + + var code:int = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + if (!negativeFace) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + var v:Vector3 = points1[0]; + v.x = tri.v0.x; + v.y = tri.v0.y; + v.z = tri.v0.z; + + v = points1[1]; + v.x = tri.v1.x; + v.y = tri.v1.y; + v.z = tri.v1.z; + + v = points1[2]; + v.x = tri.v2.x; + v.y = tri.v2.y; + v.z = tri.v2.z; + + triTransform.transformVectorsN(points1, points2, 3); + boxTransform.transformVectorsInverseN(points2, points1, 3); + + var pnum:int = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность боксу и добавляем такие точки в список контактов + var penetration:Number; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + v = points1[i]; + penetration = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace); + if (penetration > -epsilon) { + var cp:ContactPoint = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = penetration; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Вычисляет величину проникновения точки в бокс. + * + * @param hs вектор половинных размеров бокса + * @param p точка в системе координат бокса + * @param axisIndex индекс оси + * @param negativeFace если true, проверяется нижняя грань + * @return величина проникновения точки в бокс + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, negativeFace:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (negativeFace) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (negativeFace) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (negativeFace) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, вершины которой заданы в списке points1. Результат сохраняется в этом же списке. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 3; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Обрезает грань прямоугольника сторонами треугольника. Обрезка выполняется в системе координат и в проекции на + * плоскость треугольника. Входные вершины грани в количестве четырёх штук должны находиться в списке points1. + * Конечные вершины сохраняются в списке points2. + * + * @param tri треугольник + * @return количество врешин в конечном списке + */ + private function clipByTriangle(tri:CollisionTriangle):int { + var vnum:int = 4; + vnum = clipByLine(tri.v0, tri.e0, points1, vnum, points2); + if (vnum == 0) return 0; + vnum = clipByLine(tri.v1, tri.e1, points2, vnum, points1); + if (vnum == 0) return 0; + return clipByLine(tri.v2, tri.e2, points1, vnum, points2);; + } + + /** + * Обрезает полигон указанной прямой. + * + * @param linePoint точка на прямой + * @param lineDir единичный направляющий вектор прямой + * @param verticesIn список вершин исходного полигона + * @param vnum количество вершин исходного полигона + * @param verticesOut список, куда будут записаны вершины конечного полигона + * @return количество вершин конечного полигона + */ + private function clipByLine(linePoint:Vector3, lineDir:Vector3, verticesIn:Vector., vnum:int, verticesOut:Vector.):int { + var nx:Number = -lineDir.y; + var ny:Number = lineDir.x; + var offset:Number = linePoint.x*nx + linePoint.y*ny; + var v1:Vector3 = verticesIn[int(vnum - 1)]; + var offset1:Number = v1.x*nx + v1.y*ny; + var t:Number; + var v:Vector3; + var num:int = 0; + for (var i:int = 0; i < vnum; i++) { + var v2:Vector3 = verticesIn[i]; + var offset2:Number = v2.x*nx + v2.y*ny; + if (offset1 < offset) { + // Первая точка ребра во внешней полуплоскости + if (offset2 > offset) { + // Вторая точка ребра во внутренней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } else { + // Первая точка ребра во внутренней полуплоскости. Добавляем её в конечный список. + v = verticesOut[num]; + v.x = v1.x; + v.y = v1.y; + v.z = v1.z; + num++; + if (offset2 < offset) { + // Вторая точка ребра во внешней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } + v1 = v2; + offset1 = offset2; + } + return num; + } + + /** + * + * @param box + * @param tri + * @param toBox + * @param boxAxisIdx + * @param triAxisIdx + * @param contact + */ + private function findEdgesIntersection(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, boxAxisIdx:int, triAxisIdx:int, contact:Contact):Boolean { + // Определение точки и направляющего вектора ребра треугольника + var tmpx1:Number; + var tmpy1:Number; + var tmpz1:Number; + var tmpx2:Number; + var tmpy2:Number; + var tmpz2:Number; + switch (triAxisIdx) { + case 0: + tmpx1 = tri.e0.x; + tmpy1 = tri.e0.y; + tmpz1 = tri.e0.z; + tmpx2 = tri.v0.x; + tmpy2 = tri.v0.y; + tmpz2 = tri.v0.z; + break; + case 1: + tmpx1 = tri.e1.x; + tmpy1 = tri.e1.y; + tmpz1 = tri.e1.z; + tmpx2 = tri.v1.x; + tmpy2 = tri.v1.y; + tmpz2 = tri.v1.z; + break; + case 2: + tmpx1 = tri.e2.x; + tmpy1 = tri.e2.y; + tmpz1 = tri.e2.z; + tmpx2 = tri.v2.x; + tmpy2 = tri.v2.y; + tmpz2 = tri.v2.z; + break; + } + var triTransform:Matrix4 = tri.transform; + axis20.x = triTransform.a*tmpx1 + triTransform.b*tmpy1 + triTransform.c*tmpz1; + axis20.y = triTransform.e*tmpx1 + triTransform.f*tmpy1 + triTransform.g*tmpz1; + axis20.z = triTransform.i*tmpx1 + triTransform.j*tmpy1 + triTransform.k*tmpz1; + var x2:Number = triTransform.a*tmpx2 + triTransform.b*tmpy2 + triTransform.c*tmpz2 + triTransform.d; + var y2:Number = triTransform.e*tmpx2 + triTransform.f*tmpy2 + triTransform.g*tmpz2 + triTransform.h; + var z2:Number = triTransform.i*tmpx2 + triTransform.j*tmpy2 + triTransform.k*tmpz2 + triTransform.l; + + // Определение нормали контакта, точки и направляющего вектора ребра бокса + var boxTransform:Matrix4 = box.transform; + boxTransform.getAxis(boxAxisIdx, axis10); + + // Нормаль контакта + var v:Vector3 = contact.normal; + v.x = axis10.y*axis20.z - axis10.z*axis20.y; + v.y = axis10.z*axis20.x - axis10.x*axis20.z; + v.z = axis10.x*axis20.y - axis10.y*axis20.x; + k = 1/Math.sqrt(v.x*v.x + v.y*v.y + v.z*v.z); + v.x *= k; + v.y *= k; + v.z *= k; + // Разворот нормали в сторону бокса + if (v.x*toBox.x + v.y*toBox.y + v.z*toBox.z < 0) { + v.x = -v.x; + v.y = -v.y; + v.z = -v.z; + } + + var boxHalfLen:Number; + tmpx1 = box.hs.x; + tmpy1 = box.hs.y; + tmpz1 = box.hs.z; + // X + if (boxAxisIdx == 0) { + tmpx1 = 0; + boxHalfLen = box.hs.x; + } else { + if (boxTransform.a*v.x + boxTransform.e*v.y + boxTransform.i*v.z > 0) { + tmpx1 = -tmpx1; + if ((box.excludedFaces & 2) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 1) != 0) { + return false; + } + } + } + // Y + if (boxAxisIdx == 1) { + tmpy1 = 0; + boxHalfLen = box.hs.y; + } else { + if (boxTransform.b*v.x + boxTransform.f*v.y + boxTransform.j*v.z > 0) { + tmpy1 = -tmpy1; + if ((box.excludedFaces & 8) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 4) != 0) { + return false; + } + } + } + // Z + if (boxAxisIdx == 2) { + tmpz1 = 0; + boxHalfLen = box.hs.z; + } else { + if (boxTransform.c*v.x + boxTransform.g*v.y + boxTransform.k*v.z > 0) { + tmpz1 = -tmpz1; + if ((box.excludedFaces & 32) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 16) != 0) { + return false; + } + } + } + var x1:Number = boxTransform.a*tmpx1 + boxTransform.b*tmpy1 + boxTransform.c*tmpz1 + boxTransform.d; + var y1:Number = boxTransform.e*tmpx1 + boxTransform.f*tmpy1 + boxTransform.g*tmpz1 + boxTransform.h; + var z1:Number = boxTransform.i*tmpx1 + boxTransform.j*tmpy1 + boxTransform.k*tmpz1 + boxTransform.l; + + // Находим точку пересечения рёбер, решая систему уравнений + // axis10 - направляющий вектор ребра бокса + // x1, y1, z1 - средняя точка ребра бокса + // axis20 - направляющий вектор ребра треугольника + // x2, y2, z2 - начальная точка ребра треугольника + var k:Number = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + var vx:Number = x2 - x1; + var vy:Number = y2 - y1; + var vz:Number = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + + // Запись данных о контакте + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + v = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + v.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + v.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + v.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = v.x - boxTransform.d; + r.y = v.y - boxTransform.h; + r.z = v.z - boxTransform.l; + r = cp.r2; + r.x = v.x - triTransform.d; + r.y = v.y - triTransform.h; + r.z = v.z - triTransform.l; + return true; + } + + } +} diff --git a/0.0.9.2/src/alternativa/physics/collision/colliders/.svn/text-base/SpherePlaneCollider.as.svn-base b/0.0.9.2/src/alternativa/physics/collision/colliders/.svn/text-base/SpherePlaneCollider.as.svn-base new file mode 100644 index 0000000..30cd55f --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/colliders/.svn/text-base/SpherePlaneCollider.as.svn-base @@ -0,0 +1,68 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + /** + * + */ + public class SpherePlaneCollider implements ICollider { + + private var normal:Vector3 = new Vector3(); + + /** + * + */ + public function SpherePlaneCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var sphere:RigidSphere = body1 as RigidSphere; +// var plane:RigidPlane; +// if (sphere == null) { +// sphere = body2 as RigidSphere; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// +// var dist:Number = sphere.state.pos.dot(normal) - offset; +// if (dist > sphere.r) return false; +// +// collisionInfo.body1 = sphere; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// collisionInfo.pcount = 1; +// +// var cp:ContactPoint = collisionInfo.points[0]; +// cp.penetration = sphere.r - dist; +// cp.pos.copy(normal).reverse().scale(sphere.r).add(sphere.state.pos); +// cp.r1.diff(cp.pos, sphere.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/collision/colliders/.svn/text-base/SphereSphereCollider.as.svn-base b/0.0.9.2/src/alternativa/physics/collision/colliders/.svn/text-base/SphereSphereCollider.as.svn-base new file mode 100644 index 0000000..3332f3c --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/colliders/.svn/text-base/SphereSphereCollider.as.svn-base @@ -0,0 +1,70 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.math.Vector3; + use namespace altphysics; + + public class SphereSphereCollider implements ICollider { + + private var p1:Vector3 = new Vector3(); + private var p2:Vector3 = new Vector3(); + + public function SphereSphereCollider() { + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var s1:CollisionSphere; + var s2:CollisionSphere; + if (prim1.body != null) { + s1 = prim1 as CollisionSphere; + s2 = prim2 as CollisionSphere; + } else { + s1 = prim2 as CollisionSphere; + s2 = prim1 as CollisionSphere; + } + + s1.transform.getAxis(3, p1); + s2.transform.getAxis(3, p2); + var dx:Number = p1.x - p2.x; + var dy:Number = p1.y - p2.y; + var dz:Number = p1.z - p2.z; + var len:Number = dx*dx + dy*dy + dz*dz; + var sum:Number = s1.r + s2.r; + if (len > sum*sum) return false; + len = Math.sqrt(len); + dx /= len; + dy /= len; + dz /= len; + + contact.body1 = s1.body; + contact.body2 = s2.body; + contact.normal.x = dx; + contact.normal.y = dy; + contact.normal.z = dz; + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = sum - len; + cp.pos.x = p1.x - dx*s1.r; + cp.pos.y = p1.y - dy*s1.r; + cp.pos.z = p1.z - dz*s1.r; + cp.r1.vDiff(cp.pos, p1); + cp.r2.vDiff(cp.pos, p2); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/collision/colliders/BoxBoxCollider.as b/0.0.9.2/src/alternativa/physics/collision/colliders/BoxBoxCollider.as new file mode 100644 index 0000000..2d3f04c --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/colliders/BoxBoxCollider.as @@ -0,0 +1,589 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * Расчитывает точки контакта двух боксов. Нормаль контакта направляется в сторону бокса с меньшим ID. + */ + public class BoxBoxCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + private var vectorToBox1:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var bestAxisIndex:int; + private var minOverlap:Number; + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + private var tmpPoints:Vector. = new Vector.(8, true); + private var pcount:int; + + /** + * + */ + public function BoxBoxCollider() { + for (var i:int = 0; i < 8; i++) { + tmpPoints[i] = new ContactPoint(); + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + + if (bestAxisIndex < 6) { + // Контакт грань-(грань|ребро|вершина) + if (!findFaceContactPoints(box1, box2, vectorToBox1, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 6; + findEdgesIntersection(box1, box2, vectorToBox1, int(bestAxisIndex/3), bestAxisIndex%3, contact); + } + contact.body1 = box1.body; + contact.body2 = box2.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + // Вектор из центра второго бокса в центр первого + vectorToBox1.x = transform1.d - transform2.d; + vectorToBox1.y = transform1.h - transform2.h; + vectorToBox1.z = transform1.l - transform2.l; + + // Проверка пересечения по основным осям + axis10.x = transform1.a; + axis10.y = transform1.e; + axis10.z = transform1.i; + if (!testMainAxis(box1, box2, axis10, 0, vectorToBox1)) return false; + axis11.x = transform1.b; + axis11.y = transform1.f; + axis11.z = transform1.j; + if (!testMainAxis(box1, box2, axis11, 1, vectorToBox1)) return false; + axis12.x = transform1.c; + axis12.y = transform1.g; + axis12.z = transform1.k; + if (!testMainAxis(box1, box2, axis12, 2, vectorToBox1)) return false; + + axis20.x = transform2.a; + axis20.y = transform2.e; + axis20.z = transform2.i; + if (!testMainAxis(box1, box2, axis20, 3, vectorToBox1)) return false; + axis21.x = transform2.b; + axis21.y = transform2.f; + axis21.z = transform2.j; + if (!testMainAxis(box1, box2, axis21, 4, vectorToBox1)) return false; + axis22.x = transform2.c; + axis22.y = transform2.g; + axis22.z = transform2.k; + if (!testMainAxis(box1, box2, axis22, 5, vectorToBox1)) return false; + + // Проверка производных осей + if (!testDerivedAxis(box1, box2, axis10, axis20, 6, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis21, 7, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis22, 8, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis11, axis20, 9, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis21, 10, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis22, 11, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis12, axis20, 12, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis21, 13, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis22, 14, vectorToBox1)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта грани одного бокса с гранью/ребром/вершиной другого. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param faceAxisIdx индекс оси первого бокса, перпендикулярной грани, с которой произошло столкновение + * @param contactInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var swapNormal:Boolean = false; + if (faceAxisIdx > 2) { + // Столкновение с гранью второго бокса. Для дальнейших расчётов боксы меняются местами, + // но нормаль контакта всё равно должна быть направлена в сторону первоначального box1 + var tmpBox:CollisionBox = box1; + box1 = box2; + box2 = tmpBox; + vectorToBox1.x = -vectorToBox1.x; + vectorToBox1.y = -vectorToBox1.y; + vectorToBox1.z = -vectorToBox1.z; + faceAxisIdx -= 3; + swapNormal = true; + } + + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + var colAxis:Vector3 = contact.normal; + + transform1.getAxis(faceAxisIdx, colAxis); + var negativeFace:Boolean = colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z > 0; + + var code:int = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box1.excludedFaces) != 0) return false; + + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + // Ищем ось второго бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + transform2.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisDot = dot; + incidentAxisIdx = axisIdx; + } + } + // Получаем список вершин грани второго бокса, переводим их в систему координат первого бокса и выполняем обрезку + // по грани первого бокса. Таким образом получается список потенциальных точек контакта. + transform2.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box2.hs, incidentAxisIdx, incidentAxisDot < 0, points1); + + // TODO: Вычислить результирующую матрицу, затем преобразовать векторы за один заход + transform2.transformVectorsN(points1, points2, 4); + transform1.transformVectorsInverseN(points2, points1, 4); + + var pnum:int = clip(box1.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points1[i]; + if ((pen = getPointBoxPenetration(box1.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + var cp:ContactPoint = tmpPoints[pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = transform1.a*v.x + transform1.b*v.y + transform1.c*v.z + transform1.d; + cpPos.y = transform1.e*v.x + transform1.f*v.y + transform1.g*v.z + transform1.h; + cpPos.z = transform1.i*v.x + transform1.j*v.y + transform1.k*v.z + transform1.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + + cp.penetration = pen; + } + } + if (swapNormal) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (pcount > 4) { + reducePoints(); + } + for (i = 0; i < pcount; i++) { + cp = contact.points[i]; + cp.copyFrom(tmpPoints[i]); + } + contact.pcount = pcount; + return true; + } + + /** + * + * @param contactInfo + */ + private function reducePoints():void { + var i:int; + var minIdx:int; + var cp1:ContactPoint; + var cp2:ContactPoint; + while (pcount > 4) { + var minLen:Number = 1e10; + var p1:Vector3 = (tmpPoints[int(pcount - 1)] as ContactPoint).pos; + var p2:Vector3; + for (i = 0; i < pcount; i++) { + p2 = (tmpPoints[i] as ContactPoint).pos; + var dx:Number = p2.x - p1.x; + var dy:Number = p2.y - p1.y; + var dz:Number = p2.z - p1.z; + var len:Number = dx*dx + dy*dy + dz*dz; + if (len < minLen) { + minLen = len; + minIdx = i; + } + p1 = p2; + } + var ii:int = minIdx == 0 ? (pcount - 1) : (minIdx - 1); + cp1 = tmpPoints[ii]; + cp2 = tmpPoints[minIdx]; + p1 = cp1.pos; + p2 = cp2.pos; + p2.x = 0.5*(p1.x + p2.x); + p2.y = 0.5*(p1.y + p2.y); + p2.z = 0.5*(p1.z + p2.z); + cp2.penetration = 0.5*(cp1.penetration + cp2.penetration); + if (minIdx > 0) { + for (i = minIdx; i < pcount; i++) { + tmpPoints[int(i - 1)] = tmpPoints[i]; + } + tmpPoints[int(pcount - 1)] = cp1; + } + pcount--; + } + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clip(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param contactInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):void { + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + transform1.getAxis(axisIdx1, axis10); + transform2.getAxis(axisIdx2, axis20); + + var colAxis:Vector3 = contact.normal; + + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону первого бокса + if (colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + + var tx:Number = box1.hs.x; + var ty:Number = box1.hs.y; + var tz:Number = box1.hs.z; + + var x2:Number = box2.hs.x; + var y2:Number = box2.hs.y; + var z2:Number = box2.hs.z; + // x + if (axisIdx1 == 0) { + tx = 0; + halfLen1 = box1.hs.x; + } else { + if (colAxis.x*transform1.a + colAxis.y*transform1.e + colAxis.z*transform1.i > 0) { + tx = -tx; + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = box2.hs.x; + } else { + if (colAxis.x*transform2.a + colAxis.y*transform2.e + colAxis.z*transform2.i < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + ty = 0; + halfLen1 = box1.hs.y; + } else { + if (colAxis.x*transform1.b + colAxis.y*transform1.f + colAxis.z*transform1.j > 0) { + ty = -ty; + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = box2.hs.y; + } else { + if (colAxis.x*transform2.b + colAxis.y*transform2.f + colAxis.z*transform2.j < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + tz = 0; + halfLen1 = box1.hs.z; + } else { + if (colAxis.x*transform1.c + colAxis.y*transform1.g + colAxis.z*transform1.k > 0) { + tz = -tz; + } + } + if (axisIdx2 == 2) { + z2 = 0; + halfLen2 = box2.hs.z; + } else { + if (colAxis.x*transform2.c + colAxis.y*transform2.g + colAxis.z*transform2.k < 0) { + z2 = -z2; + } + } + // Получаем глобальные координаты средних точек рёбер + var x1:Number = transform1.a*tx + transform1.b*ty + transform1.c*tz + transform1.d; + var y1:Number = transform1.e*tx + transform1.f*ty + transform1.g*tz + transform1.h; + var z1:Number = transform1.i*tx + transform1.j*ty + transform1.k*tz + transform1.l; + tx = x2; + ty = y2; + tz = z2; + x2 = transform2.a*tx + transform2.b*ty + transform2.c*tz + transform2.d; + y2 = transform2.e*tx + transform2.f*ty + transform2.g*tz + transform2.h; + z2 = transform2.i*tx + transform2.j*ty + transform2.k*tz + transform2.l; + + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + tx = x2 - x1; + ty = y2 - y1; + tz = z2 - z1; + var c1:Number = axis10.x*tx + axis10.y*ty + axis10.z*tz; + var c2:Number = axis20.x*tx + axis20.y*ty + axis20.z*tz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cp.pos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cp.pos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение боксов вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box1 + * @param box2 + * @param axis + * @param axisIndex + * @param toBox1 + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box1 + * @param box2 + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox1 + * @return + */ + private function testDerivedAxis(box1:CollisionBox, box2:CollisionBox, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия двух боксов вдоль заданной оси. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param axis ось + * @param vectorToBox1 вектор, соединяющий центр второго бокса с центром первого + * @return величина перекрытия боксов вдоль оси + */ + public function overlapOnAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, vectorToBox1:Vector3):Number { + var m:Matrix4 = box1.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box1.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box1.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box1.hs.z; + if (d < 0) d = -d; + projection += d; + + m = box2.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box2.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box2.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box2.hs.z; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox1.x*axis.x + vectorToBox1.y*axis.y + vectorToBox1.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} + import alternativa.math.Vector3; + + class CollisionPointTmp { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + } diff --git a/0.0.9.2/src/alternativa/physics/collision/colliders/BoxCollider.as b/0.0.9.2/src/alternativa/physics/collision/colliders/BoxCollider.as new file mode 100644 index 0000000..2429bb1 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/colliders/BoxCollider.as @@ -0,0 +1,427 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + /** + * + */ + public class BoxCollider implements ICollider { + + /** + * + */ + public function BoxCollider() { + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + return false; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + protected function getFaceVertsByAxis(hs:Vector3, axisIdx:int, negativeFace:Boolean, result:Vector.):void { + var v:Vector3; + switch (axisIdx) { + case 0: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + } + break; + case 1: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + } + break; + case 2: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + } else { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } + break; + } + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/collision/colliders/BoxPlaneCollider.as b/0.0.9.2/src/alternativa/physics/collision/colliders/BoxPlaneCollider.as new file mode 100644 index 0000000..3bebdc3 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/colliders/BoxPlaneCollider.as @@ -0,0 +1,88 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + public class BoxPlaneCollider implements ICollider { + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var normal:Vector3 = new Vector3(); + + public function BoxPlaneCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var box:RigidBox = body1 as RigidBox; +// var plane:RigidPlane; +// if (box == null) { +// box = body2 as RigidBox; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные координаты вершин бокса +// var sx:Number = box.halfSize.x; +// var sy:Number = box.halfSize.y; +// var sz:Number = box.halfSize.z; +// (verts1[0] as Vector3).reset(-sx, -sy, -sz); +// (verts1[1] as Vector3).reset(sx, -sy, -sz); +// (verts1[2] as Vector3).reset(sx, sy, -sz); +// (verts1[3] as Vector3).reset(-sx, sy, -sz); +// (verts1[4] as Vector3).reset(-sx, -sy, sz); +// (verts1[5] as Vector3).reset(sx, -sy, sz); +// (verts1[6] as Vector3).reset(sx, sy, sz); +// (verts1[7] as Vector3).reset(-sx, sy, sz); +// +// box.transform.transformVectors(verts1, verts2); +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// // Проверяем наличие столкновений с каждой вершиной +// collisionInfo.pcount = 0; +// for (var i:int = 0; i < 8; i++) { +// // Вершина добавляется в список точек столкновения, если лежит под плоскостью +// var dist:Number = (verts2[i] as Vector3).dot(normal); +// if (dist < offset) { +// var cp:ContactPoint; +// if (collisionInfo.pcount == collisionInfo.points.length) { +// cp = new ContactPoint(); +// collisionInfo.points[collisionInfo.pcount] = cp; +// } else { +// cp = collisionInfo.points[collisionInfo.pcount]; +// } +// cp.pos.copy(verts2[i]); +// cp.r1.diff(cp.pos, box.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); +// cp.penetration = offset - dist; +// collisionInfo.pcount++; +// } +// } +// if (collisionInfo.pcount > 0) { +// collisionInfo.body1 = box; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// return true; +// } + return false; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/collision/colliders/BoxRectCollider.as b/0.0.9.2/src/alternativa/physics/collision/colliders/BoxRectCollider.as new file mode 100644 index 0000000..d6ba302 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/colliders/BoxRectCollider.as @@ -0,0 +1,575 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * + */ + public class BoxRectCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + + private var vectorToBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxRectCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, rect, vectorToBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + if (!findEdgesIntersection(box, rect, vectorToBox, int(bestAxisIndex/2), bestAxisIndex%2, contact)) { + return false; + } + } + contact.body1 = box.body; + contact.body2 = rect.body; + + // Хак для танков, чтобы исключить утыкания танков в стыки статических примитивов + if (rect.transform.k > 0.99999) { + contact.normal.x = 0; + contact.normal.y = 0; + contact.normal.z = 1; + } + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + // Вектор из центра прямоугольника в центр бокса + vectorToBox.x = boxTransform.d - rectTransform.d; + vectorToBox.y = boxTransform.h - rectTransform.h; + vectorToBox.z = boxTransform.l - rectTransform.l; + + // Проверка пересечения по нормали прямоугольника + rectTransform.getAxis(2, axis22); + if (!testMainAxis(box, rect, axis22, 0, vectorToBox)) return false; + + // Проверка пересечения по основным осям бокса + boxTransform.getAxis(0, axis10); + if (!testMainAxis(box, rect, axis10, 1, vectorToBox)) return false; + boxTransform.getAxis(1, axis11); + if (!testMainAxis(box, rect, axis11, 2, vectorToBox)) return false; + boxTransform.getAxis(2, axis12); + if (!testMainAxis(box, rect, axis12, 3, vectorToBox)) return false; + + // Получаем направляющие рёбер прямоугольника + rectTransform.getAxis(0, axis20); + rectTransform.getAxis(1, axis21); + + // Проверка производных осей + if (!testDerivedAxis(box, rect, axis10, axis20, 4, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis10, axis21, 5, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis11, axis20, 6, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis11, axis21, 7, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis12, axis20, 8, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis12, axis21, 9, vectorToBox)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта бокса с прямоугольником. + * + * @param box бокс + * @param rect прямоугольник + * @param vectorToBox вектор, направленный из центра прямоугольника в центр бокса + * @param faceAxisIdx индекс оси, идентифицирующей полскость столкновения (грань бокса или полскость прямоугольника) + * @param colInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var pnum:int; + var i:int; + var v:Vector3; + var cp:ContactPoint; + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + var colAxis:Vector3 = contact.normal; + + var negativeFace:Boolean; + var code:int; + + if (faceAxisIdx == 0) { + // Столкновение с плоскостью прямоугольника + + // Проверим положение бокса относительно плоскости прямоугольника + colAxis.x = rectTransform.c; + colAxis.y = rectTransform.g; + colAxis.z = rectTransform.k; + +// var offset:Number = colAxis.x*rectTransform.d + colAxis.y*rectTransform.h + colAxis.z*rectTransform.l; +// if (bbPos.vDot(colAxis) < offset) return false; + + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisIdx = axisIdx; + incidentAxisDot = dot; + } + } + negativeFace = incidentAxisDot > 0; + + code = 1 << (incidentAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + + // Получаем список вершин грани бокса, переводим их в систему координат прямоугольника и выполняем обрезку + // по прямоугольнику. Таким образом получается список потенциальных точек контакта. + boxTransform.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box.hs, incidentAxisIdx, negativeFace, points1); + boxTransform.transformVectorsN(points1, points2, 4); + rectTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByRect(rect.hs); + // Проверяем каждую потенциальную точку на принадлежность нижней полуплоскости прямоугольника и добавляем такие точки в список контактов + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if (v.z < epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = -v.z; + var cpPos:Vector3 = cp.pos; + cpPos.x = rectTransform.a*v.x + rectTransform.b*v.y + rectTransform.c*v.z + rectTransform.d; + cpPos.y = rectTransform.e*v.x + rectTransform.f*v.y + rectTransform.g*v.z + rectTransform.h; + cpPos.z = rectTransform.i*v.x + rectTransform.j*v.y + rectTransform.k*v.z + rectTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } else { + // Столкновение с гранью бокса + faceAxisIdx--; + boxTransform.getAxis(faceAxisIdx, colAxis); + negativeFace = colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z > 0; + + code = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) { + return false; + } + + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (rectTransform.c*colAxis.x + rectTransform.g*colAxis.y + rectTransform.k*colAxis.z < 0) return false; + + getFaceVertsByAxis(rect.hs, 2, false, points1); + rectTransform.transformVectorsN(points1, points2, 4); + boxTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if ((pen = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = pen; + cpPos = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } + return true; + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * + * @param hs + * @return + */ + private function clipByRect(hs:Vector3):int { + var pnum:int = 4; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box первый бокс + * @param rect второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param colInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + boxTransform.getAxis(axisIdx1, axis10); + rectTransform.getAxis(axisIdx2, axis20); + var colAxis:Vector3 = contact.normal; + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону бокса + if (colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + var vx:Number = box.hs.x; + var vy:Number = box.hs.y; + var vz:Number = box.hs.z; + var x2:Number = rect.hs.x; + var y2:Number = rect.hs.y; + var z2:Number = rect.hs.z; + // x + if (axisIdx1 == 0) { + vx = 0; + halfLen1 = box.hs.x; + } else { + if (boxTransform.a*colAxis.x + boxTransform.e*colAxis.y + boxTransform.i*colAxis.z > 0) { + vx = -vx; + if ((box.excludedFaces & 2) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 1) != 0) { + return false; + } + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = rect.hs.x; + } else { + if (rectTransform.a*colAxis.x + rectTransform.e*colAxis.y + rectTransform.i*colAxis.z < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + vy = 0; + halfLen1 = box.hs.y; + } else { + if (boxTransform.b*colAxis.x + boxTransform.f*colAxis.y + boxTransform.j*colAxis.z > 0) { + vy = -vy; + if ((box.excludedFaces & 8) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 4) != 0) { + return false; + } + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = rect.hs.y; + } else { + if (rectTransform.b*colAxis.x + rectTransform.f*colAxis.y + rectTransform.j*colAxis.z < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + vz = 0; + halfLen1 = box.hs.z; + } else { + if (boxTransform.c*colAxis.x + boxTransform.g*colAxis.y + boxTransform.k*colAxis.z > 0) { + vz = -vz; + if ((box.excludedFaces & 32) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 16) != 0) { + return false; + } + } + } + // Получаем глобальные координаты средних точек рёбер + + var x1:Number = boxTransform.a*vx + boxTransform.b*vy + boxTransform.c*vz + boxTransform.d; + var y1:Number = boxTransform.e*vx + boxTransform.f*vy + boxTransform.g*vz + boxTransform.h; + var z1:Number = boxTransform.i*vx + boxTransform.j*vy + boxTransform.k*vz + boxTransform.l; + vx = x2; + vy = y2; + vz = z2; + x2 = rectTransform.a*vx + rectTransform.b*vy + rectTransform.c*vz + rectTransform.d; + y2 = rectTransform.e*vx + rectTransform.f*vy + rectTransform.g*vz + rectTransform.h; + z2 = rectTransform.i*vx + rectTransform.j*vy + rectTransform.k*vz + rectTransform.l; + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + vx = x2 - x1; + vy = y2 - y1; + vz = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cpPos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cpPos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cpPos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var v:Vector3 = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + return true; + } + + /** + * Проверяет пересечение вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box + * @param rect + * @param axis + * @param axisIndex + * @param vectorToBox + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param rect + * @param axis1 + * @param axis2 + * @param axisIndex + * @param vectorToBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, rect:CollisionRect, axis1:Vector3, axis2:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия вдоль заданной оси. + * + * @param box бокс + * @param rect прямоугольник + * @param axis ось + * @param vectorToBox вектор, соединяющий центр прямоугольника с центром бокса + * @return величина перекрытия вдоль оси + */ + public function overlapOnAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, vectorToBox:Vector3):Number { + var m:Matrix4 = box.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box.hs.z; + if (d < 0) d = -d; + projection += d; + + m = rect.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*rect.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*rect.hs.y; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox.x*axis.x + vectorToBox.y*axis.y + vectorToBox.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} diff --git a/0.0.9.2/src/alternativa/physics/collision/colliders/BoxSphereCollider.as b/0.0.9.2/src/alternativa/physics/collision/colliders/BoxSphereCollider.as new file mode 100644 index 0000000..da5e36d --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/colliders/BoxSphereCollider.as @@ -0,0 +1,168 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.math.Vector3; + use namespace altphysics; + /** + * + */ + public class BoxSphereCollider implements ICollider { + + private var center:Vector3 = new Vector3(); + private var closestPt:Vector3 = new Vector3(); + + private var bPos:Vector3 = new Vector3(); + private var sPos:Vector3 = new Vector3(); + + /** + * + */ + public function BoxSphereCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + // Зафиксированно столкновение + contact.body1 = sphere.body; + contact.body2 = box.body; + contact.normal.vCopy(closestPt).vTransformBy4(box.transform).vSubtract(sPos).vNormalize().vReverse(); + contact.pcount = 1; + + var cp:ContactPoint = contact.points[0]; + cp.penetration = sphere.r - Math.sqrt(distSqr); + cp.pos.vCopy(contact.normal).vScale(-sphere.r).vAdd(sPos); + cp.r1.vDiff(cp.pos, sPos); + cp.r2.vDiff(cp.pos, bPos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + + return true; + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/collision/colliders/BoxTriangleCollider.as b/0.0.9.2/src/alternativa/physics/collision/colliders/BoxTriangleCollider.as new file mode 100644 index 0000000..935525d --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/colliders/BoxTriangleCollider.as @@ -0,0 +1,725 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionTriangle; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * + */ + public class BoxTriangleCollider extends BoxCollider { + + public var epsilon:Number = 0.001; + + private var bestAxisIndex:int; + private var minOverlap:Number; + private var toBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var colNormal:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxTriangleCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, tri, toBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + if (!findEdgesIntersection(box, tri, toBox, bestAxisIndex%3, int(bestAxisIndex/3), contact)) return false; + } + + contact.body1 = box.body; + contact.body2 = tri.body; + + // Хак для танков, чтобы исключить утыкания танков в стыки статических примитивов + if (tri.transform.k > 0.99999) { + contact.normal.x = 0; + contact.normal.y = 0; + contact.normal.z = 1; + } + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + toBox.x = boxTransform.d - triTransform.d; + toBox.y = boxTransform.h - triTransform.h; + toBox.z = boxTransform.l - triTransform.l; + + minOverlap = 1e308; + + // Сначала проверяется нормаль треугольника + axis.x = triTransform.c; + axis.y = triTransform.g; + axis.z = triTransform.k; + if (!testMainAxis(box, tri, axis, 0, toBox)) return false; + + // Проверка основных осей бокса + axis10.x = boxTransform.a; + axis10.y = boxTransform.e; + axis10.z = boxTransform.i; + if (!testMainAxis(box, tri, axis10, 1, toBox)) return false; + axis11.x = boxTransform.b; + axis11.y = boxTransform.f; + axis11.z = boxTransform.j; + if (!testMainAxis(box, tri, axis11, 2, toBox)) return false; + axis12.x = boxTransform.c; + axis12.y = boxTransform.g; + axis12.z = boxTransform.k; + if (!testMainAxis(box, tri, axis12, 3, toBox)) return false; + + // Проверка производных осей + // TODO: заменить вычисления векторных произведений инлайнами + var v:Vector3 = tri.e0; + axis20.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis20.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis20.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis20, 4, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis20, 5, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis20, 6, toBox)) return false; + + v = tri.e1; + axis21.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis21.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis21.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis21, 7, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis21, 8, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis21, 9, toBox)) return false; + + v = tri.e2; + axis22.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis22.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis22.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis22, 10, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis22, 11, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis22, 12, toBox)) return false; + + return true; + } + + /** + * Тестирует пересечение примитивов вдоль заданной оси. + * + * @param box бокс + * @param tri треугольник + * @param axis ось + * @param axisIndex индекс оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return true, если примитивы имеют перекрытие проекций вдоль указанной оси, иначе false + */ + private function testMainAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, axisIndex:int, toBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param tri + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, tri:CollisionTriangle, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Рассчитывает величину перекрытия проекций бокса и треугольника на заданную ось. + * + * @param box бокс + * @param tri треугольник + * @param axis единичный направляющий вектор оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return величина перекрытия. Положительное значение указывает на наличие перекрытия, нулевое или отрицательное значение указывает на отсутствие перекрытия. + */ + private function overlapOnAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, toBox:Vector3):Number { + var t:Matrix4 = box.transform; + var projection:Number = (t.a*axis.x + t.e*axis.y + t.i*axis.z)*box.hs.x; + if (projection < 0) projection = -projection; + var d:Number = (t.b*axis.x + t.f*axis.y + t.j*axis.z)*box.hs.y; + projection += d < 0 ? -d : d; + d = (t.c*axis.x + t.g*axis.y + t.k*axis.z)*box.hs.z; + projection += d < 0 ? -d : d; + + var vectorProjection:Number = toBox.x*axis.x + toBox.y*axis.y + toBox.z*axis.z; + // Для оптимизации ось преобразуется в систему треугольника, а не его вершины в мировую систему + t = tri.transform; + var ax:Number = t.a*axis.x + t.e*axis.y + t.i*axis.z; + var ay:Number = t.b*axis.x + t.f*axis.y + t.j*axis.z; + var az:Number = t.c*axis.x + t.g*axis.y + t.k*axis.z; + var max:Number = 0; + if (vectorProjection < 0) { + vectorProjection = -vectorProjection; + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d < max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d < max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d < max) max = d; + max = -max; + } else { + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d > max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d > max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d > max) max = d; + } + + return projection + max - vectorProjection; + } + + /** + * Определяет точки контакта бокса и треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function findFaceContactPoints(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIndex:int, contact:Contact):Boolean { + if (faceAxisIndex == 0) { + // Столкновение с плоскостью треугольника + return getBoxToTriContact(box, tri, toBox, contact); + } else { + // Столкновение с гранью бокса + return getTriToBoxContact(box, tri, toBox, faceAxisIndex, contact); + } + } + + /** + * Определяет точки контакта бокса с плоскостью треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getBoxToTriContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + colNormal.x = triTransform.c; + colNormal.y = triTransform.g; + colNormal.z = triTransform.k; + + var over:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + if (!over) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var incAxisDot:Number = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colNormal.x + axis.y*colNormal.y + axis.z*colNormal.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incAxisDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Обрезка грани + var negativeFace:Boolean = incAxisDot > 0; + + var code:int = 1 << (incFaceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + getFaceVertsByAxis(box.hs, incFaceAxisIdx, negativeFace, points1); + boxTransform.transformVectorsN(points1, points2, 4); + triTransform.transformVectorsInverseN(points2, points1, 4); + var pnum:int = clipByTriangle(tri); + // Среди конечного списка точек определяются лежащие под плоскостью треугольника + var cp:ContactPoint; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points2[i]; + if ((over && v.z < 0) || (!over && v.z > 0)) { + cp = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z + triTransform.d; + cpPos.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z + triTransform.h; + cpPos.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z + triTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = over ? -v.z : v.z; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Оперделяет точки контакта треугольника с гранью бокса. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getTriToBoxContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + faceAxisIdx--; + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + boxTransform.getAxis(faceAxisIdx, colNormal); + var negativeFace:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + + var code:int = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + if (!negativeFace) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + var v:Vector3 = points1[0]; + v.x = tri.v0.x; + v.y = tri.v0.y; + v.z = tri.v0.z; + + v = points1[1]; + v.x = tri.v1.x; + v.y = tri.v1.y; + v.z = tri.v1.z; + + v = points1[2]; + v.x = tri.v2.x; + v.y = tri.v2.y; + v.z = tri.v2.z; + + triTransform.transformVectorsN(points1, points2, 3); + boxTransform.transformVectorsInverseN(points2, points1, 3); + + var pnum:int = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность боксу и добавляем такие точки в список контактов + var penetration:Number; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + v = points1[i]; + penetration = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace); + if (penetration > -epsilon) { + var cp:ContactPoint = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = penetration; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Вычисляет величину проникновения точки в бокс. + * + * @param hs вектор половинных размеров бокса + * @param p точка в системе координат бокса + * @param axisIndex индекс оси + * @param negativeFace если true, проверяется нижняя грань + * @return величина проникновения точки в бокс + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, negativeFace:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (negativeFace) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (negativeFace) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (negativeFace) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, вершины которой заданы в списке points1. Результат сохраняется в этом же списке. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 3; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Обрезает грань прямоугольника сторонами треугольника. Обрезка выполняется в системе координат и в проекции на + * плоскость треугольника. Входные вершины грани в количестве четырёх штук должны находиться в списке points1. + * Конечные вершины сохраняются в списке points2. + * + * @param tri треугольник + * @return количество врешин в конечном списке + */ + private function clipByTriangle(tri:CollisionTriangle):int { + var vnum:int = 4; + vnum = clipByLine(tri.v0, tri.e0, points1, vnum, points2); + if (vnum == 0) return 0; + vnum = clipByLine(tri.v1, tri.e1, points2, vnum, points1); + if (vnum == 0) return 0; + return clipByLine(tri.v2, tri.e2, points1, vnum, points2);; + } + + /** + * Обрезает полигон указанной прямой. + * + * @param linePoint точка на прямой + * @param lineDir единичный направляющий вектор прямой + * @param verticesIn список вершин исходного полигона + * @param vnum количество вершин исходного полигона + * @param verticesOut список, куда будут записаны вершины конечного полигона + * @return количество вершин конечного полигона + */ + private function clipByLine(linePoint:Vector3, lineDir:Vector3, verticesIn:Vector., vnum:int, verticesOut:Vector.):int { + var nx:Number = -lineDir.y; + var ny:Number = lineDir.x; + var offset:Number = linePoint.x*nx + linePoint.y*ny; + var v1:Vector3 = verticesIn[int(vnum - 1)]; + var offset1:Number = v1.x*nx + v1.y*ny; + var t:Number; + var v:Vector3; + var num:int = 0; + for (var i:int = 0; i < vnum; i++) { + var v2:Vector3 = verticesIn[i]; + var offset2:Number = v2.x*nx + v2.y*ny; + if (offset1 < offset) { + // Первая точка ребра во внешней полуплоскости + if (offset2 > offset) { + // Вторая точка ребра во внутренней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } else { + // Первая точка ребра во внутренней полуплоскости. Добавляем её в конечный список. + v = verticesOut[num]; + v.x = v1.x; + v.y = v1.y; + v.z = v1.z; + num++; + if (offset2 < offset) { + // Вторая точка ребра во внешней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } + v1 = v2; + offset1 = offset2; + } + return num; + } + + /** + * + * @param box + * @param tri + * @param toBox + * @param boxAxisIdx + * @param triAxisIdx + * @param contact + */ + private function findEdgesIntersection(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, boxAxisIdx:int, triAxisIdx:int, contact:Contact):Boolean { + // Определение точки и направляющего вектора ребра треугольника + var tmpx1:Number; + var tmpy1:Number; + var tmpz1:Number; + var tmpx2:Number; + var tmpy2:Number; + var tmpz2:Number; + switch (triAxisIdx) { + case 0: + tmpx1 = tri.e0.x; + tmpy1 = tri.e0.y; + tmpz1 = tri.e0.z; + tmpx2 = tri.v0.x; + tmpy2 = tri.v0.y; + tmpz2 = tri.v0.z; + break; + case 1: + tmpx1 = tri.e1.x; + tmpy1 = tri.e1.y; + tmpz1 = tri.e1.z; + tmpx2 = tri.v1.x; + tmpy2 = tri.v1.y; + tmpz2 = tri.v1.z; + break; + case 2: + tmpx1 = tri.e2.x; + tmpy1 = tri.e2.y; + tmpz1 = tri.e2.z; + tmpx2 = tri.v2.x; + tmpy2 = tri.v2.y; + tmpz2 = tri.v2.z; + break; + } + var triTransform:Matrix4 = tri.transform; + axis20.x = triTransform.a*tmpx1 + triTransform.b*tmpy1 + triTransform.c*tmpz1; + axis20.y = triTransform.e*tmpx1 + triTransform.f*tmpy1 + triTransform.g*tmpz1; + axis20.z = triTransform.i*tmpx1 + triTransform.j*tmpy1 + triTransform.k*tmpz1; + var x2:Number = triTransform.a*tmpx2 + triTransform.b*tmpy2 + triTransform.c*tmpz2 + triTransform.d; + var y2:Number = triTransform.e*tmpx2 + triTransform.f*tmpy2 + triTransform.g*tmpz2 + triTransform.h; + var z2:Number = triTransform.i*tmpx2 + triTransform.j*tmpy2 + triTransform.k*tmpz2 + triTransform.l; + + // Определение нормали контакта, точки и направляющего вектора ребра бокса + var boxTransform:Matrix4 = box.transform; + boxTransform.getAxis(boxAxisIdx, axis10); + + // Нормаль контакта + var v:Vector3 = contact.normal; + v.x = axis10.y*axis20.z - axis10.z*axis20.y; + v.y = axis10.z*axis20.x - axis10.x*axis20.z; + v.z = axis10.x*axis20.y - axis10.y*axis20.x; + k = 1/Math.sqrt(v.x*v.x + v.y*v.y + v.z*v.z); + v.x *= k; + v.y *= k; + v.z *= k; + // Разворот нормали в сторону бокса + if (v.x*toBox.x + v.y*toBox.y + v.z*toBox.z < 0) { + v.x = -v.x; + v.y = -v.y; + v.z = -v.z; + } + + var boxHalfLen:Number; + tmpx1 = box.hs.x; + tmpy1 = box.hs.y; + tmpz1 = box.hs.z; + // X + if (boxAxisIdx == 0) { + tmpx1 = 0; + boxHalfLen = box.hs.x; + } else { + if (boxTransform.a*v.x + boxTransform.e*v.y + boxTransform.i*v.z > 0) { + tmpx1 = -tmpx1; + if ((box.excludedFaces & 2) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 1) != 0) { + return false; + } + } + } + // Y + if (boxAxisIdx == 1) { + tmpy1 = 0; + boxHalfLen = box.hs.y; + } else { + if (boxTransform.b*v.x + boxTransform.f*v.y + boxTransform.j*v.z > 0) { + tmpy1 = -tmpy1; + if ((box.excludedFaces & 8) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 4) != 0) { + return false; + } + } + } + // Z + if (boxAxisIdx == 2) { + tmpz1 = 0; + boxHalfLen = box.hs.z; + } else { + if (boxTransform.c*v.x + boxTransform.g*v.y + boxTransform.k*v.z > 0) { + tmpz1 = -tmpz1; + if ((box.excludedFaces & 32) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 16) != 0) { + return false; + } + } + } + var x1:Number = boxTransform.a*tmpx1 + boxTransform.b*tmpy1 + boxTransform.c*tmpz1 + boxTransform.d; + var y1:Number = boxTransform.e*tmpx1 + boxTransform.f*tmpy1 + boxTransform.g*tmpz1 + boxTransform.h; + var z1:Number = boxTransform.i*tmpx1 + boxTransform.j*tmpy1 + boxTransform.k*tmpz1 + boxTransform.l; + + // Находим точку пересечения рёбер, решая систему уравнений + // axis10 - направляющий вектор ребра бокса + // x1, y1, z1 - средняя точка ребра бокса + // axis20 - направляющий вектор ребра треугольника + // x2, y2, z2 - начальная точка ребра треугольника + var k:Number = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + var vx:Number = x2 - x1; + var vy:Number = y2 - y1; + var vz:Number = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + + // Запись данных о контакте + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + v = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + v.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + v.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + v.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = v.x - boxTransform.d; + r.y = v.y - boxTransform.h; + r.z = v.z - boxTransform.l; + r = cp.r2; + r.x = v.x - triTransform.d; + r.y = v.y - triTransform.h; + r.z = v.z - triTransform.l; + return true; + } + + } +} diff --git a/0.0.9.2/src/alternativa/physics/collision/colliders/SpherePlaneCollider.as b/0.0.9.2/src/alternativa/physics/collision/colliders/SpherePlaneCollider.as new file mode 100644 index 0000000..30cd55f --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/colliders/SpherePlaneCollider.as @@ -0,0 +1,68 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + /** + * + */ + public class SpherePlaneCollider implements ICollider { + + private var normal:Vector3 = new Vector3(); + + /** + * + */ + public function SpherePlaneCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var sphere:RigidSphere = body1 as RigidSphere; +// var plane:RigidPlane; +// if (sphere == null) { +// sphere = body2 as RigidSphere; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// +// var dist:Number = sphere.state.pos.dot(normal) - offset; +// if (dist > sphere.r) return false; +// +// collisionInfo.body1 = sphere; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// collisionInfo.pcount = 1; +// +// var cp:ContactPoint = collisionInfo.points[0]; +// cp.penetration = sphere.r - dist; +// cp.pos.copy(normal).reverse().scale(sphere.r).add(sphere.state.pos); +// cp.r1.diff(cp.pos, sphere.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/collision/colliders/SphereSphereCollider.as b/0.0.9.2/src/alternativa/physics/collision/colliders/SphereSphereCollider.as new file mode 100644 index 0000000..3332f3c --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/colliders/SphereSphereCollider.as @@ -0,0 +1,70 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.math.Vector3; + use namespace altphysics; + + public class SphereSphereCollider implements ICollider { + + private var p1:Vector3 = new Vector3(); + private var p2:Vector3 = new Vector3(); + + public function SphereSphereCollider() { + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var s1:CollisionSphere; + var s2:CollisionSphere; + if (prim1.body != null) { + s1 = prim1 as CollisionSphere; + s2 = prim2 as CollisionSphere; + } else { + s1 = prim2 as CollisionSphere; + s2 = prim1 as CollisionSphere; + } + + s1.transform.getAxis(3, p1); + s2.transform.getAxis(3, p2); + var dx:Number = p1.x - p2.x; + var dy:Number = p1.y - p2.y; + var dz:Number = p1.z - p2.z; + var len:Number = dx*dx + dy*dy + dz*dz; + var sum:Number = s1.r + s2.r; + if (len > sum*sum) return false; + len = Math.sqrt(len); + dx /= len; + dy /= len; + dz /= len; + + contact.body1 = s1.body; + contact.body2 = s2.body; + contact.normal.x = dx; + contact.normal.y = dy; + contact.normal.z = dz; + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = sum - len; + cp.pos.x = p1.x - dx*s1.r; + cp.pos.y = p1.y - dy*s1.r; + cp.pos.z = p1.z - dz*s1.r; + cp.r1.vDiff(cp.pos, p1); + cp.r2.vDiff(cp.pos, p2); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/collision/primitives/.svn/all-wcprops b/0.0.9.2/src/alternativa/physics/collision/primitives/.svn/all-wcprops new file mode 100644 index 0000000..c09549d --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/primitives/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 124 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/collision/primitives +END +CollisionTriangle.as +K 25 +svn:wc:ra_dav:version-url +V 145 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/collision/primitives/CollisionTriangle.as +END +CollisionSphere.as +K 25 +svn:wc:ra_dav:version-url +V 143 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/collision/primitives/CollisionSphere.as +END +CollisionBox.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/collision/primitives/CollisionBox.as +END +CollisionRect.as +K 25 +svn:wc:ra_dav:version-url +V 141 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/collision/primitives/CollisionRect.as +END diff --git a/0.0.9.2/src/alternativa/physics/collision/primitives/.svn/entries b/0.0.9.2/src/alternativa/physics/collision/primitives/.svn/entries new file mode 100644 index 0000000..acbc35d --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/primitives/.svn/entries @@ -0,0 +1,76 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/collision/primitives +http://svndev.alternativaplatform.com + + + +2009-11-24T08:44:15.941480Z +23758 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +CollisionTriangle.as +file + + + + +2010-10-28T04:32:44.000000Z +23ebb96cda309378ce15ead4cf865446 +2009-11-24T08:44:15.941480Z +23758 +mike + +CollisionSphere.as +file + + + + +2010-10-28T04:32:44.000000Z +335b6b72039123d6d54fee4d6b17f74e +2009-11-24T08:44:15.941480Z +23758 +mike + +CollisionBox.as +file + + + + +2010-10-28T04:32:44.000000Z +65e81a3d0034265e568d549306df1406 +2009-11-24T08:44:15.941480Z +23758 +mike + +CollisionRect.as +file + + + + +2010-10-28T04:32:44.000000Z +91b913679dca5ae4baabafb09c5aa051 +2009-11-24T08:44:15.941480Z +23758 +mike + diff --git a/0.0.9.2/src/alternativa/physics/collision/primitives/.svn/format b/0.0.9.2/src/alternativa/physics/collision/primitives/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/primitives/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.2/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base b/0.0.9.2/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base new file mode 100644 index 0000000..1febfb4 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base @@ -0,0 +1,204 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * Ориентированный бокс. + */ + public class CollisionBox extends CollisionPrimitive { + + // Половинные размеры вдоль каждой из осей + public var hs:Vector3 = new Vector3(); + + public var excludedFaces:int; + + /** + * @param hs + * @param collisionGroup + */ + public function CollisionBox(hs:Vector3, collisionGroup:int) { + super(BOX, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + var t:Matrix4 = transform; + + var xx:Number; + var yy:Number; + var zz:Number; + + xx = t.a < 0 ? -t.a : t.a; + yy = t.b < 0 ? -t.b : t.b; + zz = t.c < 0 ? -t.c : t.c; + aabb.maxX = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minX = -aabb.maxX; + + xx = t.e < 0 ? -t.e : t.e; + yy = t.f < 0 ? -t.f : t.f; + zz = t.g < 0 ? -t.g : t.g; + aabb.maxY = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minY = -aabb.maxY; + + xx = t.i < 0 ? -t.i : t.i; + yy = t.j < 0 ? -t.j : t.j; + zz = t.k < 0 ? -t.k : t.k; + aabb.maxZ = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var box:CollisionBox = source as CollisionBox; + if (box == null) return this; + super.copyFrom(box); + hs.vCopy(box.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionBox(hs, collisionGroup); + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + var tMin:Number = -1; + var tMax:Number = 1e308; + var t1:Number; + var t2:Number; + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + // X + if (vx < epsilon && vx > -epsilon) { + if (ox < -hs.x || ox > hs.x) return -1; + } else { + t1 = (-hs.x - ox)/vx; + t2 = (hs.x - ox)/vx; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.x = -1; + normal.y = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.x = 1; + normal.y = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Y + if (vy < epsilon && vy > -epsilon) { + if (oy < -hs.y || oy > hs.y) return -1; + } else { + t1 = (-hs.y - oy)/vy; + t2 = (hs.y - oy)/vy; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.y = -1; + normal.x = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.y = 1; + normal.x = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Z + if (vz < epsilon && vz > -epsilon) { + if (oz < -hs.z || oz > hs.z) return -1; + } else { + t1 = (-hs.z - oz)/vz; + t2 = (hs.z - oz)/vz; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.z = -1; + normal.x = normal.y = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.z = 1; + normal.x = normal.y = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Перевод нормали в мировую систему координат + // Inlined normal.vDeltaTransformBy4(transform); + vx = normal.x; + vy = normal.y; + vz = normal.z; + normal.x = transform.a*vx + transform.b*vy + transform.c*vz; + normal.y = transform.e*vx + transform.f*vy + transform.g*vz; + normal.z = transform.i*vx + transform.j*vy + transform.k*vz; + + return tMin; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionBox hs=" + hs + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base b/0.0.9.2/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base new file mode 100644 index 0000000..7a33b79 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base @@ -0,0 +1,143 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * Ориентированный прямоугольник. Задаётся половинами размеров вдоль осей X (ширина) и Y (длина) локальной системы + * координат примитива. Таким образом, прямоугольник лежит в плоскости XY, его стороны параллельны осям этой + * плоскости, а нормаль направлена вдоль локальной оси Z. + * + * Прямоугольник может быть одно- или двусторонним. В случае одностороннего прямоугольника, + * столкновения с внутренней стороны не регистрируются. + */ + public class CollisionRect extends CollisionPrimitive { + // Половинные размеры прямоугольника вдоль осей X и Y. Нормаль направлена вдоль оси Z. + public var hs:Vector3 = new Vector3(); + // Флаг указывает, является примитив одно- или двусторонним + public var twoSided:Boolean = true; + + // Малое значение. Используется для указания фиктивной высоты примитива и в функции определения пересечения с сегментом. + private static const EPSILON:Number = 0.005; + + /** + * Создаёт новый экземпляр примитива. + * + * @param hs половинные размерв прямоугольника вдоль осей X и Y. Значение z игнорируется. + * @param collisionGroup группа примитива + */ + public function CollisionRect(hs:Vector3, collisionGroup:int) { + super(RECT, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * Расчитывает ограничивающий бокс прямоугольника. Для избежания проблем высота примитива принимается равной + * не нулю, а малому значению. + * + * @return + */ + override public function calculateAABB():BoundBox { + // Баунд бокс прямоугольника имеет минимальную высоту, отличную от нуля во избежание проблем с построением kd-дерева + var t:Matrix4 = transform; + var xx:Number = t.a < 0 ? -t.a : t.a; + var yy:Number = t.b < 0 ? -t.b : t.b; + var zz:Number = t.c < 0 ? -t.c : t.c; + aabb.maxX = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minX = -aabb.maxX; + + xx = t.e < 0 ? -t.e : t.e; + yy = t.f < 0 ? -t.f : t.f; + zz = t.g < 0 ? -t.g : t.g; + aabb.maxY = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minY = -aabb.maxY; + + xx = t.i < 0 ? -t.i : t.i; + yy = t.j < 0 ? -t.j : t.j; + zz = t.k < 0 ? -t.k : t.k; + aabb.maxZ = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var rect:CollisionRect = source as CollisionRect; + if (rect == null) return this; + super.copyFrom(rect); + hs.vCopy(rect.hs); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionRect hs=" + hs + "]"; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionRect(hs, collisionGroup); + } + + /** + * @param origin + * @param vector + * @param collisionGroup + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + + // Проверка параллельности сегмента и плоскости примитива + if (vz > -threshold && vz < threshold) return -1; + var t:Number = -oz/vz; + if (t < 0) return -1; + // Проверка вхождения точки пересечения в прямоугольник + ox += vx*t; + oy += vy*t; + oz = 0; + if (ox < (-hs.x - threshold) || ox > (hs.x + threshold) || oy < (-hs.y - threshold) || oy > (hs.y + threshold)) return -1; + + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + return t; + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base b/0.0.9.2/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base new file mode 100644 index 0000000..b912a5b --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base @@ -0,0 +1,81 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Vector3; + use namespace altphysics; + + + /** + * Сфера. + */ + public class CollisionSphere extends CollisionPrimitive { + + // Радиус сферы + public var r:Number = 0; + + /** + * + * @param r + * @param collisionGroup + */ + public function CollisionSphere(r:Number, collisionGroup:int) { + super(SPHERE, collisionGroup); + this.r = r; + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + aabb.maxX = transform.d + r; + aabb.minX = transform.d - r; + + aabb.maxY = transform.h + r; + aabb.minY = transform.h - r; + + aabb.maxZ = transform.l + r; + aabb.minZ = transform.l - r; + + return aabb; + } + + /** + * @param origin + * @param vector + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var px:Number = origin.x - transform.d; + var py:Number = origin.y - transform.h; + var pz:Number = origin.z - transform.l; + var k:Number = vector.x*px + vector.y*py + vector.z*pz; + if (k > 0) return -1; + var a:Number = vector.x*vector.x + vector.y*vector.y + vector.z*vector.z; + var D:Number = k*k - a*(px*px + py*py + pz*pz - r*r); + if (D < 0) return -1; + return -(k + Math.sqrt(D))/a; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var sphere:CollisionSphere = source as CollisionSphere; + if (sphere == null) return this; + super.copyFrom(sphere); + r = sphere.r; + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionSphere(r, collisionGroup); + } + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base b/0.0.9.2/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base new file mode 100644 index 0000000..bdc3166 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base @@ -0,0 +1,244 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * Примитив треугольник. + */ + public class CollisionTriangle extends CollisionPrimitive { + + public var v0:Vector3 = new Vector3(); + public var v1:Vector3 = new Vector3(); + public var v2:Vector3 = new Vector3(); + + public var e0:Vector3 = new Vector3(); + public var e1:Vector3 = new Vector3(); + public var e2:Vector3 = new Vector3(); + +// public var len0:Number; +// public var len1:Number; +// public var len2:Number; +// + /** + * + * @param v0 + * @param v1 + * @param v2 + * @param collisionGroup + */ + public function CollisionTriangle(v0:Vector3, v1:Vector3, v2:Vector3, collisionGroup:int) { + super(TRIANGLE, collisionGroup); + initVertices(v0, v1, v2); + } + + /** + * Рассчитывает AABB примитива. + * + * @return ссылка на свой AABB + */ + override public function calculateAABB():BoundBox { + var epsilon:Number = 0.005; + var a:Number; + var b:Number; + var eps_c:Number = epsilon*transform.c; + var eps_g:Number = epsilon*transform.g; + var eps_k:Number = epsilon*transform.k; + + // Вершина 0 + // Ось X + a = v0.x*transform.a + v0.y*transform.b; + aabb.minX = aabb.maxX = a + eps_c; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v0.x*transform.e + v0.y*transform.f; + aabb.minY = aabb.maxY = a + eps_g; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v0.x*transform.i + v0.y*transform.j; + aabb.minZ = aabb.maxZ = a + eps_k; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 1 + // Ось X + a = v1.x*transform.a + v1.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v1.x*transform.e + v1.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v1.x*transform.i + v1.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 2 + // Ось X + a = v2.x*transform.a + v2.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v2.x*transform.e + v2.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v2.x*transform.i + v2.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + aabb.minX += transform.d; + aabb.maxX += transform.d; + + aabb.minY += transform.h; + aabb.maxY += transform.h; + + aabb.minZ += transform.l; + aabb.maxZ += transform.l; + + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + // Луч трансформируется в систему координат примитива, затем проверяется пересечение + + var vz:Number = vector.x*transform.c + vector.y*transform.g + vector.z*transform.k; + // Если луч параллелен плоскости птимитива, то пересечения нет + if (vz < epsilon && vz > -epsilon) return -1; + + var tx:Number = origin.x - transform.d; + var ty:Number = origin.y - transform.h; + var tz:Number = origin.z - transform.l; + + var oz:Number = tx*transform.c + ty*transform.g + tz*transform.k; + var t:Number = -oz/vz; + if (t < 0) return -1; + + var ox:Number = tx*transform.a + ty*transform.e + tz*transform.i; + var oy:Number = tx*transform.b + ty*transform.f + tz*transform.j; + + tx = ox + t*(vector.x*transform.a + vector.y*transform.e + vector.z*transform.i); + ty = oy + t*(vector.x*transform.b + vector.y*transform.f + vector.z*transform.j); + tz = oz + t*vz; + + // Проверка вхождения точки в треугольник + if ((e0.x*(ty - v0.y) - e0.y*(tx - v0.x) < 0) || (e1.x*(ty - v1.y) - e1.y*(tx - v1.x) < 0) || (e2.x*(ty - v2.y) - e2.y*(tx - v2.x) < 0)) return -1; + + // Запись нормали + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + + return t; + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + super.copyFrom(source); + var tri:CollisionTriangle = source as CollisionTriangle; + if (tri != null) { + v0.vCopy(tri.v0); + v1.vCopy(tri.v1); + v2.vCopy(tri.v2); + + e0.vCopy(tri.e0); + e1.vCopy(tri.e1); + e2.vCopy(tri.e2); + +// len0 = tri.len0; +// len1 = tri.len1; +// len2 = tri.len2; + } + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionTriangle v0=" + v0 + ", v1=" + v1 + ", v2=" + v2 + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionTriangle(v0, v1, v2, collisionGroup); + } + + /** + * + * @param v0 + * @param v1 + * @param v2 + */ + private function initVertices(v0:Vector3, v1:Vector3, v2:Vector3):void { + this.v0.vCopy(v0); + this.v1.vCopy(v1); + this.v2.vCopy(v2); + + e0.vDiff(v1, v0); +// len0 = e0.vLength(); + e0.vNormalize(); + + e1.vDiff(v2, v1); +// len1 = e1.vLength(); + e1.vNormalize(); + + e2.vDiff(v0, v2); +// len2 = e2.vLength(); + e2.vNormalize(); + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/collision/primitives/CollisionBox.as b/0.0.9.2/src/alternativa/physics/collision/primitives/CollisionBox.as new file mode 100644 index 0000000..1febfb4 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/primitives/CollisionBox.as @@ -0,0 +1,204 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * Ориентированный бокс. + */ + public class CollisionBox extends CollisionPrimitive { + + // Половинные размеры вдоль каждой из осей + public var hs:Vector3 = new Vector3(); + + public var excludedFaces:int; + + /** + * @param hs + * @param collisionGroup + */ + public function CollisionBox(hs:Vector3, collisionGroup:int) { + super(BOX, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + var t:Matrix4 = transform; + + var xx:Number; + var yy:Number; + var zz:Number; + + xx = t.a < 0 ? -t.a : t.a; + yy = t.b < 0 ? -t.b : t.b; + zz = t.c < 0 ? -t.c : t.c; + aabb.maxX = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minX = -aabb.maxX; + + xx = t.e < 0 ? -t.e : t.e; + yy = t.f < 0 ? -t.f : t.f; + zz = t.g < 0 ? -t.g : t.g; + aabb.maxY = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minY = -aabb.maxY; + + xx = t.i < 0 ? -t.i : t.i; + yy = t.j < 0 ? -t.j : t.j; + zz = t.k < 0 ? -t.k : t.k; + aabb.maxZ = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var box:CollisionBox = source as CollisionBox; + if (box == null) return this; + super.copyFrom(box); + hs.vCopy(box.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionBox(hs, collisionGroup); + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + var tMin:Number = -1; + var tMax:Number = 1e308; + var t1:Number; + var t2:Number; + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + // X + if (vx < epsilon && vx > -epsilon) { + if (ox < -hs.x || ox > hs.x) return -1; + } else { + t1 = (-hs.x - ox)/vx; + t2 = (hs.x - ox)/vx; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.x = -1; + normal.y = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.x = 1; + normal.y = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Y + if (vy < epsilon && vy > -epsilon) { + if (oy < -hs.y || oy > hs.y) return -1; + } else { + t1 = (-hs.y - oy)/vy; + t2 = (hs.y - oy)/vy; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.y = -1; + normal.x = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.y = 1; + normal.x = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Z + if (vz < epsilon && vz > -epsilon) { + if (oz < -hs.z || oz > hs.z) return -1; + } else { + t1 = (-hs.z - oz)/vz; + t2 = (hs.z - oz)/vz; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.z = -1; + normal.x = normal.y = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.z = 1; + normal.x = normal.y = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Перевод нормали в мировую систему координат + // Inlined normal.vDeltaTransformBy4(transform); + vx = normal.x; + vy = normal.y; + vz = normal.z; + normal.x = transform.a*vx + transform.b*vy + transform.c*vz; + normal.y = transform.e*vx + transform.f*vy + transform.g*vz; + normal.z = transform.i*vx + transform.j*vy + transform.k*vz; + + return tMin; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionBox hs=" + hs + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/collision/primitives/CollisionRect.as b/0.0.9.2/src/alternativa/physics/collision/primitives/CollisionRect.as new file mode 100644 index 0000000..7a33b79 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/primitives/CollisionRect.as @@ -0,0 +1,143 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * Ориентированный прямоугольник. Задаётся половинами размеров вдоль осей X (ширина) и Y (длина) локальной системы + * координат примитива. Таким образом, прямоугольник лежит в плоскости XY, его стороны параллельны осям этой + * плоскости, а нормаль направлена вдоль локальной оси Z. + * + * Прямоугольник может быть одно- или двусторонним. В случае одностороннего прямоугольника, + * столкновения с внутренней стороны не регистрируются. + */ + public class CollisionRect extends CollisionPrimitive { + // Половинные размеры прямоугольника вдоль осей X и Y. Нормаль направлена вдоль оси Z. + public var hs:Vector3 = new Vector3(); + // Флаг указывает, является примитив одно- или двусторонним + public var twoSided:Boolean = true; + + // Малое значение. Используется для указания фиктивной высоты примитива и в функции определения пересечения с сегментом. + private static const EPSILON:Number = 0.005; + + /** + * Создаёт новый экземпляр примитива. + * + * @param hs половинные размерв прямоугольника вдоль осей X и Y. Значение z игнорируется. + * @param collisionGroup группа примитива + */ + public function CollisionRect(hs:Vector3, collisionGroup:int) { + super(RECT, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * Расчитывает ограничивающий бокс прямоугольника. Для избежания проблем высота примитива принимается равной + * не нулю, а малому значению. + * + * @return + */ + override public function calculateAABB():BoundBox { + // Баунд бокс прямоугольника имеет минимальную высоту, отличную от нуля во избежание проблем с построением kd-дерева + var t:Matrix4 = transform; + var xx:Number = t.a < 0 ? -t.a : t.a; + var yy:Number = t.b < 0 ? -t.b : t.b; + var zz:Number = t.c < 0 ? -t.c : t.c; + aabb.maxX = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minX = -aabb.maxX; + + xx = t.e < 0 ? -t.e : t.e; + yy = t.f < 0 ? -t.f : t.f; + zz = t.g < 0 ? -t.g : t.g; + aabb.maxY = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minY = -aabb.maxY; + + xx = t.i < 0 ? -t.i : t.i; + yy = t.j < 0 ? -t.j : t.j; + zz = t.k < 0 ? -t.k : t.k; + aabb.maxZ = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var rect:CollisionRect = source as CollisionRect; + if (rect == null) return this; + super.copyFrom(rect); + hs.vCopy(rect.hs); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionRect hs=" + hs + "]"; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionRect(hs, collisionGroup); + } + + /** + * @param origin + * @param vector + * @param collisionGroup + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + + // Проверка параллельности сегмента и плоскости примитива + if (vz > -threshold && vz < threshold) return -1; + var t:Number = -oz/vz; + if (t < 0) return -1; + // Проверка вхождения точки пересечения в прямоугольник + ox += vx*t; + oy += vy*t; + oz = 0; + if (ox < (-hs.x - threshold) || ox > (hs.x + threshold) || oy < (-hs.y - threshold) || oy > (hs.y + threshold)) return -1; + + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + return t; + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/collision/primitives/CollisionSphere.as b/0.0.9.2/src/alternativa/physics/collision/primitives/CollisionSphere.as new file mode 100644 index 0000000..b912a5b --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/primitives/CollisionSphere.as @@ -0,0 +1,81 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Vector3; + use namespace altphysics; + + + /** + * Сфера. + */ + public class CollisionSphere extends CollisionPrimitive { + + // Радиус сферы + public var r:Number = 0; + + /** + * + * @param r + * @param collisionGroup + */ + public function CollisionSphere(r:Number, collisionGroup:int) { + super(SPHERE, collisionGroup); + this.r = r; + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + aabb.maxX = transform.d + r; + aabb.minX = transform.d - r; + + aabb.maxY = transform.h + r; + aabb.minY = transform.h - r; + + aabb.maxZ = transform.l + r; + aabb.minZ = transform.l - r; + + return aabb; + } + + /** + * @param origin + * @param vector + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var px:Number = origin.x - transform.d; + var py:Number = origin.y - transform.h; + var pz:Number = origin.z - transform.l; + var k:Number = vector.x*px + vector.y*py + vector.z*pz; + if (k > 0) return -1; + var a:Number = vector.x*vector.x + vector.y*vector.y + vector.z*vector.z; + var D:Number = k*k - a*(px*px + py*py + pz*pz - r*r); + if (D < 0) return -1; + return -(k + Math.sqrt(D))/a; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var sphere:CollisionSphere = source as CollisionSphere; + if (sphere == null) return this; + super.copyFrom(sphere); + r = sphere.r; + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionSphere(r, collisionGroup); + } + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/collision/primitives/CollisionTriangle.as b/0.0.9.2/src/alternativa/physics/collision/primitives/CollisionTriangle.as new file mode 100644 index 0000000..bdc3166 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/primitives/CollisionTriangle.as @@ -0,0 +1,244 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * Примитив треугольник. + */ + public class CollisionTriangle extends CollisionPrimitive { + + public var v0:Vector3 = new Vector3(); + public var v1:Vector3 = new Vector3(); + public var v2:Vector3 = new Vector3(); + + public var e0:Vector3 = new Vector3(); + public var e1:Vector3 = new Vector3(); + public var e2:Vector3 = new Vector3(); + +// public var len0:Number; +// public var len1:Number; +// public var len2:Number; +// + /** + * + * @param v0 + * @param v1 + * @param v2 + * @param collisionGroup + */ + public function CollisionTriangle(v0:Vector3, v1:Vector3, v2:Vector3, collisionGroup:int) { + super(TRIANGLE, collisionGroup); + initVertices(v0, v1, v2); + } + + /** + * Рассчитывает AABB примитива. + * + * @return ссылка на свой AABB + */ + override public function calculateAABB():BoundBox { + var epsilon:Number = 0.005; + var a:Number; + var b:Number; + var eps_c:Number = epsilon*transform.c; + var eps_g:Number = epsilon*transform.g; + var eps_k:Number = epsilon*transform.k; + + // Вершина 0 + // Ось X + a = v0.x*transform.a + v0.y*transform.b; + aabb.minX = aabb.maxX = a + eps_c; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v0.x*transform.e + v0.y*transform.f; + aabb.minY = aabb.maxY = a + eps_g; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v0.x*transform.i + v0.y*transform.j; + aabb.minZ = aabb.maxZ = a + eps_k; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 1 + // Ось X + a = v1.x*transform.a + v1.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v1.x*transform.e + v1.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v1.x*transform.i + v1.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 2 + // Ось X + a = v2.x*transform.a + v2.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v2.x*transform.e + v2.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v2.x*transform.i + v2.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + aabb.minX += transform.d; + aabb.maxX += transform.d; + + aabb.minY += transform.h; + aabb.maxY += transform.h; + + aabb.minZ += transform.l; + aabb.maxZ += transform.l; + + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + // Луч трансформируется в систему координат примитива, затем проверяется пересечение + + var vz:Number = vector.x*transform.c + vector.y*transform.g + vector.z*transform.k; + // Если луч параллелен плоскости птимитива, то пересечения нет + if (vz < epsilon && vz > -epsilon) return -1; + + var tx:Number = origin.x - transform.d; + var ty:Number = origin.y - transform.h; + var tz:Number = origin.z - transform.l; + + var oz:Number = tx*transform.c + ty*transform.g + tz*transform.k; + var t:Number = -oz/vz; + if (t < 0) return -1; + + var ox:Number = tx*transform.a + ty*transform.e + tz*transform.i; + var oy:Number = tx*transform.b + ty*transform.f + tz*transform.j; + + tx = ox + t*(vector.x*transform.a + vector.y*transform.e + vector.z*transform.i); + ty = oy + t*(vector.x*transform.b + vector.y*transform.f + vector.z*transform.j); + tz = oz + t*vz; + + // Проверка вхождения точки в треугольник + if ((e0.x*(ty - v0.y) - e0.y*(tx - v0.x) < 0) || (e1.x*(ty - v1.y) - e1.y*(tx - v1.x) < 0) || (e2.x*(ty - v2.y) - e2.y*(tx - v2.x) < 0)) return -1; + + // Запись нормали + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + + return t; + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + super.copyFrom(source); + var tri:CollisionTriangle = source as CollisionTriangle; + if (tri != null) { + v0.vCopy(tri.v0); + v1.vCopy(tri.v1); + v2.vCopy(tri.v2); + + e0.vCopy(tri.e0); + e1.vCopy(tri.e1); + e2.vCopy(tri.e2); + +// len0 = tri.len0; +// len1 = tri.len1; +// len2 = tri.len2; + } + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionTriangle v0=" + v0 + ", v1=" + v1 + ", v2=" + v2 + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionTriangle(v0, v1, v2, collisionGroup); + } + + /** + * + * @param v0 + * @param v1 + * @param v2 + */ + private function initVertices(v0:Vector3, v1:Vector3, v2:Vector3):void { + this.v0.vCopy(v0); + this.v1.vCopy(v1); + this.v2.vCopy(v2); + + e0.vDiff(v1, v0); +// len0 = e0.vLength(); + e0.vNormalize(); + + e1.vDiff(v2, v1); +// len1 = e1.vLength(); + e1.vNormalize(); + + e2.vDiff(v0, v2); +// len2 = e2.vLength(); + e2.vNormalize(); + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/collision/types/.svn/all-wcprops b/0.0.9.2/src/alternativa/physics/collision/types/.svn/all-wcprops new file mode 100644 index 0000000..989eedd --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/types/.svn/all-wcprops @@ -0,0 +1,23 @@ +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/collision/types +END +BoundBox.as +K 25 +svn:wc:ra_dav:version-url +V 131 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/collision/types/BoundBox.as +END +RayIntersection.as +K 25 +svn:wc:ra_dav:version-url +V 138 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/collision/types/RayIntersection.as +END +Ray.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/collision/types/Ray.as +END diff --git a/0.0.9.2/src/alternativa/physics/collision/types/.svn/entries b/0.0.9.2/src/alternativa/physics/collision/types/.svn/entries new file mode 100644 index 0000000..c679162 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/types/.svn/entries @@ -0,0 +1,64 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/collision/types +http://svndev.alternativaplatform.com + + + +2009-11-24T08:44:15.941480Z +23758 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +BoundBox.as +file + + + + +2010-10-28T04:32:44.000000Z +d92d4ab3dde698feb674fb9488f59678 +2009-09-09T14:02:48.134744Z +19633 +mike + +RayIntersection.as +file + + + + +2010-10-28T04:32:44.000000Z +88c0836ead77b8eb0841f11159dfb836 +2009-11-24T08:44:15.941480Z +23758 +mike + +Ray.as +file + + + + +2010-10-28T04:32:44.000000Z +c65cc8bfae377f8f0520b5c79c8b3ab9 +2009-11-24T08:44:15.941480Z +23758 +mike + diff --git a/0.0.9.2/src/alternativa/physics/collision/types/.svn/format b/0.0.9.2/src/alternativa/physics/collision/types/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/types/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.2/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base b/0.0.9.2/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base new file mode 100644 index 0000000..b4d5278 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base @@ -0,0 +1,75 @@ +package alternativa.physics.collision.types { + + /** + * Ограничивающий параллелепипед. + */ + public class BoundBox { + + public var minX:Number = 1e308; + public var minY:Number = 1e308; + public var minZ:Number = 1e308; + public var maxX:Number = -1e308; + public var maxY:Number = -1e308; + public var maxZ:Number = -1e308; + + public function setSize(minX:Number, minY:Number, minZ:Number, maxX:Number, maxY:Number, maxZ:Number):void { + this.minX = minX; + this.minY = minY; + this.minZ = minZ; + this.maxX = maxX; + this.maxY = maxY; + this.maxZ = maxZ; + } + + public function addBoundBox(boundBox:BoundBox):void { + minX = (boundBox.minX < minX) ? boundBox.minX : minX; + minY = (boundBox.minY < minY) ? boundBox.minY : minY; + minZ = (boundBox.minZ < minZ) ? boundBox.minZ : minZ; + maxX = (boundBox.maxX > maxX) ? boundBox.maxX : maxX; + maxY = (boundBox.maxY > maxY) ? boundBox.maxY : maxY; + maxZ = (boundBox.maxZ > maxZ) ? boundBox.maxZ : maxZ; + } + + public function addPoint(x:Number, y:Number, z:Number):void { + if (x < minX) minX = x; + if (x > maxX) maxX = x; + if (y < minY) minY = y; + if (y > maxY) maxY = y; + if (z < minZ) minZ = z; + if (z > maxZ) maxZ = z; + } + + public function infinity():void { + minX = 1e308; + minY = 1e308; + minZ = 1e308; + maxX = -1e308; + maxY = -1e308; + maxZ = -1e308; + } + + public function intersects(bb:BoundBox, epsilon:Number):Boolean { + return !(minX > bb.maxX + epsilon || maxX < bb.minX - epsilon || minY > bb.maxY + epsilon || maxY < bb.minY - epsilon || minZ > bb.maxZ + epsilon || maxZ < bb.minZ - epsilon); + } + + public function copyFrom(boundBox:BoundBox):void { + minX = boundBox.minX; + minY = boundBox.minY; + minZ = boundBox.minZ; + maxX = boundBox.maxX; + maxY = boundBox.maxY; + maxZ = boundBox.maxZ; + } + + public function clone():BoundBox { + var clone:BoundBox = new BoundBox(); + clone.copyFrom(this); + return clone; + } + + public function toString():String { + return "BoundBox [" + minX + ", " + minY + ", " + minZ + " : " + maxX + ", " + maxY + ", " + maxZ + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base b/0.0.9.2/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base new file mode 100644 index 0000000..fb2ffbc --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base @@ -0,0 +1,13 @@ +package alternativa.physics.collision.types { + import alternativa.math.Vector3; + + /** + * Структура, описывающая луч и сегмент прямой в пространстве. + */ + public class Ray { + // Начало луча. Начальная точка сегмента на прямой луча. + public var origin:Vector3 = new Vector3(); + // Направляющий вектор луча. Длина вектора задаёт длину сегмента на луче. Конечная точка сегмента вычисляется как origin + dir. + public var dir:Vector3 = new Vector3(); + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base b/0.0.9.2/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base new file mode 100644 index 0000000..21e9290 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base @@ -0,0 +1,25 @@ +package alternativa.physics.collision.types { + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.math.Vector3; + + /** + * Структура описывает точку пересечения луча с физической геометрией. + */ + public class RayIntersection { + // Физический примитив, с которым пересекается луч + public var primitive:CollisionPrimitive; + // Положенеи точки пересечения + public var pos:Vector3 = new Vector3(); + // Нормаль поверхности физического примитива в точке пересечения + public var normal:Vector3 = new Vector3(); + // Время пересечения (|pos - ray.origin|/|ray.dir|) + public var t:Number = 0; + + public function copy(source:RayIntersection):void { + primitive = source.primitive; + pos.vCopy(source.pos); + normal.vCopy(source.normal); + t = source.t; + } + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/collision/types/BoundBox.as b/0.0.9.2/src/alternativa/physics/collision/types/BoundBox.as new file mode 100644 index 0000000..b4d5278 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/types/BoundBox.as @@ -0,0 +1,75 @@ +package alternativa.physics.collision.types { + + /** + * Ограничивающий параллелепипед. + */ + public class BoundBox { + + public var minX:Number = 1e308; + public var minY:Number = 1e308; + public var minZ:Number = 1e308; + public var maxX:Number = -1e308; + public var maxY:Number = -1e308; + public var maxZ:Number = -1e308; + + public function setSize(minX:Number, minY:Number, minZ:Number, maxX:Number, maxY:Number, maxZ:Number):void { + this.minX = minX; + this.minY = minY; + this.minZ = minZ; + this.maxX = maxX; + this.maxY = maxY; + this.maxZ = maxZ; + } + + public function addBoundBox(boundBox:BoundBox):void { + minX = (boundBox.minX < minX) ? boundBox.minX : minX; + minY = (boundBox.minY < minY) ? boundBox.minY : minY; + minZ = (boundBox.minZ < minZ) ? boundBox.minZ : minZ; + maxX = (boundBox.maxX > maxX) ? boundBox.maxX : maxX; + maxY = (boundBox.maxY > maxY) ? boundBox.maxY : maxY; + maxZ = (boundBox.maxZ > maxZ) ? boundBox.maxZ : maxZ; + } + + public function addPoint(x:Number, y:Number, z:Number):void { + if (x < minX) minX = x; + if (x > maxX) maxX = x; + if (y < minY) minY = y; + if (y > maxY) maxY = y; + if (z < minZ) minZ = z; + if (z > maxZ) maxZ = z; + } + + public function infinity():void { + minX = 1e308; + minY = 1e308; + minZ = 1e308; + maxX = -1e308; + maxY = -1e308; + maxZ = -1e308; + } + + public function intersects(bb:BoundBox, epsilon:Number):Boolean { + return !(minX > bb.maxX + epsilon || maxX < bb.minX - epsilon || minY > bb.maxY + epsilon || maxY < bb.minY - epsilon || minZ > bb.maxZ + epsilon || maxZ < bb.minZ - epsilon); + } + + public function copyFrom(boundBox:BoundBox):void { + minX = boundBox.minX; + minY = boundBox.minY; + minZ = boundBox.minZ; + maxX = boundBox.maxX; + maxY = boundBox.maxY; + maxZ = boundBox.maxZ; + } + + public function clone():BoundBox { + var clone:BoundBox = new BoundBox(); + clone.copyFrom(this); + return clone; + } + + public function toString():String { + return "BoundBox [" + minX + ", " + minY + ", " + minZ + " : " + maxX + ", " + maxY + ", " + maxZ + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/collision/types/Ray.as b/0.0.9.2/src/alternativa/physics/collision/types/Ray.as new file mode 100644 index 0000000..fb2ffbc --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/types/Ray.as @@ -0,0 +1,13 @@ +package alternativa.physics.collision.types { + import alternativa.math.Vector3; + + /** + * Структура, описывающая луч и сегмент прямой в пространстве. + */ + public class Ray { + // Начало луча. Начальная точка сегмента на прямой луча. + public var origin:Vector3 = new Vector3(); + // Направляющий вектор луча. Длина вектора задаёт длину сегмента на луче. Конечная точка сегмента вычисляется как origin + dir. + public var dir:Vector3 = new Vector3(); + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/collision/types/RayIntersection.as b/0.0.9.2/src/alternativa/physics/collision/types/RayIntersection.as new file mode 100644 index 0000000..21e9290 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/collision/types/RayIntersection.as @@ -0,0 +1,25 @@ +package alternativa.physics.collision.types { + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.math.Vector3; + + /** + * Структура описывает точку пересечения луча с физической геометрией. + */ + public class RayIntersection { + // Физический примитив, с которым пересекается луч + public var primitive:CollisionPrimitive; + // Положенеи точки пересечения + public var pos:Vector3 = new Vector3(); + // Нормаль поверхности физического примитива в точке пересечения + public var normal:Vector3 = new Vector3(); + // Время пересечения (|pos - ray.origin|/|ray.dir|) + public var t:Number = 0; + + public function copy(source:RayIntersection):void { + primitive = source.primitive; + pos.vCopy(source.pos); + normal.vCopy(source.normal); + t = source.t; + } + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/constraints/.svn/all-wcprops b/0.0.9.2/src/alternativa/physics/constraints/.svn/all-wcprops new file mode 100644 index 0000000..7c74a9d --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/constraints/.svn/all-wcprops @@ -0,0 +1,17 @@ +K 25 +svn:wc:ra_dav:version-url +V 115 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/constraints +END +Constraint.as +K 25 +svn:wc:ra_dav:version-url +V 129 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/constraints/Constraint.as +END +MaxDistanceConstraint.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/constraints/MaxDistanceConstraint.as +END diff --git a/0.0.9.2/src/alternativa/physics/constraints/.svn/entries b/0.0.9.2/src/alternativa/physics/constraints/.svn/entries new file mode 100644 index 0000000..64a0bf4 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/constraints/.svn/entries @@ -0,0 +1,52 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/constraints +http://svndev.alternativaplatform.com + + + +2010-01-20T17:53:18.555251Z +26849 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Constraint.as +file + + + + +2010-10-28T04:32:44.000000Z +02bdbd40eda515549956d6393551a63b +2010-01-20T17:53:18.555251Z +26849 +mike + +MaxDistanceConstraint.as +file + + + + +2010-10-28T04:32:44.000000Z +dbf792986b9dd7e2a1c720cf4a578f42 +2009-11-24T08:44:15.941480Z +23758 +mike + diff --git a/0.0.9.2/src/alternativa/physics/constraints/.svn/format b/0.0.9.2/src/alternativa/physics/constraints/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/constraints/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.2/src/alternativa/physics/constraints/.svn/text-base/Constraint.as.svn-base b/0.0.9.2/src/alternativa/physics/constraints/.svn/text-base/Constraint.as.svn-base new file mode 100644 index 0000000..8607442 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/constraints/.svn/text-base/Constraint.as.svn-base @@ -0,0 +1,35 @@ +package alternativa.physics.constraints { + + import alternativa.physics.PhysicsScene; + import alternativa.physics.altphysics; + + use namespace altphysics; + + /** + * + */ + public class Constraint { + + altphysics var satisfied:Boolean; + altphysics var world:PhysicsScene; + + /** + * + */ + public function Constraint() { + } + + /** + * @param dt + */ + public function preProcess(dt:Number):void { + } + + /** + * @param dt + */ + public function apply(dt:Number):void { + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base b/0.0.9.2/src/alternativa/physics/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base new file mode 100644 index 0000000..d29f518 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base @@ -0,0 +1,183 @@ +package alternativa.physics.constraints { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * + */ + public class MaxDistanceConstraint extends Constraint { + // Первое тело ограничения + altphysics var body1:Body; + // Второе тело ограничения. Может быть null. + altphysics var body2:Body; + // Первая точка крепления в системе координат первого тела + altphysics var r1:Vector3; + // Вторая точка привязки в системе координат второго тела. В случае отсутствия второго тела, координаты задаются в мировой системе. + altphysics var r2:Vector3; + // Максимально допустимое расстояние между точками крепления + altphysics var maxDistance:Number; + // Радиус-вектор первой точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr1:Vector3 = new Vector3(); + // Радиус-вектор второй точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr2:Vector3 = new Vector3(); + // Минимальная скорость сближения для удовлетворения ограничения + private var minClosingVel:Number; + // Изменение относительной скорости на единицу импульса + private var velByUnitImpulseN:Number; + // Направление корректирующего импульса + private var impulseDirection:Vector3 = new Vector3(); + + // Вспомогательные переменные +// private static var _v:Vector3 = new Vector3(); +// private static var _v1:Vector3 = new Vector3(); +// private static var _v2:Vector3 = new Vector3(); + + /** + * @param body1 + * @param body2 + * @param r1 + * @param r2 + * @param maxDistance + */ + public function MaxDistanceConstraint(body1:Body, body2:Body, r1:Vector3, r2:Vector3, maxDistance:Number) { + super(); + this.body1 = body1; + this.body2 = body2; + this.r1 = r1.vClone(); + this.r2 = r2.vClone(); + this.maxDistance = maxDistance; + } + + /** + * @param dt + */ + override public function preProcess(dt:Number):void { + // Вычислим расстояние между точками + var m:Matrix3 = body1.baseMatrix; + wr1.x = m.a*r1.x + m.b*r1.y + m.c*r1.z; + wr1.y = m.e*r1.x + m.f*r1.y + m.g*r1.z; + wr1.z = m.i*r1.x + m.j*r1.y + m.k*r1.z; + if (body2 != null) { + m = body2.baseMatrix; + wr2.x = m.a*r2.x + m.b*r2.y + m.c*r2.z; + wr2.y = m.e*r2.x + m.f*r2.y + m.g*r2.z; + wr2.z = m.i*r2.x + m.j*r2.y + m.k*r2.z; + } else { + wr2.x = r2.x; + wr2.y = r2.y; + wr2.z = r2.z; + } + var p1:Vector3 = body1.state.pos; + impulseDirection.x = wr2.x - wr1.x - p1.x; + impulseDirection.y = wr2.y - wr1.y - p1.y; + impulseDirection.z = wr2.z - wr1.z - p1.z; + if (body2 != null) { + var p2:Vector3 = body2.state.pos; + impulseDirection.x += p2.x; + impulseDirection.y += p2.y; + impulseDirection.z += p2.z; + } + + var len:Number = Math.sqrt(impulseDirection.x*impulseDirection.x + impulseDirection.y*impulseDirection.y + impulseDirection.z*impulseDirection.z); + var delta:Number = len - maxDistance; + if (delta > 0) { + satisfied = false; + if (len < 0.001) { + impulseDirection.x = 1; + } else { + len = 1/len; + impulseDirection.x *= len; + impulseDirection.y *= len; + impulseDirection.z *= len; + } + minClosingVel = delta/(world.penResolutionSteps*dt); + if (minClosingVel > world.maxPenResolutionSpeed) { + minClosingVel = world.maxPenResolutionSpeed; + } + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + var x:Number; + var y:Number; + var z:Number; + var vx:Number; + var vy:Number; + var vz:Number; + velByUnitImpulseN = 0; + if (body1.movable) { +// velByUnitImpulseN += body1.invMass + _v.vCross2(wr1, impulseDirection).vTransformBy3(body1.invInertiaWorld).vCross(wr1).vDot(impulseDirection); + vx = wr1.y*impulseDirection.z - wr1.z*impulseDirection.y; + vy = wr1.z*impulseDirection.x - wr1.x*impulseDirection.z; + vz = wr1.x*impulseDirection.y - wr1.y*impulseDirection.x; + + m = body1.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr1.z - z*wr1.y; + vy = z*wr1.x - x*wr1.z; + vz = x*wr1.y - y*wr1.x; + velByUnitImpulseN += body1.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + if (body2 != null && body2.movable) { +// velByUnitImpulseN += body2.invMass + _v.vCross2(wr2, impulseDirection).vTransformBy3(body2.invInertiaWorld).vCross(wr2).vDot(impulseDirection); + vx = wr2.y*impulseDirection.z - wr2.z*impulseDirection.y; + vy = wr2.z*impulseDirection.x - wr2.x*impulseDirection.z; + vz = wr2.x*impulseDirection.y - wr2.y*impulseDirection.x; + + m = body2.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr2.z - z*wr2.y; + vy = z*wr2.x - x*wr2.z; + vz = x*wr2.y - y*wr2.x; + velByUnitImpulseN += body2.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + } else { + satisfied = true; + } + } + + /** + * @param dt + */ + override public function apply(dt:Number):void { + if (satisfied) return; + // Расчитываем проекцию скорости относительной скорости + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 + var vel:Vector3 = body1.state.velocity; + var rot:Vector3 = body1.state.rotation; +// v1Cross2(state.rotation, wr1); + var vx:Number = vel.x + rot.y*wr1.z - rot.z*wr1.y; + var vy:Number = vel.y + rot.z*wr1.x - rot.x*wr1.z; + var vz:Number = vel.z + rot.x*wr1.y - rot.y*wr1.x; + // V2 = V2_c + w2%r2 + if (body2 != null) { + vel = body2.state.velocity; + rot = body2.state.rotation; +// _v2.vCross2(state.rotation, wr2); + vx -= vel.x + rot.y*wr2.z - rot.z*wr2.y; + vy -= vel.y + rot.z*wr2.x - rot.x*wr2.z; + vz -= vel.z + rot.x*wr2.y - rot.y*wr2.x; + } + var closingVel:Number = vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + if (closingVel > minClosingVel) return; + var impulse:Number = (minClosingVel - closingVel)/velByUnitImpulseN; + if (body1.movable) { + body1.applyRelPosWorldImpulse(wr1, impulseDirection, impulse); + } + if (body2 != null && body2.movable) { + body2.applyRelPosWorldImpulse(wr2, impulseDirection, -impulse); + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/constraints/Constraint.as b/0.0.9.2/src/alternativa/physics/constraints/Constraint.as new file mode 100644 index 0000000..8607442 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/constraints/Constraint.as @@ -0,0 +1,35 @@ +package alternativa.physics.constraints { + + import alternativa.physics.PhysicsScene; + import alternativa.physics.altphysics; + + use namespace altphysics; + + /** + * + */ + public class Constraint { + + altphysics var satisfied:Boolean; + altphysics var world:PhysicsScene; + + /** + * + */ + public function Constraint() { + } + + /** + * @param dt + */ + public function preProcess(dt:Number):void { + } + + /** + * @param dt + */ + public function apply(dt:Number):void { + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/constraints/MaxDistanceConstraint.as b/0.0.9.2/src/alternativa/physics/constraints/MaxDistanceConstraint.as new file mode 100644 index 0000000..d29f518 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/constraints/MaxDistanceConstraint.as @@ -0,0 +1,183 @@ +package alternativa.physics.constraints { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * + */ + public class MaxDistanceConstraint extends Constraint { + // Первое тело ограничения + altphysics var body1:Body; + // Второе тело ограничения. Может быть null. + altphysics var body2:Body; + // Первая точка крепления в системе координат первого тела + altphysics var r1:Vector3; + // Вторая точка привязки в системе координат второго тела. В случае отсутствия второго тела, координаты задаются в мировой системе. + altphysics var r2:Vector3; + // Максимально допустимое расстояние между точками крепления + altphysics var maxDistance:Number; + // Радиус-вектор первой точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr1:Vector3 = new Vector3(); + // Радиус-вектор второй точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr2:Vector3 = new Vector3(); + // Минимальная скорость сближения для удовлетворения ограничения + private var minClosingVel:Number; + // Изменение относительной скорости на единицу импульса + private var velByUnitImpulseN:Number; + // Направление корректирующего импульса + private var impulseDirection:Vector3 = new Vector3(); + + // Вспомогательные переменные +// private static var _v:Vector3 = new Vector3(); +// private static var _v1:Vector3 = new Vector3(); +// private static var _v2:Vector3 = new Vector3(); + + /** + * @param body1 + * @param body2 + * @param r1 + * @param r2 + * @param maxDistance + */ + public function MaxDistanceConstraint(body1:Body, body2:Body, r1:Vector3, r2:Vector3, maxDistance:Number) { + super(); + this.body1 = body1; + this.body2 = body2; + this.r1 = r1.vClone(); + this.r2 = r2.vClone(); + this.maxDistance = maxDistance; + } + + /** + * @param dt + */ + override public function preProcess(dt:Number):void { + // Вычислим расстояние между точками + var m:Matrix3 = body1.baseMatrix; + wr1.x = m.a*r1.x + m.b*r1.y + m.c*r1.z; + wr1.y = m.e*r1.x + m.f*r1.y + m.g*r1.z; + wr1.z = m.i*r1.x + m.j*r1.y + m.k*r1.z; + if (body2 != null) { + m = body2.baseMatrix; + wr2.x = m.a*r2.x + m.b*r2.y + m.c*r2.z; + wr2.y = m.e*r2.x + m.f*r2.y + m.g*r2.z; + wr2.z = m.i*r2.x + m.j*r2.y + m.k*r2.z; + } else { + wr2.x = r2.x; + wr2.y = r2.y; + wr2.z = r2.z; + } + var p1:Vector3 = body1.state.pos; + impulseDirection.x = wr2.x - wr1.x - p1.x; + impulseDirection.y = wr2.y - wr1.y - p1.y; + impulseDirection.z = wr2.z - wr1.z - p1.z; + if (body2 != null) { + var p2:Vector3 = body2.state.pos; + impulseDirection.x += p2.x; + impulseDirection.y += p2.y; + impulseDirection.z += p2.z; + } + + var len:Number = Math.sqrt(impulseDirection.x*impulseDirection.x + impulseDirection.y*impulseDirection.y + impulseDirection.z*impulseDirection.z); + var delta:Number = len - maxDistance; + if (delta > 0) { + satisfied = false; + if (len < 0.001) { + impulseDirection.x = 1; + } else { + len = 1/len; + impulseDirection.x *= len; + impulseDirection.y *= len; + impulseDirection.z *= len; + } + minClosingVel = delta/(world.penResolutionSteps*dt); + if (minClosingVel > world.maxPenResolutionSpeed) { + minClosingVel = world.maxPenResolutionSpeed; + } + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + var x:Number; + var y:Number; + var z:Number; + var vx:Number; + var vy:Number; + var vz:Number; + velByUnitImpulseN = 0; + if (body1.movable) { +// velByUnitImpulseN += body1.invMass + _v.vCross2(wr1, impulseDirection).vTransformBy3(body1.invInertiaWorld).vCross(wr1).vDot(impulseDirection); + vx = wr1.y*impulseDirection.z - wr1.z*impulseDirection.y; + vy = wr1.z*impulseDirection.x - wr1.x*impulseDirection.z; + vz = wr1.x*impulseDirection.y - wr1.y*impulseDirection.x; + + m = body1.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr1.z - z*wr1.y; + vy = z*wr1.x - x*wr1.z; + vz = x*wr1.y - y*wr1.x; + velByUnitImpulseN += body1.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + if (body2 != null && body2.movable) { +// velByUnitImpulseN += body2.invMass + _v.vCross2(wr2, impulseDirection).vTransformBy3(body2.invInertiaWorld).vCross(wr2).vDot(impulseDirection); + vx = wr2.y*impulseDirection.z - wr2.z*impulseDirection.y; + vy = wr2.z*impulseDirection.x - wr2.x*impulseDirection.z; + vz = wr2.x*impulseDirection.y - wr2.y*impulseDirection.x; + + m = body2.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr2.z - z*wr2.y; + vy = z*wr2.x - x*wr2.z; + vz = x*wr2.y - y*wr2.x; + velByUnitImpulseN += body2.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + } else { + satisfied = true; + } + } + + /** + * @param dt + */ + override public function apply(dt:Number):void { + if (satisfied) return; + // Расчитываем проекцию скорости относительной скорости + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 + var vel:Vector3 = body1.state.velocity; + var rot:Vector3 = body1.state.rotation; +// v1Cross2(state.rotation, wr1); + var vx:Number = vel.x + rot.y*wr1.z - rot.z*wr1.y; + var vy:Number = vel.y + rot.z*wr1.x - rot.x*wr1.z; + var vz:Number = vel.z + rot.x*wr1.y - rot.y*wr1.x; + // V2 = V2_c + w2%r2 + if (body2 != null) { + vel = body2.state.velocity; + rot = body2.state.rotation; +// _v2.vCross2(state.rotation, wr2); + vx -= vel.x + rot.y*wr2.z - rot.z*wr2.y; + vy -= vel.y + rot.z*wr2.x - rot.x*wr2.z; + vz -= vel.z + rot.x*wr2.y - rot.y*wr2.x; + } + var closingVel:Number = vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + if (closingVel > minClosingVel) return; + var impulse:Number = (minClosingVel - closingVel)/velByUnitImpulseN; + if (body1.movable) { + body1.applyRelPosWorldImpulse(wr1, impulseDirection, impulse); + } + if (body2 != null && body2.movable) { + body2.applyRelPosWorldImpulse(wr2, impulseDirection, -impulse); + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/primitives/.svn/all-wcprops b/0.0.9.2/src/alternativa/physics/primitives/.svn/all-wcprops new file mode 100644 index 0000000..1951915 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/primitives/.svn/all-wcprops @@ -0,0 +1,35 @@ +K 25 +svn:wc:ra_dav:version-url +V 114 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/primitives +END +RigidPlane.as +K 25 +svn:wc:ra_dav:version-url +V 128 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/primitives/RigidPlane.as +END +RigidSphere.as +K 25 +svn:wc:ra_dav:version-url +V 129 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/primitives/RigidSphere.as +END +RigidBox.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/primitives/RigidBox.as +END +RigidCylinder.as +K 25 +svn:wc:ra_dav:version-url +V 131 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/primitives/RigidCylinder.as +END +RigidRect.as +K 25 +svn:wc:ra_dav:version-url +V 127 +/!svn/ver/29411/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/primitives/RigidRect.as +END diff --git a/0.0.9.2/src/alternativa/physics/primitives/.svn/entries b/0.0.9.2/src/alternativa/physics/primitives/.svn/entries new file mode 100644 index 0000000..647bb11 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/primitives/.svn/entries @@ -0,0 +1,88 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.2/src/alternativa/physics/primitives +http://svndev.alternativaplatform.com + + + +2009-11-24T08:44:15.941480Z +23758 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +RigidPlane.as +file + + + + +2010-10-28T04:32:44.000000Z +8faf607cf544d7388852668f5cd5672f +2009-11-24T08:44:15.941480Z +23758 +mike + +RigidSphere.as +file + + + + +2010-10-28T04:32:44.000000Z +774b0c23b084bfffb3381eb40e616deb +2009-11-24T08:44:15.941480Z +23758 +mike + +RigidBox.as +file + + + + +2010-10-28T04:32:44.000000Z +7b93f349a2818f22b76d9590cb05f84a +2009-11-24T08:44:15.941480Z +23758 +mike + +RigidCylinder.as +file + + + + +2010-10-28T04:32:44.000000Z +e9ce0eeb457428727cc10d2e53a0f164 +2009-11-24T08:44:15.941480Z +23758 +mike + +RigidRect.as +file + + + + +2010-10-28T04:32:44.000000Z +2eb6d0ebde8c2b46c4b0f810b1d64e1e +2009-11-24T08:44:15.941480Z +23758 +mike + diff --git a/0.0.9.2/src/alternativa/physics/primitives/.svn/format b/0.0.9.2/src/alternativa/physics/primitives/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/primitives/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.2/src/alternativa/physics/primitives/.svn/text-base/RigidBox.as.svn-base b/0.0.9.2/src/alternativa/physics/primitives/.svn/text-base/RigidBox.as.svn-base new file mode 100644 index 0000000..24e9b9f --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/primitives/.svn/text-base/RigidBox.as.svn-base @@ -0,0 +1,56 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * + */ + public class RigidBox extends Body { + + /** + * + * @param halfSize + * @param mass + */ + public function RigidBox(halfSize:Vector3, mass:Number) { + super(0, Matrix3.ZERO); + addCollisionPrimitive(new CollisionBox(halfSize, 1)); + setParams(halfSize, mass); + } + + /** + * @param halfSize + * @param mass + */ + public function setParams(halfSize:Vector3, mass:Number):void { + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + invInertia.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + invInertia.a = 3*invMass/(yy + zz); + invInertia.f = 3*invMass/(zz + xx); + invInertia.k = 3*invMass/(xx + yy); + } + var prim:CollisionBox = CollisionBox(collisionPrimitives.head.primitive); + prim.hs.vCopy(halfSize); + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/primitives/.svn/text-base/RigidCylinder.as.svn-base b/0.0.9.2/src/alternativa/physics/primitives/.svn/text-base/RigidCylinder.as.svn-base new file mode 100644 index 0000000..4ff65dd --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/primitives/.svn/text-base/RigidCylinder.as.svn-base @@ -0,0 +1,32 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.math.Matrix3; + + use namespace altphysics; + + public class RigidCylinder extends Body { + + altphysics var r:Number; + altphysics var h:Number; + + public function RigidCylinder(mass:Number, radius:Number, height:Number) { + super(0, Matrix3.ZERO); + setParams(mass, radius, height); + } + + public function setParams(mass:Number, radius:Number, height:Number):void { + r = radius; + h = height; + invInertiaWorld.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + invInertia.a = invInertia.f = mass*(h*h/12 + r*r/4); + invInertia.k = 0.5*mass*r*r; + invInertia.invert(); + } + } + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/primitives/.svn/text-base/RigidPlane.as.svn-base b/0.0.9.2/src/alternativa/physics/primitives/.svn/text-base/RigidPlane.as.svn-base new file mode 100644 index 0000000..d9ebf9b --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/primitives/.svn/text-base/RigidPlane.as.svn-base @@ -0,0 +1,23 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + public class RigidPlane extends Body { + + public var normal:Vector3 = new Vector3(); + public var offset:Number; + + public function RigidPlane() { + super(0, Matrix3.ZERO); + this.normal.vCopy(Vector3.Z_AXIS); + this.offset = 0; + movable = false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/primitives/.svn/text-base/RigidRect.as.svn-base b/0.0.9.2/src/alternativa/physics/primitives/.svn/text-base/RigidRect.as.svn-base new file mode 100644 index 0000000..4591931 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/primitives/.svn/text-base/RigidRect.as.svn-base @@ -0,0 +1,20 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + public class RigidRect extends Body { + + public function RigidRect(halfWidth:Number, halfLength:Number) { + super(0, Matrix3.ZERO); + movable = false; + addCollisionPrimitive(new CollisionRect(new Vector3(halfWidth, halfLength, 0), 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/primitives/.svn/text-base/RigidSphere.as.svn-base b/0.0.9.2/src/alternativa/physics/primitives/.svn/text-base/RigidSphere.as.svn-base new file mode 100644 index 0000000..8ad180c --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/primitives/.svn/text-base/RigidSphere.as.svn-base @@ -0,0 +1,29 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.math.Matrix3; + + use namespace altphysics; + + public class RigidSphere extends Body { + + public function RigidSphere(radius:Number, mass:Number) { + var m:Matrix3; + if (mass == Infinity) { + mass = 0; + m = Matrix3.ZERO; + } else { + mass = 1/mass; + var r2:Number = radius*radius; + m = new Matrix3(); + m.a = m.f = m.k = 2.5*mass/r2; + } + super(mass, m); + if (mass == 0) movable = false; + addCollisionPrimitive(new CollisionSphere(radius, 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/primitives/RigidBox.as b/0.0.9.2/src/alternativa/physics/primitives/RigidBox.as new file mode 100644 index 0000000..24e9b9f --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/primitives/RigidBox.as @@ -0,0 +1,56 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * + */ + public class RigidBox extends Body { + + /** + * + * @param halfSize + * @param mass + */ + public function RigidBox(halfSize:Vector3, mass:Number) { + super(0, Matrix3.ZERO); + addCollisionPrimitive(new CollisionBox(halfSize, 1)); + setParams(halfSize, mass); + } + + /** + * @param halfSize + * @param mass + */ + public function setParams(halfSize:Vector3, mass:Number):void { + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + invInertia.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + invInertia.a = 3*invMass/(yy + zz); + invInertia.f = 3*invMass/(zz + xx); + invInertia.k = 3*invMass/(xx + yy); + } + var prim:CollisionBox = CollisionBox(collisionPrimitives.head.primitive); + prim.hs.vCopy(halfSize); + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/primitives/RigidCylinder.as b/0.0.9.2/src/alternativa/physics/primitives/RigidCylinder.as new file mode 100644 index 0000000..4ff65dd --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/primitives/RigidCylinder.as @@ -0,0 +1,32 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.math.Matrix3; + + use namespace altphysics; + + public class RigidCylinder extends Body { + + altphysics var r:Number; + altphysics var h:Number; + + public function RigidCylinder(mass:Number, radius:Number, height:Number) { + super(0, Matrix3.ZERO); + setParams(mass, radius, height); + } + + public function setParams(mass:Number, radius:Number, height:Number):void { + r = radius; + h = height; + invInertiaWorld.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + invInertia.a = invInertia.f = mass*(h*h/12 + r*r/4); + invInertia.k = 0.5*mass*r*r; + invInertia.invert(); + } + } + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/primitives/RigidPlane.as b/0.0.9.2/src/alternativa/physics/primitives/RigidPlane.as new file mode 100644 index 0000000..d9ebf9b --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/primitives/RigidPlane.as @@ -0,0 +1,23 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + public class RigidPlane extends Body { + + public var normal:Vector3 = new Vector3(); + public var offset:Number; + + public function RigidPlane() { + super(0, Matrix3.ZERO); + this.normal.vCopy(Vector3.Z_AXIS); + this.offset = 0; + movable = false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/primitives/RigidRect.as b/0.0.9.2/src/alternativa/physics/primitives/RigidRect.as new file mode 100644 index 0000000..4591931 --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/primitives/RigidRect.as @@ -0,0 +1,20 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + public class RigidRect extends Body { + + public function RigidRect(halfWidth:Number, halfLength:Number) { + super(0, Matrix3.ZERO); + movable = false; + addCollisionPrimitive(new CollisionRect(new Vector3(halfWidth, halfLength, 0), 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.9.2/src/alternativa/physics/primitives/RigidSphere.as b/0.0.9.2/src/alternativa/physics/primitives/RigidSphere.as new file mode 100644 index 0000000..8ad180c --- /dev/null +++ b/0.0.9.2/src/alternativa/physics/primitives/RigidSphere.as @@ -0,0 +1,29 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.math.Matrix3; + + use namespace altphysics; + + public class RigidSphere extends Body { + + public function RigidSphere(radius:Number, mass:Number) { + var m:Matrix3; + if (mass == Infinity) { + mass = 0; + m = Matrix3.ZERO; + } else { + mass = 1/mass; + var r2:Number = radius*radius; + m = new Matrix3(); + m.a = m.f = m.k = 2.5*mass/r2; + } + super(mass, m); + if (mass == 0) movable = false; + addCollisionPrimitive(new CollisionSphere(radius, 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/.actionScriptProperties b/0.0.9.3/.actionScriptProperties new file mode 100644 index 0000000..f73adc8 --- /dev/null +++ b/0.0.9.3/.actionScriptProperties @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/0.0.9.3/.flexLibProperties b/0.0.9.3/.flexLibProperties new file mode 100644 index 0000000..0b65396 --- /dev/null +++ b/0.0.9.3/.flexLibProperties @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/0.0.9.3/.project b/0.0.9.3/.project new file mode 100644 index 0000000..1f1425f --- /dev/null +++ b/0.0.9.3/.project @@ -0,0 +1,19 @@ + + + AlternativaPhysics + + + + + + com.adobe.flexbuilder.project.flexbuilder + + + + + + org.maven.ide.eclipse.maven2Nature + com.adobe.flexbuilder.project.flexlibnature + com.adobe.flexbuilder.project.actionscriptnature + + diff --git a/0.0.9.3/.settings/.svn/all-wcprops b/0.0.9.3/.settings/.svn/all-wcprops new file mode 100644 index 0000000..3702089 --- /dev/null +++ b/0.0.9.3/.settings/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 89 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/.settings +END +org.eclipse.core.resources.prefs +K 25 +svn:wc:ra_dav:version-url +V 122 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/.settings/org.eclipse.core.resources.prefs +END diff --git a/0.0.9.3/.settings/.svn/entries b/0.0.9.3/.settings/.svn/entries new file mode 100644 index 0000000..025aa84 --- /dev/null +++ b/0.0.9.3/.settings/.svn/entries @@ -0,0 +1,40 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/.settings +http://svndev.alternativaplatform.com + + + +2009-04-01T12:25:29.638016Z +10301 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +org.eclipse.core.resources.prefs +file + + + + +2010-10-28T04:32:44.000000Z +4f70b476f3e5075e399505021df2f89b +2009-04-01T12:25:29.638016Z +10301 +mike + diff --git a/0.0.9.3/.settings/.svn/format b/0.0.9.3/.settings/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.3/.settings/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.3/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base b/0.0.9.3/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base new file mode 100644 index 0000000..b3d0d49 --- /dev/null +++ b/0.0.9.3/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base @@ -0,0 +1,3 @@ +#Fri Mar 20 00:19:33 YEKT 2009 +eclipse.preferences.version=1 +encoding/=UTF-8 diff --git a/0.0.9.3/.settings/org.eclipse.core.resources.prefs b/0.0.9.3/.settings/org.eclipse.core.resources.prefs new file mode 100644 index 0000000..b3d0d49 --- /dev/null +++ b/0.0.9.3/.settings/org.eclipse.core.resources.prefs @@ -0,0 +1,3 @@ +#Fri Mar 20 00:19:33 YEKT 2009 +eclipse.preferences.version=1 +encoding/=UTF-8 diff --git a/0.0.9.3/.svn/all-wcprops b/0.0.9.3/.svn/all-wcprops new file mode 100644 index 0000000..d6a5bc9 --- /dev/null +++ b/0.0.9.3/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 79 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3 +END +.flexLibProperties +K 25 +svn:wc:ra_dav:version-url +V 98 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/.flexLibProperties +END +.project +K 25 +svn:wc:ra_dav:version-url +V 88 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/.project +END +pom.xml +K 25 +svn:wc:ra_dav:version-url +V 87 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/pom.xml +END +.actionScriptProperties +K 25 +svn:wc:ra_dav:version-url +V 103 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/.actionScriptProperties +END diff --git a/0.0.9.3/.svn/dir-prop-base b/0.0.9.3/.svn/dir-prop-base new file mode 100644 index 0000000..c31ca28 --- /dev/null +++ b/0.0.9.3/.svn/dir-prop-base @@ -0,0 +1,7 @@ +K 13 +svn:mergeinfo +V 278 +/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized:19806-19860 +/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.7.0.LinkedList:20360-22217,22226-22252 +/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D:13892-14098 +END diff --git a/0.0.9.3/.svn/entries b/0.0.9.3/.svn/entries new file mode 100644 index 0000000..7bdc234 --- /dev/null +++ b/0.0.9.3/.svn/entries @@ -0,0 +1,85 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3 +http://svndev.alternativaplatform.com + + + +2010-07-06T08:27:43.797261Z +36823 +mike +has-props + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +META-INF +dir + +.flexLibProperties +file + + + + +2010-10-28T04:32:44.000000Z +b170dccb732dcb12462ac421cdca9723 +2010-02-12T12:34:25.192477Z +28424 +mike + +.project +file + + + + +2010-10-28T04:32:44.000000Z +1e90cc68b52b93173b2b5de88eb5c3a3 +2009-04-20T17:15:55.260110Z +11541 +mike + +src +dir + +pom.xml +file + + + + +2010-10-28T04:32:44.000000Z +3b890e83e31ba0e283d8cfbca016053a +2010-07-06T08:27:43.797261Z +36823 +mike + +.actionScriptProperties +file + + + + +2010-10-28T04:32:44.000000Z +b8656f882fff2baedd57a92db86fcf8c +2010-01-14T07:05:07.823716Z +26358 +mike + +.settings +dir + diff --git a/0.0.9.3/.svn/format b/0.0.9.3/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.3/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.3/.svn/text-base/.actionScriptProperties.svn-base b/0.0.9.3/.svn/text-base/.actionScriptProperties.svn-base new file mode 100644 index 0000000..f73adc8 --- /dev/null +++ b/0.0.9.3/.svn/text-base/.actionScriptProperties.svn-base @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/0.0.9.3/.svn/text-base/.flexLibProperties.svn-base b/0.0.9.3/.svn/text-base/.flexLibProperties.svn-base new file mode 100644 index 0000000..0b65396 --- /dev/null +++ b/0.0.9.3/.svn/text-base/.flexLibProperties.svn-base @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/0.0.9.3/.svn/text-base/.project.svn-base b/0.0.9.3/.svn/text-base/.project.svn-base new file mode 100644 index 0000000..1f1425f --- /dev/null +++ b/0.0.9.3/.svn/text-base/.project.svn-base @@ -0,0 +1,19 @@ + + + AlternativaPhysics + + + + + + com.adobe.flexbuilder.project.flexbuilder + + + + + + org.maven.ide.eclipse.maven2Nature + com.adobe.flexbuilder.project.flexlibnature + com.adobe.flexbuilder.project.actionscriptnature + + diff --git a/0.0.9.3/.svn/text-base/pom.xml.svn-base b/0.0.9.3/.svn/text-base/pom.xml.svn-base new file mode 100644 index 0000000..dbad7d6 --- /dev/null +++ b/0.0.9.3/.svn/text-base/pom.xml.svn-base @@ -0,0 +1,17 @@ + + 4.0.0 + platform.clients.fp10.libraries + AlternativaPhysics + swc + 0.0.9.3 + + platform.tools.maven + FlashBasePom + 2.0.1.0 + + + scm:svn:http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3 + + + + \ No newline at end of file diff --git a/0.0.9.3/META-INF/.svn/all-wcprops b/0.0.9.3/META-INF/.svn/all-wcprops new file mode 100644 index 0000000..c069fc8 --- /dev/null +++ b/0.0.9.3/META-INF/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 88 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/META-INF +END +MANIFEST.MF +K 25 +svn:wc:ra_dav:version-url +V 100 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/META-INF/MANIFEST.MF +END diff --git a/0.0.9.3/META-INF/.svn/entries b/0.0.9.3/META-INF/.svn/entries new file mode 100644 index 0000000..7ffba2a --- /dev/null +++ b/0.0.9.3/META-INF/.svn/entries @@ -0,0 +1,40 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/META-INF +http://svndev.alternativaplatform.com + + + +2010-03-06T18:33:59.318892Z +29409 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +MANIFEST.MF +file + + + + +2010-10-28T04:32:44.000000Z +7b64dceb50a6905850ff00a0bfbe77eb +2009-04-20T17:15:55.260110Z +11541 +mike + diff --git a/0.0.9.3/META-INF/.svn/format b/0.0.9.3/META-INF/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.3/META-INF/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.3/META-INF/.svn/text-base/MANIFEST.MF.svn-base b/0.0.9.3/META-INF/.svn/text-base/MANIFEST.MF.svn-base new file mode 100644 index 0000000..5066a55 --- /dev/null +++ b/0.0.9.3/META-INF/.svn/text-base/MANIFEST.MF.svn-base @@ -0,0 +1 @@ +Bundle-Name: platform.clients.fp10.libraries.AlternativaPhysics diff --git a/0.0.9.3/META-INF/MANIFEST.MF b/0.0.9.3/META-INF/MANIFEST.MF new file mode 100644 index 0000000..5066a55 --- /dev/null +++ b/0.0.9.3/META-INF/MANIFEST.MF @@ -0,0 +1 @@ +Bundle-Name: platform.clients.fp10.libraries.AlternativaPhysics diff --git a/0.0.9.3/pom.xml b/0.0.9.3/pom.xml new file mode 100644 index 0000000..dbad7d6 --- /dev/null +++ b/0.0.9.3/pom.xml @@ -0,0 +1,17 @@ + + 4.0.0 + platform.clients.fp10.libraries + AlternativaPhysics + swc + 0.0.9.3 + + platform.tools.maven + FlashBasePom + 2.0.1.0 + + + scm:svn:http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3 + + + + \ No newline at end of file diff --git a/0.0.9.3/src/.svn/all-wcprops b/0.0.9.3/src/.svn/all-wcprops new file mode 100644 index 0000000..8ff7fbe --- /dev/null +++ b/0.0.9.3/src/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 83 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src +END diff --git a/0.0.9.3/src/.svn/entries b/0.0.9.3/src/.svn/entries new file mode 100644 index 0000000..4f17d32 --- /dev/null +++ b/0.0.9.3/src/.svn/entries @@ -0,0 +1,31 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src +http://svndev.alternativaplatform.com + + + +2010-07-03T11:40:52.545629Z +36684 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +alternativa +dir + diff --git a/0.0.9.3/src/.svn/format b/0.0.9.3/src/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.3/src/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.3/src/alternativa/.svn/all-wcprops b/0.0.9.3/src/alternativa/.svn/all-wcprops new file mode 100644 index 0000000..3638cc6 --- /dev/null +++ b/0.0.9.3/src/alternativa/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 95 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa +END diff --git a/0.0.9.3/src/alternativa/.svn/entries b/0.0.9.3/src/alternativa/.svn/entries new file mode 100644 index 0000000..a9654a8 --- /dev/null +++ b/0.0.9.3/src/alternativa/.svn/entries @@ -0,0 +1,34 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa +http://svndev.alternativaplatform.com + + + +2010-07-03T11:40:52.545629Z +36684 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +physics +dir + +math +dir + diff --git a/0.0.9.3/src/alternativa/.svn/format b/0.0.9.3/src/alternativa/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.3/src/alternativa/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.3/src/alternativa/math/.svn/all-wcprops b/0.0.9.3/src/alternativa/math/.svn/all-wcprops new file mode 100644 index 0000000..dd8b27e --- /dev/null +++ b/0.0.9.3/src/alternativa/math/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 100 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/math +END +Quaternion.as +K 25 +svn:wc:ra_dav:version-url +V 114 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/math/Quaternion.as +END +Vector3.as +K 25 +svn:wc:ra_dav:version-url +V 111 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/math/Vector3.as +END +Matrix3.as +K 25 +svn:wc:ra_dav:version-url +V 111 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/math/Matrix3.as +END +Matrix4.as +K 25 +svn:wc:ra_dav:version-url +V 111 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/math/Matrix4.as +END diff --git a/0.0.9.3/src/alternativa/math/.svn/entries b/0.0.9.3/src/alternativa/math/.svn/entries new file mode 100644 index 0000000..37f3b14 --- /dev/null +++ b/0.0.9.3/src/alternativa/math/.svn/entries @@ -0,0 +1,76 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/math +http://svndev.alternativaplatform.com + + + +2010-02-12T12:34:25.192477Z +28424 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Quaternion.as +file + + + + +2010-10-28T04:32:44.000000Z +7332c9620420bd10062d913decfb9501 +2010-02-12T12:34:25.192477Z +28424 +mike + +Vector3.as +file + + + + +2010-10-28T04:32:44.000000Z +a1571b527786783cef1a9f56db3f2af6 +2010-02-12T12:34:25.192477Z +28424 +mike + +Matrix3.as +file + + + + +2010-10-28T04:32:44.000000Z +0054bd91fe694474a28c8ec17697292b +2010-02-12T12:34:25.192477Z +28424 +mike + +Matrix4.as +file + + + + +2010-10-28T04:32:44.000000Z +47b31d4350499ebe77a5973a938e4b44 +2010-02-12T12:34:25.192477Z +28424 +mike + diff --git a/0.0.9.3/src/alternativa/math/.svn/format b/0.0.9.3/src/alternativa/math/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.3/src/alternativa/math/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.3/src/alternativa/math/.svn/text-base/Matrix3.as.svn-base b/0.0.9.3/src/alternativa/math/.svn/text-base/Matrix3.as.svn-base new file mode 100644 index 0000000..244cc98 --- /dev/null +++ b/0.0.9.3/src/alternativa/math/.svn/text-base/Matrix3.as.svn-base @@ -0,0 +1,390 @@ +package alternativa.math { + import flash.geom.Vector3D; + + /** + * + */ + public class Matrix3 { + + public static const ZERO:Matrix3 = new Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0); + public static const IDENTITY:Matrix3 = new Matrix3(); + + public var a:Number; + public var b:Number; + public var c:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix3(a:Number = 1, b:Number = 0, c:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1) { + this.a = a; + this.b = b; + this.c = c; + + this.e = e; + this.f = f; + this.g = g; + + this.i = i; + this.j = j; + this.k = k; + } + + /** + * + * @return + */ + public function toIdentity():Matrix3 { + a = f = k = 1; + b = c = e = g = i = j = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + var det:Number = 1/(-cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk); + + a = (ff*kk - gg*jj)*det; + b = (cc*jj - bb*kk)*det; + c = (bb*gg - cc*ff)*det; + e = (gg*ii - ee*kk)*det; + f = (aa*kk - cc*ii)*det; + g = (cc*ee - aa*gg)*det; + i = (ee*jj - ff*ii)*det; + j = (bb*ii - aa*jj)*det; + k = (aa*ff - bb*ee)*det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prependTransposed(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.b + cc*m.c; + b = aa*m.e + bb*m.f + cc*m.g; + c = aa*m.i + bb*m.j + cc*m.k; + e = ee*m.a + ff*m.b + gg*m.c; + f = ee*m.e + ff*m.f + gg*m.g; + g = ee*m.i + ff*m.j + gg*m.k; + i = ii*m.a + jj*m.b + kk*m.c; + j = ii*m.e + jj*m.f + kk*m.g; + k = ii*m.i + jj*m.j + kk*m.k; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix3):Matrix3 { + a += m.a; + b += m.b; + c += m.c; + e += m.e; + f += m.f; + g += m.g; + i += m.i; + j += m.j; + k += m.k; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix3):Matrix3 { + a -= m.a; + b -= m.b; + c -= m.c; + e -= m.e; + f -= m.f; + g -= m.g; + i -= m.i; + j -= m.j; + k -= m.k; + + return this; + } + + /** + * + * @return + */ + public function transpose():Matrix3 { + var tmp:Number = b; + b = e; + e = tmp; + tmp = c; + c = i; + i = tmp; + tmp = g; + g = j; + j = tmp; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector3To3D(vin:Vector3, vout:Vector3D):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * + * @param matrix + */ + public function createSkewSymmetric(v:Vector3):Matrix3 { + a = f = k = 0; + b = -v.z; + c = v.y; + e = v.z; + g = -v.x; + i = -v.y; + j = v.x; + return this; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix3):Matrix3 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @return + */ + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):Matrix3 { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + + return this; + } + + /** + * Метод формирует матрицу поворота на заданный угол относительно заданной оси. + * + * @param axis нормализованный вектор, задающий ось, относительно которой выполняется поворот + * @param angle угол поворота в радианах + */ + public function fromAxisAngle(axis:Vector3, angle:Number):void { + var c1:Number = Math.cos(angle); + var s:Number = Math.sin(angle); + var t:Number = 1 - c1; + var x:Number = axis.x; + var y:Number = axis.y; + var z:Number = axis.z; + + a = t*x*x + c1; + b = t*x*y - z*s; + c = t*x*z + y*s; + + e = t*x*y + z*s; + f = t*y*y + c1; + g = t*y*z - x*s; + + i = t*x*z - y*s; + j = t*y*z + x*s; + k = t*z*z + c1; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix3 { + return new Matrix3(a, b, c, e, f, g, i, j, k); + } + + /** + * + * @return + */ + public function toString():String { + return "[Matrix3 (" + a + ", " + b + ", " + c + "), (" + e + ", " + f + ", " + g + "), (" + i + ", " + j + ", " + k + ")]"; + } + + /** + * @param angles + */ + public function getEulerAngles(angles:Vector3):void { + if (-1 < i && i < 1) { + angles.x = Math.atan2(j, k); + angles.y = -Math.asin(i); + angles.z = Math.atan2(e, a); + } else { + angles.x = 0; + angles.y = (i <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-b, f); + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/math/.svn/text-base/Matrix4.as.svn-base b/0.0.9.3/src/alternativa/math/.svn/text-base/Matrix4.as.svn-base new file mode 100644 index 0000000..d06eb5f --- /dev/null +++ b/0.0.9.3/src/alternativa/math/.svn/text-base/Matrix4.as.svn-base @@ -0,0 +1,475 @@ +package alternativa.math { + + /** + * + */ + public class Matrix4 { + + public static const IDENTITY:Matrix4 = new Matrix4(); + + public var a:Number; + public var b:Number; + public var c:Number; + public var d:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + public var h:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + public var l:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix4(a:Number = 1, b:Number = 0, c:Number = 0, d:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, h:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1, l:Number = 0) { + this.a = a; + this.b = b; + this.c = c; + this.d = d; + + this.e = e; + this.f = f; + this.g = g; + this.h = h; + + this.i = i; + this.j = j; + this.k = k; + this.l = l; + } + + /** + * + * @return + */ + public function toIdentity():Matrix4 { + a = f = k = 1; + b = c = e = g = i = j = d = h = l = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + var det:Number = -cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk; + + a = (-gg*jj + ff*kk)/det; + b = (cc*jj - bb*kk)/det; + c = (-cc*ff + bb*gg)/det; + d = (dd*gg*jj - cc*hh*jj - dd*ff*kk + bb*hh*kk + cc*ff*ll - bb*gg*ll)/det; + e = (gg*ii - ee*kk)/det; + f = (-cc*ii + aa*kk)/det; + g = (cc*ee - aa*gg)/det; + h = (cc*hh*ii - dd*gg*ii + dd*ee*kk - aa*hh*kk - cc*ee*ll + aa*gg*ll)/det; + i = (-ff*ii + ee*jj)/det; + j = (bb*ii - aa*jj)/det; + k = (-bb*ee + aa*ff)/det; + l = (dd*ff*ii - bb*hh*ii - dd*ee*jj + aa*hh*jj + bb*ee*ll - aa*ff*ll)/det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + d = m.a*dd + m.b*hh + m.c*ll + m.d; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + h = m.e*dd + m.f*hh + m.g*ll + m.h; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + l = m.i*dd + m.j*hh + m.k*ll + m.l; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + d = aa*m.d + bb*m.h + cc*m.l + dd; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + h = ee*m.d + ff*m.h + gg*m.l + hh; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + l = ii*m.d + jj*m.h + kk*m.l + ll; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix4):Matrix4 { + a += m.a; + b += m.b; + c += m.c; + d += m.d; + e += m.e; + f += m.f; + g += m.g; + h += m.h; + i += m.i; + j += m.j; + k += m.k; + l += m.l; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix4):Matrix4 { + a -= m.a; + b -= m.b; + c -= m.c; + d -= m.d; + e -= m.e; + f -= m.f; + g -= m.g; + h -= m.h; + i -= m.i; + j -= m.j; + k -= m.k; + l -= m.l; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectors(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + * @param len + */ + public function transformVectorsN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverse(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverseN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(idx:int, axis:Vector3):void { + switch (idx) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + case 3: + axis.x = d; + axis.y = h; + axis.z = l; + return; + } + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix4):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = m.d; + e = m.e; + f = m.f; + g = m.g; + h = m.h; + i = m.i; + j = m.j; + k = m.k; + l = m.l; + + return this; + } + + public function setFromMatrix3(m:Matrix3, offset:Vector3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = offset.x; + e = m.e; + f = m.f; + g = m.g; + h = offset.y; + i = m.i; + j = m.j; + k = m.k; + l = offset.z; + + return this; + } + + public function setOrientationFromMatrix3(m:Matrix3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @param angles + */ + public function getEulerAngles(angles:Vector3):void { + if (-1 < i && i < 1) { + angles.x = Math.atan2(j, k); + angles.y = -Math.asin(i); + angles.z = Math.atan2(e, a); + } else { + angles.x = 0; + angles.y = (i <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-b, f); + } + } + + /** + * Устанавливает координаты матрицы. + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + d = pos.x; + h = pos.y; + l = pos.z; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix4 { + return new Matrix4(a, b, c, d, e, f, g, h, i, j, k, l); + } + + /** + * + * @return + */ + public function toString():String { + return "[Matrix4 [" + a.toFixed(3) + " " + b.toFixed(3) + " " + c.toFixed(3) + " " + d.toFixed(3) + "] [" + e.toFixed(3) + " " + f.toFixed(3) + " " + g.toFixed(3) + " " + h.toFixed(3) + "] [" + i.toFixed(3) + " " + j.toFixed(3) + " " + k.toFixed(3) + " " + l.toFixed(3) + "]]"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/math/.svn/text-base/Quaternion.as.svn-base b/0.0.9.3/src/alternativa/math/.svn/text-base/Quaternion.as.svn-base new file mode 100644 index 0000000..ddc85f5 --- /dev/null +++ b/0.0.9.3/src/alternativa/math/.svn/text-base/Quaternion.as.svn-base @@ -0,0 +1,458 @@ +package alternativa.math { + import flash.geom.Vector3D; + + public class Quaternion { + + public var w:Number; + public var x:Number; + public var y:Number; + public var z:Number; + + public static function multiply(q1:Quaternion, q2:Quaternion, result:Quaternion):void { + result.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z; + result.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y; + result.y = q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z; + result.z = q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x; + } + + public static function createFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngle(axis, angle); + return q; + } + + public static function createFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngleComponents(x, y, z, angle); + return q; + } + + private static var _q:Quaternion = new Quaternion(); + + public function Quaternion(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0) { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + } + + public function reset(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0):Quaternion { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + return this; + } + + public function normalize():Quaternion { + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * Умножает на указанный кватернион слева: this * q + * + * @param q множитель + */ + public function prepend(q:Quaternion):Quaternion { + var ww:Number = w*q.w - x*q.x - y*q.y - z*q.z; + var xx:Number = w*q.x + x*q.w + y*q.z - z*q.y; + var yy:Number = w*q.y + y*q.w + z*q.x - x*q.z; + var zz:Number = w*q.z + z*q.w + x*q.y - y*q.x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Умножает на указанный кватернион справа: q * this + * + * @param q множитель + */ + public function append(q:Quaternion):Quaternion { + var ww:Number = q.w*w - q.x*x - q.y*y - q.z*z; + var xx:Number = q.w*x + q.x*w + q.y*z - q.z*y; + var yy:Number = q.w*y + q.y*w + q.z*x - q.x*z; + var zz:Number = q.w*z + q.z*w + q.x*y - q.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * + * @param vector + */ + public function rotateByVector(v:Vector3):Quaternion { + var ww:Number = -v.x*x - v.y*y - v.z*z; + var xx:Number = v.x*w + v.y*z - v.z*y; + var yy:Number = v.y*w + v.z*x - v.x*z; + var zz:Number = v.z*w + v.x*y - v.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Добавляет вращение, приданное вектором угловой скорости за указанное время. + * + * @param v + * @param scale + */ + public function addScaledVector(v:Vector3, scale:Number):Quaternion { + var vx:Number = v.x*scale; + var vy:Number = v.y*scale; + var vz:Number = v.z*scale; + var ww:Number = -x*vx - y*vy - z*vz; + var xx:Number = vx*w + vy*z - vz*y; + var yy:Number = vy*w + vz*x - vx*z; + var zz:Number = vz*w + vx*y - vy*x; + w += 0.5*ww; + x += 0.5*xx; + y += 0.5*yy; + z += 0.5*zz; + // inlined normalize + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param m + * @return + */ + public function toMatrix3(m:Matrix3):Quaternion { + var xx2:Number = 2.0*x*x; + var yy2:Number = 2.0*y*y; + var zz2:Number = 2.0*z*z; + var xy2:Number = 2.0*x*y; + var yz2:Number = 2.0*y*z; + var zx2:Number = 2.0*z*x; + var wx2:Number = 2.0*w*x; + var wy2:Number = 2.0*w*y; + var wz2:Number = 2.0*w*z; + + m.a = 1.0 - yy2 - zz2; + m.b = xy2 - wz2; + m.c = zx2 + wy2; + + m.e = xy2 + wz2; + m.f = 1.0 - xx2 - zz2; + m.g = yz2 - wx2; + + m.i = zx2 - wy2; + m.j = yz2 + wx2; + m.k = 1.0 - xx2 - yy2; + return this; + } + + /** + * + * @return + */ + public function length():Number { + return Math.sqrt(w*w + x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function lengthSqr():Number { + return w*w + x*x + y*y + z*z; + } + + /** + * + * @param axis + * @param angle + * @return + */ + public function setFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z); + x = axis.x*k; + y = axis.y*k; + z = axis.z*k; + return this; + } + + /** + * + * @param x + * @param y + * @param z + * @param angle + */ + public function setFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(x*x + y*y + z*z); + this.x = x*k; + this.y = y*k; + this.z = z*k; + return this; + } + + /** + * + * @param vector + */ + public function toAxisVector(v:Vector3 = null):Vector3 { + if (w < -1 || w > 1) { + normalize(); + } + if (v == null) { + v = new Vector3(); + } + if (w > -1 && w < 1) { + if (w == 0) { + v.x = x; + v.y = y; + v.z = z; + } else { + var angle:Number = 2*Math.acos(w); + var coeff:Number = 1/Math.sqrt(1 - w*w); + v.x = x*coeff*angle; + v.y = y*coeff*angle; + v.z = z*coeff*angle; + } + } else { + v.x = 0; + v.y = 0; + v.z = 0; + } + return v; + } + + /** + * + * @param rotations + */ + public function getEulerAngles(angles:Vector3):Vector3 { + var qi2:Number = 2*x*x; + var qj2:Number = 2*y*y; + var qk2:Number = 2*z*z; + var qij:Number = 2*x*y; + var qjk:Number = 2*y*z; + var qki:Number = 2*z*x; + var qri:Number = 2*w*x; + var qrj:Number = 2*w*y; + var qrk:Number = 2*w*z; + + var aa:Number = 1 - qj2 - qk2; + var bb:Number = qij - qrk; + var ee:Number = qij + qrk; + var ff:Number = 1 - qi2 - qk2; + var ii:Number = qki - qrj; + var jj:Number = qjk + qri; + var kk:Number = 1 - qi2 - qj2; + + if (-1 < ii && ii < 1) { + if (angles == null) { + angles = new Vector3(Math.atan2(jj, kk), -Math.asin(ii), Math.atan2(ee, aa)); + } else { + angles.x = Math.atan2(jj, kk); + angles.y = -Math.asin(ii); + angles.z = Math.atan2(ee, aa); + } + } else { + if (angles == null) { + angles = new Vector3(0, 0.5*((ii <= -1) ? Math.PI : -Math.PI), Math.atan2(-bb, ff)); + } else { + angles.x = 0; + angles.y = (ii <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-bb, ff); + } + } + return angles; + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setFromEulerAnglesXYZ(x:Number, y:Number, z:Number):void { + setFromAxisAngleComponents(1, 0, 0, x); + + _q.setFromAxisAngleComponents(0, 1, 0, y); + append(_q); + normalize(); + + _q.setFromAxisAngleComponents(0, 0, 1, z); + append(_q); + normalize(); + } + + /** + * + */ + public function conjugate():void { + x = -x; + y = -y; + z = -z; + } + + /** + * Выполняет линейную интерполяцию. + * + * @param q1 начало отрезка + * @param q2 конец отрезка + * @param t время, обычно задаётся в интервале [0, 1] + */ + public function nlerp(q1:Quaternion, q2:Quaternion, t:Number):Quaternion { + var d:Number = 1 - t; + w = q1.w*d + q2.w*t; + x = q1.x*d + q2.x*t; + y = q1.y*d + q2.y*t; + z = q1.z*d + q2.z*t; + // inlined normalize + d = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param q + * @return + */ + public function subtract(q:Quaternion):Quaternion { + w -= q.w; + x -= q.x; + y -= q.y; + z -= q.z; + return this; + } + + /** + * + * @param q1 + * @param q2 + * @return + */ + public function diff(q1:Quaternion, q2:Quaternion):Quaternion { + w = q2.w - q1.w; + x = q2.x - q1.x; + y = q2.y - q1.y; + z = q2.z - q1.z; + return this; + } + + /** + * + * @param q + * @return + */ + public function copy(q:Quaternion):Quaternion { + w = q.w; + x = q.x; + y = q.y; + z = q.z; + return this; + } + + /** + * @param reslult + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + result.w = w; + return result; + } + + /** + * + * @return + */ + public function clone():Quaternion { + return new Quaternion(w, x, y, z); + } + + /** + * + * @return + */ + public function toString():String { + return "[" + w + ", " + x + ", " + y + ", " + z + "]"; + } + + /** + * Выполняет сферическую интерполяцию между двумя заданными кватернионами по наименьшему расстоянию. + * + * @param a первый кватерион + * @param b второй кватернион + * @param t параметр интерполяции, обычно принадлежит отрезку [0, 1] + * @return this + */ + public function slerp(a:Quaternion, b:Quaternion, t:Number):Quaternion { + var flip:Number = 1; + // Так как одна и та же ориентация представляется двумя значениями q и -q, нужно сменить знак одного из кватернионов + // если скалярное произведение отрицательно. Иначе будет получено интерполированное значение по наибольшему расстоянию. + var cosine:Number = a.w*b.w + a.x*b.x + a.y*b.y + a.z*b.z; + if (cosine < 0) { + cosine = -cosine; + flip = -1; + } + + if ((1 - cosine) < 0.001) { + // Вблизи нуля используется линейная интерполяция + var k1:Number = 1 - t; + var k2:Number = t*flip; + w = a.w*k1 + b.w*k2; + x = a.x*k1 + b.x*k2; + y = a.y*k1 + b.y*k2; + z = a.z*k1 + b.z*k2; + normalize(); + } else { + var theta:Number = Math.acos(cosine); + var sine:Number = Math.sin(theta); + var beta:Number = Math.sin((1 - t)*theta)/sine; + var alpha:Number = Math.sin(t*theta)/sine*flip; + w = a.w*beta + b.w*alpha; + x = a.x*beta + b.x*alpha; + y = a.y*beta + b.y*alpha; + z = a.z*beta + b.z*alpha; + } + + return this; + } + + } +} diff --git a/0.0.9.3/src/alternativa/math/.svn/text-base/Vector3.as.svn-base b/0.0.9.3/src/alternativa/math/.svn/text-base/Vector3.as.svn-base new file mode 100644 index 0000000..89010a8 --- /dev/null +++ b/0.0.9.3/src/alternativa/math/.svn/text-base/Vector3.as.svn-base @@ -0,0 +1,334 @@ +package alternativa.math { + import flash.geom.Vector3D; + + /** + * + */ + public class Vector3 extends Vector3D { + + public static const ZERO:Vector3 = new Vector3(0, 0, 0); + public static const X_AXIS:Vector3 = new Vector3(1, 0, 0); + public static const Y_AXIS:Vector3 = new Vector3(0, 1, 0); + public static const Z_AXIS:Vector3 = new Vector3(0, 0, 1); + + /** + * + * @param x + * @param y + * @param z + */ + public function Vector3(x:Number = 0, y:Number = 0, z:Number = 0) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * + * @return + */ + public function vLength():Number { + return Math.sqrt(x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function vLengthSqr():Number { + return x*x + y*y + z*z; + } + + /** + * + * @param length + * @return + */ + public function vSetLength(length:Number):Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = length; + } else { + var k:Number = length/Math.sqrt(x*x + y*y + z*z); + x *= k; + y *= k; + z *= k; + } + return this; + } + + /** + * + * @return + */ + public function vNormalize():Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = 1; + } else { + d = Math.sqrt(d); + x /= d; + y /= d; + z /= d; + } + return this; + } + + /** + * Сложение координат. + * + * @param v точка, координаты которой прибавляются к собственным + */ + public function vAdd(v:Vector3):Vector3 { + x += v.x; + y += v.y; + z += v.z; + return this; + } + + /** + * + * @param k + * @param v + * @return + */ + public function vAddScaled(k:Number, v:Vector3):Vector3 { + x += k*v.x; + y += k*v.y; + z += k*v.z; + return this; + } + + /** + * Вычитание координат. + * + * @param v точка, координаты которой вычитаются из собственных + */ + public function vSubtract(v:Vector3):Vector3 { + x -= v.x; + y -= v.y; + z -= v.z; + return this; + } + + /** + * Вычисляет сумму векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + * @return this + */ + public function vSum(a:Vector3, b:Vector3):Vector3 { + x = a.x + b.x; + y = a.y + b.y; + z = a.z + b.z; + return this; + } + + /** + * Вычисление разности векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + */ + public function vDiff(a:Vector3, b:Vector3):Vector3 { + x = a.x - b.x; + y = a.y - b.y; + z = a.z - b.z; + return this; + } + + /** + * Умножение на скаляр. + * + * @param k число, на которое умножаются координаты + */ + public function vScale(k:Number):Vector3 { + x *= k; + y *= k; + z *= k; + return this; + } + + /** + * Инвертирование вектора. + */ + public function vReverse():Vector3 { + x = -x; + y = -y; + z = -z; + return this; + } + + /** + * + * @param v + * @return + */ + public function vDot(v:Vector3):Number { + return x*v.x + y*v.y + z*v.z; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross(v:Vector3):Vector3 { + var xx:Number = y*v.z - z*v.y; + var yy:Number = z*v.x - x*v.z; + var zz:Number = x*v.y - y*v.x; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross2(a:Vector3, b:Vector3):Vector3 { + x = a.y*b.z - a.z*b.y; + y = a.z*b.x - a.x*b.z; + z = a.x*b.y - a.y*b.x; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy3(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * + * @param matrix + */ + public function vTransformBy3Tr(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz + m.d; + y = m.e*xx + m.f*yy + m.g*zz + m.h; + z = m.i*xx + m.j*yy + m.k*zz + m.l; + return this; + } + + /** + * + * @param m + * @return + */ + public function vTransformInverseBy4(m:Matrix4):Vector3 { + var xx:Number = x - m.d; + var yy:Number = y - m.h; + var zz:Number = z - m.l; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * + * @param m + */ + public function vDeltaTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * Установка координат. + */ + public function vReset(x:Number = 0, y:Number = 0, z:Number = 0):Vector3 { + this.x = x; + this.y = y; + this.z = z; + return this; + } + + /** + * Копирование координат точки. + * + * @param v точка, координаты которой копируются + */ + public function vCopy(v:Vector3):Vector3 { + x = v.x; + y = v.y; + z = v.z; + return this; + } + + /** + * Клонирование точки. + * + * @return клонированная точка + */ + public function vClone():Vector3 { + return new Vector3(x, y, z); + } + + /** + * Копирует координаты вектора в экземпляр класса Vector3D. + * + * @param result экземпляр, в который копируются координаты + * @return переданный параметр + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + return result; + } + + /** + * @param source + * @return + */ + public function copyFromVector3D(source:Vector3D):Vector3 { + x = source.x; + y = source.y; + z = source.z; + return this; + } + + override public function toString():String { + return "Vector3(" + x + "," + y + ", " + z + ")"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/math/Matrix3.as b/0.0.9.3/src/alternativa/math/Matrix3.as new file mode 100644 index 0000000..244cc98 --- /dev/null +++ b/0.0.9.3/src/alternativa/math/Matrix3.as @@ -0,0 +1,390 @@ +package alternativa.math { + import flash.geom.Vector3D; + + /** + * + */ + public class Matrix3 { + + public static const ZERO:Matrix3 = new Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0); + public static const IDENTITY:Matrix3 = new Matrix3(); + + public var a:Number; + public var b:Number; + public var c:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix3(a:Number = 1, b:Number = 0, c:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1) { + this.a = a; + this.b = b; + this.c = c; + + this.e = e; + this.f = f; + this.g = g; + + this.i = i; + this.j = j; + this.k = k; + } + + /** + * + * @return + */ + public function toIdentity():Matrix3 { + a = f = k = 1; + b = c = e = g = i = j = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + var det:Number = 1/(-cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk); + + a = (ff*kk - gg*jj)*det; + b = (cc*jj - bb*kk)*det; + c = (bb*gg - cc*ff)*det; + e = (gg*ii - ee*kk)*det; + f = (aa*kk - cc*ii)*det; + g = (cc*ee - aa*gg)*det; + i = (ee*jj - ff*ii)*det; + j = (bb*ii - aa*jj)*det; + k = (aa*ff - bb*ee)*det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prependTransposed(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.b + cc*m.c; + b = aa*m.e + bb*m.f + cc*m.g; + c = aa*m.i + bb*m.j + cc*m.k; + e = ee*m.a + ff*m.b + gg*m.c; + f = ee*m.e + ff*m.f + gg*m.g; + g = ee*m.i + ff*m.j + gg*m.k; + i = ii*m.a + jj*m.b + kk*m.c; + j = ii*m.e + jj*m.f + kk*m.g; + k = ii*m.i + jj*m.j + kk*m.k; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix3):Matrix3 { + a += m.a; + b += m.b; + c += m.c; + e += m.e; + f += m.f; + g += m.g; + i += m.i; + j += m.j; + k += m.k; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix3):Matrix3 { + a -= m.a; + b -= m.b; + c -= m.c; + e -= m.e; + f -= m.f; + g -= m.g; + i -= m.i; + j -= m.j; + k -= m.k; + + return this; + } + + /** + * + * @return + */ + public function transpose():Matrix3 { + var tmp:Number = b; + b = e; + e = tmp; + tmp = c; + c = i; + i = tmp; + tmp = g; + g = j; + j = tmp; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector3To3D(vin:Vector3, vout:Vector3D):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * + * @param matrix + */ + public function createSkewSymmetric(v:Vector3):Matrix3 { + a = f = k = 0; + b = -v.z; + c = v.y; + e = v.z; + g = -v.x; + i = -v.y; + j = v.x; + return this; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix3):Matrix3 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @return + */ + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):Matrix3 { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + + return this; + } + + /** + * Метод формирует матрицу поворота на заданный угол относительно заданной оси. + * + * @param axis нормализованный вектор, задающий ось, относительно которой выполняется поворот + * @param angle угол поворота в радианах + */ + public function fromAxisAngle(axis:Vector3, angle:Number):void { + var c1:Number = Math.cos(angle); + var s:Number = Math.sin(angle); + var t:Number = 1 - c1; + var x:Number = axis.x; + var y:Number = axis.y; + var z:Number = axis.z; + + a = t*x*x + c1; + b = t*x*y - z*s; + c = t*x*z + y*s; + + e = t*x*y + z*s; + f = t*y*y + c1; + g = t*y*z - x*s; + + i = t*x*z - y*s; + j = t*y*z + x*s; + k = t*z*z + c1; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix3 { + return new Matrix3(a, b, c, e, f, g, i, j, k); + } + + /** + * + * @return + */ + public function toString():String { + return "[Matrix3 (" + a + ", " + b + ", " + c + "), (" + e + ", " + f + ", " + g + "), (" + i + ", " + j + ", " + k + ")]"; + } + + /** + * @param angles + */ + public function getEulerAngles(angles:Vector3):void { + if (-1 < i && i < 1) { + angles.x = Math.atan2(j, k); + angles.y = -Math.asin(i); + angles.z = Math.atan2(e, a); + } else { + angles.x = 0; + angles.y = (i <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-b, f); + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/math/Matrix4.as b/0.0.9.3/src/alternativa/math/Matrix4.as new file mode 100644 index 0000000..d06eb5f --- /dev/null +++ b/0.0.9.3/src/alternativa/math/Matrix4.as @@ -0,0 +1,475 @@ +package alternativa.math { + + /** + * + */ + public class Matrix4 { + + public static const IDENTITY:Matrix4 = new Matrix4(); + + public var a:Number; + public var b:Number; + public var c:Number; + public var d:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + public var h:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + public var l:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix4(a:Number = 1, b:Number = 0, c:Number = 0, d:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, h:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1, l:Number = 0) { + this.a = a; + this.b = b; + this.c = c; + this.d = d; + + this.e = e; + this.f = f; + this.g = g; + this.h = h; + + this.i = i; + this.j = j; + this.k = k; + this.l = l; + } + + /** + * + * @return + */ + public function toIdentity():Matrix4 { + a = f = k = 1; + b = c = e = g = i = j = d = h = l = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + var det:Number = -cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk; + + a = (-gg*jj + ff*kk)/det; + b = (cc*jj - bb*kk)/det; + c = (-cc*ff + bb*gg)/det; + d = (dd*gg*jj - cc*hh*jj - dd*ff*kk + bb*hh*kk + cc*ff*ll - bb*gg*ll)/det; + e = (gg*ii - ee*kk)/det; + f = (-cc*ii + aa*kk)/det; + g = (cc*ee - aa*gg)/det; + h = (cc*hh*ii - dd*gg*ii + dd*ee*kk - aa*hh*kk - cc*ee*ll + aa*gg*ll)/det; + i = (-ff*ii + ee*jj)/det; + j = (bb*ii - aa*jj)/det; + k = (-bb*ee + aa*ff)/det; + l = (dd*ff*ii - bb*hh*ii - dd*ee*jj + aa*hh*jj + bb*ee*ll - aa*ff*ll)/det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + d = m.a*dd + m.b*hh + m.c*ll + m.d; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + h = m.e*dd + m.f*hh + m.g*ll + m.h; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + l = m.i*dd + m.j*hh + m.k*ll + m.l; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + d = aa*m.d + bb*m.h + cc*m.l + dd; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + h = ee*m.d + ff*m.h + gg*m.l + hh; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + l = ii*m.d + jj*m.h + kk*m.l + ll; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix4):Matrix4 { + a += m.a; + b += m.b; + c += m.c; + d += m.d; + e += m.e; + f += m.f; + g += m.g; + h += m.h; + i += m.i; + j += m.j; + k += m.k; + l += m.l; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix4):Matrix4 { + a -= m.a; + b -= m.b; + c -= m.c; + d -= m.d; + e -= m.e; + f -= m.f; + g -= m.g; + h -= m.h; + i -= m.i; + j -= m.j; + k -= m.k; + l -= m.l; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectors(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + * @param len + */ + public function transformVectorsN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverse(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverseN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(idx:int, axis:Vector3):void { + switch (idx) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + case 3: + axis.x = d; + axis.y = h; + axis.z = l; + return; + } + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix4):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = m.d; + e = m.e; + f = m.f; + g = m.g; + h = m.h; + i = m.i; + j = m.j; + k = m.k; + l = m.l; + + return this; + } + + public function setFromMatrix3(m:Matrix3, offset:Vector3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = offset.x; + e = m.e; + f = m.f; + g = m.g; + h = offset.y; + i = m.i; + j = m.j; + k = m.k; + l = offset.z; + + return this; + } + + public function setOrientationFromMatrix3(m:Matrix3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @param angles + */ + public function getEulerAngles(angles:Vector3):void { + if (-1 < i && i < 1) { + angles.x = Math.atan2(j, k); + angles.y = -Math.asin(i); + angles.z = Math.atan2(e, a); + } else { + angles.x = 0; + angles.y = (i <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-b, f); + } + } + + /** + * Устанавливает координаты матрицы. + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + d = pos.x; + h = pos.y; + l = pos.z; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix4 { + return new Matrix4(a, b, c, d, e, f, g, h, i, j, k, l); + } + + /** + * + * @return + */ + public function toString():String { + return "[Matrix4 [" + a.toFixed(3) + " " + b.toFixed(3) + " " + c.toFixed(3) + " " + d.toFixed(3) + "] [" + e.toFixed(3) + " " + f.toFixed(3) + " " + g.toFixed(3) + " " + h.toFixed(3) + "] [" + i.toFixed(3) + " " + j.toFixed(3) + " " + k.toFixed(3) + " " + l.toFixed(3) + "]]"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/math/Quaternion.as b/0.0.9.3/src/alternativa/math/Quaternion.as new file mode 100644 index 0000000..ddc85f5 --- /dev/null +++ b/0.0.9.3/src/alternativa/math/Quaternion.as @@ -0,0 +1,458 @@ +package alternativa.math { + import flash.geom.Vector3D; + + public class Quaternion { + + public var w:Number; + public var x:Number; + public var y:Number; + public var z:Number; + + public static function multiply(q1:Quaternion, q2:Quaternion, result:Quaternion):void { + result.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z; + result.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y; + result.y = q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z; + result.z = q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x; + } + + public static function createFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngle(axis, angle); + return q; + } + + public static function createFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngleComponents(x, y, z, angle); + return q; + } + + private static var _q:Quaternion = new Quaternion(); + + public function Quaternion(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0) { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + } + + public function reset(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0):Quaternion { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + return this; + } + + public function normalize():Quaternion { + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * Умножает на указанный кватернион слева: this * q + * + * @param q множитель + */ + public function prepend(q:Quaternion):Quaternion { + var ww:Number = w*q.w - x*q.x - y*q.y - z*q.z; + var xx:Number = w*q.x + x*q.w + y*q.z - z*q.y; + var yy:Number = w*q.y + y*q.w + z*q.x - x*q.z; + var zz:Number = w*q.z + z*q.w + x*q.y - y*q.x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Умножает на указанный кватернион справа: q * this + * + * @param q множитель + */ + public function append(q:Quaternion):Quaternion { + var ww:Number = q.w*w - q.x*x - q.y*y - q.z*z; + var xx:Number = q.w*x + q.x*w + q.y*z - q.z*y; + var yy:Number = q.w*y + q.y*w + q.z*x - q.x*z; + var zz:Number = q.w*z + q.z*w + q.x*y - q.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * + * @param vector + */ + public function rotateByVector(v:Vector3):Quaternion { + var ww:Number = -v.x*x - v.y*y - v.z*z; + var xx:Number = v.x*w + v.y*z - v.z*y; + var yy:Number = v.y*w + v.z*x - v.x*z; + var zz:Number = v.z*w + v.x*y - v.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Добавляет вращение, приданное вектором угловой скорости за указанное время. + * + * @param v + * @param scale + */ + public function addScaledVector(v:Vector3, scale:Number):Quaternion { + var vx:Number = v.x*scale; + var vy:Number = v.y*scale; + var vz:Number = v.z*scale; + var ww:Number = -x*vx - y*vy - z*vz; + var xx:Number = vx*w + vy*z - vz*y; + var yy:Number = vy*w + vz*x - vx*z; + var zz:Number = vz*w + vx*y - vy*x; + w += 0.5*ww; + x += 0.5*xx; + y += 0.5*yy; + z += 0.5*zz; + // inlined normalize + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param m + * @return + */ + public function toMatrix3(m:Matrix3):Quaternion { + var xx2:Number = 2.0*x*x; + var yy2:Number = 2.0*y*y; + var zz2:Number = 2.0*z*z; + var xy2:Number = 2.0*x*y; + var yz2:Number = 2.0*y*z; + var zx2:Number = 2.0*z*x; + var wx2:Number = 2.0*w*x; + var wy2:Number = 2.0*w*y; + var wz2:Number = 2.0*w*z; + + m.a = 1.0 - yy2 - zz2; + m.b = xy2 - wz2; + m.c = zx2 + wy2; + + m.e = xy2 + wz2; + m.f = 1.0 - xx2 - zz2; + m.g = yz2 - wx2; + + m.i = zx2 - wy2; + m.j = yz2 + wx2; + m.k = 1.0 - xx2 - yy2; + return this; + } + + /** + * + * @return + */ + public function length():Number { + return Math.sqrt(w*w + x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function lengthSqr():Number { + return w*w + x*x + y*y + z*z; + } + + /** + * + * @param axis + * @param angle + * @return + */ + public function setFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z); + x = axis.x*k; + y = axis.y*k; + z = axis.z*k; + return this; + } + + /** + * + * @param x + * @param y + * @param z + * @param angle + */ + public function setFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(x*x + y*y + z*z); + this.x = x*k; + this.y = y*k; + this.z = z*k; + return this; + } + + /** + * + * @param vector + */ + public function toAxisVector(v:Vector3 = null):Vector3 { + if (w < -1 || w > 1) { + normalize(); + } + if (v == null) { + v = new Vector3(); + } + if (w > -1 && w < 1) { + if (w == 0) { + v.x = x; + v.y = y; + v.z = z; + } else { + var angle:Number = 2*Math.acos(w); + var coeff:Number = 1/Math.sqrt(1 - w*w); + v.x = x*coeff*angle; + v.y = y*coeff*angle; + v.z = z*coeff*angle; + } + } else { + v.x = 0; + v.y = 0; + v.z = 0; + } + return v; + } + + /** + * + * @param rotations + */ + public function getEulerAngles(angles:Vector3):Vector3 { + var qi2:Number = 2*x*x; + var qj2:Number = 2*y*y; + var qk2:Number = 2*z*z; + var qij:Number = 2*x*y; + var qjk:Number = 2*y*z; + var qki:Number = 2*z*x; + var qri:Number = 2*w*x; + var qrj:Number = 2*w*y; + var qrk:Number = 2*w*z; + + var aa:Number = 1 - qj2 - qk2; + var bb:Number = qij - qrk; + var ee:Number = qij + qrk; + var ff:Number = 1 - qi2 - qk2; + var ii:Number = qki - qrj; + var jj:Number = qjk + qri; + var kk:Number = 1 - qi2 - qj2; + + if (-1 < ii && ii < 1) { + if (angles == null) { + angles = new Vector3(Math.atan2(jj, kk), -Math.asin(ii), Math.atan2(ee, aa)); + } else { + angles.x = Math.atan2(jj, kk); + angles.y = -Math.asin(ii); + angles.z = Math.atan2(ee, aa); + } + } else { + if (angles == null) { + angles = new Vector3(0, 0.5*((ii <= -1) ? Math.PI : -Math.PI), Math.atan2(-bb, ff)); + } else { + angles.x = 0; + angles.y = (ii <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-bb, ff); + } + } + return angles; + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setFromEulerAnglesXYZ(x:Number, y:Number, z:Number):void { + setFromAxisAngleComponents(1, 0, 0, x); + + _q.setFromAxisAngleComponents(0, 1, 0, y); + append(_q); + normalize(); + + _q.setFromAxisAngleComponents(0, 0, 1, z); + append(_q); + normalize(); + } + + /** + * + */ + public function conjugate():void { + x = -x; + y = -y; + z = -z; + } + + /** + * Выполняет линейную интерполяцию. + * + * @param q1 начало отрезка + * @param q2 конец отрезка + * @param t время, обычно задаётся в интервале [0, 1] + */ + public function nlerp(q1:Quaternion, q2:Quaternion, t:Number):Quaternion { + var d:Number = 1 - t; + w = q1.w*d + q2.w*t; + x = q1.x*d + q2.x*t; + y = q1.y*d + q2.y*t; + z = q1.z*d + q2.z*t; + // inlined normalize + d = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param q + * @return + */ + public function subtract(q:Quaternion):Quaternion { + w -= q.w; + x -= q.x; + y -= q.y; + z -= q.z; + return this; + } + + /** + * + * @param q1 + * @param q2 + * @return + */ + public function diff(q1:Quaternion, q2:Quaternion):Quaternion { + w = q2.w - q1.w; + x = q2.x - q1.x; + y = q2.y - q1.y; + z = q2.z - q1.z; + return this; + } + + /** + * + * @param q + * @return + */ + public function copy(q:Quaternion):Quaternion { + w = q.w; + x = q.x; + y = q.y; + z = q.z; + return this; + } + + /** + * @param reslult + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + result.w = w; + return result; + } + + /** + * + * @return + */ + public function clone():Quaternion { + return new Quaternion(w, x, y, z); + } + + /** + * + * @return + */ + public function toString():String { + return "[" + w + ", " + x + ", " + y + ", " + z + "]"; + } + + /** + * Выполняет сферическую интерполяцию между двумя заданными кватернионами по наименьшему расстоянию. + * + * @param a первый кватерион + * @param b второй кватернион + * @param t параметр интерполяции, обычно принадлежит отрезку [0, 1] + * @return this + */ + public function slerp(a:Quaternion, b:Quaternion, t:Number):Quaternion { + var flip:Number = 1; + // Так как одна и та же ориентация представляется двумя значениями q и -q, нужно сменить знак одного из кватернионов + // если скалярное произведение отрицательно. Иначе будет получено интерполированное значение по наибольшему расстоянию. + var cosine:Number = a.w*b.w + a.x*b.x + a.y*b.y + a.z*b.z; + if (cosine < 0) { + cosine = -cosine; + flip = -1; + } + + if ((1 - cosine) < 0.001) { + // Вблизи нуля используется линейная интерполяция + var k1:Number = 1 - t; + var k2:Number = t*flip; + w = a.w*k1 + b.w*k2; + x = a.x*k1 + b.x*k2; + y = a.y*k1 + b.y*k2; + z = a.z*k1 + b.z*k2; + normalize(); + } else { + var theta:Number = Math.acos(cosine); + var sine:Number = Math.sin(theta); + var beta:Number = Math.sin((1 - t)*theta)/sine; + var alpha:Number = Math.sin(t*theta)/sine*flip; + w = a.w*beta + b.w*alpha; + x = a.x*beta + b.x*alpha; + y = a.y*beta + b.y*alpha; + z = a.z*beta + b.z*alpha; + } + + return this; + } + + } +} diff --git a/0.0.9.3/src/alternativa/math/Vector3.as b/0.0.9.3/src/alternativa/math/Vector3.as new file mode 100644 index 0000000..89010a8 --- /dev/null +++ b/0.0.9.3/src/alternativa/math/Vector3.as @@ -0,0 +1,334 @@ +package alternativa.math { + import flash.geom.Vector3D; + + /** + * + */ + public class Vector3 extends Vector3D { + + public static const ZERO:Vector3 = new Vector3(0, 0, 0); + public static const X_AXIS:Vector3 = new Vector3(1, 0, 0); + public static const Y_AXIS:Vector3 = new Vector3(0, 1, 0); + public static const Z_AXIS:Vector3 = new Vector3(0, 0, 1); + + /** + * + * @param x + * @param y + * @param z + */ + public function Vector3(x:Number = 0, y:Number = 0, z:Number = 0) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * + * @return + */ + public function vLength():Number { + return Math.sqrt(x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function vLengthSqr():Number { + return x*x + y*y + z*z; + } + + /** + * + * @param length + * @return + */ + public function vSetLength(length:Number):Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = length; + } else { + var k:Number = length/Math.sqrt(x*x + y*y + z*z); + x *= k; + y *= k; + z *= k; + } + return this; + } + + /** + * + * @return + */ + public function vNormalize():Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = 1; + } else { + d = Math.sqrt(d); + x /= d; + y /= d; + z /= d; + } + return this; + } + + /** + * Сложение координат. + * + * @param v точка, координаты которой прибавляются к собственным + */ + public function vAdd(v:Vector3):Vector3 { + x += v.x; + y += v.y; + z += v.z; + return this; + } + + /** + * + * @param k + * @param v + * @return + */ + public function vAddScaled(k:Number, v:Vector3):Vector3 { + x += k*v.x; + y += k*v.y; + z += k*v.z; + return this; + } + + /** + * Вычитание координат. + * + * @param v точка, координаты которой вычитаются из собственных + */ + public function vSubtract(v:Vector3):Vector3 { + x -= v.x; + y -= v.y; + z -= v.z; + return this; + } + + /** + * Вычисляет сумму векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + * @return this + */ + public function vSum(a:Vector3, b:Vector3):Vector3 { + x = a.x + b.x; + y = a.y + b.y; + z = a.z + b.z; + return this; + } + + /** + * Вычисление разности векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + */ + public function vDiff(a:Vector3, b:Vector3):Vector3 { + x = a.x - b.x; + y = a.y - b.y; + z = a.z - b.z; + return this; + } + + /** + * Умножение на скаляр. + * + * @param k число, на которое умножаются координаты + */ + public function vScale(k:Number):Vector3 { + x *= k; + y *= k; + z *= k; + return this; + } + + /** + * Инвертирование вектора. + */ + public function vReverse():Vector3 { + x = -x; + y = -y; + z = -z; + return this; + } + + /** + * + * @param v + * @return + */ + public function vDot(v:Vector3):Number { + return x*v.x + y*v.y + z*v.z; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross(v:Vector3):Vector3 { + var xx:Number = y*v.z - z*v.y; + var yy:Number = z*v.x - x*v.z; + var zz:Number = x*v.y - y*v.x; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross2(a:Vector3, b:Vector3):Vector3 { + x = a.y*b.z - a.z*b.y; + y = a.z*b.x - a.x*b.z; + z = a.x*b.y - a.y*b.x; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy3(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * + * @param matrix + */ + public function vTransformBy3Tr(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz + m.d; + y = m.e*xx + m.f*yy + m.g*zz + m.h; + z = m.i*xx + m.j*yy + m.k*zz + m.l; + return this; + } + + /** + * + * @param m + * @return + */ + public function vTransformInverseBy4(m:Matrix4):Vector3 { + var xx:Number = x - m.d; + var yy:Number = y - m.h; + var zz:Number = z - m.l; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * + * @param m + */ + public function vDeltaTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * Установка координат. + */ + public function vReset(x:Number = 0, y:Number = 0, z:Number = 0):Vector3 { + this.x = x; + this.y = y; + this.z = z; + return this; + } + + /** + * Копирование координат точки. + * + * @param v точка, координаты которой копируются + */ + public function vCopy(v:Vector3):Vector3 { + x = v.x; + y = v.y; + z = v.z; + return this; + } + + /** + * Клонирование точки. + * + * @return клонированная точка + */ + public function vClone():Vector3 { + return new Vector3(x, y, z); + } + + /** + * Копирует координаты вектора в экземпляр класса Vector3D. + * + * @param result экземпляр, в который копируются координаты + * @return переданный параметр + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + return result; + } + + /** + * @param source + * @return + */ + public function copyFromVector3D(source:Vector3D):Vector3 { + x = source.x; + y = source.y; + z = source.z; + return this; + } + + override public function toString():String { + return "Vector3(" + x + "," + y + ", " + z + ")"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/.svn/all-wcprops b/0.0.9.3/src/alternativa/physics/.svn/all-wcprops new file mode 100644 index 0000000..eae727c --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/.svn/all-wcprops @@ -0,0 +1,77 @@ +K 25 +svn:wc:ra_dav:version-url +V 103 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics +END +Body.as +K 25 +svn:wc:ra_dav:version-url +V 111 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/Body.as +END +BodyState.as +K 25 +svn:wc:ra_dav:version-url +V 116 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/BodyState.as +END +CollisionPrimitiveListItem.as +K 25 +svn:wc:ra_dav:version-url +V 133 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/CollisionPrimitiveListItem.as +END +CollisionPrimitiveList.as +K 25 +svn:wc:ra_dav:version-url +V 129 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/CollisionPrimitiveList.as +END +PhysicsScene.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/PhysicsScene.as +END +PhysicsUtils.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/PhysicsUtils.as +END +altphysics.as +K 25 +svn:wc:ra_dav:version-url +V 117 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/altphysics.as +END +ContactPoint.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/ContactPoint.as +END +BodyListItem.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/BodyListItem.as +END +BodyList.as +K 25 +svn:wc:ra_dav:version-url +V 115 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/BodyList.as +END +Contact.as +K 25 +svn:wc:ra_dav:version-url +V 114 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/Contact.as +END +BodyMaterial.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/BodyMaterial.as +END diff --git a/0.0.9.3/src/alternativa/physics/.svn/entries b/0.0.9.3/src/alternativa/physics/.svn/entries new file mode 100644 index 0000000..5a02f64 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/.svn/entries @@ -0,0 +1,181 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics +http://svndev.alternativaplatform.com + + + +2010-07-03T11:40:52.545629Z +36684 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +CollisionPrimitiveList.as +file + + + + +2010-10-28T04:32:44.000000Z +5752b1f4909c5146720b58fbfffed468 +2009-11-01T12:35:52.270571Z +22955 +mike + +altphysics.as +file + + + + +2010-10-28T04:32:44.000000Z +04fbe40a27502dbbd0dba02a76b2df50 +2009-04-20T20:09:14.715403Z +11579 +mike + +collision +dir + +primitives +dir + +BodyList.as +file + + + + +2010-10-28T04:32:44.000000Z +08a90332660bd0273728006d1402dd25 +2009-11-01T12:35:52.270571Z +22955 +mike + +Contact.as +file + + + + +2010-10-28T04:32:44.000000Z +eb89f4f37bcf2d21eadc733f244fcf64 +2010-02-12T12:34:25.192477Z +28424 +mike + +Body.as +file + + + + +2010-10-28T04:32:44.000000Z +355d0fb70dbb4946d4090a7c94c0d58a +2010-02-12T12:34:25.192477Z +28424 +mike + +BodyState.as +file + + + + +2010-10-28T04:32:44.000000Z +5be68820e6f6185d62b3928c39dde285 +2009-11-24T08:44:15.941480Z +23758 +mike + +CollisionPrimitiveListItem.as +file + + + + +2010-10-28T04:32:44.000000Z +cec5dbfefef812d87481478455f367b1 +2009-10-19T07:17:33.113306Z +22253 +mike + +PhysicsScene.as +file + + + + +2010-10-28T04:32:44.000000Z +96c74784bb71c06141cc469eec57ccb8 +2010-02-19T13:09:00.412754Z +28731 +mike + +PhysicsUtils.as +file + + + + +2010-10-28T04:32:44.000000Z +dce0a94f8a861291a2a8942ed0ba54a5 +2009-11-24T08:44:15.941480Z +23758 +mike + +constraints +dir + +ContactPoint.as +file + + + + +2010-10-28T04:32:44.000000Z +14f23db94810328dedd7da2992812ced +2009-11-24T08:44:15.941480Z +23758 +mike + +BodyListItem.as +file + + + + +2010-10-28T04:32:44.000000Z +2fc2b598310b24aef3f8b90e14a8ba4f +2009-10-19T07:17:33.113306Z +22253 +mike + +BodyMaterial.as +file + + + + +2010-10-28T04:32:44.000000Z +9b700692ebb8017882f3e8cec095e1a9 +2009-10-19T07:17:33.113306Z +22253 +mike + diff --git a/0.0.9.3/src/alternativa/physics/.svn/format b/0.0.9.3/src/alternativa/physics/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.3/src/alternativa/physics/.svn/text-base/Body.as.svn-base b/0.0.9.3/src/alternativa/physics/.svn/text-base/Body.as.svn-base new file mode 100644 index 0000000..cd22c5b --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/.svn/text-base/Body.as.svn-base @@ -0,0 +1,405 @@ +package alternativa.physics { + + import alternativa.math.Matrix3; + import alternativa.math.Matrix4; + import alternativa.math.Quaternion; + import alternativa.math.Vector3; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.IBodyCollisionPredicate; + import alternativa.physics.collision.types.BoundBox; + + use namespace altphysics; + + /** + * + */ + public class Body { + + public static var linDamping:Number = 0.997; + public static var rotDamping:Number = 0.997; + + // Идентификатор тела, уникальный в пределах мира + public var id:int; + // Имя тела + public var name:String; + // Мир, в котором находится тело + public var world:PhysicsScene; + // Флаг подвижности тела + public var movable:Boolean = true; + // Флаг указывает, может ли тело быть заморожено + public var canFreeze:Boolean = false; + + public var freezeCounter:int; + public var frozen:Boolean = false; + // Ограничивающий бокс тела + public var aabb:BoundBox = new BoundBox(); + // Предикат, через который тело получает сообщения о столкновениях + public var postCollisionPredicate:IBodyCollisionPredicate; + + // Текущее состояние тела + public var state:BodyState = new BodyState(); + // Предыдущее состояние тела + public var prevState:BodyState = new BodyState(); + // Линейное ускорение тела на текущем шаге симуляции + public var accel:Vector3 = new Vector3(); + // Угловое ускорение тела на текущем шаге симуляции + public var angleAccel:Vector3 = new Vector3(); + // Физический материал тела + public var material:BodyMaterial = new BodyMaterial(); + // Обратная масса тела + public var invMass:Number = 1; + // Обратная матрица тензора инерции в локальных координатах + public var invInertia:Matrix3 = new Matrix3(); + // Обратная матрица тензора инерции в мировых координатах + public var invInertiaWorld:Matrix3 = new Matrix3(); + // Базисная матрица тела в мировых координатах + public var baseMatrix:Matrix3 = new Matrix3(); + + public const MAX_CONTACTS:int = 20; + public var contacts:Vector. = new Vector.(MAX_CONTACTS); + public var contactsNum:int; + + public var collisionPrimitives:CollisionPrimitiveList; + + // Аккумулятор сил + public var forceAccum:Vector3 = new Vector3(); + // Аккумулятор моментов + public var torqueAccum:Vector3 = new Vector3(); + + // Внутренние переменные для избежания создания экземпляров + private static var _r:Vector3 = new Vector3(); + private static var _f:Vector3 = new Vector3(); + + /** + * + * @param invMass + * @param invInertia + */ + public function Body(invMass:Number, invInertia:Matrix3) { + this.invMass = invMass; + this.invInertia.copy(invInertia); + } + + /** + * @param primitive + * @param localTransform + */ + public function addCollisionPrimitive(primitive:CollisionPrimitive, localTransform:Matrix4 = null):void { + if (primitive == null) { + throw new ArgumentError("Primitive cannot be null"); + } + if (collisionPrimitives == null) { + collisionPrimitives = new CollisionPrimitiveList(); + } + collisionPrimitives.append(primitive); + primitive.setBody(this, localTransform); + } + + /** + * + * @param primitive + */ + public function removeCollisionPrimitive(primitive:CollisionPrimitive):void { + if (collisionPrimitives == null) return; + primitive.setBody(null); + collisionPrimitives.remove(primitive); + if (collisionPrimitives.size == 0) { + collisionPrimitives = null; + } + } + + /** + * @param t + * @param result + */ + public function interpolate(t:Number, pos:Vector3, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.w = prevState.orientation.w*t1 + state.orientation.w*t; + orientation.x = prevState.orientation.x*t1 + state.orientation.x*t; + orientation.y = prevState.orientation.y*t1 + state.orientation.y*t; + orientation.z = prevState.orientation.z*t1 + state.orientation.z*t; + } + + /** + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + state.pos.vCopy(pos); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setPositionXYZ(x:Number, y:Number, z:Number):void { + state.pos.vReset(x, y, z); + } + + /** + * + * @param vel + */ + public function setVelocity(vel:Vector3):void { + state.velocity.vCopy(vel); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setVelocityXYZ(x:Number, y:Number, z:Number):void { + state.velocity.vReset(x, y, z); + } + + /** + * + * @param rot + */ + public function setRotation(rot:Vector3):void { + state.rotation.vCopy(rot); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setRotationXYZ(x:Number, y:Number, z:Number):void { + state.rotation.vReset(x, y, z); + } + + /** + * + * @param q + */ + public function setOrientation(q:Quaternion):void { + state.orientation.copy(q); + } + + /** + * @param r + * @param dir + * @param magnitude + */ + public function applyRelPosWorldImpulse(r:Vector3, dir:Vector3, magnitude:Number):void { + var d:Number = magnitude*invMass; + // Линейная часть + state.velocity.x += d*dir.x; + state.velocity.y += d*dir.y; + state.velocity.z += d*dir.z; + + // Вращательная часть + var x:Number = (r.y*dir.z - r.z*dir.y)*magnitude; + var y:Number = (r.z*dir.x - r.x*dir.z)*magnitude; + var z:Number = (r.x*dir.y - r.y*dir.x)*magnitude; + + state.rotation.x += invInertiaWorld.a*x + invInertiaWorld.b*y + invInertiaWorld.c*z; + state.rotation.y += invInertiaWorld.e*x + invInertiaWorld.f*y + invInertiaWorld.g*z; + state.rotation.z += invInertiaWorld.i*x + invInertiaWorld.j*y + invInertiaWorld.k*z; + } + + /** + * @param f + */ + public function addForce(f:Vector3):void { + forceAccum.vAdd(f); + } + + /** + * + * @param fx + * @param fy + * @param fz + */ + public function addForceXYZ(fx:Number, fy:Number, fz:Number):void { + forceAccum.x += fx; + forceAccum.y += fy; + forceAccum.z += fz; + } + + /** + * @param pos + * @param f + */ + public function addWorldForceXYZ(px:Number, py:Number, pz:Number, fx:Number, fy:Number, fz:Number):void { + forceAccum.x += fx; + forceAccum.y += fy; + forceAccum.z += fz; + + var pos:Vector3 = state.pos; + var rx:Number = px - pos.x; + var ry:Number = py - pos.y; + var rz:Number = pz - pos.z; + +// var x:Number = ry*fz - rz*fy; +// var y:Number = rz*fx - rx*fz; +// var z:Number = rx*fy - ry*fx; + + torqueAccum.x += ry*fz - rz*fy; + torqueAccum.y += rz*fx - rx*fz; + torqueAccum.z += rx*fy - ry*fx; + } + + /** + * @param pos + * @param f + */ + public function addWorldForce(pos:Vector3, force:Vector3):void { + forceAccum.vAdd(force); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(force)); + } + + /** + * @param pos + * @param f + */ + public function addWorldForceScaled(pos:Vector3, force:Vector3, scale:Number):void { + _f.x = scale*force.x; + _f.y = scale*force.y; + _f.z = scale*force.z; + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(_f)); + } + + /** + * @param pos + * @param f + */ + public function addLocalForce(pos:Vector3, force:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(pos, _r); + // Трансформируем вектор силы в мировую систему + baseMatrix.transformVector(force, _f); + // Добавляем силу и момент + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vCross(_f)); + } + + /** + * + * @param localPos + * @param worldForce + */ + public function addWorldForceAtLocalPoint(localPos:Vector3, worldForce:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(localPos, _r); + // Добавляем силу и момент + forceAccum.vAdd(worldForce); + torqueAccum.vAdd(_r.vCross(worldForce)); + } + + /** + * @param dt + */ + public function beforePhysicsStep(dt:Number):void { + } + + /** + * @param t + */ + public function addTorque(t:Vector3):void { + torqueAccum.vAdd(t); + } + + /** + * + */ + altphysics function clearAccumulators():void { + forceAccum.x = forceAccum.y = forceAccum.z = 0; + torqueAccum.x = torqueAccum.y = torqueAccum.z = 0; + } + + /** + * + */ + altphysics function calcAccelerations():void { + accel.x = forceAccum.x*invMass; + accel.y = forceAccum.y*invMass; + accel.z = forceAccum.z*invMass; + angleAccel.x = invInertiaWorld.a*torqueAccum.x + invInertiaWorld.b*torqueAccum.y + invInertiaWorld.c*torqueAccum.z; + angleAccel.y = invInertiaWorld.e*torqueAccum.x + invInertiaWorld.f*torqueAccum.y + invInertiaWorld.g*torqueAccum.z; + angleAccel.z = invInertiaWorld.i*torqueAccum.x + invInertiaWorld.j*torqueAccum.y + invInertiaWorld.k*torqueAccum.z; + } + + /** + * Вычисляет производные данные. + */ + public function calcDerivedData():void { + // Вычисление базисной матрицы и обратного тензора инерции в мировых координатах + state.orientation.toMatrix3(baseMatrix); + invInertiaWorld.copy(invInertia).append(baseMatrix).prependTransposed(baseMatrix); + if (collisionPrimitives != null) { + aabb.infinity(); + var item:CollisionPrimitiveListItem = collisionPrimitives.head; + while (item != null) { + var primitive:CollisionPrimitive = item.primitive; + primitive.transform.setFromMatrix3(baseMatrix, state.pos); + if (primitive.localTransform != null) { + primitive.transform.prepend(primitive.localTransform); + } + primitive.calculateAABB(); + aabb.addBoundBox(primitive.aabb); + item = item.next; + } + } + } + + /** + * + */ + altphysics function saveState():void { + prevState.copy(state); + } + + /** + * + */ + altphysics function restoreState():void { + state.copy(prevState); + } + + /** + * @param dt + */ + altphysics function integrateVelocity(dt:Number):void { + // v = v + a*t + state.velocity.x += accel.x*dt; + state.velocity.y += accel.y*dt; + state.velocity.z += accel.z*dt; + // rot = rot + eps*t + state.rotation.x += angleAccel.x*dt; + state.rotation.y += angleAccel.y*dt; + state.rotation.z += angleAccel.z*dt; + + state.velocity.x *= linDamping; + state.velocity.y *= linDamping; + state.velocity.z *= linDamping; + + state.rotation.x *= rotDamping; + state.rotation.y *= rotDamping; + state.rotation.z *= rotDamping; + } + + /** + * + */ + altphysics function integratePosition(dt:Number):void { + // pos = pos + v*t + state.pos.x += state.velocity.x*dt; + state.pos.y += state.velocity.y*dt; + state.pos.z += state.velocity.z*dt; + // q = q + 0.5*rot*q + state.orientation.addScaledVector(state.rotation, dt); + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/.svn/text-base/BodyList.as.svn-base b/0.0.9.3/src/alternativa/physics/.svn/text-base/BodyList.as.svn-base new file mode 100644 index 0000000..aa8f79f --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/.svn/text-base/BodyList.as.svn-base @@ -0,0 +1,76 @@ +package alternativa.physics { + + /** + * + */ + public class BodyList { + + public var head:BodyListItem; + public var tail:BodyListItem; + public var size:int; + + /** + * + */ + public function BodyList() { + } + + /** + * + * @param body + */ + public function append(body:Body):void { + var item:BodyListItem = BodyListItem.create(body); + if (head == null) { + head = tail = item; + } else { + tail.next = item; + item.prev = tail; + tail = item; + } + size++; + } + + /** + * + * @param body + */ + public function remove(body:Body):Boolean { + var item:BodyListItem = findItem(body); + if (item == null) return false; + if (item == head) { + if (size == 1) { + head = tail = null; + } else { + head = item.next; + head.prev = null; + } + } else { + if (item == tail) { + tail = item.prev; + tail.next = null; + } else { + item.prev.next = item.next; + item.next.prev = item.prev; + } + } + item.dispose(); + size--; + return true; + } + + /** + * + * @param body + * @return + */ + public function findItem(body:Body):BodyListItem { + var item:BodyListItem = head; + while (item != null && item.body != body) { + item = item.next; + } + return item; + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/.svn/text-base/BodyListItem.as.svn-base b/0.0.9.3/src/alternativa/physics/.svn/text-base/BodyListItem.as.svn-base new file mode 100644 index 0000000..382e11c --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/.svn/text-base/BodyListItem.as.svn-base @@ -0,0 +1,74 @@ +package alternativa.physics { + + /** + * + */ + public class BodyListItem { + + // Верхний элемент хранилища + private static var poolTop:BodyListItem; + + /** + * Создаёт новый элемент списка. + * + * @param primitive примитив, содержащийся в элементе + * @return новый элемент списка + */ + public static function create(body:Body):BodyListItem { + var item:BodyListItem; + if (poolTop == null) { + item = new BodyListItem(body); + } else { + item = poolTop; + poolTop = item.next; + item.next = null; + item.body = body; + } + return item; + } + + /** + * Очищает хранилище. + */ + public static function clearPool():void { + var item:BodyListItem = poolTop; + while (item != null) { + poolTop = item.next; + item.next = null; + item = poolTop; + } + } + + /** + * + */ + public var body:Body; + /** + * + */ + public var next:BodyListItem; + /** + * + */ + public var prev:BodyListItem; + + /** + * + * @param body + */ + public function BodyListItem(body:Body) { + this.body = body; + } + + /** + * + */ + public function dispose():void { + body = null; + prev = null; + next = poolTop; + poolTop = this; + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/.svn/text-base/BodyMaterial.as.svn-base b/0.0.9.3/src/alternativa/physics/.svn/text-base/BodyMaterial.as.svn-base new file mode 100644 index 0000000..7a7c47d --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/.svn/text-base/BodyMaterial.as.svn-base @@ -0,0 +1,11 @@ +package alternativa.physics { + + public class BodyMaterial { + + public var restitution:Number = 0; + public var friction:Number = 0.3; +// public var dynamicFriction:Number = 0.2; +// public var dynamicFriction:Number = 0.2; + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/.svn/text-base/BodyState.as.svn-base b/0.0.9.3/src/alternativa/physics/.svn/text-base/BodyState.as.svn-base new file mode 100644 index 0000000..3987388 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/.svn/text-base/BodyState.as.svn-base @@ -0,0 +1,39 @@ +package alternativa.physics { + import alternativa.math.Quaternion; + import alternativa.math.Vector3; + + /** + * Класс описывает состояние твёрдого тела. + */ + public class BodyState { + /** + * Положение тела. + */ + public var pos:Vector3 = new Vector3(); + /** + * Ориентация тела. + */ + public var orientation:Quaternion = new Quaternion(); + /** + * Скорость тела. + */ + public var velocity:Vector3 = new Vector3(); + /** + * Угловая скорость тела. + */ + public var rotation:Vector3 = new Vector3(); + + /** + * Копирует значение указанного объекта. + * + * @param state + */ + public function copy(state:BodyState):void { + pos.vCopy(state.pos); + orientation.copy(state.orientation); + velocity.vCopy(state.velocity); + rotation.vCopy(state.rotation); + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/.svn/text-base/CollisionPrimitiveList.as.svn-base b/0.0.9.3/src/alternativa/physics/.svn/text-base/CollisionPrimitiveList.as.svn-base new file mode 100644 index 0000000..5d5304f --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/.svn/text-base/CollisionPrimitiveList.as.svn-base @@ -0,0 +1,89 @@ +package alternativa.physics { + import alternativa.physics.collision.CollisionPrimitive; + + /** + * + */ + public class CollisionPrimitiveList { + + public var head:CollisionPrimitiveListItem; + public var tail:CollisionPrimitiveListItem; + public var size:int; + + /** + * + */ + public function CollisionPrimitiveList() { + } + + /** + * + * @param primitive + */ + public function append(primitive:CollisionPrimitive):void { + var item:CollisionPrimitiveListItem = CollisionPrimitiveListItem.create(primitive); + if (head == null) { + head = tail = item; + } else { + tail.next = item; + item.prev = tail; + tail = item; + } + size++; + } + + /** + * + * @param primitve + */ + public function remove(primitve:CollisionPrimitive):void { + var item:CollisionPrimitiveListItem = findItem(primitve); + if (item == null) return; + if (item == head) { + if (size == 1) { + head = tail = null; + } else { + head = item.next; + head.prev = null; + } + } else { + if (item == tail) { + tail = tail.prev; + tail.next = null; + } else { + item.prev.next = item.next; + item.next.prev = item.prev; + } + } + item.dispose(); + size--; + } + + /** + * + * @param primitive + * @return + */ + public function findItem(primitive:CollisionPrimitive):CollisionPrimitiveListItem { + var item:CollisionPrimitiveListItem = head; + while (item != null && item.primitive != primitive) { + item = item.next; + } + return item; + } + + /** + * + */ + public function clear():void { + while (head != null) { + var item:CollisionPrimitiveListItem = head; + head = head.next; + item.dispose(); + } + tail = null; + size = 0; + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/.svn/text-base/CollisionPrimitiveListItem.as.svn-base b/0.0.9.3/src/alternativa/physics/.svn/text-base/CollisionPrimitiveListItem.as.svn-base new file mode 100644 index 0000000..d204532 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/.svn/text-base/CollisionPrimitiveListItem.as.svn-base @@ -0,0 +1,76 @@ +package alternativa.physics { + import alternativa.physics.collision.CollisionPrimitive; + + /** + * Элемент списка примитивов. + */ + public class CollisionPrimitiveListItem { + + // Верхний элемент хранилища + private static var poolTop:CollisionPrimitiveListItem; + + /** + * Создаёт новый элемент списка. + * + * @param primitive примитив, содержащийся в элементе + * @return новый элемент списка + */ + public static function create(primitive:CollisionPrimitive):CollisionPrimitiveListItem { + var item:CollisionPrimitiveListItem; + if (poolTop == null) { + item = new CollisionPrimitiveListItem(primitive); + } else { + item = poolTop; + item.primitive = primitive; + poolTop = item.next; + item.next = null; + } + return item; + } + + /** + * Очищает хранилище. + */ + public static function clearPool():void { + var curr:CollisionPrimitiveListItem = poolTop; + while (curr != null) { + poolTop = curr.next; + curr.next = null; + curr = poolTop; + } + } + + /** + * Примитив, хранящийся в элементе списка. + */ + public var primitive:CollisionPrimitive; + /** + * Ссылка не следующий элемент списка. + */ + public var next:CollisionPrimitiveListItem; + /** + * Ссылка не предыдущий элемент списка. + */ + public var prev:CollisionPrimitiveListItem; + + /** + * Создаёт новый экземпляр. + * + * @param primitive примитив, хранящийся в элементе списка + */ + public function CollisionPrimitiveListItem(primitive:CollisionPrimitive) { + this.primitive = primitive; + } + + /** + * Очищает внутренние ссылки и помещает элемент в хранилище для дальнейшего использования. + */ + public function dispose():void { + primitive = null; + prev = null; + next = poolTop; + poolTop = this; + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/.svn/text-base/Contact.as.svn-base b/0.0.9.3/src/alternativa/physics/.svn/text-base/Contact.as.svn-base new file mode 100644 index 0000000..3f010ec --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/.svn/text-base/Contact.as.svn-base @@ -0,0 +1,69 @@ +package alternativa.physics { + + + import alternativa.math.Vector3; + + /** + * Описывает контакт одного или двух тел. + */ + public class Contact { + /** + * Первое тело контакта, не может быть равно null. + */ + public var body1:Body; + /** + * Второе тело контакта, может быть равно null. + */ + public var body2:Body; + /** + * Предрассчитанный взаимный коэффициент отскока. + */ + public var restitution:Number; + /** + * Предрассчитанный взаимный коэффициент трения. + */ + public var friction:Number; + /** + * Нормаль контакта. Направлена к первому телу. + */ + public var normal:Vector3 = new Vector3(); + /** + * Список точек контакта. + */ + public var points:Vector. = new Vector.(MAX_POINTS, true); + /** + * Количество точек контакта. + */ + public var pcount:int; + /** + * Максимальная глубина пересечения. + */ + public var maxPenetration:Number = 0; + /** + * Флаг показывает, разрешён контакт или нет. + */ + public var satisfied:Boolean; + /** + * Следующий контакт в списке. + */ + public var next:Contact; + /** + * Индекс контакта. Первый контакт в списке имеет индекс 0. + */ + public var index:int; + + // Максимальное количество точек контакта + private const MAX_POINTS:int = 8; + + /** + * + */ + public function Contact(index:int) { + this.index = index; + for (var i:int = 0; i < MAX_POINTS; i++) { + points[i] = new ContactPoint(); + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/.svn/text-base/ContactPoint.as.svn-base b/0.0.9.3/src/alternativa/physics/.svn/text-base/ContactPoint.as.svn-base new file mode 100644 index 0000000..395d2dc --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/.svn/text-base/ContactPoint.as.svn-base @@ -0,0 +1,55 @@ +package alternativa.physics { + import alternativa.math.Vector3; + + /** + * + */ + public class ContactPoint { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + + // Величины, расчитываемые перед началом фазы решения контактов + + // Требуемая проекция конечной скорости на нормаль для упругого контакта + public var normalVel:Number; + // Минимальная скорость разделения неупругого контакта + public var minSepVel:Number; + // Изменение проекции скорости на единицу нормального импульса + public var velByUnitImpulseN:Number; + + public var angularInertia1:Number; + public var angularInertia2:Number; + + // Радиус-вектор точки контакта относительно центра первого тела + public var r1:Vector3 = new Vector3(); + // Радиус-вектор точки контакта относительно центра второго тела + public var r2:Vector3 = new Vector3(); + + // Величины, накапливаемые во время фазы решения контактов + + // Накопленный импульс, применяемый для получения требуемой относительной скорости в точке контакта + public var accumImpulseN:Number; + // Накопленный импульс, применяемый для разделения тел в точке контакта. Не создаёт момента. +// public var accumSepImpulse:Number; + + public var satisfied:Boolean; + + /** + * + * @param cp + */ + public function copyFrom(cp:ContactPoint):void { + pos.vCopy(cp.pos); + penetration = cp.penetration; + feature1 = cp.feature1; + feature2 = cp.feature2; + r1.vCopy(cp.r1); + r2.vCopy(cp.r2); + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/.svn/text-base/PhysicsScene.as.svn-base b/0.0.9.3/src/alternativa/physics/.svn/text-base/PhysicsScene.as.svn-base new file mode 100644 index 0000000..abdc762 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/.svn/text-base/PhysicsScene.as.svn-base @@ -0,0 +1,743 @@ +package alternativa.physics { + + + import alternativa.physics.collision.ICollisionDetector; + import alternativa.physics.collision.KdTreeCollisionDetector; + import alternativa.physics.constraints.Constraint; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * Класс реализует физическую симуляцию поведения твёрдых тел. + */ + public class PhysicsScene { + + private static var lastBodyId:int; + + // Максимальное количество контактов + public const MAX_CONTACTS:int = 1000; + + ////////////////////////////////////////////////////////////////// + // Настроечные параметры симуляции. Могут быть изменены в любой + // момент времени без нарушения корректной работы симуляции. + ////////////////////////////////////////////////////////////////// + // Количество шагов, за которое пересекающиеся тела должны разделиться + public var penResolutionSteps:int = 10; + // Величина допустимой глубины пересечения + public var allowedPenetration:Number = 0.1; + // Максимальная скорость, добавляемая с целью разделения тел + public var maxPenResolutionSpeed:Number = 0.5; + // Количество итераций для обработки упругих контактов + public var collisionIterations:int = 5; + // Количество итераций для обработки неупругих контактов + public var contactIterations:int = 5; + // Флаг использования предсказания состояний + public var usePrediction:Boolean = false; + + public var freezeSteps:int = 10; + public var linSpeedFreezeLimit:Number = 1; + public var angSpeedFreezeLimit:Number = 0.01; + + // Переменные для процедуры разделения тел путём непосредственного их перемещенеия. + // !!!!!!!!!!!!!!!!!!!!!!!! + // !!! Экспериментально !!! + // !!!!!!!!!!!!!!!!!!!!!!!! + public var staticSeparationIterations:int = 10; + public var staticSeparationSteps:int = 10; + public var maxAngleMove:Number = 10; + public var useStaticSeparation:Boolean = false; + + // Вектор гравитации + public var _gravity:Vector3 = new Vector3(0, 0, -9.8); + // Модуль вектора гравитации + public var _gravityMagnitude:Number = 9.8; + + // Использующийся детектор столкновений + public var collisionDetector:ICollisionDetector; + + // Список тел, участвующих в симуляции + public var bodies:BodyList = new BodyList(); + + // Список контактов на текущем шаге симуляции + altphysics var contacts:Contact; + // Количество контактов на текущем шаге симуляции +// altphysics var contactsNum:int; + // Список ограничений + altphysics var constraints:Vector. = new Vector.(); + // Количество ограничений + altphysics var constraintsNum:int; + // Временная метка. Число прошедших шагов с начала симуляции. + public var timeStamp:int; + // Время с начала симуляции, мс + public var time:int; + // Первый неиспользованный контакт на текущем шаге симуляции + private var borderContact:Contact; + + // Временные переменные для избежания создания экземпляров + private var _r:Vector3 = new Vector3(); + private var _t:Vector3 = new Vector3(); + private var _v:Vector3 = new Vector3(); + private var _v1:Vector3 = new Vector3(); + private var _v2:Vector3 = new Vector3(); + + /** + * + */ + public function PhysicsScene() { + contacts = new Contact(0); + var contact:Contact = contacts; + for (var i:int = 1; i < MAX_CONTACTS; i++) { + contact.next = new Contact(i); + contact = contact.next; + } + collisionDetector = new KdTreeCollisionDetector(); + } + + /** + * Вектор гравитации. + */ + public function get gravity():Vector3 { + return _gravity.vClone(); + } + + /** + * @private + */ + public function set gravity(value:Vector3):void { + _gravity.vCopy(value); + _gravityMagnitude = _gravity.vLength(); + } + + /** + * Добавляет тело в симуляцию. + * + * @param body + */ + public function addBody(body:Body):void { + body.id = lastBodyId++; + body.world = this; + bodies.append(body); + } + + /** + * Удаляет тело из симуляции. + * @param body + * @return + */ + public function removeBody(body:Body):void { + if (bodies.remove(body)) { + body.world = null; + } + } + + /** + * Добавляет ограничение. + * @param c + */ + public function addConstraint(c:Constraint):void { + constraints[constraintsNum++] = c; + c.world = this; + } + + /** + * Удаляет ограничение. + * @param c + */ + public function removeConstraint(c:Constraint):Boolean { + var idx:int = constraints.indexOf(c); + if (idx < 0) return false; + constraints.splice(idx, 1) + constraintsNum--; + c.world = null; + return true; + } + + /** + * Применяет к телам действующте на них силы. Аккумуляторы сил и моментов тел очищаются после завершения шага + * симуляции, поэтому на момент вызова метода могут уже содержать некоторые значения. + * + * @param dt промежуток времени, в течении которого действуют силы + */ + private function applyForces(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + body.beforePhysicsStep(dt); + body.calcAccelerations(); + // Ускорение свободного падения применяется только к подвижным телам во избежание некорректного изменения + // фиктивной скорости неподвижных тел. + if (body.movable && !body.frozen) { + body.accel.x += _gravity.x; + body.accel.y += _gravity.y; + body.accel.z += _gravity.z; + } + item = item.next; + } + } + + /** + * Определяет все столкновения на текущем шаге симуляции и заполняет список получившихся контактов. + * + * @param dt длительность шага симуляции + */ + private function detectCollisions(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + if (!body.frozen) { + body.contactsNum = 0; + body.saveState(); + // При включённом режиме предсказания состояние тел интегрируется на один шаг вперёд + if (usePrediction) { + body.integrateVelocity(dt); + body.integratePosition(dt); + } + body.calcDerivedData(); + } + item = item.next; + } + + borderContact = collisionDetector.getAllContacts(contacts); + + // Расчёт относительных векторов точки контакта вынесен сюда из-за необходимости учитывать + // положение тел в предсказанном состоянии + var contact:Contact = contacts; + while (contact != borderContact) { + var b1:Body = contact.body1; + var b2:Body = contact.body2; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + var bPos:Vector3 = b1.state.pos; + cp.r1.x = cp.pos.x - bPos.x; + cp.r1.y = cp.pos.y - bPos.y; + cp.r1.z = cp.pos.z - bPos.z; + if (b2 != null) { + bPos = b2.state.pos; + cp.r2.x = cp.pos.x - bPos.x; + cp.r2.y = cp.pos.y - bPos.y; + cp.r2.z = cp.pos.z - bPos.z; + } + } + contact = contact.next; + } + + // Восстановление состояния тел + if (usePrediction) { + item = bodies.head; + while (item != null) { + body = item.body; + if (!body.frozen) { + body.restoreState(); + body.calcDerivedData(); + } + item = item.next; + } + } + } + + /** + * Подготваливает полученные из детектора столкновений контакты, расчитывая значения, не меняющиеся + * в ходе шага симуляции. + */ + private function preProcessContacts(dt:Number):void { + var contact:Contact = contacts; + while (contact != borderContact) { + var b1:Body = contact.body1; + var b2:Body = contact.body2; + // Столкнувшиеся тела размораживаются + if (b1.frozen) { + b1.frozen = false; + b1.freezeCounter = 0; + } + if (b2 != null && b2.frozen) { + b2.frozen = false; + b2.freezeCounter = 0; + } + contact.restitution = b1.material.restitution; + if (b2 != null && b2.material.restitution < contact.restitution) contact.restitution = b2.material.restitution; + contact.friction = b1.material.friction; + if (b2 != null && b2.material.friction < contact.friction) contact.friction = b2.material.friction; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.accumImpulseN = 0; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + cp.velByUnitImpulseN = 0; + if (b1.movable) { + cp.angularInertia1 = _v.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1).vDot(contact.normal); + cp.velByUnitImpulseN += b1.invMass + cp.angularInertia1; + } + if (b2 != null && b2.movable) { + cp.angularInertia2 = _v.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2).vDot(contact.normal); + cp.velByUnitImpulseN += b2.invMass + cp.angularInertia2; + } + // Расчёт требуемой конечной скорости для упругого контакта + calcSepVelocity(b1, b2, cp, _v); + cp.normalVel = _v.vDot(contact.normal); + if (cp.normalVel < 0) cp.normalVel = - contact.restitution*cp.normalVel; + // Скорость разделения неупругого контакта + cp.minSepVel = cp.penetration > allowedPenetration ? (cp.penetration - allowedPenetration)/(penResolutionSteps*dt) : 0; + if (cp.minSepVel > maxPenResolutionSpeed) cp.minSepVel = maxPenResolutionSpeed; + } + contact = contact.next; + } + for (var i:int = 0; i < constraintsNum; i++) { + var constraint:Constraint = constraints[i]; + constraint.preProcess(dt); + } + } + + /** + * + * @param dt + * @param forceInelastic + */ + private function processContacts(dt:Number, forceInelastic:Boolean):void { + var iterNum:int = forceInelastic ? contactIterations : collisionIterations; + var i:int; + var forwardLoop:Boolean = false; + for (var iter:int = 0; iter < iterNum; iter++) { + forwardLoop = !forwardLoop; + var contact:Contact = contacts; + while (contact != borderContact) { + resolveContact(contact, forceInelastic, forwardLoop); + contact = contact.next; + } + // Ограничения + for (i = 0; i < constraintsNum; i++) { + var constraint:Constraint = constraints[i]; + constraint.apply(dt); + } + } + + // Разделение контактов путём непосредственного изменения координат и ориентации. Экспериментально. +// if (forceInelastic && useStaticSeparation) performStaticSeparation(); + } + + /** + * + */ + private function resolveContact(contactInfo:Contact, forceInelastic:Boolean, forwardLoop:Boolean):void { + var b1:Body = contactInfo.body1; + var b2:Body = contactInfo.body2; + var normal:Vector3 = contactInfo.normal; + var i:int; + if (forwardLoop) { + for (i = 0; i < contactInfo.pcount; i++) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } else { + for (i = contactInfo.pcount - 1; i >= 0; i--) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } + } + + /** + * + * @param idx + * @param b1 + * @param b2 + * @param colInfo + * @param normal + * @param forceInelastic + */ + private function resolveContactPoint(idx:int, b1:Body, b2:Body, contact:Contact, normal:Vector3, forceInelastic:Boolean):void { + var cp:ContactPoint = contact.points[idx]; + if (!forceInelastic) { + cp.satisfied = true; + } + + var newVel:Number = 0; + calcSepVelocity(b1, b2, cp, _v); + var cnormal:Vector3 = contact.normal; + var sepVel:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + if (forceInelastic) { + var minSpeVel:Number = useStaticSeparation ? 0 : cp.minSepVel; + if (sepVel < minSpeVel) { + cp.satisfied = false; + } else if (cp.satisfied) return; + newVel = minSpeVel; + } else { + newVel = cp.normalVel; + } + var deltaVel:Number = newVel - sepVel; + var impulse:Number = deltaVel/cp.velByUnitImpulseN; + var accumImpulse:Number = cp.accumImpulseN + impulse; + if (accumImpulse < 0) { + accumImpulse = 0; + } + var deltaImpulse:Number = accumImpulse - cp.accumImpulseN; + cp.accumImpulseN = accumImpulse; + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, normal, deltaImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, normal, -deltaImpulse); + } + + // Учёт силы трения + calcSepVelocity(b1, b2, cp, _v); + // Расчитываем изменение касательной скорости на единицу касательного импульса + var tanSpeedByUnitImpulse:Number = 0; + +// _v.vAddScaled(-_v.vDot(contact.normal), contact.normal); + var dot:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + _v.x -= dot*cnormal.x; + _v.y -= dot*cnormal.y; + _v.z -= dot*cnormal.z; + + var tanSpeed:Number = _v.vLength(); + if (tanSpeed < 0.001) return; + +// _t.vCopy(_v).vNormalize().vReverse(); + _t.x = -_v.x; + _t.y = -_v.y; + _t.z = -_v.z; + _t.vNormalize(); + + var r:Vector3; + var m:Matrix3; + var xx:Number; + var yy:Number; + var zz:Number; + // dV = b.invMass + ((invI * (r % t)) % r) * t + if (b1.movable) { +// _v.vCross2(cp.r1, _t).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1); + r = cp.r1; + m = b1.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b1.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + if (b2 != null && b2.movable) { +// _v.vCross2(cp.r2, _t).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2); + + r = cp.r2; + m = b2.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b2.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + + var tanImpulse:Number = tanSpeed/tanSpeedByUnitImpulse; + var max:Number = contact.friction*cp.accumImpulseN; + if (max < 0) { + if (tanImpulse < max) tanImpulse = max; + } else { + if (tanImpulse > max) tanImpulse = max; + } + + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, _t, tanImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, _t, -tanImpulse); + } + } + + /** + * + * @param cp + * @param normal + * @return + */ + private function calcSepVelocity(body1:Body, body2:Body, cp:ContactPoint, result:Vector3):void { + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 +// result.vCopy(body1.state.velocity).vAdd(_v1.vCross2(body1.state.rotation, cp.r1)); + var rot:Vector3 = body1.state.rotation; + var v:Vector3 = cp.r1; + var x:Number = rot.y*v.z - rot.z*v.y; + var y:Number = rot.z*v.x - rot.x*v.z; + var z:Number = rot.x*v.y - rot.y*v.x; + v = body1.state.velocity; + result.x = v.x + x; + result.y = v.y + y; + result.z = v.z + z; + // V2 = V2_c + w2%r2 + if (body2 != null) { +// result.vSubtract(body2.state.velocity).vSubtract(_v2.vCross2(body2.state.rotation, cp.r2)); + rot = body2.state.rotation; + v = cp.r2; + x = rot.y*v.z - rot.z*v.y; + y = rot.z*v.x - rot.x*v.z; + z = rot.x*v.y - rot.y*v.x; + v = body2.state.velocity; + result.x -= v.x + x; + result.y -= v.y + y; + result.z -= v.z + z; + } + } + + /** + * + * @param dt + */ + private function intergateVelocities(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + item.body.integrateVelocity(dt); + item = item.next; + } + } + + /** + * + * @param dt + */ + private function integratePositions(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + if (body.movable && !body.frozen) { + body.integratePosition(dt); + } + item = item.next; + } + } + + /** + * + */ + private function performStaticSeparation():void { + var iterNum:int = staticSeparationIterations; +// iterNum = 100; + + // 1. В начале каждой итерации для всех контактов сбрасывается флаг satisfied с одновременным поиском наихудшего контакта, т.е. имеющего наибольшую величину пересечения тел. + // 2. Если найденный контакт имеет величину пересечения меньше предельно допустимой процедура прерывается, т.к. разделение контактов не требуется. + // 3. Если процедура продолжается, то выполняется разделение найденного контакта и установка его флага satisfied в true, после чего запускается внутренний цикл, состоящий + // из contactsNum - 1 итераций. На каждой итерации ищется наихудший контакт среди оставшихся (satisfied == false). Если такой контакт найден, то выполняется его разделение + // и переход к следующей итерации внутреннего цикла, иначе внутренний цикл прерывается и выполняется переход к следующей итерации внешнего цикла (шаг 1.). +// for (var iter:int = 0; iter < iterNum; iter++) { +// // Ищем контакт с максимальной величиной пересечения тел, одновременно сбрасывая флаги satisfied в false +// var worstContact:Contact = contacts[0]; +// var i:int; +// for (i = 1; i < contactsNum; i++) { +// var contact:Contact = contacts[i]; +// contact.satisfied = false; +// if (contact.maxPenetration > worstContact.maxPenetration) worstContact = contact; +// } +// if (worstContact.maxPenetration <= allowedPenetration) return; +// +// resolveInterpenetration(worstContact); +// // Внутренний цикл по оставшимся контактам +// for (i = 1; i < contactsNum; i++) { +// worstContact = getWorstContact(); +// if (worstContact == null) break; +// resolveInterpenetration(worstContact); +// } +// } + } + + /** + * + */ +// private function getWorstContact():Contact { +// var maxPen:Number = 0; +// var worst:Contact = null; +// for (var i:int = 0; i < contactsNum; i++) { +// var c:Contact = contacts[i]; +// if (!c.satisfied && c.maxPenetration > maxPen) { +// worst = c; +// maxPen = c.maxPenetration; +// } +// } +// return maxPen > allowedPenetration ? worst : null; +// } + + /** + * Разделяет указанный контакт, выполняя staticSeparationSteps итераций по списку точек контакта. + * 1. В начале каждой итерации ищется точка с наибольшим пересечением, одновременно сбрасываются флаги satisfied у точек. + * 2. Для найденной точки выполняется процедура разделения. + * 3. После выполняется pcount - 1 итерация по списку точек, каждый раз ищется наихудшая среди имеющих satisfied == false, для которой выполняется процедура разделения. + * Если наихудшая точка не найдена, то выполнется переход к следующей итерации внешнего цикла (шаг 1). + * + * В конце процедуры разделения обновляются значения пересечений для остальных точек контакта, а также для всех прочих контактов, относящихся к телам текущего. + * + * @param contact контакт для разделения + */ + private function resolveInterpenetration(contact:Contact):void { + contact.satisfied = true; + + for (var step:int = 0; step < staticSeparationSteps; step++) { + var worstCp:ContactPoint = contact.points[0]; + var cp:ContactPoint; + var i:int; + for (i = 1; i < contact.pcount; i++) { + cp = contact.points[i]; + cp.satisfied = false; + if (cp.penetration > worstCp.penetration) worstCp = cp; + } + if (worstCp.penetration <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + // Разделяем оставшиеся точки + var maxPen:Number = 0; + for (i = 1; i < contact.pcount; i++) { + // Поиск наихудшей точки + for (var j:int = 0; j < contact.pcount; j++) { + cp = contact.points[j]; + if (cp.satisfied) continue; + if (cp.penetration > maxPen) { + maxPen = cp.penetration; + worstCp = cp; + } + } + if (maxPen <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + } + } + } + + /** + * + * @param cp + * @param contact + */ + private function separateContactPoint(cp:ContactPoint, contact:Contact):void { +// cp.satisfied = true; +// +// var b1:Body = contact.body1; +// var b2:Body = contact.body2; +// var totalMove:Number = cp.penetration - allowedPenetration; +// var moveCoeff:Number = totalMove/cp.velByUnitImpulseN; +// var linMove1:Number; +// var angleMove1:Number; +// if (b1.movable) { +// linMove1 = b1.invMass*moveCoeff; +// angleMove1 = cp.angularInertia1*moveCoeff; +// if (angleMove1 > maxAngleMove) { +// linMove1 += angleMove1 - maxAngleMove; +// angleMove1 -= maxAngleMove; +// } +// b1.state.pos.vAddScaled(linMove1, contact.normal); +// _v1.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vScale(angleMove1); +// b1.state.orientation.addScaledVector(_v1, 1); +// } +// var linMove2:Number; +// var angleMove2:Number; +// if (b2 != null && b2.movable) { +// linMove2 = b2.invMass*moveCoeff; +// angleMove2 = cp.angularInertia2*moveCoeff; +// if (angleMove2 > maxAngleMove) { +// linMove2 += angleMove2 - maxAngleMove; +// angleMove2 -= maxAngleMove; +// } +// b2.state.pos.vAddScaled(-linMove2, contact.normal); +// _v2.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vScale(angleMove2); +// _v2.vReverse(); +// b2.state.orientation.addScaledVector(_v2, 1); +// } +// cp.penetration = allowedPenetration; +// // Обновляем пересечения в других точках +// var i:int; +// for (i = 0; i < contact.pcount; i++) { +// var cp1:ContactPoint = contact.points[i]; +// if (cp1 == cp) continue; +// var angularMove:Number; +// if (b1.movable) { +// angularMove = _v.vCross2(_v1, cp1.r1).vDot(contact.normal); +// cp1.penetration -= linMove1 + angularMove; +// } +// if (b2 != null && b2.movable) { +// angularMove = _v.vCross2(_v2, cp1.r2).vDot(contact.normal); +// cp1.penetration -= linMove2 - angularMove; +// } +// // Обновление максимального значения для контакта +// if (cp1.penetration > contact.maxPenetration) contact.maxPenetration = cp1.penetration; +// } +// // Обновляем пересечения для других контактов +// var c:Contact; +// var j:int; +// if (b1.movable) { +// for (i = 0; i < b1.contactsNum; i++) { +// c = b1.contacts[i]; +// if (c == contact) continue; +// for (j = 0; j < c.pcount; j++) { +// cp1 = c.points[j]; +// if (b1 == c.body1) cp1.penetration -= linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r1).vDot(c.normal); +// else cp1.penetration += linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r2).vDot(c.normal); +// if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; +// } +// } +// } +// if (b2 != null && b2.movable) { +// for (i = 0; i < b2.contactsNum; i++) { +// c = b2.contacts[i]; +// if (c == contact) continue; +// for (j = 0; j < c.pcount; j++) { +// cp1 = c.points[j]; +// if (b2 == c.body1) cp1.penetration -= linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r1).vDot(c.normal); +// else cp1.penetration += linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r2).vDot(c.normal); +// if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; +// } +// } +// } + } + + /** + * + */ + private function postPhysics():void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + body.clearAccumulators(); + body.calcDerivedData(); + if (body.canFreeze) { + if (body.state.velocity.vLength() < linSpeedFreezeLimit && body.state.rotation.vLength() < angSpeedFreezeLimit) { + if (!body.frozen) { + body.freezeCounter++; + if (body.freezeCounter >= freezeSteps) body.frozen = true; + } + } else { + body.freezeCounter = 0; + body.frozen = false; + } + } + item = item.next; + } + } + + /** + * + * @param delta + */ + public function update(delta:int):void { + timeStamp++; + time += delta; + var dt:Number = 0.001*delta; + applyForces(dt); + detectCollisions(dt); + preProcessContacts(dt); + processContacts(dt, false); + intergateVelocities(dt); + processContacts(dt, true); + integratePositions(dt); + postPhysics(); + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/.svn/text-base/PhysicsUtils.as.svn-base b/0.0.9.3/src/alternativa/physics/.svn/text-base/PhysicsUtils.as.svn-base new file mode 100644 index 0000000..5a2e46f --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/.svn/text-base/PhysicsUtils.as.svn-base @@ -0,0 +1,49 @@ +package alternativa.physics { + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + public class PhysicsUtils { + + /** + * @param mass + * @param halfSize + * @param result + */ + public static function getBoxInvInertia(mass:Number, halfSize:Vector3, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + result.a = 3/(mass*(yy + zz)); + result.f = 3/(mass*(zz + xx)); + result.k = 3/(mass*(xx + yy)); + } + + /** + * @param mass + * @param r + * @param h + * @param result + */ + public static function getCylinderInvInertia(mass:Number, r:Number, h:Number, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + + result.a = result.f = 1/(mass*(h*h/12 + r*r/4)); + result.k = 2/(mass*r*r); + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base b/0.0.9.3/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base new file mode 100644 index 0000000..2715307 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base @@ -0,0 +1,3 @@ +package alternativa.physics { + public namespace altphysics = "http://alternativaplatform.com/en/altphysics"; +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/Body.as b/0.0.9.3/src/alternativa/physics/Body.as new file mode 100644 index 0000000..cd22c5b --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/Body.as @@ -0,0 +1,405 @@ +package alternativa.physics { + + import alternativa.math.Matrix3; + import alternativa.math.Matrix4; + import alternativa.math.Quaternion; + import alternativa.math.Vector3; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.IBodyCollisionPredicate; + import alternativa.physics.collision.types.BoundBox; + + use namespace altphysics; + + /** + * + */ + public class Body { + + public static var linDamping:Number = 0.997; + public static var rotDamping:Number = 0.997; + + // Идентификатор тела, уникальный в пределах мира + public var id:int; + // Имя тела + public var name:String; + // Мир, в котором находится тело + public var world:PhysicsScene; + // Флаг подвижности тела + public var movable:Boolean = true; + // Флаг указывает, может ли тело быть заморожено + public var canFreeze:Boolean = false; + + public var freezeCounter:int; + public var frozen:Boolean = false; + // Ограничивающий бокс тела + public var aabb:BoundBox = new BoundBox(); + // Предикат, через который тело получает сообщения о столкновениях + public var postCollisionPredicate:IBodyCollisionPredicate; + + // Текущее состояние тела + public var state:BodyState = new BodyState(); + // Предыдущее состояние тела + public var prevState:BodyState = new BodyState(); + // Линейное ускорение тела на текущем шаге симуляции + public var accel:Vector3 = new Vector3(); + // Угловое ускорение тела на текущем шаге симуляции + public var angleAccel:Vector3 = new Vector3(); + // Физический материал тела + public var material:BodyMaterial = new BodyMaterial(); + // Обратная масса тела + public var invMass:Number = 1; + // Обратная матрица тензора инерции в локальных координатах + public var invInertia:Matrix3 = new Matrix3(); + // Обратная матрица тензора инерции в мировых координатах + public var invInertiaWorld:Matrix3 = new Matrix3(); + // Базисная матрица тела в мировых координатах + public var baseMatrix:Matrix3 = new Matrix3(); + + public const MAX_CONTACTS:int = 20; + public var contacts:Vector. = new Vector.(MAX_CONTACTS); + public var contactsNum:int; + + public var collisionPrimitives:CollisionPrimitiveList; + + // Аккумулятор сил + public var forceAccum:Vector3 = new Vector3(); + // Аккумулятор моментов + public var torqueAccum:Vector3 = new Vector3(); + + // Внутренние переменные для избежания создания экземпляров + private static var _r:Vector3 = new Vector3(); + private static var _f:Vector3 = new Vector3(); + + /** + * + * @param invMass + * @param invInertia + */ + public function Body(invMass:Number, invInertia:Matrix3) { + this.invMass = invMass; + this.invInertia.copy(invInertia); + } + + /** + * @param primitive + * @param localTransform + */ + public function addCollisionPrimitive(primitive:CollisionPrimitive, localTransform:Matrix4 = null):void { + if (primitive == null) { + throw new ArgumentError("Primitive cannot be null"); + } + if (collisionPrimitives == null) { + collisionPrimitives = new CollisionPrimitiveList(); + } + collisionPrimitives.append(primitive); + primitive.setBody(this, localTransform); + } + + /** + * + * @param primitive + */ + public function removeCollisionPrimitive(primitive:CollisionPrimitive):void { + if (collisionPrimitives == null) return; + primitive.setBody(null); + collisionPrimitives.remove(primitive); + if (collisionPrimitives.size == 0) { + collisionPrimitives = null; + } + } + + /** + * @param t + * @param result + */ + public function interpolate(t:Number, pos:Vector3, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.w = prevState.orientation.w*t1 + state.orientation.w*t; + orientation.x = prevState.orientation.x*t1 + state.orientation.x*t; + orientation.y = prevState.orientation.y*t1 + state.orientation.y*t; + orientation.z = prevState.orientation.z*t1 + state.orientation.z*t; + } + + /** + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + state.pos.vCopy(pos); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setPositionXYZ(x:Number, y:Number, z:Number):void { + state.pos.vReset(x, y, z); + } + + /** + * + * @param vel + */ + public function setVelocity(vel:Vector3):void { + state.velocity.vCopy(vel); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setVelocityXYZ(x:Number, y:Number, z:Number):void { + state.velocity.vReset(x, y, z); + } + + /** + * + * @param rot + */ + public function setRotation(rot:Vector3):void { + state.rotation.vCopy(rot); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setRotationXYZ(x:Number, y:Number, z:Number):void { + state.rotation.vReset(x, y, z); + } + + /** + * + * @param q + */ + public function setOrientation(q:Quaternion):void { + state.orientation.copy(q); + } + + /** + * @param r + * @param dir + * @param magnitude + */ + public function applyRelPosWorldImpulse(r:Vector3, dir:Vector3, magnitude:Number):void { + var d:Number = magnitude*invMass; + // Линейная часть + state.velocity.x += d*dir.x; + state.velocity.y += d*dir.y; + state.velocity.z += d*dir.z; + + // Вращательная часть + var x:Number = (r.y*dir.z - r.z*dir.y)*magnitude; + var y:Number = (r.z*dir.x - r.x*dir.z)*magnitude; + var z:Number = (r.x*dir.y - r.y*dir.x)*magnitude; + + state.rotation.x += invInertiaWorld.a*x + invInertiaWorld.b*y + invInertiaWorld.c*z; + state.rotation.y += invInertiaWorld.e*x + invInertiaWorld.f*y + invInertiaWorld.g*z; + state.rotation.z += invInertiaWorld.i*x + invInertiaWorld.j*y + invInertiaWorld.k*z; + } + + /** + * @param f + */ + public function addForce(f:Vector3):void { + forceAccum.vAdd(f); + } + + /** + * + * @param fx + * @param fy + * @param fz + */ + public function addForceXYZ(fx:Number, fy:Number, fz:Number):void { + forceAccum.x += fx; + forceAccum.y += fy; + forceAccum.z += fz; + } + + /** + * @param pos + * @param f + */ + public function addWorldForceXYZ(px:Number, py:Number, pz:Number, fx:Number, fy:Number, fz:Number):void { + forceAccum.x += fx; + forceAccum.y += fy; + forceAccum.z += fz; + + var pos:Vector3 = state.pos; + var rx:Number = px - pos.x; + var ry:Number = py - pos.y; + var rz:Number = pz - pos.z; + +// var x:Number = ry*fz - rz*fy; +// var y:Number = rz*fx - rx*fz; +// var z:Number = rx*fy - ry*fx; + + torqueAccum.x += ry*fz - rz*fy; + torqueAccum.y += rz*fx - rx*fz; + torqueAccum.z += rx*fy - ry*fx; + } + + /** + * @param pos + * @param f + */ + public function addWorldForce(pos:Vector3, force:Vector3):void { + forceAccum.vAdd(force); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(force)); + } + + /** + * @param pos + * @param f + */ + public function addWorldForceScaled(pos:Vector3, force:Vector3, scale:Number):void { + _f.x = scale*force.x; + _f.y = scale*force.y; + _f.z = scale*force.z; + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(_f)); + } + + /** + * @param pos + * @param f + */ + public function addLocalForce(pos:Vector3, force:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(pos, _r); + // Трансформируем вектор силы в мировую систему + baseMatrix.transformVector(force, _f); + // Добавляем силу и момент + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vCross(_f)); + } + + /** + * + * @param localPos + * @param worldForce + */ + public function addWorldForceAtLocalPoint(localPos:Vector3, worldForce:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(localPos, _r); + // Добавляем силу и момент + forceAccum.vAdd(worldForce); + torqueAccum.vAdd(_r.vCross(worldForce)); + } + + /** + * @param dt + */ + public function beforePhysicsStep(dt:Number):void { + } + + /** + * @param t + */ + public function addTorque(t:Vector3):void { + torqueAccum.vAdd(t); + } + + /** + * + */ + altphysics function clearAccumulators():void { + forceAccum.x = forceAccum.y = forceAccum.z = 0; + torqueAccum.x = torqueAccum.y = torqueAccum.z = 0; + } + + /** + * + */ + altphysics function calcAccelerations():void { + accel.x = forceAccum.x*invMass; + accel.y = forceAccum.y*invMass; + accel.z = forceAccum.z*invMass; + angleAccel.x = invInertiaWorld.a*torqueAccum.x + invInertiaWorld.b*torqueAccum.y + invInertiaWorld.c*torqueAccum.z; + angleAccel.y = invInertiaWorld.e*torqueAccum.x + invInertiaWorld.f*torqueAccum.y + invInertiaWorld.g*torqueAccum.z; + angleAccel.z = invInertiaWorld.i*torqueAccum.x + invInertiaWorld.j*torqueAccum.y + invInertiaWorld.k*torqueAccum.z; + } + + /** + * Вычисляет производные данные. + */ + public function calcDerivedData():void { + // Вычисление базисной матрицы и обратного тензора инерции в мировых координатах + state.orientation.toMatrix3(baseMatrix); + invInertiaWorld.copy(invInertia).append(baseMatrix).prependTransposed(baseMatrix); + if (collisionPrimitives != null) { + aabb.infinity(); + var item:CollisionPrimitiveListItem = collisionPrimitives.head; + while (item != null) { + var primitive:CollisionPrimitive = item.primitive; + primitive.transform.setFromMatrix3(baseMatrix, state.pos); + if (primitive.localTransform != null) { + primitive.transform.prepend(primitive.localTransform); + } + primitive.calculateAABB(); + aabb.addBoundBox(primitive.aabb); + item = item.next; + } + } + } + + /** + * + */ + altphysics function saveState():void { + prevState.copy(state); + } + + /** + * + */ + altphysics function restoreState():void { + state.copy(prevState); + } + + /** + * @param dt + */ + altphysics function integrateVelocity(dt:Number):void { + // v = v + a*t + state.velocity.x += accel.x*dt; + state.velocity.y += accel.y*dt; + state.velocity.z += accel.z*dt; + // rot = rot + eps*t + state.rotation.x += angleAccel.x*dt; + state.rotation.y += angleAccel.y*dt; + state.rotation.z += angleAccel.z*dt; + + state.velocity.x *= linDamping; + state.velocity.y *= linDamping; + state.velocity.z *= linDamping; + + state.rotation.x *= rotDamping; + state.rotation.y *= rotDamping; + state.rotation.z *= rotDamping; + } + + /** + * + */ + altphysics function integratePosition(dt:Number):void { + // pos = pos + v*t + state.pos.x += state.velocity.x*dt; + state.pos.y += state.velocity.y*dt; + state.pos.z += state.velocity.z*dt; + // q = q + 0.5*rot*q + state.orientation.addScaledVector(state.rotation, dt); + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/BodyList.as b/0.0.9.3/src/alternativa/physics/BodyList.as new file mode 100644 index 0000000..aa8f79f --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/BodyList.as @@ -0,0 +1,76 @@ +package alternativa.physics { + + /** + * + */ + public class BodyList { + + public var head:BodyListItem; + public var tail:BodyListItem; + public var size:int; + + /** + * + */ + public function BodyList() { + } + + /** + * + * @param body + */ + public function append(body:Body):void { + var item:BodyListItem = BodyListItem.create(body); + if (head == null) { + head = tail = item; + } else { + tail.next = item; + item.prev = tail; + tail = item; + } + size++; + } + + /** + * + * @param body + */ + public function remove(body:Body):Boolean { + var item:BodyListItem = findItem(body); + if (item == null) return false; + if (item == head) { + if (size == 1) { + head = tail = null; + } else { + head = item.next; + head.prev = null; + } + } else { + if (item == tail) { + tail = item.prev; + tail.next = null; + } else { + item.prev.next = item.next; + item.next.prev = item.prev; + } + } + item.dispose(); + size--; + return true; + } + + /** + * + * @param body + * @return + */ + public function findItem(body:Body):BodyListItem { + var item:BodyListItem = head; + while (item != null && item.body != body) { + item = item.next; + } + return item; + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/BodyListItem.as b/0.0.9.3/src/alternativa/physics/BodyListItem.as new file mode 100644 index 0000000..382e11c --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/BodyListItem.as @@ -0,0 +1,74 @@ +package alternativa.physics { + + /** + * + */ + public class BodyListItem { + + // Верхний элемент хранилища + private static var poolTop:BodyListItem; + + /** + * Создаёт новый элемент списка. + * + * @param primitive примитив, содержащийся в элементе + * @return новый элемент списка + */ + public static function create(body:Body):BodyListItem { + var item:BodyListItem; + if (poolTop == null) { + item = new BodyListItem(body); + } else { + item = poolTop; + poolTop = item.next; + item.next = null; + item.body = body; + } + return item; + } + + /** + * Очищает хранилище. + */ + public static function clearPool():void { + var item:BodyListItem = poolTop; + while (item != null) { + poolTop = item.next; + item.next = null; + item = poolTop; + } + } + + /** + * + */ + public var body:Body; + /** + * + */ + public var next:BodyListItem; + /** + * + */ + public var prev:BodyListItem; + + /** + * + * @param body + */ + public function BodyListItem(body:Body) { + this.body = body; + } + + /** + * + */ + public function dispose():void { + body = null; + prev = null; + next = poolTop; + poolTop = this; + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/BodyMaterial.as b/0.0.9.3/src/alternativa/physics/BodyMaterial.as new file mode 100644 index 0000000..7a7c47d --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/BodyMaterial.as @@ -0,0 +1,11 @@ +package alternativa.physics { + + public class BodyMaterial { + + public var restitution:Number = 0; + public var friction:Number = 0.3; +// public var dynamicFriction:Number = 0.2; +// public var dynamicFriction:Number = 0.2; + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/BodyState.as b/0.0.9.3/src/alternativa/physics/BodyState.as new file mode 100644 index 0000000..3987388 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/BodyState.as @@ -0,0 +1,39 @@ +package alternativa.physics { + import alternativa.math.Quaternion; + import alternativa.math.Vector3; + + /** + * Класс описывает состояние твёрдого тела. + */ + public class BodyState { + /** + * Положение тела. + */ + public var pos:Vector3 = new Vector3(); + /** + * Ориентация тела. + */ + public var orientation:Quaternion = new Quaternion(); + /** + * Скорость тела. + */ + public var velocity:Vector3 = new Vector3(); + /** + * Угловая скорость тела. + */ + public var rotation:Vector3 = new Vector3(); + + /** + * Копирует значение указанного объекта. + * + * @param state + */ + public function copy(state:BodyState):void { + pos.vCopy(state.pos); + orientation.copy(state.orientation); + velocity.vCopy(state.velocity); + rotation.vCopy(state.rotation); + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/CollisionPrimitiveList.as b/0.0.9.3/src/alternativa/physics/CollisionPrimitiveList.as new file mode 100644 index 0000000..5d5304f --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/CollisionPrimitiveList.as @@ -0,0 +1,89 @@ +package alternativa.physics { + import alternativa.physics.collision.CollisionPrimitive; + + /** + * + */ + public class CollisionPrimitiveList { + + public var head:CollisionPrimitiveListItem; + public var tail:CollisionPrimitiveListItem; + public var size:int; + + /** + * + */ + public function CollisionPrimitiveList() { + } + + /** + * + * @param primitive + */ + public function append(primitive:CollisionPrimitive):void { + var item:CollisionPrimitiveListItem = CollisionPrimitiveListItem.create(primitive); + if (head == null) { + head = tail = item; + } else { + tail.next = item; + item.prev = tail; + tail = item; + } + size++; + } + + /** + * + * @param primitve + */ + public function remove(primitve:CollisionPrimitive):void { + var item:CollisionPrimitiveListItem = findItem(primitve); + if (item == null) return; + if (item == head) { + if (size == 1) { + head = tail = null; + } else { + head = item.next; + head.prev = null; + } + } else { + if (item == tail) { + tail = tail.prev; + tail.next = null; + } else { + item.prev.next = item.next; + item.next.prev = item.prev; + } + } + item.dispose(); + size--; + } + + /** + * + * @param primitive + * @return + */ + public function findItem(primitive:CollisionPrimitive):CollisionPrimitiveListItem { + var item:CollisionPrimitiveListItem = head; + while (item != null && item.primitive != primitive) { + item = item.next; + } + return item; + } + + /** + * + */ + public function clear():void { + while (head != null) { + var item:CollisionPrimitiveListItem = head; + head = head.next; + item.dispose(); + } + tail = null; + size = 0; + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/CollisionPrimitiveListItem.as b/0.0.9.3/src/alternativa/physics/CollisionPrimitiveListItem.as new file mode 100644 index 0000000..d204532 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/CollisionPrimitiveListItem.as @@ -0,0 +1,76 @@ +package alternativa.physics { + import alternativa.physics.collision.CollisionPrimitive; + + /** + * Элемент списка примитивов. + */ + public class CollisionPrimitiveListItem { + + // Верхний элемент хранилища + private static var poolTop:CollisionPrimitiveListItem; + + /** + * Создаёт новый элемент списка. + * + * @param primitive примитив, содержащийся в элементе + * @return новый элемент списка + */ + public static function create(primitive:CollisionPrimitive):CollisionPrimitiveListItem { + var item:CollisionPrimitiveListItem; + if (poolTop == null) { + item = new CollisionPrimitiveListItem(primitive); + } else { + item = poolTop; + item.primitive = primitive; + poolTop = item.next; + item.next = null; + } + return item; + } + + /** + * Очищает хранилище. + */ + public static function clearPool():void { + var curr:CollisionPrimitiveListItem = poolTop; + while (curr != null) { + poolTop = curr.next; + curr.next = null; + curr = poolTop; + } + } + + /** + * Примитив, хранящийся в элементе списка. + */ + public var primitive:CollisionPrimitive; + /** + * Ссылка не следующий элемент списка. + */ + public var next:CollisionPrimitiveListItem; + /** + * Ссылка не предыдущий элемент списка. + */ + public var prev:CollisionPrimitiveListItem; + + /** + * Создаёт новый экземпляр. + * + * @param primitive примитив, хранящийся в элементе списка + */ + public function CollisionPrimitiveListItem(primitive:CollisionPrimitive) { + this.primitive = primitive; + } + + /** + * Очищает внутренние ссылки и помещает элемент в хранилище для дальнейшего использования. + */ + public function dispose():void { + primitive = null; + prev = null; + next = poolTop; + poolTop = this; + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/Contact.as b/0.0.9.3/src/alternativa/physics/Contact.as new file mode 100644 index 0000000..3f010ec --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/Contact.as @@ -0,0 +1,69 @@ +package alternativa.physics { + + + import alternativa.math.Vector3; + + /** + * Описывает контакт одного или двух тел. + */ + public class Contact { + /** + * Первое тело контакта, не может быть равно null. + */ + public var body1:Body; + /** + * Второе тело контакта, может быть равно null. + */ + public var body2:Body; + /** + * Предрассчитанный взаимный коэффициент отскока. + */ + public var restitution:Number; + /** + * Предрассчитанный взаимный коэффициент трения. + */ + public var friction:Number; + /** + * Нормаль контакта. Направлена к первому телу. + */ + public var normal:Vector3 = new Vector3(); + /** + * Список точек контакта. + */ + public var points:Vector. = new Vector.(MAX_POINTS, true); + /** + * Количество точек контакта. + */ + public var pcount:int; + /** + * Максимальная глубина пересечения. + */ + public var maxPenetration:Number = 0; + /** + * Флаг показывает, разрешён контакт или нет. + */ + public var satisfied:Boolean; + /** + * Следующий контакт в списке. + */ + public var next:Contact; + /** + * Индекс контакта. Первый контакт в списке имеет индекс 0. + */ + public var index:int; + + // Максимальное количество точек контакта + private const MAX_POINTS:int = 8; + + /** + * + */ + public function Contact(index:int) { + this.index = index; + for (var i:int = 0; i < MAX_POINTS; i++) { + points[i] = new ContactPoint(); + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/ContactPoint.as b/0.0.9.3/src/alternativa/physics/ContactPoint.as new file mode 100644 index 0000000..395d2dc --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/ContactPoint.as @@ -0,0 +1,55 @@ +package alternativa.physics { + import alternativa.math.Vector3; + + /** + * + */ + public class ContactPoint { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + + // Величины, расчитываемые перед началом фазы решения контактов + + // Требуемая проекция конечной скорости на нормаль для упругого контакта + public var normalVel:Number; + // Минимальная скорость разделения неупругого контакта + public var minSepVel:Number; + // Изменение проекции скорости на единицу нормального импульса + public var velByUnitImpulseN:Number; + + public var angularInertia1:Number; + public var angularInertia2:Number; + + // Радиус-вектор точки контакта относительно центра первого тела + public var r1:Vector3 = new Vector3(); + // Радиус-вектор точки контакта относительно центра второго тела + public var r2:Vector3 = new Vector3(); + + // Величины, накапливаемые во время фазы решения контактов + + // Накопленный импульс, применяемый для получения требуемой относительной скорости в точке контакта + public var accumImpulseN:Number; + // Накопленный импульс, применяемый для разделения тел в точке контакта. Не создаёт момента. +// public var accumSepImpulse:Number; + + public var satisfied:Boolean; + + /** + * + * @param cp + */ + public function copyFrom(cp:ContactPoint):void { + pos.vCopy(cp.pos); + penetration = cp.penetration; + feature1 = cp.feature1; + feature2 = cp.feature2; + r1.vCopy(cp.r1); + r2.vCopy(cp.r2); + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/PhysicsScene.as b/0.0.9.3/src/alternativa/physics/PhysicsScene.as new file mode 100644 index 0000000..abdc762 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/PhysicsScene.as @@ -0,0 +1,743 @@ +package alternativa.physics { + + + import alternativa.physics.collision.ICollisionDetector; + import alternativa.physics.collision.KdTreeCollisionDetector; + import alternativa.physics.constraints.Constraint; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * Класс реализует физическую симуляцию поведения твёрдых тел. + */ + public class PhysicsScene { + + private static var lastBodyId:int; + + // Максимальное количество контактов + public const MAX_CONTACTS:int = 1000; + + ////////////////////////////////////////////////////////////////// + // Настроечные параметры симуляции. Могут быть изменены в любой + // момент времени без нарушения корректной работы симуляции. + ////////////////////////////////////////////////////////////////// + // Количество шагов, за которое пересекающиеся тела должны разделиться + public var penResolutionSteps:int = 10; + // Величина допустимой глубины пересечения + public var allowedPenetration:Number = 0.1; + // Максимальная скорость, добавляемая с целью разделения тел + public var maxPenResolutionSpeed:Number = 0.5; + // Количество итераций для обработки упругих контактов + public var collisionIterations:int = 5; + // Количество итераций для обработки неупругих контактов + public var contactIterations:int = 5; + // Флаг использования предсказания состояний + public var usePrediction:Boolean = false; + + public var freezeSteps:int = 10; + public var linSpeedFreezeLimit:Number = 1; + public var angSpeedFreezeLimit:Number = 0.01; + + // Переменные для процедуры разделения тел путём непосредственного их перемещенеия. + // !!!!!!!!!!!!!!!!!!!!!!!! + // !!! Экспериментально !!! + // !!!!!!!!!!!!!!!!!!!!!!!! + public var staticSeparationIterations:int = 10; + public var staticSeparationSteps:int = 10; + public var maxAngleMove:Number = 10; + public var useStaticSeparation:Boolean = false; + + // Вектор гравитации + public var _gravity:Vector3 = new Vector3(0, 0, -9.8); + // Модуль вектора гравитации + public var _gravityMagnitude:Number = 9.8; + + // Использующийся детектор столкновений + public var collisionDetector:ICollisionDetector; + + // Список тел, участвующих в симуляции + public var bodies:BodyList = new BodyList(); + + // Список контактов на текущем шаге симуляции + altphysics var contacts:Contact; + // Количество контактов на текущем шаге симуляции +// altphysics var contactsNum:int; + // Список ограничений + altphysics var constraints:Vector. = new Vector.(); + // Количество ограничений + altphysics var constraintsNum:int; + // Временная метка. Число прошедших шагов с начала симуляции. + public var timeStamp:int; + // Время с начала симуляции, мс + public var time:int; + // Первый неиспользованный контакт на текущем шаге симуляции + private var borderContact:Contact; + + // Временные переменные для избежания создания экземпляров + private var _r:Vector3 = new Vector3(); + private var _t:Vector3 = new Vector3(); + private var _v:Vector3 = new Vector3(); + private var _v1:Vector3 = new Vector3(); + private var _v2:Vector3 = new Vector3(); + + /** + * + */ + public function PhysicsScene() { + contacts = new Contact(0); + var contact:Contact = contacts; + for (var i:int = 1; i < MAX_CONTACTS; i++) { + contact.next = new Contact(i); + contact = contact.next; + } + collisionDetector = new KdTreeCollisionDetector(); + } + + /** + * Вектор гравитации. + */ + public function get gravity():Vector3 { + return _gravity.vClone(); + } + + /** + * @private + */ + public function set gravity(value:Vector3):void { + _gravity.vCopy(value); + _gravityMagnitude = _gravity.vLength(); + } + + /** + * Добавляет тело в симуляцию. + * + * @param body + */ + public function addBody(body:Body):void { + body.id = lastBodyId++; + body.world = this; + bodies.append(body); + } + + /** + * Удаляет тело из симуляции. + * @param body + * @return + */ + public function removeBody(body:Body):void { + if (bodies.remove(body)) { + body.world = null; + } + } + + /** + * Добавляет ограничение. + * @param c + */ + public function addConstraint(c:Constraint):void { + constraints[constraintsNum++] = c; + c.world = this; + } + + /** + * Удаляет ограничение. + * @param c + */ + public function removeConstraint(c:Constraint):Boolean { + var idx:int = constraints.indexOf(c); + if (idx < 0) return false; + constraints.splice(idx, 1) + constraintsNum--; + c.world = null; + return true; + } + + /** + * Применяет к телам действующте на них силы. Аккумуляторы сил и моментов тел очищаются после завершения шага + * симуляции, поэтому на момент вызова метода могут уже содержать некоторые значения. + * + * @param dt промежуток времени, в течении которого действуют силы + */ + private function applyForces(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + body.beforePhysicsStep(dt); + body.calcAccelerations(); + // Ускорение свободного падения применяется только к подвижным телам во избежание некорректного изменения + // фиктивной скорости неподвижных тел. + if (body.movable && !body.frozen) { + body.accel.x += _gravity.x; + body.accel.y += _gravity.y; + body.accel.z += _gravity.z; + } + item = item.next; + } + } + + /** + * Определяет все столкновения на текущем шаге симуляции и заполняет список получившихся контактов. + * + * @param dt длительность шага симуляции + */ + private function detectCollisions(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + if (!body.frozen) { + body.contactsNum = 0; + body.saveState(); + // При включённом режиме предсказания состояние тел интегрируется на один шаг вперёд + if (usePrediction) { + body.integrateVelocity(dt); + body.integratePosition(dt); + } + body.calcDerivedData(); + } + item = item.next; + } + + borderContact = collisionDetector.getAllContacts(contacts); + + // Расчёт относительных векторов точки контакта вынесен сюда из-за необходимости учитывать + // положение тел в предсказанном состоянии + var contact:Contact = contacts; + while (contact != borderContact) { + var b1:Body = contact.body1; + var b2:Body = contact.body2; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + var bPos:Vector3 = b1.state.pos; + cp.r1.x = cp.pos.x - bPos.x; + cp.r1.y = cp.pos.y - bPos.y; + cp.r1.z = cp.pos.z - bPos.z; + if (b2 != null) { + bPos = b2.state.pos; + cp.r2.x = cp.pos.x - bPos.x; + cp.r2.y = cp.pos.y - bPos.y; + cp.r2.z = cp.pos.z - bPos.z; + } + } + contact = contact.next; + } + + // Восстановление состояния тел + if (usePrediction) { + item = bodies.head; + while (item != null) { + body = item.body; + if (!body.frozen) { + body.restoreState(); + body.calcDerivedData(); + } + item = item.next; + } + } + } + + /** + * Подготваливает полученные из детектора столкновений контакты, расчитывая значения, не меняющиеся + * в ходе шага симуляции. + */ + private function preProcessContacts(dt:Number):void { + var contact:Contact = contacts; + while (contact != borderContact) { + var b1:Body = contact.body1; + var b2:Body = contact.body2; + // Столкнувшиеся тела размораживаются + if (b1.frozen) { + b1.frozen = false; + b1.freezeCounter = 0; + } + if (b2 != null && b2.frozen) { + b2.frozen = false; + b2.freezeCounter = 0; + } + contact.restitution = b1.material.restitution; + if (b2 != null && b2.material.restitution < contact.restitution) contact.restitution = b2.material.restitution; + contact.friction = b1.material.friction; + if (b2 != null && b2.material.friction < contact.friction) contact.friction = b2.material.friction; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.accumImpulseN = 0; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + cp.velByUnitImpulseN = 0; + if (b1.movable) { + cp.angularInertia1 = _v.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1).vDot(contact.normal); + cp.velByUnitImpulseN += b1.invMass + cp.angularInertia1; + } + if (b2 != null && b2.movable) { + cp.angularInertia2 = _v.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2).vDot(contact.normal); + cp.velByUnitImpulseN += b2.invMass + cp.angularInertia2; + } + // Расчёт требуемой конечной скорости для упругого контакта + calcSepVelocity(b1, b2, cp, _v); + cp.normalVel = _v.vDot(contact.normal); + if (cp.normalVel < 0) cp.normalVel = - contact.restitution*cp.normalVel; + // Скорость разделения неупругого контакта + cp.minSepVel = cp.penetration > allowedPenetration ? (cp.penetration - allowedPenetration)/(penResolutionSteps*dt) : 0; + if (cp.minSepVel > maxPenResolutionSpeed) cp.minSepVel = maxPenResolutionSpeed; + } + contact = contact.next; + } + for (var i:int = 0; i < constraintsNum; i++) { + var constraint:Constraint = constraints[i]; + constraint.preProcess(dt); + } + } + + /** + * + * @param dt + * @param forceInelastic + */ + private function processContacts(dt:Number, forceInelastic:Boolean):void { + var iterNum:int = forceInelastic ? contactIterations : collisionIterations; + var i:int; + var forwardLoop:Boolean = false; + for (var iter:int = 0; iter < iterNum; iter++) { + forwardLoop = !forwardLoop; + var contact:Contact = contacts; + while (contact != borderContact) { + resolveContact(contact, forceInelastic, forwardLoop); + contact = contact.next; + } + // Ограничения + for (i = 0; i < constraintsNum; i++) { + var constraint:Constraint = constraints[i]; + constraint.apply(dt); + } + } + + // Разделение контактов путём непосредственного изменения координат и ориентации. Экспериментально. +// if (forceInelastic && useStaticSeparation) performStaticSeparation(); + } + + /** + * + */ + private function resolveContact(contactInfo:Contact, forceInelastic:Boolean, forwardLoop:Boolean):void { + var b1:Body = contactInfo.body1; + var b2:Body = contactInfo.body2; + var normal:Vector3 = contactInfo.normal; + var i:int; + if (forwardLoop) { + for (i = 0; i < contactInfo.pcount; i++) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } else { + for (i = contactInfo.pcount - 1; i >= 0; i--) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } + } + + /** + * + * @param idx + * @param b1 + * @param b2 + * @param colInfo + * @param normal + * @param forceInelastic + */ + private function resolveContactPoint(idx:int, b1:Body, b2:Body, contact:Contact, normal:Vector3, forceInelastic:Boolean):void { + var cp:ContactPoint = contact.points[idx]; + if (!forceInelastic) { + cp.satisfied = true; + } + + var newVel:Number = 0; + calcSepVelocity(b1, b2, cp, _v); + var cnormal:Vector3 = contact.normal; + var sepVel:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + if (forceInelastic) { + var minSpeVel:Number = useStaticSeparation ? 0 : cp.minSepVel; + if (sepVel < minSpeVel) { + cp.satisfied = false; + } else if (cp.satisfied) return; + newVel = minSpeVel; + } else { + newVel = cp.normalVel; + } + var deltaVel:Number = newVel - sepVel; + var impulse:Number = deltaVel/cp.velByUnitImpulseN; + var accumImpulse:Number = cp.accumImpulseN + impulse; + if (accumImpulse < 0) { + accumImpulse = 0; + } + var deltaImpulse:Number = accumImpulse - cp.accumImpulseN; + cp.accumImpulseN = accumImpulse; + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, normal, deltaImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, normal, -deltaImpulse); + } + + // Учёт силы трения + calcSepVelocity(b1, b2, cp, _v); + // Расчитываем изменение касательной скорости на единицу касательного импульса + var tanSpeedByUnitImpulse:Number = 0; + +// _v.vAddScaled(-_v.vDot(contact.normal), contact.normal); + var dot:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + _v.x -= dot*cnormal.x; + _v.y -= dot*cnormal.y; + _v.z -= dot*cnormal.z; + + var tanSpeed:Number = _v.vLength(); + if (tanSpeed < 0.001) return; + +// _t.vCopy(_v).vNormalize().vReverse(); + _t.x = -_v.x; + _t.y = -_v.y; + _t.z = -_v.z; + _t.vNormalize(); + + var r:Vector3; + var m:Matrix3; + var xx:Number; + var yy:Number; + var zz:Number; + // dV = b.invMass + ((invI * (r % t)) % r) * t + if (b1.movable) { +// _v.vCross2(cp.r1, _t).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1); + r = cp.r1; + m = b1.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b1.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + if (b2 != null && b2.movable) { +// _v.vCross2(cp.r2, _t).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2); + + r = cp.r2; + m = b2.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b2.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + + var tanImpulse:Number = tanSpeed/tanSpeedByUnitImpulse; + var max:Number = contact.friction*cp.accumImpulseN; + if (max < 0) { + if (tanImpulse < max) tanImpulse = max; + } else { + if (tanImpulse > max) tanImpulse = max; + } + + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, _t, tanImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, _t, -tanImpulse); + } + } + + /** + * + * @param cp + * @param normal + * @return + */ + private function calcSepVelocity(body1:Body, body2:Body, cp:ContactPoint, result:Vector3):void { + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 +// result.vCopy(body1.state.velocity).vAdd(_v1.vCross2(body1.state.rotation, cp.r1)); + var rot:Vector3 = body1.state.rotation; + var v:Vector3 = cp.r1; + var x:Number = rot.y*v.z - rot.z*v.y; + var y:Number = rot.z*v.x - rot.x*v.z; + var z:Number = rot.x*v.y - rot.y*v.x; + v = body1.state.velocity; + result.x = v.x + x; + result.y = v.y + y; + result.z = v.z + z; + // V2 = V2_c + w2%r2 + if (body2 != null) { +// result.vSubtract(body2.state.velocity).vSubtract(_v2.vCross2(body2.state.rotation, cp.r2)); + rot = body2.state.rotation; + v = cp.r2; + x = rot.y*v.z - rot.z*v.y; + y = rot.z*v.x - rot.x*v.z; + z = rot.x*v.y - rot.y*v.x; + v = body2.state.velocity; + result.x -= v.x + x; + result.y -= v.y + y; + result.z -= v.z + z; + } + } + + /** + * + * @param dt + */ + private function intergateVelocities(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + item.body.integrateVelocity(dt); + item = item.next; + } + } + + /** + * + * @param dt + */ + private function integratePositions(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + if (body.movable && !body.frozen) { + body.integratePosition(dt); + } + item = item.next; + } + } + + /** + * + */ + private function performStaticSeparation():void { + var iterNum:int = staticSeparationIterations; +// iterNum = 100; + + // 1. В начале каждой итерации для всех контактов сбрасывается флаг satisfied с одновременным поиском наихудшего контакта, т.е. имеющего наибольшую величину пересечения тел. + // 2. Если найденный контакт имеет величину пересечения меньше предельно допустимой процедура прерывается, т.к. разделение контактов не требуется. + // 3. Если процедура продолжается, то выполняется разделение найденного контакта и установка его флага satisfied в true, после чего запускается внутренний цикл, состоящий + // из contactsNum - 1 итераций. На каждой итерации ищется наихудший контакт среди оставшихся (satisfied == false). Если такой контакт найден, то выполняется его разделение + // и переход к следующей итерации внутреннего цикла, иначе внутренний цикл прерывается и выполняется переход к следующей итерации внешнего цикла (шаг 1.). +// for (var iter:int = 0; iter < iterNum; iter++) { +// // Ищем контакт с максимальной величиной пересечения тел, одновременно сбрасывая флаги satisfied в false +// var worstContact:Contact = contacts[0]; +// var i:int; +// for (i = 1; i < contactsNum; i++) { +// var contact:Contact = contacts[i]; +// contact.satisfied = false; +// if (contact.maxPenetration > worstContact.maxPenetration) worstContact = contact; +// } +// if (worstContact.maxPenetration <= allowedPenetration) return; +// +// resolveInterpenetration(worstContact); +// // Внутренний цикл по оставшимся контактам +// for (i = 1; i < contactsNum; i++) { +// worstContact = getWorstContact(); +// if (worstContact == null) break; +// resolveInterpenetration(worstContact); +// } +// } + } + + /** + * + */ +// private function getWorstContact():Contact { +// var maxPen:Number = 0; +// var worst:Contact = null; +// for (var i:int = 0; i < contactsNum; i++) { +// var c:Contact = contacts[i]; +// if (!c.satisfied && c.maxPenetration > maxPen) { +// worst = c; +// maxPen = c.maxPenetration; +// } +// } +// return maxPen > allowedPenetration ? worst : null; +// } + + /** + * Разделяет указанный контакт, выполняя staticSeparationSteps итераций по списку точек контакта. + * 1. В начале каждой итерации ищется точка с наибольшим пересечением, одновременно сбрасываются флаги satisfied у точек. + * 2. Для найденной точки выполняется процедура разделения. + * 3. После выполняется pcount - 1 итерация по списку точек, каждый раз ищется наихудшая среди имеющих satisfied == false, для которой выполняется процедура разделения. + * Если наихудшая точка не найдена, то выполнется переход к следующей итерации внешнего цикла (шаг 1). + * + * В конце процедуры разделения обновляются значения пересечений для остальных точек контакта, а также для всех прочих контактов, относящихся к телам текущего. + * + * @param contact контакт для разделения + */ + private function resolveInterpenetration(contact:Contact):void { + contact.satisfied = true; + + for (var step:int = 0; step < staticSeparationSteps; step++) { + var worstCp:ContactPoint = contact.points[0]; + var cp:ContactPoint; + var i:int; + for (i = 1; i < contact.pcount; i++) { + cp = contact.points[i]; + cp.satisfied = false; + if (cp.penetration > worstCp.penetration) worstCp = cp; + } + if (worstCp.penetration <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + // Разделяем оставшиеся точки + var maxPen:Number = 0; + for (i = 1; i < contact.pcount; i++) { + // Поиск наихудшей точки + for (var j:int = 0; j < contact.pcount; j++) { + cp = contact.points[j]; + if (cp.satisfied) continue; + if (cp.penetration > maxPen) { + maxPen = cp.penetration; + worstCp = cp; + } + } + if (maxPen <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + } + } + } + + /** + * + * @param cp + * @param contact + */ + private function separateContactPoint(cp:ContactPoint, contact:Contact):void { +// cp.satisfied = true; +// +// var b1:Body = contact.body1; +// var b2:Body = contact.body2; +// var totalMove:Number = cp.penetration - allowedPenetration; +// var moveCoeff:Number = totalMove/cp.velByUnitImpulseN; +// var linMove1:Number; +// var angleMove1:Number; +// if (b1.movable) { +// linMove1 = b1.invMass*moveCoeff; +// angleMove1 = cp.angularInertia1*moveCoeff; +// if (angleMove1 > maxAngleMove) { +// linMove1 += angleMove1 - maxAngleMove; +// angleMove1 -= maxAngleMove; +// } +// b1.state.pos.vAddScaled(linMove1, contact.normal); +// _v1.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vScale(angleMove1); +// b1.state.orientation.addScaledVector(_v1, 1); +// } +// var linMove2:Number; +// var angleMove2:Number; +// if (b2 != null && b2.movable) { +// linMove2 = b2.invMass*moveCoeff; +// angleMove2 = cp.angularInertia2*moveCoeff; +// if (angleMove2 > maxAngleMove) { +// linMove2 += angleMove2 - maxAngleMove; +// angleMove2 -= maxAngleMove; +// } +// b2.state.pos.vAddScaled(-linMove2, contact.normal); +// _v2.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vScale(angleMove2); +// _v2.vReverse(); +// b2.state.orientation.addScaledVector(_v2, 1); +// } +// cp.penetration = allowedPenetration; +// // Обновляем пересечения в других точках +// var i:int; +// for (i = 0; i < contact.pcount; i++) { +// var cp1:ContactPoint = contact.points[i]; +// if (cp1 == cp) continue; +// var angularMove:Number; +// if (b1.movable) { +// angularMove = _v.vCross2(_v1, cp1.r1).vDot(contact.normal); +// cp1.penetration -= linMove1 + angularMove; +// } +// if (b2 != null && b2.movable) { +// angularMove = _v.vCross2(_v2, cp1.r2).vDot(contact.normal); +// cp1.penetration -= linMove2 - angularMove; +// } +// // Обновление максимального значения для контакта +// if (cp1.penetration > contact.maxPenetration) contact.maxPenetration = cp1.penetration; +// } +// // Обновляем пересечения для других контактов +// var c:Contact; +// var j:int; +// if (b1.movable) { +// for (i = 0; i < b1.contactsNum; i++) { +// c = b1.contacts[i]; +// if (c == contact) continue; +// for (j = 0; j < c.pcount; j++) { +// cp1 = c.points[j]; +// if (b1 == c.body1) cp1.penetration -= linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r1).vDot(c.normal); +// else cp1.penetration += linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r2).vDot(c.normal); +// if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; +// } +// } +// } +// if (b2 != null && b2.movable) { +// for (i = 0; i < b2.contactsNum; i++) { +// c = b2.contacts[i]; +// if (c == contact) continue; +// for (j = 0; j < c.pcount; j++) { +// cp1 = c.points[j]; +// if (b2 == c.body1) cp1.penetration -= linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r1).vDot(c.normal); +// else cp1.penetration += linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r2).vDot(c.normal); +// if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; +// } +// } +// } + } + + /** + * + */ + private function postPhysics():void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + body.clearAccumulators(); + body.calcDerivedData(); + if (body.canFreeze) { + if (body.state.velocity.vLength() < linSpeedFreezeLimit && body.state.rotation.vLength() < angSpeedFreezeLimit) { + if (!body.frozen) { + body.freezeCounter++; + if (body.freezeCounter >= freezeSteps) body.frozen = true; + } + } else { + body.freezeCounter = 0; + body.frozen = false; + } + } + item = item.next; + } + } + + /** + * + * @param delta + */ + public function update(delta:int):void { + timeStamp++; + time += delta; + var dt:Number = 0.001*delta; + applyForces(dt); + detectCollisions(dt); + preProcessContacts(dt); + processContacts(dt, false); + intergateVelocities(dt); + processContacts(dt, true); + integratePositions(dt); + postPhysics(); + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/PhysicsUtils.as b/0.0.9.3/src/alternativa/physics/PhysicsUtils.as new file mode 100644 index 0000000..5a2e46f --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/PhysicsUtils.as @@ -0,0 +1,49 @@ +package alternativa.physics { + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + public class PhysicsUtils { + + /** + * @param mass + * @param halfSize + * @param result + */ + public static function getBoxInvInertia(mass:Number, halfSize:Vector3, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + result.a = 3/(mass*(yy + zz)); + result.f = 3/(mass*(zz + xx)); + result.k = 3/(mass*(xx + yy)); + } + + /** + * @param mass + * @param r + * @param h + * @param result + */ + public static function getCylinderInvInertia(mass:Number, r:Number, h:Number, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + + result.a = result.f = 1/(mass*(h*h/12 + r*r/4)); + result.k = 2/(mass*r*r); + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/altphysics.as b/0.0.9.3/src/alternativa/physics/altphysics.as new file mode 100644 index 0000000..2715307 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/altphysics.as @@ -0,0 +1,3 @@ +package alternativa.physics { + public namespace altphysics = "http://alternativaplatform.com/en/altphysics"; +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/collision/.svn/all-wcprops b/0.0.9.3/src/alternativa/physics/collision/.svn/all-wcprops new file mode 100644 index 0000000..ee0e5cf --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/.svn/all-wcprops @@ -0,0 +1,65 @@ +K 25 +svn:wc:ra_dav:version-url +V 113 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/collision +END +ICollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 135 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/collision/ICollisionDetector.as +END +CollisionKdNode.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/collision/CollisionKdNode.as +END +CollisionKdTree2D.as +K 25 +svn:wc:ra_dav:version-url +V 134 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/collision/CollisionKdTree2D.as +END +IRayCollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 139 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/collision/IRayCollisionPredicate.as +END +IBodyCollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/collision/IBodyCollisionPredicate.as +END +CollisionPrimitive.as +K 25 +svn:wc:ra_dav:version-url +V 135 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/collision/CollisionPrimitive.as +END +KdTreeCollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/collision/KdTreeCollisionDetector.as +END +ICollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 136 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/collision/ICollisionPredicate.as +END +ICollider.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/collision/ICollider.as +END +CollisionKdTree.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/collision/CollisionKdTree.as +END diff --git a/0.0.9.3/src/alternativa/physics/collision/.svn/entries b/0.0.9.3/src/alternativa/physics/collision/.svn/entries new file mode 100644 index 0000000..7518a14 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/.svn/entries @@ -0,0 +1,157 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/collision +http://svndev.alternativaplatform.com + + + +2010-07-03T11:40:52.545629Z +36684 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +colliders +dir + +ICollisionDetector.as +file + + + + +2010-10-28T04:32:44.000000Z +9f76503bdaf92b4170b05b6e66c45de1 +2010-02-12T12:34:25.192477Z +28424 +mike + +CollisionKdNode.as +file + + + + +2010-10-28T04:32:44.000000Z +a6fab491cd9bd45a5472bf4ffc321e08 +2010-02-12T12:34:25.192477Z +28424 +mike + +CollisionKdTree2D.as +file + + + + +2010-10-28T04:32:44.000000Z +82bb636d8b49cdfbc22694dd8b01e71d +2010-02-12T12:34:25.192477Z +28424 +mike + +IRayCollisionPredicate.as +file + + + + +2010-10-28T04:32:44.000000Z +32f9b1eaeacfcb40e7ae1927f52b8ab8 +2009-10-19T07:17:33.113306Z +22253 +mike + +types +dir + +IBodyCollisionPredicate.as +file + + + + +2010-10-28T04:32:44.000000Z +618f907a588979a8d06d4f4bcf6618a0 +2009-10-19T07:17:33.113306Z +22253 +mike + +CollisionPrimitive.as +file + + + + +2010-10-28T04:32:44.000000Z +8501ce34a3effa2380c472cc9b823afa +2009-11-24T08:44:15.941480Z +23758 +mike + +KdTreeCollisionDetector.as +file + + + + +2010-10-28T04:32:44.000000Z +ea670ad5d0f72f6a6df5163d65f204f9 +2010-02-12T12:34:25.192477Z +28424 +mike + +ICollisionPredicate.as +file + + + + +2010-10-28T04:32:44.000000Z +65f3250d36556fffc457c740fb7d38b3 +2009-10-19T07:17:33.113306Z +22253 +mike + +ICollider.as +file + + + + +2010-10-28T04:32:44.000000Z +128a7d7479e13caeccecb45161b27b22 +2009-10-19T07:17:33.113306Z +22253 +mike + +primitives +dir + +CollisionKdTree.as +file + + + + +2010-10-28T04:32:44.000000Z +8e815c1f1e527b0884b3f0124257a603 +2010-02-12T12:34:25.192477Z +28424 +mike + diff --git a/0.0.9.3/src/alternativa/physics/collision/.svn/format b/0.0.9.3/src/alternativa/physics/collision/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.3/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base b/0.0.9.3/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base new file mode 100644 index 0000000..f450f1e --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base @@ -0,0 +1,18 @@ +package alternativa.physics.collision { + + + import alternativa.physics.collision.types.BoundBox; + + public class CollisionKdNode { + public var indices:Vector.; + public var splitIndices:Vector.; + public var boundBox:BoundBox; + public var parent:CollisionKdNode; + public var splitTree:CollisionKdTree2D; + + public var axis:int = -1; // 0 - x, 1 - y, 2 - z + public var coord:Number; + public var positiveNode:CollisionKdNode; + public var negativeNode:CollisionKdNode; + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base b/0.0.9.3/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base new file mode 100644 index 0000000..d3dc307 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base @@ -0,0 +1,261 @@ +package alternativa.physics.collision { + + + + import alternativa.physics.collision.types.BoundBox; + + /** + * @author mike + */ + public class CollisionKdTree { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + public var rootNode:CollisionKdNode; + public var staticChildren:Vector.; + public var numStaticChildren:int; + public var staticBoundBoxes:Vector. = new Vector.(); + + private var splitAxis:int; + private var splitCoord:Number; + private var splitCost:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + + /** + * @param boundBox + */ + public function createTree(collisionPrimitives:Vector., boundBox:BoundBox = null):void { + staticChildren = collisionPrimitives.concat(); + numStaticChildren = staticChildren.length; + // Создаём корневую ноду + rootNode = new CollisionKdNode(); + rootNode.indices = new Vector.(); + // Расчитываем баунды объектов и рутовой ноды + var rootNodeBoundBox:BoundBox = rootNode.boundBox = (boundBox != null) ? boundBox : new BoundBox(); + for (var i:int = 0; i < numStaticChildren; ++i) { + var child:CollisionPrimitive = staticChildren[i]; + var childBoundBox:BoundBox = staticBoundBoxes[i] = child.calculateAABB(); + rootNodeBoundBox.addBoundBox(childBoundBox); + rootNode.indices[i] = i; + } + staticBoundBoxes.length = numStaticChildren; + // Разделяем рутовую ноду + splitNode(rootNode); + + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + var indices:Vector. = node.indices; + var numPrimitives:int = indices.length; + if (numPrimitives <= minPrimitivesPerNode) return; + + // Подготовка баунда с погрешностями + var nodeBoundBox:BoundBox = node.boundBox; + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + // Собираем опорные координаты + var i:int; + var j:int; + var numSplitCoordsX:int = 0 + var numSplitCoordsY:int = 0; + var numSplitCoordsZ:int = 0; + for (i = 0; i < numPrimitives; ++i) { + var boundBox:BoundBox = staticBoundBoxes[indices[i]]; + + if (boundBox.maxX - boundBox.minX <= doubleThreshold) { + if (boundBox.minX <= nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.minX; + else if (boundBox.maxX >= nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.maxX; + else splitCoordsX[numSplitCoordsX++] = (boundBox.minX + boundBox.maxX)*0.5; + } else { + if (boundBox.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = boundBox.minX; + if (boundBox.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = boundBox.maxX; + } + + if (boundBox.maxY - boundBox.minY <= doubleThreshold) { + if (boundBox.minY <= nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.minY; + else if (boundBox.maxY >= nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.maxY; + else splitCoordsY[numSplitCoordsY++] = (boundBox.minY + boundBox.maxY)*0.5; + } else { + if (boundBox.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = boundBox.minY; + if (boundBox.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = boundBox.maxY; + } + + if (boundBox.maxZ - boundBox.minZ <= doubleThreshold) { + if (boundBox.minZ <= nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.minZ; + else if (boundBox.maxZ >= nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.maxZ; + else splitCoordsZ[numSplitCoordsZ++] = (boundBox.minZ + boundBox.maxZ)*0.5; + } else { + if (boundBox.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.minZ; + if (boundBox.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numPrimitives; ++i) { + boundBox = staticBoundBoxes[indices[i]]; + var min:Number = axisX ? boundBox.minX : (axisY ? boundBox.minY : boundBox.minZ); + var max:Number = axisX ? boundBox.maxX : (axisY ? boundBox.maxY : boundBox.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(indices[i]); + indices[i] = -1; + } else { + if (node.splitIndices == null) node.splitIndices = new Vector.(); + node.splitIndices.push(indices[i]); + indices[i] = -1; + } + } else { + if (min >= coordMin) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(indices[i]); + indices[i] = -1; + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numPrimitives; ++i) { + if (indices[i] >= 0) indices[j++] = indices[i]; + } + if (j > 0) indices.length = j; + else node.indices = null; + + if (node.splitIndices != null) { + node.splitTree = new CollisionKdTree2D(this, node); + node.splitTree.createTree(); + } + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + /** + * + */ + public function traceTree():void { + traceNode("", rootNode); + } + + /** + * @param str + * @param node + */ + private function traceNode(str:String, node:CollisionKdNode):void { + if (node == null) return; + trace(str, node.axis == -1 ? "end" : ((node.axis == 0) ? "X" : ((node.axis == 1) ? "Y" : "Z")), "splitCoord=" + splitCoord, "bound", node.boundBox, "objs:", node.indices); + traceNode(str + "-", node.negativeNode); + traceNode(str + "+", node.positiveNode); + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base b/0.0.9.3/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base new file mode 100644 index 0000000..2621517 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base @@ -0,0 +1,226 @@ +package alternativa.physics.collision { + + + import alternativa.physics.collision.types.BoundBox; + + /** + * + */ + public class CollisionKdTree2D { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + + public var parentTree:CollisionKdTree; + public var parentNode:CollisionKdNode; + public var rootNode:CollisionKdNode; + + private var splitAxis:int; + private var splitCost:Number; + private var splitCoord:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + /** + * + * @param parentTree + * @param parentNode + */ + public function CollisionKdTree2D(parentTree:CollisionKdTree, parentNode:CollisionKdNode) { + this.parentTree = parentTree; + this.parentNode = parentNode; + } + + /** + * + */ + public function createTree():void { + rootNode = new CollisionKdNode(); + rootNode.boundBox = parentNode.boundBox.clone(); + rootNode.indices = new Vector.(); + var numObjects:int = parentNode.splitIndices.length; + for (var i:int = 0; i < numObjects; ++i) rootNode.indices[i] = parentNode.splitIndices[i]; + + splitNode(rootNode); + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + if (node.indices.length <= minPrimitivesPerNode) return; + + var objects:Vector. = node.indices; + var i:int; + var j:int; + + var nodeBoundBox:BoundBox = node.boundBox; + + // Подготовка баунда с погрешностями + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + // Собираем опорные координаты + var numSplitCoordsX:int; + var numSplitCoordsY:int; + var numSplitCoordsZ:int; + var numObjects:int = objects.length; + for (i = 0; i < numObjects; ++i) { + var bb:BoundBox = staticBoundBoxes[objects[i]]; + + if (parentNode.axis != 0) { + if (bb.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = bb.minX; + if (bb.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = bb.maxX; + } + + if (parentNode.axis != 1) { + if (bb.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = bb.minY; + if (bb.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = bb.maxY; + } + + if (parentNode.axis != 2) { + if (bb.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = bb.minZ; + if (bb.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = bb.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + if (parentNode.axis != 0) checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + if (parentNode.axis != 1) checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + if (parentNode.axis != 2) checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numObjects; ++i) { + bb = staticBoundBoxes[objects[i]]; + var min:Number = axisX ? bb.minX : (axisY ? bb.minY : bb.minZ); + var max:Number = axisX ? bb.maxX : (axisY ? bb.maxY : bb.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(objects[i]); + objects[i] = -1; + } + } else { + if (min >= coordMin) { + if (max > coordMax) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(objects[i]); + objects[i] = -1; + } + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numObjects; ++i) { + if (objects[i] >= 0) objects[j++] = objects[i]; + } + if (j > 0) objects.length = j; + else node.indices = null; + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/collision/.svn/text-base/CollisionPrimitive.as.svn-base b/0.0.9.3/src/alternativa/physics/collision/.svn/text-base/CollisionPrimitive.as.svn-base new file mode 100644 index 0000000..e03cfee --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/.svn/text-base/CollisionPrimitive.as.svn-base @@ -0,0 +1,140 @@ +package alternativa.physics.collision { + + import alternativa.physics.Body; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * Базовый класс для примитивов, использующихся детектором столкновений. + */ + public class CollisionPrimitive { + // Константы типов примитива + public static const BOX:int = 1; + public static const PLANE:int = 2; + public static const SPHERE:int = 3; + public static const RECT:int = 4; + public static const TRIANGLE:int = 5; + + // Тип примитива + public var type:int; + // Группы примитива. Каждая группа определяется установленным битом. Столкновения проверяются только для примитивов, + // имеющих хотя бы одну общую группу. + public var collisionGroup:int; + // Предикат примитива, вызывающийся детектором при нахождении столкновения. В зависимости от возвращённого + // предикатом результата столкновение либо регистрируется, либо игнорируется. + // Nullable + public var postCollisionPredicate:ICollisionPredicate; + // Тело, владеющее примитивом. Поле сделано открытым для быстрого доступа на чтение. Установка значения должна выполняться вызовом метода setBody(). + // Nullable + public var body:Body; + // Трансформация примитива в системе координат тела, если оно указано. Не допускается масштабирование матрицы. + public var localTransform:Matrix4; + // Полная трансформация примитива. Не допускается масштабирование матрицы. + public var transform:Matrix4 = new Matrix4(); + // AABB в мировой системе координат. Расчитывается системой вызовом функции calculateBoundBox(). + public var aabb:BoundBox = new BoundBox(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param type тип примитива + * @param collisionGroup группа примитива + */ + public function CollisionPrimitive(type:int, collisionGroup:int) { + this.type = type; + this.collisionGroup = collisionGroup; + } + + /** + * Устанавливает тело, владеющее примитивом. + * + * @param body тело, которое владеет примитивом + * @param localTransform трансформация примитива в системе координат тела. Указание значения null равносильно + * заданию единичной матрицы, однако в первом случае не будет дополнительного умножения матриц при вычислении полной трансформации примитива. + */ + public function setBody(body:Body, localTransform:Matrix4 = null):void { + if (this.body == body) return; + this.body = body; + if (body != null) { + if (localTransform != null) { + if (this.localTransform == null) { + this.localTransform = new Matrix4(); + } + this.localTransform.copy(localTransform); + } else { + this.localTransform = null; + } + } + } + + /** + * Рассчитывает AABB примитива. Наследники должны переопределять этот метод, реализуя в нём корректный рассчёт. + * + * @return ссылка на свой AABB + */ + public function calculateAABB():BoundBox { + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + return -1; + } + + /** + * Клонирует примитив. Переопределять не рекомендуется. Вместо этого переопределяются методы createPrimitive() и copyFrom(). + * + * @return клон примитива + */ + public function clone():CollisionPrimitive { + var p:CollisionPrimitive = createPrimitive(); + return p.copyFrom(this); + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + if (source == null) { + throw new ArgumentError("Parameter source cannot be null"); + } + type = source.type; + transform.copy(source.transform); + collisionGroup = source.collisionGroup; + setBody(source.body, source.localTransform); + aabb.copyFrom(source.aabb); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + public function toString():String { + return "[CollisionPrimitive type=" + type + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + protected function createPrimitive():CollisionPrimitive { + return new CollisionPrimitive(type, collisionGroup); + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base b/0.0.9.3/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base new file mode 100644 index 0000000..4ace681 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.Body; + + public interface IBodyCollisionPredicate { + function considerBodies(body1:Body, body2:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base b/0.0.9.3/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base new file mode 100644 index 0000000..e6aac2a --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base @@ -0,0 +1,29 @@ +package alternativa.physics.collision { + + import alternativa.physics.Contact; + + /** + * Интерфейс определителя столкновений между двумя примитивами. + */ + public interface ICollider { + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base b/0.0.9.3/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base new file mode 100644 index 0000000..916f67b --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base @@ -0,0 +1,64 @@ +package alternativa.physics.collision { + import alternativa.physics.Contact; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.math.Vector3; + + /** + * Интерфейс детектора столкновений. + */ + public interface ICollisionDetector { + + /** + * Получает все столкновения в текущей конфигурации физической геометрии. + * + * @param contacts список контактов, в кторые будет записана информация о столкновении + * @return количество найденных столкновений + */ + function getAllContacts(contacts:Contact):Contact; + + /** + * Тестирует луч на пересечение с физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param direction направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRay(origin:Vector3, direction:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * + * @param origin + * @param direction + * @param collisionGroup + * @param maxTime + * @param predicate + * @param result + * @return + */ + function intersectRayWithStatic(origin:Vector3, direction:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @return + */ + function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base b/0.0.9.3/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base new file mode 100644 index 0000000..33d30ca --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base @@ -0,0 +1,8 @@ +package alternativa.physics.collision { + + public interface ICollisionPredicate { + + function considerCollision(primitive:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base b/0.0.9.3/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base new file mode 100644 index 0000000..b319f62 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.Body; + + public interface IRayCollisionPredicate { + function considerBody(body:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base b/0.0.9.3/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base new file mode 100644 index 0000000..9ca4c2a --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base @@ -0,0 +1,486 @@ +package alternativa.physics.collision { + + + 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.math.Vector3; + + use namespace altphysics; + + /** + * Детектор, хранящий статическую геометрию в kD-дереве и использующий дерево для ускорения тестов на пересечения. + */ + public class KdTreeCollisionDetector implements ICollisionDetector { + + altphysics var tree:CollisionKdTree; + altphysics var dynamicPrimitives:Vector.; + 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.(); + + 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.):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. = tree.staticChildren; +// var indices:Vector. = 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., indices:Vector., 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; + } \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/collision/CollisionKdNode.as b/0.0.9.3/src/alternativa/physics/collision/CollisionKdNode.as new file mode 100644 index 0000000..f450f1e --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/CollisionKdNode.as @@ -0,0 +1,18 @@ +package alternativa.physics.collision { + + + import alternativa.physics.collision.types.BoundBox; + + public class CollisionKdNode { + public var indices:Vector.; + public var splitIndices:Vector.; + public var boundBox:BoundBox; + public var parent:CollisionKdNode; + public var splitTree:CollisionKdTree2D; + + public var axis:int = -1; // 0 - x, 1 - y, 2 - z + public var coord:Number; + public var positiveNode:CollisionKdNode; + public var negativeNode:CollisionKdNode; + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/collision/CollisionKdTree.as b/0.0.9.3/src/alternativa/physics/collision/CollisionKdTree.as new file mode 100644 index 0000000..d3dc307 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/CollisionKdTree.as @@ -0,0 +1,261 @@ +package alternativa.physics.collision { + + + + import alternativa.physics.collision.types.BoundBox; + + /** + * @author mike + */ + public class CollisionKdTree { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + public var rootNode:CollisionKdNode; + public var staticChildren:Vector.; + public var numStaticChildren:int; + public var staticBoundBoxes:Vector. = new Vector.(); + + private var splitAxis:int; + private var splitCoord:Number; + private var splitCost:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + + /** + * @param boundBox + */ + public function createTree(collisionPrimitives:Vector., boundBox:BoundBox = null):void { + staticChildren = collisionPrimitives.concat(); + numStaticChildren = staticChildren.length; + // Создаём корневую ноду + rootNode = new CollisionKdNode(); + rootNode.indices = new Vector.(); + // Расчитываем баунды объектов и рутовой ноды + var rootNodeBoundBox:BoundBox = rootNode.boundBox = (boundBox != null) ? boundBox : new BoundBox(); + for (var i:int = 0; i < numStaticChildren; ++i) { + var child:CollisionPrimitive = staticChildren[i]; + var childBoundBox:BoundBox = staticBoundBoxes[i] = child.calculateAABB(); + rootNodeBoundBox.addBoundBox(childBoundBox); + rootNode.indices[i] = i; + } + staticBoundBoxes.length = numStaticChildren; + // Разделяем рутовую ноду + splitNode(rootNode); + + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + var indices:Vector. = node.indices; + var numPrimitives:int = indices.length; + if (numPrimitives <= minPrimitivesPerNode) return; + + // Подготовка баунда с погрешностями + var nodeBoundBox:BoundBox = node.boundBox; + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + // Собираем опорные координаты + var i:int; + var j:int; + var numSplitCoordsX:int = 0 + var numSplitCoordsY:int = 0; + var numSplitCoordsZ:int = 0; + for (i = 0; i < numPrimitives; ++i) { + var boundBox:BoundBox = staticBoundBoxes[indices[i]]; + + if (boundBox.maxX - boundBox.minX <= doubleThreshold) { + if (boundBox.minX <= nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.minX; + else if (boundBox.maxX >= nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.maxX; + else splitCoordsX[numSplitCoordsX++] = (boundBox.minX + boundBox.maxX)*0.5; + } else { + if (boundBox.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = boundBox.minX; + if (boundBox.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = boundBox.maxX; + } + + if (boundBox.maxY - boundBox.minY <= doubleThreshold) { + if (boundBox.minY <= nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.minY; + else if (boundBox.maxY >= nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.maxY; + else splitCoordsY[numSplitCoordsY++] = (boundBox.minY + boundBox.maxY)*0.5; + } else { + if (boundBox.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = boundBox.minY; + if (boundBox.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = boundBox.maxY; + } + + if (boundBox.maxZ - boundBox.minZ <= doubleThreshold) { + if (boundBox.minZ <= nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.minZ; + else if (boundBox.maxZ >= nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.maxZ; + else splitCoordsZ[numSplitCoordsZ++] = (boundBox.minZ + boundBox.maxZ)*0.5; + } else { + if (boundBox.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.minZ; + if (boundBox.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numPrimitives; ++i) { + boundBox = staticBoundBoxes[indices[i]]; + var min:Number = axisX ? boundBox.minX : (axisY ? boundBox.minY : boundBox.minZ); + var max:Number = axisX ? boundBox.maxX : (axisY ? boundBox.maxY : boundBox.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(indices[i]); + indices[i] = -1; + } else { + if (node.splitIndices == null) node.splitIndices = new Vector.(); + node.splitIndices.push(indices[i]); + indices[i] = -1; + } + } else { + if (min >= coordMin) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(indices[i]); + indices[i] = -1; + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numPrimitives; ++i) { + if (indices[i] >= 0) indices[j++] = indices[i]; + } + if (j > 0) indices.length = j; + else node.indices = null; + + if (node.splitIndices != null) { + node.splitTree = new CollisionKdTree2D(this, node); + node.splitTree.createTree(); + } + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + /** + * + */ + public function traceTree():void { + traceNode("", rootNode); + } + + /** + * @param str + * @param node + */ + private function traceNode(str:String, node:CollisionKdNode):void { + if (node == null) return; + trace(str, node.axis == -1 ? "end" : ((node.axis == 0) ? "X" : ((node.axis == 1) ? "Y" : "Z")), "splitCoord=" + splitCoord, "bound", node.boundBox, "objs:", node.indices); + traceNode(str + "-", node.negativeNode); + traceNode(str + "+", node.positiveNode); + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/collision/CollisionKdTree2D.as b/0.0.9.3/src/alternativa/physics/collision/CollisionKdTree2D.as new file mode 100644 index 0000000..2621517 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/CollisionKdTree2D.as @@ -0,0 +1,226 @@ +package alternativa.physics.collision { + + + import alternativa.physics.collision.types.BoundBox; + + /** + * + */ + public class CollisionKdTree2D { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + + public var parentTree:CollisionKdTree; + public var parentNode:CollisionKdNode; + public var rootNode:CollisionKdNode; + + private var splitAxis:int; + private var splitCost:Number; + private var splitCoord:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + /** + * + * @param parentTree + * @param parentNode + */ + public function CollisionKdTree2D(parentTree:CollisionKdTree, parentNode:CollisionKdNode) { + this.parentTree = parentTree; + this.parentNode = parentNode; + } + + /** + * + */ + public function createTree():void { + rootNode = new CollisionKdNode(); + rootNode.boundBox = parentNode.boundBox.clone(); + rootNode.indices = new Vector.(); + var numObjects:int = parentNode.splitIndices.length; + for (var i:int = 0; i < numObjects; ++i) rootNode.indices[i] = parentNode.splitIndices[i]; + + splitNode(rootNode); + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + if (node.indices.length <= minPrimitivesPerNode) return; + + var objects:Vector. = node.indices; + var i:int; + var j:int; + + var nodeBoundBox:BoundBox = node.boundBox; + + // Подготовка баунда с погрешностями + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + // Собираем опорные координаты + var numSplitCoordsX:int; + var numSplitCoordsY:int; + var numSplitCoordsZ:int; + var numObjects:int = objects.length; + for (i = 0; i < numObjects; ++i) { + var bb:BoundBox = staticBoundBoxes[objects[i]]; + + if (parentNode.axis != 0) { + if (bb.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = bb.minX; + if (bb.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = bb.maxX; + } + + if (parentNode.axis != 1) { + if (bb.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = bb.minY; + if (bb.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = bb.maxY; + } + + if (parentNode.axis != 2) { + if (bb.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = bb.minZ; + if (bb.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = bb.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + if (parentNode.axis != 0) checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + if (parentNode.axis != 1) checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + if (parentNode.axis != 2) checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numObjects; ++i) { + bb = staticBoundBoxes[objects[i]]; + var min:Number = axisX ? bb.minX : (axisY ? bb.minY : bb.minZ); + var max:Number = axisX ? bb.maxX : (axisY ? bb.maxY : bb.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(objects[i]); + objects[i] = -1; + } + } else { + if (min >= coordMin) { + if (max > coordMax) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(objects[i]); + objects[i] = -1; + } + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numObjects; ++i) { + if (objects[i] >= 0) objects[j++] = objects[i]; + } + if (j > 0) objects.length = j; + else node.indices = null; + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/collision/CollisionPrimitive.as b/0.0.9.3/src/alternativa/physics/collision/CollisionPrimitive.as new file mode 100644 index 0000000..e03cfee --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/CollisionPrimitive.as @@ -0,0 +1,140 @@ +package alternativa.physics.collision { + + import alternativa.physics.Body; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * Базовый класс для примитивов, использующихся детектором столкновений. + */ + public class CollisionPrimitive { + // Константы типов примитива + public static const BOX:int = 1; + public static const PLANE:int = 2; + public static const SPHERE:int = 3; + public static const RECT:int = 4; + public static const TRIANGLE:int = 5; + + // Тип примитива + public var type:int; + // Группы примитива. Каждая группа определяется установленным битом. Столкновения проверяются только для примитивов, + // имеющих хотя бы одну общую группу. + public var collisionGroup:int; + // Предикат примитива, вызывающийся детектором при нахождении столкновения. В зависимости от возвращённого + // предикатом результата столкновение либо регистрируется, либо игнорируется. + // Nullable + public var postCollisionPredicate:ICollisionPredicate; + // Тело, владеющее примитивом. Поле сделано открытым для быстрого доступа на чтение. Установка значения должна выполняться вызовом метода setBody(). + // Nullable + public var body:Body; + // Трансформация примитива в системе координат тела, если оно указано. Не допускается масштабирование матрицы. + public var localTransform:Matrix4; + // Полная трансформация примитива. Не допускается масштабирование матрицы. + public var transform:Matrix4 = new Matrix4(); + // AABB в мировой системе координат. Расчитывается системой вызовом функции calculateBoundBox(). + public var aabb:BoundBox = new BoundBox(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param type тип примитива + * @param collisionGroup группа примитива + */ + public function CollisionPrimitive(type:int, collisionGroup:int) { + this.type = type; + this.collisionGroup = collisionGroup; + } + + /** + * Устанавливает тело, владеющее примитивом. + * + * @param body тело, которое владеет примитивом + * @param localTransform трансформация примитива в системе координат тела. Указание значения null равносильно + * заданию единичной матрицы, однако в первом случае не будет дополнительного умножения матриц при вычислении полной трансформации примитива. + */ + public function setBody(body:Body, localTransform:Matrix4 = null):void { + if (this.body == body) return; + this.body = body; + if (body != null) { + if (localTransform != null) { + if (this.localTransform == null) { + this.localTransform = new Matrix4(); + } + this.localTransform.copy(localTransform); + } else { + this.localTransform = null; + } + } + } + + /** + * Рассчитывает AABB примитива. Наследники должны переопределять этот метод, реализуя в нём корректный рассчёт. + * + * @return ссылка на свой AABB + */ + public function calculateAABB():BoundBox { + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + return -1; + } + + /** + * Клонирует примитив. Переопределять не рекомендуется. Вместо этого переопределяются методы createPrimitive() и copyFrom(). + * + * @return клон примитива + */ + public function clone():CollisionPrimitive { + var p:CollisionPrimitive = createPrimitive(); + return p.copyFrom(this); + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + if (source == null) { + throw new ArgumentError("Parameter source cannot be null"); + } + type = source.type; + transform.copy(source.transform); + collisionGroup = source.collisionGroup; + setBody(source.body, source.localTransform); + aabb.copyFrom(source.aabb); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + public function toString():String { + return "[CollisionPrimitive type=" + type + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + protected function createPrimitive():CollisionPrimitive { + return new CollisionPrimitive(type, collisionGroup); + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/collision/IBodyCollisionPredicate.as b/0.0.9.3/src/alternativa/physics/collision/IBodyCollisionPredicate.as new file mode 100644 index 0000000..4ace681 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/IBodyCollisionPredicate.as @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.Body; + + public interface IBodyCollisionPredicate { + function considerBodies(body1:Body, body2:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/collision/ICollider.as b/0.0.9.3/src/alternativa/physics/collision/ICollider.as new file mode 100644 index 0000000..e6aac2a --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/ICollider.as @@ -0,0 +1,29 @@ +package alternativa.physics.collision { + + import alternativa.physics.Contact; + + /** + * Интерфейс определителя столкновений между двумя примитивами. + */ + public interface ICollider { + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/collision/ICollisionDetector.as b/0.0.9.3/src/alternativa/physics/collision/ICollisionDetector.as new file mode 100644 index 0000000..916f67b --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/ICollisionDetector.as @@ -0,0 +1,64 @@ +package alternativa.physics.collision { + import alternativa.physics.Contact; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.math.Vector3; + + /** + * Интерфейс детектора столкновений. + */ + public interface ICollisionDetector { + + /** + * Получает все столкновения в текущей конфигурации физической геометрии. + * + * @param contacts список контактов, в кторые будет записана информация о столкновении + * @return количество найденных столкновений + */ + function getAllContacts(contacts:Contact):Contact; + + /** + * Тестирует луч на пересечение с физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param direction направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRay(origin:Vector3, direction:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * + * @param origin + * @param direction + * @param collisionGroup + * @param maxTime + * @param predicate + * @param result + * @return + */ + function intersectRayWithStatic(origin:Vector3, direction:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @return + */ + function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/collision/ICollisionPredicate.as b/0.0.9.3/src/alternativa/physics/collision/ICollisionPredicate.as new file mode 100644 index 0000000..33d30ca --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/ICollisionPredicate.as @@ -0,0 +1,8 @@ +package alternativa.physics.collision { + + public interface ICollisionPredicate { + + function considerCollision(primitive:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/collision/IRayCollisionPredicate.as b/0.0.9.3/src/alternativa/physics/collision/IRayCollisionPredicate.as new file mode 100644 index 0000000..b319f62 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/IRayCollisionPredicate.as @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.Body; + + public interface IRayCollisionPredicate { + function considerBody(body:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/collision/KdTreeCollisionDetector.as b/0.0.9.3/src/alternativa/physics/collision/KdTreeCollisionDetector.as new file mode 100644 index 0000000..9ca4c2a --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/KdTreeCollisionDetector.as @@ -0,0 +1,486 @@ +package alternativa.physics.collision { + + + 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.math.Vector3; + + use namespace altphysics; + + /** + * Детектор, хранящий статическую геометрию в kD-дереве и использующий дерево для ускорения тестов на пересечения. + */ + public class KdTreeCollisionDetector implements ICollisionDetector { + + altphysics var tree:CollisionKdTree; + altphysics var dynamicPrimitives:Vector.; + 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.(); + + 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.):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. = tree.staticChildren; +// var indices:Vector. = 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., indices:Vector., 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; + } \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/collision/colliders/.svn/all-wcprops b/0.0.9.3/src/alternativa/physics/collision/colliders/.svn/all-wcprops new file mode 100644 index 0000000..6ced510 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/colliders/.svn/all-wcprops @@ -0,0 +1,53 @@ +K 25 +svn:wc:ra_dav:version-url +V 123 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/collision/colliders +END +BoxSphereCollider.as +K 25 +svn:wc:ra_dav:version-url +V 144 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/collision/colliders/BoxSphereCollider.as +END +BoxBoxCollider.as +K 25 +svn:wc:ra_dav:version-url +V 141 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/collision/colliders/BoxBoxCollider.as +END +BoxRectCollider.as +K 25 +svn:wc:ra_dav:version-url +V 142 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/collision/colliders/BoxRectCollider.as +END +SpherePlaneCollider.as +K 25 +svn:wc:ra_dav:version-url +V 146 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/collision/colliders/SpherePlaneCollider.as +END +BoxCollider.as +K 25 +svn:wc:ra_dav:version-url +V 138 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/collision/colliders/BoxCollider.as +END +BoxPlaneCollider.as +K 25 +svn:wc:ra_dav:version-url +V 143 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/collision/colliders/BoxPlaneCollider.as +END +SphereSphereCollider.as +K 25 +svn:wc:ra_dav:version-url +V 147 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/collision/colliders/SphereSphereCollider.as +END +BoxTriangleCollider.as +K 25 +svn:wc:ra_dav:version-url +V 146 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/collision/colliders/BoxTriangleCollider.as +END diff --git a/0.0.9.3/src/alternativa/physics/collision/colliders/.svn/entries b/0.0.9.3/src/alternativa/physics/collision/colliders/.svn/entries new file mode 100644 index 0000000..1885975 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/colliders/.svn/entries @@ -0,0 +1,124 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/collision/colliders +http://svndev.alternativaplatform.com + + + +2010-03-06T16:51:19.781174Z +29404 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +BoxSphereCollider.as +file + + + + +2010-10-28T04:32:44.000000Z +25170bcb9a1eec2a0941f4b50a06a554 +2009-12-08T15:53:11.094448Z +24506 +mike + +BoxBoxCollider.as +file + + + + +2010-10-28T04:32:44.000000Z +15da13b1383e9c1892edc205c8c055b0 +2010-02-12T12:34:25.192477Z +28424 +mike + +BoxRectCollider.as +file + + + + +2010-10-28T04:32:44.000000Z +4a35f94d3ef1af0a5b42e1a100580f6d +2010-03-06T16:51:19.781174Z +29404 +mike + +SpherePlaneCollider.as +file + + + + +2010-10-28T04:32:44.000000Z +67621ed47d7e90b72498acf2802c4e2d +2009-11-24T08:44:15.941480Z +23758 +mike + +BoxCollider.as +file + + + + +2010-10-28T04:32:44.000000Z +52e72ed30a24a3a43ce16a19930546b5 +2010-02-12T12:34:25.192477Z +28424 +mike + +BoxPlaneCollider.as +file + + + + +2010-10-28T04:32:44.000000Z +204c7b02a9a1913269c558658bad4262 +2010-02-12T12:34:25.192477Z +28424 +mike + +SphereSphereCollider.as +file + + + + +2010-10-28T04:32:44.000000Z +301c7102f91a7f3ede83e35ca202db0b +2009-11-24T08:44:15.941480Z +23758 +mike + +BoxTriangleCollider.as +file + + + + +2010-10-28T04:32:44.000000Z +448502271cac4f09491ca128b4bb485a +2010-03-06T16:51:19.781174Z +29404 +mike + diff --git a/0.0.9.3/src/alternativa/physics/collision/colliders/.svn/format b/0.0.9.3/src/alternativa/physics/collision/colliders/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/colliders/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.3/src/alternativa/physics/collision/colliders/.svn/text-base/BoxBoxCollider.as.svn-base b/0.0.9.3/src/alternativa/physics/collision/colliders/.svn/text-base/BoxBoxCollider.as.svn-base new file mode 100644 index 0000000..2d3f04c --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/colliders/.svn/text-base/BoxBoxCollider.as.svn-base @@ -0,0 +1,589 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * Расчитывает точки контакта двух боксов. Нормаль контакта направляется в сторону бокса с меньшим ID. + */ + public class BoxBoxCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + private var vectorToBox1:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var bestAxisIndex:int; + private var minOverlap:Number; + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + private var tmpPoints:Vector. = new Vector.(8, true); + private var pcount:int; + + /** + * + */ + public function BoxBoxCollider() { + for (var i:int = 0; i < 8; i++) { + tmpPoints[i] = new ContactPoint(); + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + + if (bestAxisIndex < 6) { + // Контакт грань-(грань|ребро|вершина) + if (!findFaceContactPoints(box1, box2, vectorToBox1, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 6; + findEdgesIntersection(box1, box2, vectorToBox1, int(bestAxisIndex/3), bestAxisIndex%3, contact); + } + contact.body1 = box1.body; + contact.body2 = box2.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + // Вектор из центра второго бокса в центр первого + vectorToBox1.x = transform1.d - transform2.d; + vectorToBox1.y = transform1.h - transform2.h; + vectorToBox1.z = transform1.l - transform2.l; + + // Проверка пересечения по основным осям + axis10.x = transform1.a; + axis10.y = transform1.e; + axis10.z = transform1.i; + if (!testMainAxis(box1, box2, axis10, 0, vectorToBox1)) return false; + axis11.x = transform1.b; + axis11.y = transform1.f; + axis11.z = transform1.j; + if (!testMainAxis(box1, box2, axis11, 1, vectorToBox1)) return false; + axis12.x = transform1.c; + axis12.y = transform1.g; + axis12.z = transform1.k; + if (!testMainAxis(box1, box2, axis12, 2, vectorToBox1)) return false; + + axis20.x = transform2.a; + axis20.y = transform2.e; + axis20.z = transform2.i; + if (!testMainAxis(box1, box2, axis20, 3, vectorToBox1)) return false; + axis21.x = transform2.b; + axis21.y = transform2.f; + axis21.z = transform2.j; + if (!testMainAxis(box1, box2, axis21, 4, vectorToBox1)) return false; + axis22.x = transform2.c; + axis22.y = transform2.g; + axis22.z = transform2.k; + if (!testMainAxis(box1, box2, axis22, 5, vectorToBox1)) return false; + + // Проверка производных осей + if (!testDerivedAxis(box1, box2, axis10, axis20, 6, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis21, 7, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis22, 8, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis11, axis20, 9, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis21, 10, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis22, 11, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis12, axis20, 12, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis21, 13, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis22, 14, vectorToBox1)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта грани одного бокса с гранью/ребром/вершиной другого. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param faceAxisIdx индекс оси первого бокса, перпендикулярной грани, с которой произошло столкновение + * @param contactInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var swapNormal:Boolean = false; + if (faceAxisIdx > 2) { + // Столкновение с гранью второго бокса. Для дальнейших расчётов боксы меняются местами, + // но нормаль контакта всё равно должна быть направлена в сторону первоначального box1 + var tmpBox:CollisionBox = box1; + box1 = box2; + box2 = tmpBox; + vectorToBox1.x = -vectorToBox1.x; + vectorToBox1.y = -vectorToBox1.y; + vectorToBox1.z = -vectorToBox1.z; + faceAxisIdx -= 3; + swapNormal = true; + } + + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + var colAxis:Vector3 = contact.normal; + + transform1.getAxis(faceAxisIdx, colAxis); + var negativeFace:Boolean = colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z > 0; + + var code:int = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box1.excludedFaces) != 0) return false; + + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + // Ищем ось второго бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + transform2.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisDot = dot; + incidentAxisIdx = axisIdx; + } + } + // Получаем список вершин грани второго бокса, переводим их в систему координат первого бокса и выполняем обрезку + // по грани первого бокса. Таким образом получается список потенциальных точек контакта. + transform2.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box2.hs, incidentAxisIdx, incidentAxisDot < 0, points1); + + // TODO: Вычислить результирующую матрицу, затем преобразовать векторы за один заход + transform2.transformVectorsN(points1, points2, 4); + transform1.transformVectorsInverseN(points2, points1, 4); + + var pnum:int = clip(box1.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points1[i]; + if ((pen = getPointBoxPenetration(box1.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + var cp:ContactPoint = tmpPoints[pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = transform1.a*v.x + transform1.b*v.y + transform1.c*v.z + transform1.d; + cpPos.y = transform1.e*v.x + transform1.f*v.y + transform1.g*v.z + transform1.h; + cpPos.z = transform1.i*v.x + transform1.j*v.y + transform1.k*v.z + transform1.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + + cp.penetration = pen; + } + } + if (swapNormal) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (pcount > 4) { + reducePoints(); + } + for (i = 0; i < pcount; i++) { + cp = contact.points[i]; + cp.copyFrom(tmpPoints[i]); + } + contact.pcount = pcount; + return true; + } + + /** + * + * @param contactInfo + */ + private function reducePoints():void { + var i:int; + var minIdx:int; + var cp1:ContactPoint; + var cp2:ContactPoint; + while (pcount > 4) { + var minLen:Number = 1e10; + var p1:Vector3 = (tmpPoints[int(pcount - 1)] as ContactPoint).pos; + var p2:Vector3; + for (i = 0; i < pcount; i++) { + p2 = (tmpPoints[i] as ContactPoint).pos; + var dx:Number = p2.x - p1.x; + var dy:Number = p2.y - p1.y; + var dz:Number = p2.z - p1.z; + var len:Number = dx*dx + dy*dy + dz*dz; + if (len < minLen) { + minLen = len; + minIdx = i; + } + p1 = p2; + } + var ii:int = minIdx == 0 ? (pcount - 1) : (minIdx - 1); + cp1 = tmpPoints[ii]; + cp2 = tmpPoints[minIdx]; + p1 = cp1.pos; + p2 = cp2.pos; + p2.x = 0.5*(p1.x + p2.x); + p2.y = 0.5*(p1.y + p2.y); + p2.z = 0.5*(p1.z + p2.z); + cp2.penetration = 0.5*(cp1.penetration + cp2.penetration); + if (minIdx > 0) { + for (i = minIdx; i < pcount; i++) { + tmpPoints[int(i - 1)] = tmpPoints[i]; + } + tmpPoints[int(pcount - 1)] = cp1; + } + pcount--; + } + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clip(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param contactInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):void { + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + transform1.getAxis(axisIdx1, axis10); + transform2.getAxis(axisIdx2, axis20); + + var colAxis:Vector3 = contact.normal; + + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону первого бокса + if (colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + + var tx:Number = box1.hs.x; + var ty:Number = box1.hs.y; + var tz:Number = box1.hs.z; + + var x2:Number = box2.hs.x; + var y2:Number = box2.hs.y; + var z2:Number = box2.hs.z; + // x + if (axisIdx1 == 0) { + tx = 0; + halfLen1 = box1.hs.x; + } else { + if (colAxis.x*transform1.a + colAxis.y*transform1.e + colAxis.z*transform1.i > 0) { + tx = -tx; + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = box2.hs.x; + } else { + if (colAxis.x*transform2.a + colAxis.y*transform2.e + colAxis.z*transform2.i < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + ty = 0; + halfLen1 = box1.hs.y; + } else { + if (colAxis.x*transform1.b + colAxis.y*transform1.f + colAxis.z*transform1.j > 0) { + ty = -ty; + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = box2.hs.y; + } else { + if (colAxis.x*transform2.b + colAxis.y*transform2.f + colAxis.z*transform2.j < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + tz = 0; + halfLen1 = box1.hs.z; + } else { + if (colAxis.x*transform1.c + colAxis.y*transform1.g + colAxis.z*transform1.k > 0) { + tz = -tz; + } + } + if (axisIdx2 == 2) { + z2 = 0; + halfLen2 = box2.hs.z; + } else { + if (colAxis.x*transform2.c + colAxis.y*transform2.g + colAxis.z*transform2.k < 0) { + z2 = -z2; + } + } + // Получаем глобальные координаты средних точек рёбер + var x1:Number = transform1.a*tx + transform1.b*ty + transform1.c*tz + transform1.d; + var y1:Number = transform1.e*tx + transform1.f*ty + transform1.g*tz + transform1.h; + var z1:Number = transform1.i*tx + transform1.j*ty + transform1.k*tz + transform1.l; + tx = x2; + ty = y2; + tz = z2; + x2 = transform2.a*tx + transform2.b*ty + transform2.c*tz + transform2.d; + y2 = transform2.e*tx + transform2.f*ty + transform2.g*tz + transform2.h; + z2 = transform2.i*tx + transform2.j*ty + transform2.k*tz + transform2.l; + + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + tx = x2 - x1; + ty = y2 - y1; + tz = z2 - z1; + var c1:Number = axis10.x*tx + axis10.y*ty + axis10.z*tz; + var c2:Number = axis20.x*tx + axis20.y*ty + axis20.z*tz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cp.pos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cp.pos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение боксов вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box1 + * @param box2 + * @param axis + * @param axisIndex + * @param toBox1 + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box1 + * @param box2 + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox1 + * @return + */ + private function testDerivedAxis(box1:CollisionBox, box2:CollisionBox, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия двух боксов вдоль заданной оси. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param axis ось + * @param vectorToBox1 вектор, соединяющий центр второго бокса с центром первого + * @return величина перекрытия боксов вдоль оси + */ + public function overlapOnAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, vectorToBox1:Vector3):Number { + var m:Matrix4 = box1.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box1.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box1.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box1.hs.z; + if (d < 0) d = -d; + projection += d; + + m = box2.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box2.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box2.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box2.hs.z; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox1.x*axis.x + vectorToBox1.y*axis.y + vectorToBox1.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} + import alternativa.math.Vector3; + + class CollisionPointTmp { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + } diff --git a/0.0.9.3/src/alternativa/physics/collision/colliders/.svn/text-base/BoxCollider.as.svn-base b/0.0.9.3/src/alternativa/physics/collision/colliders/.svn/text-base/BoxCollider.as.svn-base new file mode 100644 index 0000000..2429bb1 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/colliders/.svn/text-base/BoxCollider.as.svn-base @@ -0,0 +1,427 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + /** + * + */ + public class BoxCollider implements ICollider { + + /** + * + */ + public function BoxCollider() { + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + return false; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + protected function getFaceVertsByAxis(hs:Vector3, axisIdx:int, negativeFace:Boolean, result:Vector.):void { + var v:Vector3; + switch (axisIdx) { + case 0: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + } + break; + case 1: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + } + break; + case 2: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + } else { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } + break; + } + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/collision/colliders/.svn/text-base/BoxPlaneCollider.as.svn-base b/0.0.9.3/src/alternativa/physics/collision/colliders/.svn/text-base/BoxPlaneCollider.as.svn-base new file mode 100644 index 0000000..3bebdc3 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/colliders/.svn/text-base/BoxPlaneCollider.as.svn-base @@ -0,0 +1,88 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + public class BoxPlaneCollider implements ICollider { + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var normal:Vector3 = new Vector3(); + + public function BoxPlaneCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var box:RigidBox = body1 as RigidBox; +// var plane:RigidPlane; +// if (box == null) { +// box = body2 as RigidBox; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные координаты вершин бокса +// var sx:Number = box.halfSize.x; +// var sy:Number = box.halfSize.y; +// var sz:Number = box.halfSize.z; +// (verts1[0] as Vector3).reset(-sx, -sy, -sz); +// (verts1[1] as Vector3).reset(sx, -sy, -sz); +// (verts1[2] as Vector3).reset(sx, sy, -sz); +// (verts1[3] as Vector3).reset(-sx, sy, -sz); +// (verts1[4] as Vector3).reset(-sx, -sy, sz); +// (verts1[5] as Vector3).reset(sx, -sy, sz); +// (verts1[6] as Vector3).reset(sx, sy, sz); +// (verts1[7] as Vector3).reset(-sx, sy, sz); +// +// box.transform.transformVectors(verts1, verts2); +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// // Проверяем наличие столкновений с каждой вершиной +// collisionInfo.pcount = 0; +// for (var i:int = 0; i < 8; i++) { +// // Вершина добавляется в список точек столкновения, если лежит под плоскостью +// var dist:Number = (verts2[i] as Vector3).dot(normal); +// if (dist < offset) { +// var cp:ContactPoint; +// if (collisionInfo.pcount == collisionInfo.points.length) { +// cp = new ContactPoint(); +// collisionInfo.points[collisionInfo.pcount] = cp; +// } else { +// cp = collisionInfo.points[collisionInfo.pcount]; +// } +// cp.pos.copy(verts2[i]); +// cp.r1.diff(cp.pos, box.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); +// cp.penetration = offset - dist; +// collisionInfo.pcount++; +// } +// } +// if (collisionInfo.pcount > 0) { +// collisionInfo.body1 = box; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// return true; +// } + return false; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/collision/colliders/.svn/text-base/BoxRectCollider.as.svn-base b/0.0.9.3/src/alternativa/physics/collision/colliders/.svn/text-base/BoxRectCollider.as.svn-base new file mode 100644 index 0000000..d6ba302 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/colliders/.svn/text-base/BoxRectCollider.as.svn-base @@ -0,0 +1,575 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * + */ + public class BoxRectCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + + private var vectorToBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxRectCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, rect, vectorToBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + if (!findEdgesIntersection(box, rect, vectorToBox, int(bestAxisIndex/2), bestAxisIndex%2, contact)) { + return false; + } + } + contact.body1 = box.body; + contact.body2 = rect.body; + + // Хак для танков, чтобы исключить утыкания танков в стыки статических примитивов + if (rect.transform.k > 0.99999) { + contact.normal.x = 0; + contact.normal.y = 0; + contact.normal.z = 1; + } + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + // Вектор из центра прямоугольника в центр бокса + vectorToBox.x = boxTransform.d - rectTransform.d; + vectorToBox.y = boxTransform.h - rectTransform.h; + vectorToBox.z = boxTransform.l - rectTransform.l; + + // Проверка пересечения по нормали прямоугольника + rectTransform.getAxis(2, axis22); + if (!testMainAxis(box, rect, axis22, 0, vectorToBox)) return false; + + // Проверка пересечения по основным осям бокса + boxTransform.getAxis(0, axis10); + if (!testMainAxis(box, rect, axis10, 1, vectorToBox)) return false; + boxTransform.getAxis(1, axis11); + if (!testMainAxis(box, rect, axis11, 2, vectorToBox)) return false; + boxTransform.getAxis(2, axis12); + if (!testMainAxis(box, rect, axis12, 3, vectorToBox)) return false; + + // Получаем направляющие рёбер прямоугольника + rectTransform.getAxis(0, axis20); + rectTransform.getAxis(1, axis21); + + // Проверка производных осей + if (!testDerivedAxis(box, rect, axis10, axis20, 4, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis10, axis21, 5, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis11, axis20, 6, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis11, axis21, 7, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis12, axis20, 8, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis12, axis21, 9, vectorToBox)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта бокса с прямоугольником. + * + * @param box бокс + * @param rect прямоугольник + * @param vectorToBox вектор, направленный из центра прямоугольника в центр бокса + * @param faceAxisIdx индекс оси, идентифицирующей полскость столкновения (грань бокса или полскость прямоугольника) + * @param colInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var pnum:int; + var i:int; + var v:Vector3; + var cp:ContactPoint; + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + var colAxis:Vector3 = contact.normal; + + var negativeFace:Boolean; + var code:int; + + if (faceAxisIdx == 0) { + // Столкновение с плоскостью прямоугольника + + // Проверим положение бокса относительно плоскости прямоугольника + colAxis.x = rectTransform.c; + colAxis.y = rectTransform.g; + colAxis.z = rectTransform.k; + +// var offset:Number = colAxis.x*rectTransform.d + colAxis.y*rectTransform.h + colAxis.z*rectTransform.l; +// if (bbPos.vDot(colAxis) < offset) return false; + + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisIdx = axisIdx; + incidentAxisDot = dot; + } + } + negativeFace = incidentAxisDot > 0; + + code = 1 << (incidentAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + + // Получаем список вершин грани бокса, переводим их в систему координат прямоугольника и выполняем обрезку + // по прямоугольнику. Таким образом получается список потенциальных точек контакта. + boxTransform.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box.hs, incidentAxisIdx, negativeFace, points1); + boxTransform.transformVectorsN(points1, points2, 4); + rectTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByRect(rect.hs); + // Проверяем каждую потенциальную точку на принадлежность нижней полуплоскости прямоугольника и добавляем такие точки в список контактов + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if (v.z < epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = -v.z; + var cpPos:Vector3 = cp.pos; + cpPos.x = rectTransform.a*v.x + rectTransform.b*v.y + rectTransform.c*v.z + rectTransform.d; + cpPos.y = rectTransform.e*v.x + rectTransform.f*v.y + rectTransform.g*v.z + rectTransform.h; + cpPos.z = rectTransform.i*v.x + rectTransform.j*v.y + rectTransform.k*v.z + rectTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } else { + // Столкновение с гранью бокса + faceAxisIdx--; + boxTransform.getAxis(faceAxisIdx, colAxis); + negativeFace = colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z > 0; + + code = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) { + return false; + } + + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (rectTransform.c*colAxis.x + rectTransform.g*colAxis.y + rectTransform.k*colAxis.z < 0) return false; + + getFaceVertsByAxis(rect.hs, 2, false, points1); + rectTransform.transformVectorsN(points1, points2, 4); + boxTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if ((pen = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = pen; + cpPos = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } + return true; + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * + * @param hs + * @return + */ + private function clipByRect(hs:Vector3):int { + var pnum:int = 4; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box первый бокс + * @param rect второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param colInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + boxTransform.getAxis(axisIdx1, axis10); + rectTransform.getAxis(axisIdx2, axis20); + var colAxis:Vector3 = contact.normal; + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону бокса + if (colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + var vx:Number = box.hs.x; + var vy:Number = box.hs.y; + var vz:Number = box.hs.z; + var x2:Number = rect.hs.x; + var y2:Number = rect.hs.y; + var z2:Number = rect.hs.z; + // x + if (axisIdx1 == 0) { + vx = 0; + halfLen1 = box.hs.x; + } else { + if (boxTransform.a*colAxis.x + boxTransform.e*colAxis.y + boxTransform.i*colAxis.z > 0) { + vx = -vx; + if ((box.excludedFaces & 2) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 1) != 0) { + return false; + } + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = rect.hs.x; + } else { + if (rectTransform.a*colAxis.x + rectTransform.e*colAxis.y + rectTransform.i*colAxis.z < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + vy = 0; + halfLen1 = box.hs.y; + } else { + if (boxTransform.b*colAxis.x + boxTransform.f*colAxis.y + boxTransform.j*colAxis.z > 0) { + vy = -vy; + if ((box.excludedFaces & 8) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 4) != 0) { + return false; + } + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = rect.hs.y; + } else { + if (rectTransform.b*colAxis.x + rectTransform.f*colAxis.y + rectTransform.j*colAxis.z < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + vz = 0; + halfLen1 = box.hs.z; + } else { + if (boxTransform.c*colAxis.x + boxTransform.g*colAxis.y + boxTransform.k*colAxis.z > 0) { + vz = -vz; + if ((box.excludedFaces & 32) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 16) != 0) { + return false; + } + } + } + // Получаем глобальные координаты средних точек рёбер + + var x1:Number = boxTransform.a*vx + boxTransform.b*vy + boxTransform.c*vz + boxTransform.d; + var y1:Number = boxTransform.e*vx + boxTransform.f*vy + boxTransform.g*vz + boxTransform.h; + var z1:Number = boxTransform.i*vx + boxTransform.j*vy + boxTransform.k*vz + boxTransform.l; + vx = x2; + vy = y2; + vz = z2; + x2 = rectTransform.a*vx + rectTransform.b*vy + rectTransform.c*vz + rectTransform.d; + y2 = rectTransform.e*vx + rectTransform.f*vy + rectTransform.g*vz + rectTransform.h; + z2 = rectTransform.i*vx + rectTransform.j*vy + rectTransform.k*vz + rectTransform.l; + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + vx = x2 - x1; + vy = y2 - y1; + vz = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cpPos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cpPos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cpPos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var v:Vector3 = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + return true; + } + + /** + * Проверяет пересечение вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box + * @param rect + * @param axis + * @param axisIndex + * @param vectorToBox + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param rect + * @param axis1 + * @param axis2 + * @param axisIndex + * @param vectorToBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, rect:CollisionRect, axis1:Vector3, axis2:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия вдоль заданной оси. + * + * @param box бокс + * @param rect прямоугольник + * @param axis ось + * @param vectorToBox вектор, соединяющий центр прямоугольника с центром бокса + * @return величина перекрытия вдоль оси + */ + public function overlapOnAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, vectorToBox:Vector3):Number { + var m:Matrix4 = box.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box.hs.z; + if (d < 0) d = -d; + projection += d; + + m = rect.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*rect.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*rect.hs.y; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox.x*axis.x + vectorToBox.y*axis.y + vectorToBox.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} diff --git a/0.0.9.3/src/alternativa/physics/collision/colliders/.svn/text-base/BoxSphereCollider.as.svn-base b/0.0.9.3/src/alternativa/physics/collision/colliders/.svn/text-base/BoxSphereCollider.as.svn-base new file mode 100644 index 0000000..da5e36d --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/colliders/.svn/text-base/BoxSphereCollider.as.svn-base @@ -0,0 +1,168 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.math.Vector3; + use namespace altphysics; + /** + * + */ + public class BoxSphereCollider implements ICollider { + + private var center:Vector3 = new Vector3(); + private var closestPt:Vector3 = new Vector3(); + + private var bPos:Vector3 = new Vector3(); + private var sPos:Vector3 = new Vector3(); + + /** + * + */ + public function BoxSphereCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + // Зафиксированно столкновение + contact.body1 = sphere.body; + contact.body2 = box.body; + contact.normal.vCopy(closestPt).vTransformBy4(box.transform).vSubtract(sPos).vNormalize().vReverse(); + contact.pcount = 1; + + var cp:ContactPoint = contact.points[0]; + cp.penetration = sphere.r - Math.sqrt(distSqr); + cp.pos.vCopy(contact.normal).vScale(-sphere.r).vAdd(sPos); + cp.r1.vDiff(cp.pos, sPos); + cp.r2.vDiff(cp.pos, bPos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + + return true; + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/collision/colliders/.svn/text-base/BoxTriangleCollider.as.svn-base b/0.0.9.3/src/alternativa/physics/collision/colliders/.svn/text-base/BoxTriangleCollider.as.svn-base new file mode 100644 index 0000000..935525d --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/colliders/.svn/text-base/BoxTriangleCollider.as.svn-base @@ -0,0 +1,725 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionTriangle; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * + */ + public class BoxTriangleCollider extends BoxCollider { + + public var epsilon:Number = 0.001; + + private var bestAxisIndex:int; + private var minOverlap:Number; + private var toBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var colNormal:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxTriangleCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, tri, toBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + if (!findEdgesIntersection(box, tri, toBox, bestAxisIndex%3, int(bestAxisIndex/3), contact)) return false; + } + + contact.body1 = box.body; + contact.body2 = tri.body; + + // Хак для танков, чтобы исключить утыкания танков в стыки статических примитивов + if (tri.transform.k > 0.99999) { + contact.normal.x = 0; + contact.normal.y = 0; + contact.normal.z = 1; + } + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + toBox.x = boxTransform.d - triTransform.d; + toBox.y = boxTransform.h - triTransform.h; + toBox.z = boxTransform.l - triTransform.l; + + minOverlap = 1e308; + + // Сначала проверяется нормаль треугольника + axis.x = triTransform.c; + axis.y = triTransform.g; + axis.z = triTransform.k; + if (!testMainAxis(box, tri, axis, 0, toBox)) return false; + + // Проверка основных осей бокса + axis10.x = boxTransform.a; + axis10.y = boxTransform.e; + axis10.z = boxTransform.i; + if (!testMainAxis(box, tri, axis10, 1, toBox)) return false; + axis11.x = boxTransform.b; + axis11.y = boxTransform.f; + axis11.z = boxTransform.j; + if (!testMainAxis(box, tri, axis11, 2, toBox)) return false; + axis12.x = boxTransform.c; + axis12.y = boxTransform.g; + axis12.z = boxTransform.k; + if (!testMainAxis(box, tri, axis12, 3, toBox)) return false; + + // Проверка производных осей + // TODO: заменить вычисления векторных произведений инлайнами + var v:Vector3 = tri.e0; + axis20.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis20.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis20.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis20, 4, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis20, 5, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis20, 6, toBox)) return false; + + v = tri.e1; + axis21.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis21.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis21.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis21, 7, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis21, 8, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis21, 9, toBox)) return false; + + v = tri.e2; + axis22.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis22.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis22.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis22, 10, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis22, 11, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis22, 12, toBox)) return false; + + return true; + } + + /** + * Тестирует пересечение примитивов вдоль заданной оси. + * + * @param box бокс + * @param tri треугольник + * @param axis ось + * @param axisIndex индекс оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return true, если примитивы имеют перекрытие проекций вдоль указанной оси, иначе false + */ + private function testMainAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, axisIndex:int, toBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param tri + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, tri:CollisionTriangle, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Рассчитывает величину перекрытия проекций бокса и треугольника на заданную ось. + * + * @param box бокс + * @param tri треугольник + * @param axis единичный направляющий вектор оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return величина перекрытия. Положительное значение указывает на наличие перекрытия, нулевое или отрицательное значение указывает на отсутствие перекрытия. + */ + private function overlapOnAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, toBox:Vector3):Number { + var t:Matrix4 = box.transform; + var projection:Number = (t.a*axis.x + t.e*axis.y + t.i*axis.z)*box.hs.x; + if (projection < 0) projection = -projection; + var d:Number = (t.b*axis.x + t.f*axis.y + t.j*axis.z)*box.hs.y; + projection += d < 0 ? -d : d; + d = (t.c*axis.x + t.g*axis.y + t.k*axis.z)*box.hs.z; + projection += d < 0 ? -d : d; + + var vectorProjection:Number = toBox.x*axis.x + toBox.y*axis.y + toBox.z*axis.z; + // Для оптимизации ось преобразуется в систему треугольника, а не его вершины в мировую систему + t = tri.transform; + var ax:Number = t.a*axis.x + t.e*axis.y + t.i*axis.z; + var ay:Number = t.b*axis.x + t.f*axis.y + t.j*axis.z; + var az:Number = t.c*axis.x + t.g*axis.y + t.k*axis.z; + var max:Number = 0; + if (vectorProjection < 0) { + vectorProjection = -vectorProjection; + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d < max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d < max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d < max) max = d; + max = -max; + } else { + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d > max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d > max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d > max) max = d; + } + + return projection + max - vectorProjection; + } + + /** + * Определяет точки контакта бокса и треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function findFaceContactPoints(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIndex:int, contact:Contact):Boolean { + if (faceAxisIndex == 0) { + // Столкновение с плоскостью треугольника + return getBoxToTriContact(box, tri, toBox, contact); + } else { + // Столкновение с гранью бокса + return getTriToBoxContact(box, tri, toBox, faceAxisIndex, contact); + } + } + + /** + * Определяет точки контакта бокса с плоскостью треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getBoxToTriContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + colNormal.x = triTransform.c; + colNormal.y = triTransform.g; + colNormal.z = triTransform.k; + + var over:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + if (!over) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var incAxisDot:Number = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colNormal.x + axis.y*colNormal.y + axis.z*colNormal.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incAxisDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Обрезка грани + var negativeFace:Boolean = incAxisDot > 0; + + var code:int = 1 << (incFaceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + getFaceVertsByAxis(box.hs, incFaceAxisIdx, negativeFace, points1); + boxTransform.transformVectorsN(points1, points2, 4); + triTransform.transformVectorsInverseN(points2, points1, 4); + var pnum:int = clipByTriangle(tri); + // Среди конечного списка точек определяются лежащие под плоскостью треугольника + var cp:ContactPoint; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points2[i]; + if ((over && v.z < 0) || (!over && v.z > 0)) { + cp = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z + triTransform.d; + cpPos.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z + triTransform.h; + cpPos.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z + triTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = over ? -v.z : v.z; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Оперделяет точки контакта треугольника с гранью бокса. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getTriToBoxContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + faceAxisIdx--; + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + boxTransform.getAxis(faceAxisIdx, colNormal); + var negativeFace:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + + var code:int = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + if (!negativeFace) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + var v:Vector3 = points1[0]; + v.x = tri.v0.x; + v.y = tri.v0.y; + v.z = tri.v0.z; + + v = points1[1]; + v.x = tri.v1.x; + v.y = tri.v1.y; + v.z = tri.v1.z; + + v = points1[2]; + v.x = tri.v2.x; + v.y = tri.v2.y; + v.z = tri.v2.z; + + triTransform.transformVectorsN(points1, points2, 3); + boxTransform.transformVectorsInverseN(points2, points1, 3); + + var pnum:int = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность боксу и добавляем такие точки в список контактов + var penetration:Number; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + v = points1[i]; + penetration = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace); + if (penetration > -epsilon) { + var cp:ContactPoint = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = penetration; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Вычисляет величину проникновения точки в бокс. + * + * @param hs вектор половинных размеров бокса + * @param p точка в системе координат бокса + * @param axisIndex индекс оси + * @param negativeFace если true, проверяется нижняя грань + * @return величина проникновения точки в бокс + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, negativeFace:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (negativeFace) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (negativeFace) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (negativeFace) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, вершины которой заданы в списке points1. Результат сохраняется в этом же списке. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 3; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Обрезает грань прямоугольника сторонами треугольника. Обрезка выполняется в системе координат и в проекции на + * плоскость треугольника. Входные вершины грани в количестве четырёх штук должны находиться в списке points1. + * Конечные вершины сохраняются в списке points2. + * + * @param tri треугольник + * @return количество врешин в конечном списке + */ + private function clipByTriangle(tri:CollisionTriangle):int { + var vnum:int = 4; + vnum = clipByLine(tri.v0, tri.e0, points1, vnum, points2); + if (vnum == 0) return 0; + vnum = clipByLine(tri.v1, tri.e1, points2, vnum, points1); + if (vnum == 0) return 0; + return clipByLine(tri.v2, tri.e2, points1, vnum, points2);; + } + + /** + * Обрезает полигон указанной прямой. + * + * @param linePoint точка на прямой + * @param lineDir единичный направляющий вектор прямой + * @param verticesIn список вершин исходного полигона + * @param vnum количество вершин исходного полигона + * @param verticesOut список, куда будут записаны вершины конечного полигона + * @return количество вершин конечного полигона + */ + private function clipByLine(linePoint:Vector3, lineDir:Vector3, verticesIn:Vector., vnum:int, verticesOut:Vector.):int { + var nx:Number = -lineDir.y; + var ny:Number = lineDir.x; + var offset:Number = linePoint.x*nx + linePoint.y*ny; + var v1:Vector3 = verticesIn[int(vnum - 1)]; + var offset1:Number = v1.x*nx + v1.y*ny; + var t:Number; + var v:Vector3; + var num:int = 0; + for (var i:int = 0; i < vnum; i++) { + var v2:Vector3 = verticesIn[i]; + var offset2:Number = v2.x*nx + v2.y*ny; + if (offset1 < offset) { + // Первая точка ребра во внешней полуплоскости + if (offset2 > offset) { + // Вторая точка ребра во внутренней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } else { + // Первая точка ребра во внутренней полуплоскости. Добавляем её в конечный список. + v = verticesOut[num]; + v.x = v1.x; + v.y = v1.y; + v.z = v1.z; + num++; + if (offset2 < offset) { + // Вторая точка ребра во внешней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } + v1 = v2; + offset1 = offset2; + } + return num; + } + + /** + * + * @param box + * @param tri + * @param toBox + * @param boxAxisIdx + * @param triAxisIdx + * @param contact + */ + private function findEdgesIntersection(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, boxAxisIdx:int, triAxisIdx:int, contact:Contact):Boolean { + // Определение точки и направляющего вектора ребра треугольника + var tmpx1:Number; + var tmpy1:Number; + var tmpz1:Number; + var tmpx2:Number; + var tmpy2:Number; + var tmpz2:Number; + switch (triAxisIdx) { + case 0: + tmpx1 = tri.e0.x; + tmpy1 = tri.e0.y; + tmpz1 = tri.e0.z; + tmpx2 = tri.v0.x; + tmpy2 = tri.v0.y; + tmpz2 = tri.v0.z; + break; + case 1: + tmpx1 = tri.e1.x; + tmpy1 = tri.e1.y; + tmpz1 = tri.e1.z; + tmpx2 = tri.v1.x; + tmpy2 = tri.v1.y; + tmpz2 = tri.v1.z; + break; + case 2: + tmpx1 = tri.e2.x; + tmpy1 = tri.e2.y; + tmpz1 = tri.e2.z; + tmpx2 = tri.v2.x; + tmpy2 = tri.v2.y; + tmpz2 = tri.v2.z; + break; + } + var triTransform:Matrix4 = tri.transform; + axis20.x = triTransform.a*tmpx1 + triTransform.b*tmpy1 + triTransform.c*tmpz1; + axis20.y = triTransform.e*tmpx1 + triTransform.f*tmpy1 + triTransform.g*tmpz1; + axis20.z = triTransform.i*tmpx1 + triTransform.j*tmpy1 + triTransform.k*tmpz1; + var x2:Number = triTransform.a*tmpx2 + triTransform.b*tmpy2 + triTransform.c*tmpz2 + triTransform.d; + var y2:Number = triTransform.e*tmpx2 + triTransform.f*tmpy2 + triTransform.g*tmpz2 + triTransform.h; + var z2:Number = triTransform.i*tmpx2 + triTransform.j*tmpy2 + triTransform.k*tmpz2 + triTransform.l; + + // Определение нормали контакта, точки и направляющего вектора ребра бокса + var boxTransform:Matrix4 = box.transform; + boxTransform.getAxis(boxAxisIdx, axis10); + + // Нормаль контакта + var v:Vector3 = contact.normal; + v.x = axis10.y*axis20.z - axis10.z*axis20.y; + v.y = axis10.z*axis20.x - axis10.x*axis20.z; + v.z = axis10.x*axis20.y - axis10.y*axis20.x; + k = 1/Math.sqrt(v.x*v.x + v.y*v.y + v.z*v.z); + v.x *= k; + v.y *= k; + v.z *= k; + // Разворот нормали в сторону бокса + if (v.x*toBox.x + v.y*toBox.y + v.z*toBox.z < 0) { + v.x = -v.x; + v.y = -v.y; + v.z = -v.z; + } + + var boxHalfLen:Number; + tmpx1 = box.hs.x; + tmpy1 = box.hs.y; + tmpz1 = box.hs.z; + // X + if (boxAxisIdx == 0) { + tmpx1 = 0; + boxHalfLen = box.hs.x; + } else { + if (boxTransform.a*v.x + boxTransform.e*v.y + boxTransform.i*v.z > 0) { + tmpx1 = -tmpx1; + if ((box.excludedFaces & 2) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 1) != 0) { + return false; + } + } + } + // Y + if (boxAxisIdx == 1) { + tmpy1 = 0; + boxHalfLen = box.hs.y; + } else { + if (boxTransform.b*v.x + boxTransform.f*v.y + boxTransform.j*v.z > 0) { + tmpy1 = -tmpy1; + if ((box.excludedFaces & 8) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 4) != 0) { + return false; + } + } + } + // Z + if (boxAxisIdx == 2) { + tmpz1 = 0; + boxHalfLen = box.hs.z; + } else { + if (boxTransform.c*v.x + boxTransform.g*v.y + boxTransform.k*v.z > 0) { + tmpz1 = -tmpz1; + if ((box.excludedFaces & 32) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 16) != 0) { + return false; + } + } + } + var x1:Number = boxTransform.a*tmpx1 + boxTransform.b*tmpy1 + boxTransform.c*tmpz1 + boxTransform.d; + var y1:Number = boxTransform.e*tmpx1 + boxTransform.f*tmpy1 + boxTransform.g*tmpz1 + boxTransform.h; + var z1:Number = boxTransform.i*tmpx1 + boxTransform.j*tmpy1 + boxTransform.k*tmpz1 + boxTransform.l; + + // Находим точку пересечения рёбер, решая систему уравнений + // axis10 - направляющий вектор ребра бокса + // x1, y1, z1 - средняя точка ребра бокса + // axis20 - направляющий вектор ребра треугольника + // x2, y2, z2 - начальная точка ребра треугольника + var k:Number = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + var vx:Number = x2 - x1; + var vy:Number = y2 - y1; + var vz:Number = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + + // Запись данных о контакте + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + v = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + v.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + v.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + v.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = v.x - boxTransform.d; + r.y = v.y - boxTransform.h; + r.z = v.z - boxTransform.l; + r = cp.r2; + r.x = v.x - triTransform.d; + r.y = v.y - triTransform.h; + r.z = v.z - triTransform.l; + return true; + } + + } +} diff --git a/0.0.9.3/src/alternativa/physics/collision/colliders/.svn/text-base/SpherePlaneCollider.as.svn-base b/0.0.9.3/src/alternativa/physics/collision/colliders/.svn/text-base/SpherePlaneCollider.as.svn-base new file mode 100644 index 0000000..30cd55f --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/colliders/.svn/text-base/SpherePlaneCollider.as.svn-base @@ -0,0 +1,68 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + /** + * + */ + public class SpherePlaneCollider implements ICollider { + + private var normal:Vector3 = new Vector3(); + + /** + * + */ + public function SpherePlaneCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var sphere:RigidSphere = body1 as RigidSphere; +// var plane:RigidPlane; +// if (sphere == null) { +// sphere = body2 as RigidSphere; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// +// var dist:Number = sphere.state.pos.dot(normal) - offset; +// if (dist > sphere.r) return false; +// +// collisionInfo.body1 = sphere; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// collisionInfo.pcount = 1; +// +// var cp:ContactPoint = collisionInfo.points[0]; +// cp.penetration = sphere.r - dist; +// cp.pos.copy(normal).reverse().scale(sphere.r).add(sphere.state.pos); +// cp.r1.diff(cp.pos, sphere.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/collision/colliders/.svn/text-base/SphereSphereCollider.as.svn-base b/0.0.9.3/src/alternativa/physics/collision/colliders/.svn/text-base/SphereSphereCollider.as.svn-base new file mode 100644 index 0000000..3332f3c --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/colliders/.svn/text-base/SphereSphereCollider.as.svn-base @@ -0,0 +1,70 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.math.Vector3; + use namespace altphysics; + + public class SphereSphereCollider implements ICollider { + + private var p1:Vector3 = new Vector3(); + private var p2:Vector3 = new Vector3(); + + public function SphereSphereCollider() { + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var s1:CollisionSphere; + var s2:CollisionSphere; + if (prim1.body != null) { + s1 = prim1 as CollisionSphere; + s2 = prim2 as CollisionSphere; + } else { + s1 = prim2 as CollisionSphere; + s2 = prim1 as CollisionSphere; + } + + s1.transform.getAxis(3, p1); + s2.transform.getAxis(3, p2); + var dx:Number = p1.x - p2.x; + var dy:Number = p1.y - p2.y; + var dz:Number = p1.z - p2.z; + var len:Number = dx*dx + dy*dy + dz*dz; + var sum:Number = s1.r + s2.r; + if (len > sum*sum) return false; + len = Math.sqrt(len); + dx /= len; + dy /= len; + dz /= len; + + contact.body1 = s1.body; + contact.body2 = s2.body; + contact.normal.x = dx; + contact.normal.y = dy; + contact.normal.z = dz; + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = sum - len; + cp.pos.x = p1.x - dx*s1.r; + cp.pos.y = p1.y - dy*s1.r; + cp.pos.z = p1.z - dz*s1.r; + cp.r1.vDiff(cp.pos, p1); + cp.r2.vDiff(cp.pos, p2); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/collision/colliders/BoxBoxCollider.as b/0.0.9.3/src/alternativa/physics/collision/colliders/BoxBoxCollider.as new file mode 100644 index 0000000..2d3f04c --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/colliders/BoxBoxCollider.as @@ -0,0 +1,589 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * Расчитывает точки контакта двух боксов. Нормаль контакта направляется в сторону бокса с меньшим ID. + */ + public class BoxBoxCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + private var vectorToBox1:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var bestAxisIndex:int; + private var minOverlap:Number; + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + private var tmpPoints:Vector. = new Vector.(8, true); + private var pcount:int; + + /** + * + */ + public function BoxBoxCollider() { + for (var i:int = 0; i < 8; i++) { + tmpPoints[i] = new ContactPoint(); + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + + if (bestAxisIndex < 6) { + // Контакт грань-(грань|ребро|вершина) + if (!findFaceContactPoints(box1, box2, vectorToBox1, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 6; + findEdgesIntersection(box1, box2, vectorToBox1, int(bestAxisIndex/3), bestAxisIndex%3, contact); + } + contact.body1 = box1.body; + contact.body2 = box2.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + // Вектор из центра второго бокса в центр первого + vectorToBox1.x = transform1.d - transform2.d; + vectorToBox1.y = transform1.h - transform2.h; + vectorToBox1.z = transform1.l - transform2.l; + + // Проверка пересечения по основным осям + axis10.x = transform1.a; + axis10.y = transform1.e; + axis10.z = transform1.i; + if (!testMainAxis(box1, box2, axis10, 0, vectorToBox1)) return false; + axis11.x = transform1.b; + axis11.y = transform1.f; + axis11.z = transform1.j; + if (!testMainAxis(box1, box2, axis11, 1, vectorToBox1)) return false; + axis12.x = transform1.c; + axis12.y = transform1.g; + axis12.z = transform1.k; + if (!testMainAxis(box1, box2, axis12, 2, vectorToBox1)) return false; + + axis20.x = transform2.a; + axis20.y = transform2.e; + axis20.z = transform2.i; + if (!testMainAxis(box1, box2, axis20, 3, vectorToBox1)) return false; + axis21.x = transform2.b; + axis21.y = transform2.f; + axis21.z = transform2.j; + if (!testMainAxis(box1, box2, axis21, 4, vectorToBox1)) return false; + axis22.x = transform2.c; + axis22.y = transform2.g; + axis22.z = transform2.k; + if (!testMainAxis(box1, box2, axis22, 5, vectorToBox1)) return false; + + // Проверка производных осей + if (!testDerivedAxis(box1, box2, axis10, axis20, 6, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis21, 7, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis22, 8, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis11, axis20, 9, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis21, 10, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis22, 11, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis12, axis20, 12, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis21, 13, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis22, 14, vectorToBox1)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта грани одного бокса с гранью/ребром/вершиной другого. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param faceAxisIdx индекс оси первого бокса, перпендикулярной грани, с которой произошло столкновение + * @param contactInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var swapNormal:Boolean = false; + if (faceAxisIdx > 2) { + // Столкновение с гранью второго бокса. Для дальнейших расчётов боксы меняются местами, + // но нормаль контакта всё равно должна быть направлена в сторону первоначального box1 + var tmpBox:CollisionBox = box1; + box1 = box2; + box2 = tmpBox; + vectorToBox1.x = -vectorToBox1.x; + vectorToBox1.y = -vectorToBox1.y; + vectorToBox1.z = -vectorToBox1.z; + faceAxisIdx -= 3; + swapNormal = true; + } + + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + var colAxis:Vector3 = contact.normal; + + transform1.getAxis(faceAxisIdx, colAxis); + var negativeFace:Boolean = colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z > 0; + + var code:int = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box1.excludedFaces) != 0) return false; + + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + // Ищем ось второго бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + transform2.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisDot = dot; + incidentAxisIdx = axisIdx; + } + } + // Получаем список вершин грани второго бокса, переводим их в систему координат первого бокса и выполняем обрезку + // по грани первого бокса. Таким образом получается список потенциальных точек контакта. + transform2.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box2.hs, incidentAxisIdx, incidentAxisDot < 0, points1); + + // TODO: Вычислить результирующую матрицу, затем преобразовать векторы за один заход + transform2.transformVectorsN(points1, points2, 4); + transform1.transformVectorsInverseN(points2, points1, 4); + + var pnum:int = clip(box1.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points1[i]; + if ((pen = getPointBoxPenetration(box1.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + var cp:ContactPoint = tmpPoints[pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = transform1.a*v.x + transform1.b*v.y + transform1.c*v.z + transform1.d; + cpPos.y = transform1.e*v.x + transform1.f*v.y + transform1.g*v.z + transform1.h; + cpPos.z = transform1.i*v.x + transform1.j*v.y + transform1.k*v.z + transform1.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + + cp.penetration = pen; + } + } + if (swapNormal) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (pcount > 4) { + reducePoints(); + } + for (i = 0; i < pcount; i++) { + cp = contact.points[i]; + cp.copyFrom(tmpPoints[i]); + } + contact.pcount = pcount; + return true; + } + + /** + * + * @param contactInfo + */ + private function reducePoints():void { + var i:int; + var minIdx:int; + var cp1:ContactPoint; + var cp2:ContactPoint; + while (pcount > 4) { + var minLen:Number = 1e10; + var p1:Vector3 = (tmpPoints[int(pcount - 1)] as ContactPoint).pos; + var p2:Vector3; + for (i = 0; i < pcount; i++) { + p2 = (tmpPoints[i] as ContactPoint).pos; + var dx:Number = p2.x - p1.x; + var dy:Number = p2.y - p1.y; + var dz:Number = p2.z - p1.z; + var len:Number = dx*dx + dy*dy + dz*dz; + if (len < minLen) { + minLen = len; + minIdx = i; + } + p1 = p2; + } + var ii:int = minIdx == 0 ? (pcount - 1) : (minIdx - 1); + cp1 = tmpPoints[ii]; + cp2 = tmpPoints[minIdx]; + p1 = cp1.pos; + p2 = cp2.pos; + p2.x = 0.5*(p1.x + p2.x); + p2.y = 0.5*(p1.y + p2.y); + p2.z = 0.5*(p1.z + p2.z); + cp2.penetration = 0.5*(cp1.penetration + cp2.penetration); + if (minIdx > 0) { + for (i = minIdx; i < pcount; i++) { + tmpPoints[int(i - 1)] = tmpPoints[i]; + } + tmpPoints[int(pcount - 1)] = cp1; + } + pcount--; + } + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clip(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param contactInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):void { + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + transform1.getAxis(axisIdx1, axis10); + transform2.getAxis(axisIdx2, axis20); + + var colAxis:Vector3 = contact.normal; + + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону первого бокса + if (colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + + var tx:Number = box1.hs.x; + var ty:Number = box1.hs.y; + var tz:Number = box1.hs.z; + + var x2:Number = box2.hs.x; + var y2:Number = box2.hs.y; + var z2:Number = box2.hs.z; + // x + if (axisIdx1 == 0) { + tx = 0; + halfLen1 = box1.hs.x; + } else { + if (colAxis.x*transform1.a + colAxis.y*transform1.e + colAxis.z*transform1.i > 0) { + tx = -tx; + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = box2.hs.x; + } else { + if (colAxis.x*transform2.a + colAxis.y*transform2.e + colAxis.z*transform2.i < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + ty = 0; + halfLen1 = box1.hs.y; + } else { + if (colAxis.x*transform1.b + colAxis.y*transform1.f + colAxis.z*transform1.j > 0) { + ty = -ty; + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = box2.hs.y; + } else { + if (colAxis.x*transform2.b + colAxis.y*transform2.f + colAxis.z*transform2.j < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + tz = 0; + halfLen1 = box1.hs.z; + } else { + if (colAxis.x*transform1.c + colAxis.y*transform1.g + colAxis.z*transform1.k > 0) { + tz = -tz; + } + } + if (axisIdx2 == 2) { + z2 = 0; + halfLen2 = box2.hs.z; + } else { + if (colAxis.x*transform2.c + colAxis.y*transform2.g + colAxis.z*transform2.k < 0) { + z2 = -z2; + } + } + // Получаем глобальные координаты средних точек рёбер + var x1:Number = transform1.a*tx + transform1.b*ty + transform1.c*tz + transform1.d; + var y1:Number = transform1.e*tx + transform1.f*ty + transform1.g*tz + transform1.h; + var z1:Number = transform1.i*tx + transform1.j*ty + transform1.k*tz + transform1.l; + tx = x2; + ty = y2; + tz = z2; + x2 = transform2.a*tx + transform2.b*ty + transform2.c*tz + transform2.d; + y2 = transform2.e*tx + transform2.f*ty + transform2.g*tz + transform2.h; + z2 = transform2.i*tx + transform2.j*ty + transform2.k*tz + transform2.l; + + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + tx = x2 - x1; + ty = y2 - y1; + tz = z2 - z1; + var c1:Number = axis10.x*tx + axis10.y*ty + axis10.z*tz; + var c2:Number = axis20.x*tx + axis20.y*ty + axis20.z*tz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cp.pos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cp.pos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение боксов вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box1 + * @param box2 + * @param axis + * @param axisIndex + * @param toBox1 + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box1 + * @param box2 + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox1 + * @return + */ + private function testDerivedAxis(box1:CollisionBox, box2:CollisionBox, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия двух боксов вдоль заданной оси. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param axis ось + * @param vectorToBox1 вектор, соединяющий центр второго бокса с центром первого + * @return величина перекрытия боксов вдоль оси + */ + public function overlapOnAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, vectorToBox1:Vector3):Number { + var m:Matrix4 = box1.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box1.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box1.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box1.hs.z; + if (d < 0) d = -d; + projection += d; + + m = box2.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box2.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box2.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box2.hs.z; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox1.x*axis.x + vectorToBox1.y*axis.y + vectorToBox1.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} + import alternativa.math.Vector3; + + class CollisionPointTmp { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + } diff --git a/0.0.9.3/src/alternativa/physics/collision/colliders/BoxCollider.as b/0.0.9.3/src/alternativa/physics/collision/colliders/BoxCollider.as new file mode 100644 index 0000000..2429bb1 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/colliders/BoxCollider.as @@ -0,0 +1,427 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + /** + * + */ + public class BoxCollider implements ICollider { + + /** + * + */ + public function BoxCollider() { + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + return false; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + protected function getFaceVertsByAxis(hs:Vector3, axisIdx:int, negativeFace:Boolean, result:Vector.):void { + var v:Vector3; + switch (axisIdx) { + case 0: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + } + break; + case 1: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + } + break; + case 2: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + } else { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } + break; + } + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/collision/colliders/BoxPlaneCollider.as b/0.0.9.3/src/alternativa/physics/collision/colliders/BoxPlaneCollider.as new file mode 100644 index 0000000..3bebdc3 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/colliders/BoxPlaneCollider.as @@ -0,0 +1,88 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + public class BoxPlaneCollider implements ICollider { + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var normal:Vector3 = new Vector3(); + + public function BoxPlaneCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var box:RigidBox = body1 as RigidBox; +// var plane:RigidPlane; +// if (box == null) { +// box = body2 as RigidBox; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные координаты вершин бокса +// var sx:Number = box.halfSize.x; +// var sy:Number = box.halfSize.y; +// var sz:Number = box.halfSize.z; +// (verts1[0] as Vector3).reset(-sx, -sy, -sz); +// (verts1[1] as Vector3).reset(sx, -sy, -sz); +// (verts1[2] as Vector3).reset(sx, sy, -sz); +// (verts1[3] as Vector3).reset(-sx, sy, -sz); +// (verts1[4] as Vector3).reset(-sx, -sy, sz); +// (verts1[5] as Vector3).reset(sx, -sy, sz); +// (verts1[6] as Vector3).reset(sx, sy, sz); +// (verts1[7] as Vector3).reset(-sx, sy, sz); +// +// box.transform.transformVectors(verts1, verts2); +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// // Проверяем наличие столкновений с каждой вершиной +// collisionInfo.pcount = 0; +// for (var i:int = 0; i < 8; i++) { +// // Вершина добавляется в список точек столкновения, если лежит под плоскостью +// var dist:Number = (verts2[i] as Vector3).dot(normal); +// if (dist < offset) { +// var cp:ContactPoint; +// if (collisionInfo.pcount == collisionInfo.points.length) { +// cp = new ContactPoint(); +// collisionInfo.points[collisionInfo.pcount] = cp; +// } else { +// cp = collisionInfo.points[collisionInfo.pcount]; +// } +// cp.pos.copy(verts2[i]); +// cp.r1.diff(cp.pos, box.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); +// cp.penetration = offset - dist; +// collisionInfo.pcount++; +// } +// } +// if (collisionInfo.pcount > 0) { +// collisionInfo.body1 = box; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// return true; +// } + return false; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/collision/colliders/BoxRectCollider.as b/0.0.9.3/src/alternativa/physics/collision/colliders/BoxRectCollider.as new file mode 100644 index 0000000..d6ba302 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/colliders/BoxRectCollider.as @@ -0,0 +1,575 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * + */ + public class BoxRectCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + + private var vectorToBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxRectCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, rect, vectorToBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + if (!findEdgesIntersection(box, rect, vectorToBox, int(bestAxisIndex/2), bestAxisIndex%2, contact)) { + return false; + } + } + contact.body1 = box.body; + contact.body2 = rect.body; + + // Хак для танков, чтобы исключить утыкания танков в стыки статических примитивов + if (rect.transform.k > 0.99999) { + contact.normal.x = 0; + contact.normal.y = 0; + contact.normal.z = 1; + } + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + // Вектор из центра прямоугольника в центр бокса + vectorToBox.x = boxTransform.d - rectTransform.d; + vectorToBox.y = boxTransform.h - rectTransform.h; + vectorToBox.z = boxTransform.l - rectTransform.l; + + // Проверка пересечения по нормали прямоугольника + rectTransform.getAxis(2, axis22); + if (!testMainAxis(box, rect, axis22, 0, vectorToBox)) return false; + + // Проверка пересечения по основным осям бокса + boxTransform.getAxis(0, axis10); + if (!testMainAxis(box, rect, axis10, 1, vectorToBox)) return false; + boxTransform.getAxis(1, axis11); + if (!testMainAxis(box, rect, axis11, 2, vectorToBox)) return false; + boxTransform.getAxis(2, axis12); + if (!testMainAxis(box, rect, axis12, 3, vectorToBox)) return false; + + // Получаем направляющие рёбер прямоугольника + rectTransform.getAxis(0, axis20); + rectTransform.getAxis(1, axis21); + + // Проверка производных осей + if (!testDerivedAxis(box, rect, axis10, axis20, 4, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis10, axis21, 5, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis11, axis20, 6, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis11, axis21, 7, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis12, axis20, 8, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis12, axis21, 9, vectorToBox)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта бокса с прямоугольником. + * + * @param box бокс + * @param rect прямоугольник + * @param vectorToBox вектор, направленный из центра прямоугольника в центр бокса + * @param faceAxisIdx индекс оси, идентифицирующей полскость столкновения (грань бокса или полскость прямоугольника) + * @param colInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var pnum:int; + var i:int; + var v:Vector3; + var cp:ContactPoint; + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + var colAxis:Vector3 = contact.normal; + + var negativeFace:Boolean; + var code:int; + + if (faceAxisIdx == 0) { + // Столкновение с плоскостью прямоугольника + + // Проверим положение бокса относительно плоскости прямоугольника + colAxis.x = rectTransform.c; + colAxis.y = rectTransform.g; + colAxis.z = rectTransform.k; + +// var offset:Number = colAxis.x*rectTransform.d + colAxis.y*rectTransform.h + colAxis.z*rectTransform.l; +// if (bbPos.vDot(colAxis) < offset) return false; + + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisIdx = axisIdx; + incidentAxisDot = dot; + } + } + negativeFace = incidentAxisDot > 0; + + code = 1 << (incidentAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + + // Получаем список вершин грани бокса, переводим их в систему координат прямоугольника и выполняем обрезку + // по прямоугольнику. Таким образом получается список потенциальных точек контакта. + boxTransform.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box.hs, incidentAxisIdx, negativeFace, points1); + boxTransform.transformVectorsN(points1, points2, 4); + rectTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByRect(rect.hs); + // Проверяем каждую потенциальную точку на принадлежность нижней полуплоскости прямоугольника и добавляем такие точки в список контактов + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if (v.z < epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = -v.z; + var cpPos:Vector3 = cp.pos; + cpPos.x = rectTransform.a*v.x + rectTransform.b*v.y + rectTransform.c*v.z + rectTransform.d; + cpPos.y = rectTransform.e*v.x + rectTransform.f*v.y + rectTransform.g*v.z + rectTransform.h; + cpPos.z = rectTransform.i*v.x + rectTransform.j*v.y + rectTransform.k*v.z + rectTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } else { + // Столкновение с гранью бокса + faceAxisIdx--; + boxTransform.getAxis(faceAxisIdx, colAxis); + negativeFace = colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z > 0; + + code = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) { + return false; + } + + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (rectTransform.c*colAxis.x + rectTransform.g*colAxis.y + rectTransform.k*colAxis.z < 0) return false; + + getFaceVertsByAxis(rect.hs, 2, false, points1); + rectTransform.transformVectorsN(points1, points2, 4); + boxTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if ((pen = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = pen; + cpPos = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } + return true; + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * + * @param hs + * @return + */ + private function clipByRect(hs:Vector3):int { + var pnum:int = 4; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box первый бокс + * @param rect второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param colInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + boxTransform.getAxis(axisIdx1, axis10); + rectTransform.getAxis(axisIdx2, axis20); + var colAxis:Vector3 = contact.normal; + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону бокса + if (colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + var vx:Number = box.hs.x; + var vy:Number = box.hs.y; + var vz:Number = box.hs.z; + var x2:Number = rect.hs.x; + var y2:Number = rect.hs.y; + var z2:Number = rect.hs.z; + // x + if (axisIdx1 == 0) { + vx = 0; + halfLen1 = box.hs.x; + } else { + if (boxTransform.a*colAxis.x + boxTransform.e*colAxis.y + boxTransform.i*colAxis.z > 0) { + vx = -vx; + if ((box.excludedFaces & 2) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 1) != 0) { + return false; + } + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = rect.hs.x; + } else { + if (rectTransform.a*colAxis.x + rectTransform.e*colAxis.y + rectTransform.i*colAxis.z < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + vy = 0; + halfLen1 = box.hs.y; + } else { + if (boxTransform.b*colAxis.x + boxTransform.f*colAxis.y + boxTransform.j*colAxis.z > 0) { + vy = -vy; + if ((box.excludedFaces & 8) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 4) != 0) { + return false; + } + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = rect.hs.y; + } else { + if (rectTransform.b*colAxis.x + rectTransform.f*colAxis.y + rectTransform.j*colAxis.z < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + vz = 0; + halfLen1 = box.hs.z; + } else { + if (boxTransform.c*colAxis.x + boxTransform.g*colAxis.y + boxTransform.k*colAxis.z > 0) { + vz = -vz; + if ((box.excludedFaces & 32) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 16) != 0) { + return false; + } + } + } + // Получаем глобальные координаты средних точек рёбер + + var x1:Number = boxTransform.a*vx + boxTransform.b*vy + boxTransform.c*vz + boxTransform.d; + var y1:Number = boxTransform.e*vx + boxTransform.f*vy + boxTransform.g*vz + boxTransform.h; + var z1:Number = boxTransform.i*vx + boxTransform.j*vy + boxTransform.k*vz + boxTransform.l; + vx = x2; + vy = y2; + vz = z2; + x2 = rectTransform.a*vx + rectTransform.b*vy + rectTransform.c*vz + rectTransform.d; + y2 = rectTransform.e*vx + rectTransform.f*vy + rectTransform.g*vz + rectTransform.h; + z2 = rectTransform.i*vx + rectTransform.j*vy + rectTransform.k*vz + rectTransform.l; + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + vx = x2 - x1; + vy = y2 - y1; + vz = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cpPos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cpPos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cpPos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var v:Vector3 = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + return true; + } + + /** + * Проверяет пересечение вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box + * @param rect + * @param axis + * @param axisIndex + * @param vectorToBox + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param rect + * @param axis1 + * @param axis2 + * @param axisIndex + * @param vectorToBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, rect:CollisionRect, axis1:Vector3, axis2:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия вдоль заданной оси. + * + * @param box бокс + * @param rect прямоугольник + * @param axis ось + * @param vectorToBox вектор, соединяющий центр прямоугольника с центром бокса + * @return величина перекрытия вдоль оси + */ + public function overlapOnAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, vectorToBox:Vector3):Number { + var m:Matrix4 = box.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box.hs.z; + if (d < 0) d = -d; + projection += d; + + m = rect.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*rect.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*rect.hs.y; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox.x*axis.x + vectorToBox.y*axis.y + vectorToBox.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} diff --git a/0.0.9.3/src/alternativa/physics/collision/colliders/BoxSphereCollider.as b/0.0.9.3/src/alternativa/physics/collision/colliders/BoxSphereCollider.as new file mode 100644 index 0000000..da5e36d --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/colliders/BoxSphereCollider.as @@ -0,0 +1,168 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.math.Vector3; + use namespace altphysics; + /** + * + */ + public class BoxSphereCollider implements ICollider { + + private var center:Vector3 = new Vector3(); + private var closestPt:Vector3 = new Vector3(); + + private var bPos:Vector3 = new Vector3(); + private var sPos:Vector3 = new Vector3(); + + /** + * + */ + public function BoxSphereCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + // Зафиксированно столкновение + contact.body1 = sphere.body; + contact.body2 = box.body; + contact.normal.vCopy(closestPt).vTransformBy4(box.transform).vSubtract(sPos).vNormalize().vReverse(); + contact.pcount = 1; + + var cp:ContactPoint = contact.points[0]; + cp.penetration = sphere.r - Math.sqrt(distSqr); + cp.pos.vCopy(contact.normal).vScale(-sphere.r).vAdd(sPos); + cp.r1.vDiff(cp.pos, sPos); + cp.r2.vDiff(cp.pos, bPos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + + return true; + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/collision/colliders/BoxTriangleCollider.as b/0.0.9.3/src/alternativa/physics/collision/colliders/BoxTriangleCollider.as new file mode 100644 index 0000000..935525d --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/colliders/BoxTriangleCollider.as @@ -0,0 +1,725 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionTriangle; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * + */ + public class BoxTriangleCollider extends BoxCollider { + + public var epsilon:Number = 0.001; + + private var bestAxisIndex:int; + private var minOverlap:Number; + private var toBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var colNormal:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxTriangleCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, tri, toBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + if (!findEdgesIntersection(box, tri, toBox, bestAxisIndex%3, int(bestAxisIndex/3), contact)) return false; + } + + contact.body1 = box.body; + contact.body2 = tri.body; + + // Хак для танков, чтобы исключить утыкания танков в стыки статических примитивов + if (tri.transform.k > 0.99999) { + contact.normal.x = 0; + contact.normal.y = 0; + contact.normal.z = 1; + } + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + toBox.x = boxTransform.d - triTransform.d; + toBox.y = boxTransform.h - triTransform.h; + toBox.z = boxTransform.l - triTransform.l; + + minOverlap = 1e308; + + // Сначала проверяется нормаль треугольника + axis.x = triTransform.c; + axis.y = triTransform.g; + axis.z = triTransform.k; + if (!testMainAxis(box, tri, axis, 0, toBox)) return false; + + // Проверка основных осей бокса + axis10.x = boxTransform.a; + axis10.y = boxTransform.e; + axis10.z = boxTransform.i; + if (!testMainAxis(box, tri, axis10, 1, toBox)) return false; + axis11.x = boxTransform.b; + axis11.y = boxTransform.f; + axis11.z = boxTransform.j; + if (!testMainAxis(box, tri, axis11, 2, toBox)) return false; + axis12.x = boxTransform.c; + axis12.y = boxTransform.g; + axis12.z = boxTransform.k; + if (!testMainAxis(box, tri, axis12, 3, toBox)) return false; + + // Проверка производных осей + // TODO: заменить вычисления векторных произведений инлайнами + var v:Vector3 = tri.e0; + axis20.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis20.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis20.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis20, 4, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis20, 5, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis20, 6, toBox)) return false; + + v = tri.e1; + axis21.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis21.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis21.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis21, 7, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis21, 8, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis21, 9, toBox)) return false; + + v = tri.e2; + axis22.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis22.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis22.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis22, 10, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis22, 11, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis22, 12, toBox)) return false; + + return true; + } + + /** + * Тестирует пересечение примитивов вдоль заданной оси. + * + * @param box бокс + * @param tri треугольник + * @param axis ось + * @param axisIndex индекс оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return true, если примитивы имеют перекрытие проекций вдоль указанной оси, иначе false + */ + private function testMainAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, axisIndex:int, toBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param tri + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, tri:CollisionTriangle, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Рассчитывает величину перекрытия проекций бокса и треугольника на заданную ось. + * + * @param box бокс + * @param tri треугольник + * @param axis единичный направляющий вектор оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return величина перекрытия. Положительное значение указывает на наличие перекрытия, нулевое или отрицательное значение указывает на отсутствие перекрытия. + */ + private function overlapOnAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, toBox:Vector3):Number { + var t:Matrix4 = box.transform; + var projection:Number = (t.a*axis.x + t.e*axis.y + t.i*axis.z)*box.hs.x; + if (projection < 0) projection = -projection; + var d:Number = (t.b*axis.x + t.f*axis.y + t.j*axis.z)*box.hs.y; + projection += d < 0 ? -d : d; + d = (t.c*axis.x + t.g*axis.y + t.k*axis.z)*box.hs.z; + projection += d < 0 ? -d : d; + + var vectorProjection:Number = toBox.x*axis.x + toBox.y*axis.y + toBox.z*axis.z; + // Для оптимизации ось преобразуется в систему треугольника, а не его вершины в мировую систему + t = tri.transform; + var ax:Number = t.a*axis.x + t.e*axis.y + t.i*axis.z; + var ay:Number = t.b*axis.x + t.f*axis.y + t.j*axis.z; + var az:Number = t.c*axis.x + t.g*axis.y + t.k*axis.z; + var max:Number = 0; + if (vectorProjection < 0) { + vectorProjection = -vectorProjection; + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d < max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d < max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d < max) max = d; + max = -max; + } else { + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d > max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d > max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d > max) max = d; + } + + return projection + max - vectorProjection; + } + + /** + * Определяет точки контакта бокса и треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function findFaceContactPoints(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIndex:int, contact:Contact):Boolean { + if (faceAxisIndex == 0) { + // Столкновение с плоскостью треугольника + return getBoxToTriContact(box, tri, toBox, contact); + } else { + // Столкновение с гранью бокса + return getTriToBoxContact(box, tri, toBox, faceAxisIndex, contact); + } + } + + /** + * Определяет точки контакта бокса с плоскостью треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getBoxToTriContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + colNormal.x = triTransform.c; + colNormal.y = triTransform.g; + colNormal.z = triTransform.k; + + var over:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + if (!over) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var incAxisDot:Number = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colNormal.x + axis.y*colNormal.y + axis.z*colNormal.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incAxisDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Обрезка грани + var negativeFace:Boolean = incAxisDot > 0; + + var code:int = 1 << (incFaceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + getFaceVertsByAxis(box.hs, incFaceAxisIdx, negativeFace, points1); + boxTransform.transformVectorsN(points1, points2, 4); + triTransform.transformVectorsInverseN(points2, points1, 4); + var pnum:int = clipByTriangle(tri); + // Среди конечного списка точек определяются лежащие под плоскостью треугольника + var cp:ContactPoint; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points2[i]; + if ((over && v.z < 0) || (!over && v.z > 0)) { + cp = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z + triTransform.d; + cpPos.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z + triTransform.h; + cpPos.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z + triTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = over ? -v.z : v.z; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Оперделяет точки контакта треугольника с гранью бокса. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getTriToBoxContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + faceAxisIdx--; + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + boxTransform.getAxis(faceAxisIdx, colNormal); + var negativeFace:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + + var code:int = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + if (!negativeFace) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + var v:Vector3 = points1[0]; + v.x = tri.v0.x; + v.y = tri.v0.y; + v.z = tri.v0.z; + + v = points1[1]; + v.x = tri.v1.x; + v.y = tri.v1.y; + v.z = tri.v1.z; + + v = points1[2]; + v.x = tri.v2.x; + v.y = tri.v2.y; + v.z = tri.v2.z; + + triTransform.transformVectorsN(points1, points2, 3); + boxTransform.transformVectorsInverseN(points2, points1, 3); + + var pnum:int = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность боксу и добавляем такие точки в список контактов + var penetration:Number; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + v = points1[i]; + penetration = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace); + if (penetration > -epsilon) { + var cp:ContactPoint = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = penetration; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Вычисляет величину проникновения точки в бокс. + * + * @param hs вектор половинных размеров бокса + * @param p точка в системе координат бокса + * @param axisIndex индекс оси + * @param negativeFace если true, проверяется нижняя грань + * @return величина проникновения точки в бокс + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, negativeFace:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (negativeFace) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (negativeFace) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (negativeFace) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, вершины которой заданы в списке points1. Результат сохраняется в этом же списке. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 3; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Обрезает грань прямоугольника сторонами треугольника. Обрезка выполняется в системе координат и в проекции на + * плоскость треугольника. Входные вершины грани в количестве четырёх штук должны находиться в списке points1. + * Конечные вершины сохраняются в списке points2. + * + * @param tri треугольник + * @return количество врешин в конечном списке + */ + private function clipByTriangle(tri:CollisionTriangle):int { + var vnum:int = 4; + vnum = clipByLine(tri.v0, tri.e0, points1, vnum, points2); + if (vnum == 0) return 0; + vnum = clipByLine(tri.v1, tri.e1, points2, vnum, points1); + if (vnum == 0) return 0; + return clipByLine(tri.v2, tri.e2, points1, vnum, points2);; + } + + /** + * Обрезает полигон указанной прямой. + * + * @param linePoint точка на прямой + * @param lineDir единичный направляющий вектор прямой + * @param verticesIn список вершин исходного полигона + * @param vnum количество вершин исходного полигона + * @param verticesOut список, куда будут записаны вершины конечного полигона + * @return количество вершин конечного полигона + */ + private function clipByLine(linePoint:Vector3, lineDir:Vector3, verticesIn:Vector., vnum:int, verticesOut:Vector.):int { + var nx:Number = -lineDir.y; + var ny:Number = lineDir.x; + var offset:Number = linePoint.x*nx + linePoint.y*ny; + var v1:Vector3 = verticesIn[int(vnum - 1)]; + var offset1:Number = v1.x*nx + v1.y*ny; + var t:Number; + var v:Vector3; + var num:int = 0; + for (var i:int = 0; i < vnum; i++) { + var v2:Vector3 = verticesIn[i]; + var offset2:Number = v2.x*nx + v2.y*ny; + if (offset1 < offset) { + // Первая точка ребра во внешней полуплоскости + if (offset2 > offset) { + // Вторая точка ребра во внутренней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } else { + // Первая точка ребра во внутренней полуплоскости. Добавляем её в конечный список. + v = verticesOut[num]; + v.x = v1.x; + v.y = v1.y; + v.z = v1.z; + num++; + if (offset2 < offset) { + // Вторая точка ребра во внешней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } + v1 = v2; + offset1 = offset2; + } + return num; + } + + /** + * + * @param box + * @param tri + * @param toBox + * @param boxAxisIdx + * @param triAxisIdx + * @param contact + */ + private function findEdgesIntersection(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, boxAxisIdx:int, triAxisIdx:int, contact:Contact):Boolean { + // Определение точки и направляющего вектора ребра треугольника + var tmpx1:Number; + var tmpy1:Number; + var tmpz1:Number; + var tmpx2:Number; + var tmpy2:Number; + var tmpz2:Number; + switch (triAxisIdx) { + case 0: + tmpx1 = tri.e0.x; + tmpy1 = tri.e0.y; + tmpz1 = tri.e0.z; + tmpx2 = tri.v0.x; + tmpy2 = tri.v0.y; + tmpz2 = tri.v0.z; + break; + case 1: + tmpx1 = tri.e1.x; + tmpy1 = tri.e1.y; + tmpz1 = tri.e1.z; + tmpx2 = tri.v1.x; + tmpy2 = tri.v1.y; + tmpz2 = tri.v1.z; + break; + case 2: + tmpx1 = tri.e2.x; + tmpy1 = tri.e2.y; + tmpz1 = tri.e2.z; + tmpx2 = tri.v2.x; + tmpy2 = tri.v2.y; + tmpz2 = tri.v2.z; + break; + } + var triTransform:Matrix4 = tri.transform; + axis20.x = triTransform.a*tmpx1 + triTransform.b*tmpy1 + triTransform.c*tmpz1; + axis20.y = triTransform.e*tmpx1 + triTransform.f*tmpy1 + triTransform.g*tmpz1; + axis20.z = triTransform.i*tmpx1 + triTransform.j*tmpy1 + triTransform.k*tmpz1; + var x2:Number = triTransform.a*tmpx2 + triTransform.b*tmpy2 + triTransform.c*tmpz2 + triTransform.d; + var y2:Number = triTransform.e*tmpx2 + triTransform.f*tmpy2 + triTransform.g*tmpz2 + triTransform.h; + var z2:Number = triTransform.i*tmpx2 + triTransform.j*tmpy2 + triTransform.k*tmpz2 + triTransform.l; + + // Определение нормали контакта, точки и направляющего вектора ребра бокса + var boxTransform:Matrix4 = box.transform; + boxTransform.getAxis(boxAxisIdx, axis10); + + // Нормаль контакта + var v:Vector3 = contact.normal; + v.x = axis10.y*axis20.z - axis10.z*axis20.y; + v.y = axis10.z*axis20.x - axis10.x*axis20.z; + v.z = axis10.x*axis20.y - axis10.y*axis20.x; + k = 1/Math.sqrt(v.x*v.x + v.y*v.y + v.z*v.z); + v.x *= k; + v.y *= k; + v.z *= k; + // Разворот нормали в сторону бокса + if (v.x*toBox.x + v.y*toBox.y + v.z*toBox.z < 0) { + v.x = -v.x; + v.y = -v.y; + v.z = -v.z; + } + + var boxHalfLen:Number; + tmpx1 = box.hs.x; + tmpy1 = box.hs.y; + tmpz1 = box.hs.z; + // X + if (boxAxisIdx == 0) { + tmpx1 = 0; + boxHalfLen = box.hs.x; + } else { + if (boxTransform.a*v.x + boxTransform.e*v.y + boxTransform.i*v.z > 0) { + tmpx1 = -tmpx1; + if ((box.excludedFaces & 2) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 1) != 0) { + return false; + } + } + } + // Y + if (boxAxisIdx == 1) { + tmpy1 = 0; + boxHalfLen = box.hs.y; + } else { + if (boxTransform.b*v.x + boxTransform.f*v.y + boxTransform.j*v.z > 0) { + tmpy1 = -tmpy1; + if ((box.excludedFaces & 8) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 4) != 0) { + return false; + } + } + } + // Z + if (boxAxisIdx == 2) { + tmpz1 = 0; + boxHalfLen = box.hs.z; + } else { + if (boxTransform.c*v.x + boxTransform.g*v.y + boxTransform.k*v.z > 0) { + tmpz1 = -tmpz1; + if ((box.excludedFaces & 32) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 16) != 0) { + return false; + } + } + } + var x1:Number = boxTransform.a*tmpx1 + boxTransform.b*tmpy1 + boxTransform.c*tmpz1 + boxTransform.d; + var y1:Number = boxTransform.e*tmpx1 + boxTransform.f*tmpy1 + boxTransform.g*tmpz1 + boxTransform.h; + var z1:Number = boxTransform.i*tmpx1 + boxTransform.j*tmpy1 + boxTransform.k*tmpz1 + boxTransform.l; + + // Находим точку пересечения рёбер, решая систему уравнений + // axis10 - направляющий вектор ребра бокса + // x1, y1, z1 - средняя точка ребра бокса + // axis20 - направляющий вектор ребра треугольника + // x2, y2, z2 - начальная точка ребра треугольника + var k:Number = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + var vx:Number = x2 - x1; + var vy:Number = y2 - y1; + var vz:Number = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + + // Запись данных о контакте + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + v = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + v.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + v.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + v.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = v.x - boxTransform.d; + r.y = v.y - boxTransform.h; + r.z = v.z - boxTransform.l; + r = cp.r2; + r.x = v.x - triTransform.d; + r.y = v.y - triTransform.h; + r.z = v.z - triTransform.l; + return true; + } + + } +} diff --git a/0.0.9.3/src/alternativa/physics/collision/colliders/SpherePlaneCollider.as b/0.0.9.3/src/alternativa/physics/collision/colliders/SpherePlaneCollider.as new file mode 100644 index 0000000..30cd55f --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/colliders/SpherePlaneCollider.as @@ -0,0 +1,68 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + /** + * + */ + public class SpherePlaneCollider implements ICollider { + + private var normal:Vector3 = new Vector3(); + + /** + * + */ + public function SpherePlaneCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var sphere:RigidSphere = body1 as RigidSphere; +// var plane:RigidPlane; +// if (sphere == null) { +// sphere = body2 as RigidSphere; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// +// var dist:Number = sphere.state.pos.dot(normal) - offset; +// if (dist > sphere.r) return false; +// +// collisionInfo.body1 = sphere; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// collisionInfo.pcount = 1; +// +// var cp:ContactPoint = collisionInfo.points[0]; +// cp.penetration = sphere.r - dist; +// cp.pos.copy(normal).reverse().scale(sphere.r).add(sphere.state.pos); +// cp.r1.diff(cp.pos, sphere.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/collision/colliders/SphereSphereCollider.as b/0.0.9.3/src/alternativa/physics/collision/colliders/SphereSphereCollider.as new file mode 100644 index 0000000..3332f3c --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/colliders/SphereSphereCollider.as @@ -0,0 +1,70 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.math.Vector3; + use namespace altphysics; + + public class SphereSphereCollider implements ICollider { + + private var p1:Vector3 = new Vector3(); + private var p2:Vector3 = new Vector3(); + + public function SphereSphereCollider() { + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var s1:CollisionSphere; + var s2:CollisionSphere; + if (prim1.body != null) { + s1 = prim1 as CollisionSphere; + s2 = prim2 as CollisionSphere; + } else { + s1 = prim2 as CollisionSphere; + s2 = prim1 as CollisionSphere; + } + + s1.transform.getAxis(3, p1); + s2.transform.getAxis(3, p2); + var dx:Number = p1.x - p2.x; + var dy:Number = p1.y - p2.y; + var dz:Number = p1.z - p2.z; + var len:Number = dx*dx + dy*dy + dz*dz; + var sum:Number = s1.r + s2.r; + if (len > sum*sum) return false; + len = Math.sqrt(len); + dx /= len; + dy /= len; + dz /= len; + + contact.body1 = s1.body; + contact.body2 = s2.body; + contact.normal.x = dx; + contact.normal.y = dy; + contact.normal.z = dz; + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = sum - len; + cp.pos.x = p1.x - dx*s1.r; + cp.pos.y = p1.y - dy*s1.r; + cp.pos.z = p1.z - dz*s1.r; + cp.r1.vDiff(cp.pos, p1); + cp.r2.vDiff(cp.pos, p2); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/collision/primitives/.svn/all-wcprops b/0.0.9.3/src/alternativa/physics/collision/primitives/.svn/all-wcprops new file mode 100644 index 0000000..bb50f77 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/primitives/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 124 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/collision/primitives +END +CollisionTriangle.as +K 25 +svn:wc:ra_dav:version-url +V 145 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/collision/primitives/CollisionTriangle.as +END +CollisionSphere.as +K 25 +svn:wc:ra_dav:version-url +V 143 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/collision/primitives/CollisionSphere.as +END +CollisionBox.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/collision/primitives/CollisionBox.as +END +CollisionRect.as +K 25 +svn:wc:ra_dav:version-url +V 141 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/collision/primitives/CollisionRect.as +END diff --git a/0.0.9.3/src/alternativa/physics/collision/primitives/.svn/entries b/0.0.9.3/src/alternativa/physics/collision/primitives/.svn/entries new file mode 100644 index 0000000..aa776f7 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/primitives/.svn/entries @@ -0,0 +1,76 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/collision/primitives +http://svndev.alternativaplatform.com + + + +2010-07-03T11:40:52.545629Z +36684 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +CollisionTriangle.as +file + + + + +2010-10-28T04:32:44.000000Z +23ebb96cda309378ce15ead4cf865446 +2009-11-24T08:44:15.941480Z +23758 +mike + +CollisionSphere.as +file + + + + +2010-10-28T04:32:44.000000Z +335b6b72039123d6d54fee4d6b17f74e +2009-11-24T08:44:15.941480Z +23758 +mike + +CollisionBox.as +file + + + + +2010-10-28T04:32:44.000000Z +d1552d6d5cc026233dd769249d679970 +2010-07-03T11:40:52.545629Z +36684 +mike + +CollisionRect.as +file + + + + +2010-10-28T04:32:44.000000Z +91b913679dca5ae4baabafb09c5aa051 +2009-11-24T08:44:15.941480Z +23758 +mike + diff --git a/0.0.9.3/src/alternativa/physics/collision/primitives/.svn/format b/0.0.9.3/src/alternativa/physics/collision/primitives/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/primitives/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.3/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base b/0.0.9.3/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base new file mode 100644 index 0000000..844c65d --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base @@ -0,0 +1,215 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * Ориентированный бокс. + */ + public class CollisionBox extends CollisionPrimitive { + + // Половинные размеры вдоль каждой из осей + public var hs:Vector3 = new Vector3(); + + public var excludedFaces:int; + + /** + * @param hs + * @param collisionGroup + */ + public function CollisionBox(hs:Vector3, collisionGroup:int) { + super(BOX, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * @return - + */ + override public function calculateAABB():BoundBox { + var t:Matrix4 = transform; + + var xx:Number; + var yy:Number; + var zz:Number; + + xx = t.a < 0 ? -t.a : t.a; + yy = t.b < 0 ? -t.b : t.b; + zz = t.c < 0 ? -t.c : t.c; + aabb.maxX = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minX = -aabb.maxX; + + xx = t.e < 0 ? -t.e : t.e; + yy = t.f < 0 ? -t.f : t.f; + zz = t.g < 0 ? -t.g : t.g; + aabb.maxY = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minY = -aabb.maxY; + + xx = t.i < 0 ? -t.i : t.i; + yy = t.j < 0 ? -t.j : t.j; + zz = t.k < 0 ? -t.k : t.k; + aabb.maxZ = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return - + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var box:CollisionBox = source as CollisionBox; + if (box == null) return this; + super.copyFrom(box); + hs.vCopy(box.hs); + return this; + } + + /** + * @return - + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionBox(hs, collisionGroup); + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + var tMin:Number = -1; + var tMax:Number = 1e308; + var t1:Number; + var t2:Number; + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + + // Проверка, не находится ли начало луча внутри бокса. В этом случае точкой пересечения будет начало луча, + // а нормалью будет нормированный вектор, противоположный вектору луча. + if (ox >= -hs.x && ox <= hs.x && oy >= -hs.y && oy <= hs.y && oz >= -hs.z && oz <= hs.z) { + normal.x = -vector.x; + normal.y = -vector.y; + normal.z = -vector.z; + normal.vNormalize(); + return 0; + } + + // X + if (vx < epsilon && vx > -epsilon) { + if (ox < -hs.x || ox > hs.x) return -1; + } else { + t1 = (-hs.x - ox)/vx; + t2 = (hs.x - ox)/vx; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.x = -1; + normal.y = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.x = 1; + normal.y = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Y + if (vy < epsilon && vy > -epsilon) { + if (oy < -hs.y || oy > hs.y) return -1; + } else { + t1 = (-hs.y - oy)/vy; + t2 = (hs.y - oy)/vy; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.y = -1; + normal.x = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.y = 1; + normal.x = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Z + if (vz < epsilon && vz > -epsilon) { + if (oz < -hs.z || oz > hs.z) return -1; + } else { + t1 = (-hs.z - oz)/vz; + t2 = (hs.z - oz)/vz; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.z = -1; + normal.x = normal.y = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.z = 1; + normal.x = normal.y = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Перевод нормали в мировую систему координат + // Inlined normal.vDeltaTransformBy4(transform); + vx = normal.x; + vy = normal.y; + vz = normal.z; + normal.x = transform.a*vx + transform.b*vy + transform.c*vz; + normal.y = transform.e*vx + transform.f*vy + transform.g*vz; + normal.z = transform.i*vx + transform.j*vy + transform.k*vz; + + return tMin; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionBox hs=" + hs + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base b/0.0.9.3/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base new file mode 100644 index 0000000..7a33b79 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base @@ -0,0 +1,143 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * Ориентированный прямоугольник. Задаётся половинами размеров вдоль осей X (ширина) и Y (длина) локальной системы + * координат примитива. Таким образом, прямоугольник лежит в плоскости XY, его стороны параллельны осям этой + * плоскости, а нормаль направлена вдоль локальной оси Z. + * + * Прямоугольник может быть одно- или двусторонним. В случае одностороннего прямоугольника, + * столкновения с внутренней стороны не регистрируются. + */ + public class CollisionRect extends CollisionPrimitive { + // Половинные размеры прямоугольника вдоль осей X и Y. Нормаль направлена вдоль оси Z. + public var hs:Vector3 = new Vector3(); + // Флаг указывает, является примитив одно- или двусторонним + public var twoSided:Boolean = true; + + // Малое значение. Используется для указания фиктивной высоты примитива и в функции определения пересечения с сегментом. + private static const EPSILON:Number = 0.005; + + /** + * Создаёт новый экземпляр примитива. + * + * @param hs половинные размерв прямоугольника вдоль осей X и Y. Значение z игнорируется. + * @param collisionGroup группа примитива + */ + public function CollisionRect(hs:Vector3, collisionGroup:int) { + super(RECT, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * Расчитывает ограничивающий бокс прямоугольника. Для избежания проблем высота примитива принимается равной + * не нулю, а малому значению. + * + * @return + */ + override public function calculateAABB():BoundBox { + // Баунд бокс прямоугольника имеет минимальную высоту, отличную от нуля во избежание проблем с построением kd-дерева + var t:Matrix4 = transform; + var xx:Number = t.a < 0 ? -t.a : t.a; + var yy:Number = t.b < 0 ? -t.b : t.b; + var zz:Number = t.c < 0 ? -t.c : t.c; + aabb.maxX = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minX = -aabb.maxX; + + xx = t.e < 0 ? -t.e : t.e; + yy = t.f < 0 ? -t.f : t.f; + zz = t.g < 0 ? -t.g : t.g; + aabb.maxY = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minY = -aabb.maxY; + + xx = t.i < 0 ? -t.i : t.i; + yy = t.j < 0 ? -t.j : t.j; + zz = t.k < 0 ? -t.k : t.k; + aabb.maxZ = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var rect:CollisionRect = source as CollisionRect; + if (rect == null) return this; + super.copyFrom(rect); + hs.vCopy(rect.hs); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionRect hs=" + hs + "]"; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionRect(hs, collisionGroup); + } + + /** + * @param origin + * @param vector + * @param collisionGroup + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + + // Проверка параллельности сегмента и плоскости примитива + if (vz > -threshold && vz < threshold) return -1; + var t:Number = -oz/vz; + if (t < 0) return -1; + // Проверка вхождения точки пересечения в прямоугольник + ox += vx*t; + oy += vy*t; + oz = 0; + if (ox < (-hs.x - threshold) || ox > (hs.x + threshold) || oy < (-hs.y - threshold) || oy > (hs.y + threshold)) return -1; + + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + return t; + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base b/0.0.9.3/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base new file mode 100644 index 0000000..b912a5b --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base @@ -0,0 +1,81 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Vector3; + use namespace altphysics; + + + /** + * Сфера. + */ + public class CollisionSphere extends CollisionPrimitive { + + // Радиус сферы + public var r:Number = 0; + + /** + * + * @param r + * @param collisionGroup + */ + public function CollisionSphere(r:Number, collisionGroup:int) { + super(SPHERE, collisionGroup); + this.r = r; + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + aabb.maxX = transform.d + r; + aabb.minX = transform.d - r; + + aabb.maxY = transform.h + r; + aabb.minY = transform.h - r; + + aabb.maxZ = transform.l + r; + aabb.minZ = transform.l - r; + + return aabb; + } + + /** + * @param origin + * @param vector + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var px:Number = origin.x - transform.d; + var py:Number = origin.y - transform.h; + var pz:Number = origin.z - transform.l; + var k:Number = vector.x*px + vector.y*py + vector.z*pz; + if (k > 0) return -1; + var a:Number = vector.x*vector.x + vector.y*vector.y + vector.z*vector.z; + var D:Number = k*k - a*(px*px + py*py + pz*pz - r*r); + if (D < 0) return -1; + return -(k + Math.sqrt(D))/a; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var sphere:CollisionSphere = source as CollisionSphere; + if (sphere == null) return this; + super.copyFrom(sphere); + r = sphere.r; + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionSphere(r, collisionGroup); + } + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base b/0.0.9.3/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base new file mode 100644 index 0000000..bdc3166 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base @@ -0,0 +1,244 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * Примитив треугольник. + */ + public class CollisionTriangle extends CollisionPrimitive { + + public var v0:Vector3 = new Vector3(); + public var v1:Vector3 = new Vector3(); + public var v2:Vector3 = new Vector3(); + + public var e0:Vector3 = new Vector3(); + public var e1:Vector3 = new Vector3(); + public var e2:Vector3 = new Vector3(); + +// public var len0:Number; +// public var len1:Number; +// public var len2:Number; +// + /** + * + * @param v0 + * @param v1 + * @param v2 + * @param collisionGroup + */ + public function CollisionTriangle(v0:Vector3, v1:Vector3, v2:Vector3, collisionGroup:int) { + super(TRIANGLE, collisionGroup); + initVertices(v0, v1, v2); + } + + /** + * Рассчитывает AABB примитива. + * + * @return ссылка на свой AABB + */ + override public function calculateAABB():BoundBox { + var epsilon:Number = 0.005; + var a:Number; + var b:Number; + var eps_c:Number = epsilon*transform.c; + var eps_g:Number = epsilon*transform.g; + var eps_k:Number = epsilon*transform.k; + + // Вершина 0 + // Ось X + a = v0.x*transform.a + v0.y*transform.b; + aabb.minX = aabb.maxX = a + eps_c; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v0.x*transform.e + v0.y*transform.f; + aabb.minY = aabb.maxY = a + eps_g; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v0.x*transform.i + v0.y*transform.j; + aabb.minZ = aabb.maxZ = a + eps_k; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 1 + // Ось X + a = v1.x*transform.a + v1.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v1.x*transform.e + v1.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v1.x*transform.i + v1.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 2 + // Ось X + a = v2.x*transform.a + v2.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v2.x*transform.e + v2.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v2.x*transform.i + v2.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + aabb.minX += transform.d; + aabb.maxX += transform.d; + + aabb.minY += transform.h; + aabb.maxY += transform.h; + + aabb.minZ += transform.l; + aabb.maxZ += transform.l; + + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + // Луч трансформируется в систему координат примитива, затем проверяется пересечение + + var vz:Number = vector.x*transform.c + vector.y*transform.g + vector.z*transform.k; + // Если луч параллелен плоскости птимитива, то пересечения нет + if (vz < epsilon && vz > -epsilon) return -1; + + var tx:Number = origin.x - transform.d; + var ty:Number = origin.y - transform.h; + var tz:Number = origin.z - transform.l; + + var oz:Number = tx*transform.c + ty*transform.g + tz*transform.k; + var t:Number = -oz/vz; + if (t < 0) return -1; + + var ox:Number = tx*transform.a + ty*transform.e + tz*transform.i; + var oy:Number = tx*transform.b + ty*transform.f + tz*transform.j; + + tx = ox + t*(vector.x*transform.a + vector.y*transform.e + vector.z*transform.i); + ty = oy + t*(vector.x*transform.b + vector.y*transform.f + vector.z*transform.j); + tz = oz + t*vz; + + // Проверка вхождения точки в треугольник + if ((e0.x*(ty - v0.y) - e0.y*(tx - v0.x) < 0) || (e1.x*(ty - v1.y) - e1.y*(tx - v1.x) < 0) || (e2.x*(ty - v2.y) - e2.y*(tx - v2.x) < 0)) return -1; + + // Запись нормали + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + + return t; + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + super.copyFrom(source); + var tri:CollisionTriangle = source as CollisionTriangle; + if (tri != null) { + v0.vCopy(tri.v0); + v1.vCopy(tri.v1); + v2.vCopy(tri.v2); + + e0.vCopy(tri.e0); + e1.vCopy(tri.e1); + e2.vCopy(tri.e2); + +// len0 = tri.len0; +// len1 = tri.len1; +// len2 = tri.len2; + } + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionTriangle v0=" + v0 + ", v1=" + v1 + ", v2=" + v2 + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionTriangle(v0, v1, v2, collisionGroup); + } + + /** + * + * @param v0 + * @param v1 + * @param v2 + */ + private function initVertices(v0:Vector3, v1:Vector3, v2:Vector3):void { + this.v0.vCopy(v0); + this.v1.vCopy(v1); + this.v2.vCopy(v2); + + e0.vDiff(v1, v0); +// len0 = e0.vLength(); + e0.vNormalize(); + + e1.vDiff(v2, v1); +// len1 = e1.vLength(); + e1.vNormalize(); + + e2.vDiff(v0, v2); +// len2 = e2.vLength(); + e2.vNormalize(); + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/collision/primitives/CollisionBox.as b/0.0.9.3/src/alternativa/physics/collision/primitives/CollisionBox.as new file mode 100644 index 0000000..844c65d --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/primitives/CollisionBox.as @@ -0,0 +1,215 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * Ориентированный бокс. + */ + public class CollisionBox extends CollisionPrimitive { + + // Половинные размеры вдоль каждой из осей + public var hs:Vector3 = new Vector3(); + + public var excludedFaces:int; + + /** + * @param hs + * @param collisionGroup + */ + public function CollisionBox(hs:Vector3, collisionGroup:int) { + super(BOX, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * @return - + */ + override public function calculateAABB():BoundBox { + var t:Matrix4 = transform; + + var xx:Number; + var yy:Number; + var zz:Number; + + xx = t.a < 0 ? -t.a : t.a; + yy = t.b < 0 ? -t.b : t.b; + zz = t.c < 0 ? -t.c : t.c; + aabb.maxX = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minX = -aabb.maxX; + + xx = t.e < 0 ? -t.e : t.e; + yy = t.f < 0 ? -t.f : t.f; + zz = t.g < 0 ? -t.g : t.g; + aabb.maxY = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minY = -aabb.maxY; + + xx = t.i < 0 ? -t.i : t.i; + yy = t.j < 0 ? -t.j : t.j; + zz = t.k < 0 ? -t.k : t.k; + aabb.maxZ = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return - + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var box:CollisionBox = source as CollisionBox; + if (box == null) return this; + super.copyFrom(box); + hs.vCopy(box.hs); + return this; + } + + /** + * @return - + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionBox(hs, collisionGroup); + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + var tMin:Number = -1; + var tMax:Number = 1e308; + var t1:Number; + var t2:Number; + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + + // Проверка, не находится ли начало луча внутри бокса. В этом случае точкой пересечения будет начало луча, + // а нормалью будет нормированный вектор, противоположный вектору луча. + if (ox >= -hs.x && ox <= hs.x && oy >= -hs.y && oy <= hs.y && oz >= -hs.z && oz <= hs.z) { + normal.x = -vector.x; + normal.y = -vector.y; + normal.z = -vector.z; + normal.vNormalize(); + return 0; + } + + // X + if (vx < epsilon && vx > -epsilon) { + if (ox < -hs.x || ox > hs.x) return -1; + } else { + t1 = (-hs.x - ox)/vx; + t2 = (hs.x - ox)/vx; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.x = -1; + normal.y = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.x = 1; + normal.y = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Y + if (vy < epsilon && vy > -epsilon) { + if (oy < -hs.y || oy > hs.y) return -1; + } else { + t1 = (-hs.y - oy)/vy; + t2 = (hs.y - oy)/vy; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.y = -1; + normal.x = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.y = 1; + normal.x = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Z + if (vz < epsilon && vz > -epsilon) { + if (oz < -hs.z || oz > hs.z) return -1; + } else { + t1 = (-hs.z - oz)/vz; + t2 = (hs.z - oz)/vz; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.z = -1; + normal.x = normal.y = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.z = 1; + normal.x = normal.y = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Перевод нормали в мировую систему координат + // Inlined normal.vDeltaTransformBy4(transform); + vx = normal.x; + vy = normal.y; + vz = normal.z; + normal.x = transform.a*vx + transform.b*vy + transform.c*vz; + normal.y = transform.e*vx + transform.f*vy + transform.g*vz; + normal.z = transform.i*vx + transform.j*vy + transform.k*vz; + + return tMin; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionBox hs=" + hs + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/collision/primitives/CollisionRect.as b/0.0.9.3/src/alternativa/physics/collision/primitives/CollisionRect.as new file mode 100644 index 0000000..7a33b79 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/primitives/CollisionRect.as @@ -0,0 +1,143 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * Ориентированный прямоугольник. Задаётся половинами размеров вдоль осей X (ширина) и Y (длина) локальной системы + * координат примитива. Таким образом, прямоугольник лежит в плоскости XY, его стороны параллельны осям этой + * плоскости, а нормаль направлена вдоль локальной оси Z. + * + * Прямоугольник может быть одно- или двусторонним. В случае одностороннего прямоугольника, + * столкновения с внутренней стороны не регистрируются. + */ + public class CollisionRect extends CollisionPrimitive { + // Половинные размеры прямоугольника вдоль осей X и Y. Нормаль направлена вдоль оси Z. + public var hs:Vector3 = new Vector3(); + // Флаг указывает, является примитив одно- или двусторонним + public var twoSided:Boolean = true; + + // Малое значение. Используется для указания фиктивной высоты примитива и в функции определения пересечения с сегментом. + private static const EPSILON:Number = 0.005; + + /** + * Создаёт новый экземпляр примитива. + * + * @param hs половинные размерв прямоугольника вдоль осей X и Y. Значение z игнорируется. + * @param collisionGroup группа примитива + */ + public function CollisionRect(hs:Vector3, collisionGroup:int) { + super(RECT, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * Расчитывает ограничивающий бокс прямоугольника. Для избежания проблем высота примитива принимается равной + * не нулю, а малому значению. + * + * @return + */ + override public function calculateAABB():BoundBox { + // Баунд бокс прямоугольника имеет минимальную высоту, отличную от нуля во избежание проблем с построением kd-дерева + var t:Matrix4 = transform; + var xx:Number = t.a < 0 ? -t.a : t.a; + var yy:Number = t.b < 0 ? -t.b : t.b; + var zz:Number = t.c < 0 ? -t.c : t.c; + aabb.maxX = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minX = -aabb.maxX; + + xx = t.e < 0 ? -t.e : t.e; + yy = t.f < 0 ? -t.f : t.f; + zz = t.g < 0 ? -t.g : t.g; + aabb.maxY = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minY = -aabb.maxY; + + xx = t.i < 0 ? -t.i : t.i; + yy = t.j < 0 ? -t.j : t.j; + zz = t.k < 0 ? -t.k : t.k; + aabb.maxZ = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var rect:CollisionRect = source as CollisionRect; + if (rect == null) return this; + super.copyFrom(rect); + hs.vCopy(rect.hs); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionRect hs=" + hs + "]"; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionRect(hs, collisionGroup); + } + + /** + * @param origin + * @param vector + * @param collisionGroup + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + + // Проверка параллельности сегмента и плоскости примитива + if (vz > -threshold && vz < threshold) return -1; + var t:Number = -oz/vz; + if (t < 0) return -1; + // Проверка вхождения точки пересечения в прямоугольник + ox += vx*t; + oy += vy*t; + oz = 0; + if (ox < (-hs.x - threshold) || ox > (hs.x + threshold) || oy < (-hs.y - threshold) || oy > (hs.y + threshold)) return -1; + + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + return t; + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/collision/primitives/CollisionSphere.as b/0.0.9.3/src/alternativa/physics/collision/primitives/CollisionSphere.as new file mode 100644 index 0000000..b912a5b --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/primitives/CollisionSphere.as @@ -0,0 +1,81 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Vector3; + use namespace altphysics; + + + /** + * Сфера. + */ + public class CollisionSphere extends CollisionPrimitive { + + // Радиус сферы + public var r:Number = 0; + + /** + * + * @param r + * @param collisionGroup + */ + public function CollisionSphere(r:Number, collisionGroup:int) { + super(SPHERE, collisionGroup); + this.r = r; + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + aabb.maxX = transform.d + r; + aabb.minX = transform.d - r; + + aabb.maxY = transform.h + r; + aabb.minY = transform.h - r; + + aabb.maxZ = transform.l + r; + aabb.minZ = transform.l - r; + + return aabb; + } + + /** + * @param origin + * @param vector + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var px:Number = origin.x - transform.d; + var py:Number = origin.y - transform.h; + var pz:Number = origin.z - transform.l; + var k:Number = vector.x*px + vector.y*py + vector.z*pz; + if (k > 0) return -1; + var a:Number = vector.x*vector.x + vector.y*vector.y + vector.z*vector.z; + var D:Number = k*k - a*(px*px + py*py + pz*pz - r*r); + if (D < 0) return -1; + return -(k + Math.sqrt(D))/a; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var sphere:CollisionSphere = source as CollisionSphere; + if (sphere == null) return this; + super.copyFrom(sphere); + r = sphere.r; + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionSphere(r, collisionGroup); + } + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/collision/primitives/CollisionTriangle.as b/0.0.9.3/src/alternativa/physics/collision/primitives/CollisionTriangle.as new file mode 100644 index 0000000..bdc3166 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/primitives/CollisionTriangle.as @@ -0,0 +1,244 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * Примитив треугольник. + */ + public class CollisionTriangle extends CollisionPrimitive { + + public var v0:Vector3 = new Vector3(); + public var v1:Vector3 = new Vector3(); + public var v2:Vector3 = new Vector3(); + + public var e0:Vector3 = new Vector3(); + public var e1:Vector3 = new Vector3(); + public var e2:Vector3 = new Vector3(); + +// public var len0:Number; +// public var len1:Number; +// public var len2:Number; +// + /** + * + * @param v0 + * @param v1 + * @param v2 + * @param collisionGroup + */ + public function CollisionTriangle(v0:Vector3, v1:Vector3, v2:Vector3, collisionGroup:int) { + super(TRIANGLE, collisionGroup); + initVertices(v0, v1, v2); + } + + /** + * Рассчитывает AABB примитива. + * + * @return ссылка на свой AABB + */ + override public function calculateAABB():BoundBox { + var epsilon:Number = 0.005; + var a:Number; + var b:Number; + var eps_c:Number = epsilon*transform.c; + var eps_g:Number = epsilon*transform.g; + var eps_k:Number = epsilon*transform.k; + + // Вершина 0 + // Ось X + a = v0.x*transform.a + v0.y*transform.b; + aabb.minX = aabb.maxX = a + eps_c; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v0.x*transform.e + v0.y*transform.f; + aabb.minY = aabb.maxY = a + eps_g; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v0.x*transform.i + v0.y*transform.j; + aabb.minZ = aabb.maxZ = a + eps_k; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 1 + // Ось X + a = v1.x*transform.a + v1.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v1.x*transform.e + v1.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v1.x*transform.i + v1.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 2 + // Ось X + a = v2.x*transform.a + v2.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v2.x*transform.e + v2.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v2.x*transform.i + v2.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + aabb.minX += transform.d; + aabb.maxX += transform.d; + + aabb.minY += transform.h; + aabb.maxY += transform.h; + + aabb.minZ += transform.l; + aabb.maxZ += transform.l; + + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + // Луч трансформируется в систему координат примитива, затем проверяется пересечение + + var vz:Number = vector.x*transform.c + vector.y*transform.g + vector.z*transform.k; + // Если луч параллелен плоскости птимитива, то пересечения нет + if (vz < epsilon && vz > -epsilon) return -1; + + var tx:Number = origin.x - transform.d; + var ty:Number = origin.y - transform.h; + var tz:Number = origin.z - transform.l; + + var oz:Number = tx*transform.c + ty*transform.g + tz*transform.k; + var t:Number = -oz/vz; + if (t < 0) return -1; + + var ox:Number = tx*transform.a + ty*transform.e + tz*transform.i; + var oy:Number = tx*transform.b + ty*transform.f + tz*transform.j; + + tx = ox + t*(vector.x*transform.a + vector.y*transform.e + vector.z*transform.i); + ty = oy + t*(vector.x*transform.b + vector.y*transform.f + vector.z*transform.j); + tz = oz + t*vz; + + // Проверка вхождения точки в треугольник + if ((e0.x*(ty - v0.y) - e0.y*(tx - v0.x) < 0) || (e1.x*(ty - v1.y) - e1.y*(tx - v1.x) < 0) || (e2.x*(ty - v2.y) - e2.y*(tx - v2.x) < 0)) return -1; + + // Запись нормали + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + + return t; + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + super.copyFrom(source); + var tri:CollisionTriangle = source as CollisionTriangle; + if (tri != null) { + v0.vCopy(tri.v0); + v1.vCopy(tri.v1); + v2.vCopy(tri.v2); + + e0.vCopy(tri.e0); + e1.vCopy(tri.e1); + e2.vCopy(tri.e2); + +// len0 = tri.len0; +// len1 = tri.len1; +// len2 = tri.len2; + } + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionTriangle v0=" + v0 + ", v1=" + v1 + ", v2=" + v2 + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionTriangle(v0, v1, v2, collisionGroup); + } + + /** + * + * @param v0 + * @param v1 + * @param v2 + */ + private function initVertices(v0:Vector3, v1:Vector3, v2:Vector3):void { + this.v0.vCopy(v0); + this.v1.vCopy(v1); + this.v2.vCopy(v2); + + e0.vDiff(v1, v0); +// len0 = e0.vLength(); + e0.vNormalize(); + + e1.vDiff(v2, v1); +// len1 = e1.vLength(); + e1.vNormalize(); + + e2.vDiff(v0, v2); +// len2 = e2.vLength(); + e2.vNormalize(); + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/collision/types/.svn/all-wcprops b/0.0.9.3/src/alternativa/physics/collision/types/.svn/all-wcprops new file mode 100644 index 0000000..9aac09e --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/types/.svn/all-wcprops @@ -0,0 +1,23 @@ +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/collision/types +END +BoundBox.as +K 25 +svn:wc:ra_dav:version-url +V 131 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/collision/types/BoundBox.as +END +RayIntersection.as +K 25 +svn:wc:ra_dav:version-url +V 138 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/collision/types/RayIntersection.as +END +Ray.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/collision/types/Ray.as +END diff --git a/0.0.9.3/src/alternativa/physics/collision/types/.svn/entries b/0.0.9.3/src/alternativa/physics/collision/types/.svn/entries new file mode 100644 index 0000000..2bcc524 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/types/.svn/entries @@ -0,0 +1,64 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/collision/types +http://svndev.alternativaplatform.com + + + +2009-11-24T08:44:15.941480Z +23758 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +BoundBox.as +file + + + + +2010-10-28T04:32:44.000000Z +d92d4ab3dde698feb674fb9488f59678 +2009-09-09T14:02:48.134744Z +19633 +mike + +RayIntersection.as +file + + + + +2010-10-28T04:32:44.000000Z +88c0836ead77b8eb0841f11159dfb836 +2009-11-24T08:44:15.941480Z +23758 +mike + +Ray.as +file + + + + +2010-10-28T04:32:44.000000Z +c65cc8bfae377f8f0520b5c79c8b3ab9 +2009-11-24T08:44:15.941480Z +23758 +mike + diff --git a/0.0.9.3/src/alternativa/physics/collision/types/.svn/format b/0.0.9.3/src/alternativa/physics/collision/types/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/types/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.3/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base b/0.0.9.3/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base new file mode 100644 index 0000000..b4d5278 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base @@ -0,0 +1,75 @@ +package alternativa.physics.collision.types { + + /** + * Ограничивающий параллелепипед. + */ + public class BoundBox { + + public var minX:Number = 1e308; + public var minY:Number = 1e308; + public var minZ:Number = 1e308; + public var maxX:Number = -1e308; + public var maxY:Number = -1e308; + public var maxZ:Number = -1e308; + + public function setSize(minX:Number, minY:Number, minZ:Number, maxX:Number, maxY:Number, maxZ:Number):void { + this.minX = minX; + this.minY = minY; + this.minZ = minZ; + this.maxX = maxX; + this.maxY = maxY; + this.maxZ = maxZ; + } + + public function addBoundBox(boundBox:BoundBox):void { + minX = (boundBox.minX < minX) ? boundBox.minX : minX; + minY = (boundBox.minY < minY) ? boundBox.minY : minY; + minZ = (boundBox.minZ < minZ) ? boundBox.minZ : minZ; + maxX = (boundBox.maxX > maxX) ? boundBox.maxX : maxX; + maxY = (boundBox.maxY > maxY) ? boundBox.maxY : maxY; + maxZ = (boundBox.maxZ > maxZ) ? boundBox.maxZ : maxZ; + } + + public function addPoint(x:Number, y:Number, z:Number):void { + if (x < minX) minX = x; + if (x > maxX) maxX = x; + if (y < minY) minY = y; + if (y > maxY) maxY = y; + if (z < minZ) minZ = z; + if (z > maxZ) maxZ = z; + } + + public function infinity():void { + minX = 1e308; + minY = 1e308; + minZ = 1e308; + maxX = -1e308; + maxY = -1e308; + maxZ = -1e308; + } + + public function intersects(bb:BoundBox, epsilon:Number):Boolean { + return !(minX > bb.maxX + epsilon || maxX < bb.minX - epsilon || minY > bb.maxY + epsilon || maxY < bb.minY - epsilon || minZ > bb.maxZ + epsilon || maxZ < bb.minZ - epsilon); + } + + public function copyFrom(boundBox:BoundBox):void { + minX = boundBox.minX; + minY = boundBox.minY; + minZ = boundBox.minZ; + maxX = boundBox.maxX; + maxY = boundBox.maxY; + maxZ = boundBox.maxZ; + } + + public function clone():BoundBox { + var clone:BoundBox = new BoundBox(); + clone.copyFrom(this); + return clone; + } + + public function toString():String { + return "BoundBox [" + minX + ", " + minY + ", " + minZ + " : " + maxX + ", " + maxY + ", " + maxZ + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base b/0.0.9.3/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base new file mode 100644 index 0000000..fb2ffbc --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base @@ -0,0 +1,13 @@ +package alternativa.physics.collision.types { + import alternativa.math.Vector3; + + /** + * Структура, описывающая луч и сегмент прямой в пространстве. + */ + public class Ray { + // Начало луча. Начальная точка сегмента на прямой луча. + public var origin:Vector3 = new Vector3(); + // Направляющий вектор луча. Длина вектора задаёт длину сегмента на луче. Конечная точка сегмента вычисляется как origin + dir. + public var dir:Vector3 = new Vector3(); + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base b/0.0.9.3/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base new file mode 100644 index 0000000..21e9290 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base @@ -0,0 +1,25 @@ +package alternativa.physics.collision.types { + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.math.Vector3; + + /** + * Структура описывает точку пересечения луча с физической геометрией. + */ + public class RayIntersection { + // Физический примитив, с которым пересекается луч + public var primitive:CollisionPrimitive; + // Положенеи точки пересечения + public var pos:Vector3 = new Vector3(); + // Нормаль поверхности физического примитива в точке пересечения + public var normal:Vector3 = new Vector3(); + // Время пересечения (|pos - ray.origin|/|ray.dir|) + public var t:Number = 0; + + public function copy(source:RayIntersection):void { + primitive = source.primitive; + pos.vCopy(source.pos); + normal.vCopy(source.normal); + t = source.t; + } + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/collision/types/BoundBox.as b/0.0.9.3/src/alternativa/physics/collision/types/BoundBox.as new file mode 100644 index 0000000..b4d5278 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/types/BoundBox.as @@ -0,0 +1,75 @@ +package alternativa.physics.collision.types { + + /** + * Ограничивающий параллелепипед. + */ + public class BoundBox { + + public var minX:Number = 1e308; + public var minY:Number = 1e308; + public var minZ:Number = 1e308; + public var maxX:Number = -1e308; + public var maxY:Number = -1e308; + public var maxZ:Number = -1e308; + + public function setSize(minX:Number, minY:Number, minZ:Number, maxX:Number, maxY:Number, maxZ:Number):void { + this.minX = minX; + this.minY = minY; + this.minZ = minZ; + this.maxX = maxX; + this.maxY = maxY; + this.maxZ = maxZ; + } + + public function addBoundBox(boundBox:BoundBox):void { + minX = (boundBox.minX < minX) ? boundBox.minX : minX; + minY = (boundBox.minY < minY) ? boundBox.minY : minY; + minZ = (boundBox.minZ < minZ) ? boundBox.minZ : minZ; + maxX = (boundBox.maxX > maxX) ? boundBox.maxX : maxX; + maxY = (boundBox.maxY > maxY) ? boundBox.maxY : maxY; + maxZ = (boundBox.maxZ > maxZ) ? boundBox.maxZ : maxZ; + } + + public function addPoint(x:Number, y:Number, z:Number):void { + if (x < minX) minX = x; + if (x > maxX) maxX = x; + if (y < minY) minY = y; + if (y > maxY) maxY = y; + if (z < minZ) minZ = z; + if (z > maxZ) maxZ = z; + } + + public function infinity():void { + minX = 1e308; + minY = 1e308; + minZ = 1e308; + maxX = -1e308; + maxY = -1e308; + maxZ = -1e308; + } + + public function intersects(bb:BoundBox, epsilon:Number):Boolean { + return !(minX > bb.maxX + epsilon || maxX < bb.minX - epsilon || minY > bb.maxY + epsilon || maxY < bb.minY - epsilon || minZ > bb.maxZ + epsilon || maxZ < bb.minZ - epsilon); + } + + public function copyFrom(boundBox:BoundBox):void { + minX = boundBox.minX; + minY = boundBox.minY; + minZ = boundBox.minZ; + maxX = boundBox.maxX; + maxY = boundBox.maxY; + maxZ = boundBox.maxZ; + } + + public function clone():BoundBox { + var clone:BoundBox = new BoundBox(); + clone.copyFrom(this); + return clone; + } + + public function toString():String { + return "BoundBox [" + minX + ", " + minY + ", " + minZ + " : " + maxX + ", " + maxY + ", " + maxZ + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/collision/types/Ray.as b/0.0.9.3/src/alternativa/physics/collision/types/Ray.as new file mode 100644 index 0000000..fb2ffbc --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/types/Ray.as @@ -0,0 +1,13 @@ +package alternativa.physics.collision.types { + import alternativa.math.Vector3; + + /** + * Структура, описывающая луч и сегмент прямой в пространстве. + */ + public class Ray { + // Начало луча. Начальная точка сегмента на прямой луча. + public var origin:Vector3 = new Vector3(); + // Направляющий вектор луча. Длина вектора задаёт длину сегмента на луче. Конечная точка сегмента вычисляется как origin + dir. + public var dir:Vector3 = new Vector3(); + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/collision/types/RayIntersection.as b/0.0.9.3/src/alternativa/physics/collision/types/RayIntersection.as new file mode 100644 index 0000000..21e9290 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/collision/types/RayIntersection.as @@ -0,0 +1,25 @@ +package alternativa.physics.collision.types { + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.math.Vector3; + + /** + * Структура описывает точку пересечения луча с физической геометрией. + */ + public class RayIntersection { + // Физический примитив, с которым пересекается луч + public var primitive:CollisionPrimitive; + // Положенеи точки пересечения + public var pos:Vector3 = new Vector3(); + // Нормаль поверхности физического примитива в точке пересечения + public var normal:Vector3 = new Vector3(); + // Время пересечения (|pos - ray.origin|/|ray.dir|) + public var t:Number = 0; + + public function copy(source:RayIntersection):void { + primitive = source.primitive; + pos.vCopy(source.pos); + normal.vCopy(source.normal); + t = source.t; + } + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/constraints/.svn/all-wcprops b/0.0.9.3/src/alternativa/physics/constraints/.svn/all-wcprops new file mode 100644 index 0000000..d701343 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/constraints/.svn/all-wcprops @@ -0,0 +1,17 @@ +K 25 +svn:wc:ra_dav:version-url +V 115 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/constraints +END +Constraint.as +K 25 +svn:wc:ra_dav:version-url +V 129 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/constraints/Constraint.as +END +MaxDistanceConstraint.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/constraints/MaxDistanceConstraint.as +END diff --git a/0.0.9.3/src/alternativa/physics/constraints/.svn/entries b/0.0.9.3/src/alternativa/physics/constraints/.svn/entries new file mode 100644 index 0000000..f7d6f94 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/constraints/.svn/entries @@ -0,0 +1,52 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/constraints +http://svndev.alternativaplatform.com + + + +2010-01-20T17:53:18.555251Z +26849 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Constraint.as +file + + + + +2010-10-28T04:32:44.000000Z +02bdbd40eda515549956d6393551a63b +2010-01-20T17:53:18.555251Z +26849 +mike + +MaxDistanceConstraint.as +file + + + + +2010-10-28T04:32:44.000000Z +dbf792986b9dd7e2a1c720cf4a578f42 +2009-11-24T08:44:15.941480Z +23758 +mike + diff --git a/0.0.9.3/src/alternativa/physics/constraints/.svn/format b/0.0.9.3/src/alternativa/physics/constraints/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/constraints/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.3/src/alternativa/physics/constraints/.svn/text-base/Constraint.as.svn-base b/0.0.9.3/src/alternativa/physics/constraints/.svn/text-base/Constraint.as.svn-base new file mode 100644 index 0000000..8607442 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/constraints/.svn/text-base/Constraint.as.svn-base @@ -0,0 +1,35 @@ +package alternativa.physics.constraints { + + import alternativa.physics.PhysicsScene; + import alternativa.physics.altphysics; + + use namespace altphysics; + + /** + * + */ + public class Constraint { + + altphysics var satisfied:Boolean; + altphysics var world:PhysicsScene; + + /** + * + */ + public function Constraint() { + } + + /** + * @param dt + */ + public function preProcess(dt:Number):void { + } + + /** + * @param dt + */ + public function apply(dt:Number):void { + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base b/0.0.9.3/src/alternativa/physics/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base new file mode 100644 index 0000000..d29f518 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base @@ -0,0 +1,183 @@ +package alternativa.physics.constraints { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * + */ + public class MaxDistanceConstraint extends Constraint { + // Первое тело ограничения + altphysics var body1:Body; + // Второе тело ограничения. Может быть null. + altphysics var body2:Body; + // Первая точка крепления в системе координат первого тела + altphysics var r1:Vector3; + // Вторая точка привязки в системе координат второго тела. В случае отсутствия второго тела, координаты задаются в мировой системе. + altphysics var r2:Vector3; + // Максимально допустимое расстояние между точками крепления + altphysics var maxDistance:Number; + // Радиус-вектор первой точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr1:Vector3 = new Vector3(); + // Радиус-вектор второй точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr2:Vector3 = new Vector3(); + // Минимальная скорость сближения для удовлетворения ограничения + private var minClosingVel:Number; + // Изменение относительной скорости на единицу импульса + private var velByUnitImpulseN:Number; + // Направление корректирующего импульса + private var impulseDirection:Vector3 = new Vector3(); + + // Вспомогательные переменные +// private static var _v:Vector3 = new Vector3(); +// private static var _v1:Vector3 = new Vector3(); +// private static var _v2:Vector3 = new Vector3(); + + /** + * @param body1 + * @param body2 + * @param r1 + * @param r2 + * @param maxDistance + */ + public function MaxDistanceConstraint(body1:Body, body2:Body, r1:Vector3, r2:Vector3, maxDistance:Number) { + super(); + this.body1 = body1; + this.body2 = body2; + this.r1 = r1.vClone(); + this.r2 = r2.vClone(); + this.maxDistance = maxDistance; + } + + /** + * @param dt + */ + override public function preProcess(dt:Number):void { + // Вычислим расстояние между точками + var m:Matrix3 = body1.baseMatrix; + wr1.x = m.a*r1.x + m.b*r1.y + m.c*r1.z; + wr1.y = m.e*r1.x + m.f*r1.y + m.g*r1.z; + wr1.z = m.i*r1.x + m.j*r1.y + m.k*r1.z; + if (body2 != null) { + m = body2.baseMatrix; + wr2.x = m.a*r2.x + m.b*r2.y + m.c*r2.z; + wr2.y = m.e*r2.x + m.f*r2.y + m.g*r2.z; + wr2.z = m.i*r2.x + m.j*r2.y + m.k*r2.z; + } else { + wr2.x = r2.x; + wr2.y = r2.y; + wr2.z = r2.z; + } + var p1:Vector3 = body1.state.pos; + impulseDirection.x = wr2.x - wr1.x - p1.x; + impulseDirection.y = wr2.y - wr1.y - p1.y; + impulseDirection.z = wr2.z - wr1.z - p1.z; + if (body2 != null) { + var p2:Vector3 = body2.state.pos; + impulseDirection.x += p2.x; + impulseDirection.y += p2.y; + impulseDirection.z += p2.z; + } + + var len:Number = Math.sqrt(impulseDirection.x*impulseDirection.x + impulseDirection.y*impulseDirection.y + impulseDirection.z*impulseDirection.z); + var delta:Number = len - maxDistance; + if (delta > 0) { + satisfied = false; + if (len < 0.001) { + impulseDirection.x = 1; + } else { + len = 1/len; + impulseDirection.x *= len; + impulseDirection.y *= len; + impulseDirection.z *= len; + } + minClosingVel = delta/(world.penResolutionSteps*dt); + if (minClosingVel > world.maxPenResolutionSpeed) { + minClosingVel = world.maxPenResolutionSpeed; + } + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + var x:Number; + var y:Number; + var z:Number; + var vx:Number; + var vy:Number; + var vz:Number; + velByUnitImpulseN = 0; + if (body1.movable) { +// velByUnitImpulseN += body1.invMass + _v.vCross2(wr1, impulseDirection).vTransformBy3(body1.invInertiaWorld).vCross(wr1).vDot(impulseDirection); + vx = wr1.y*impulseDirection.z - wr1.z*impulseDirection.y; + vy = wr1.z*impulseDirection.x - wr1.x*impulseDirection.z; + vz = wr1.x*impulseDirection.y - wr1.y*impulseDirection.x; + + m = body1.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr1.z - z*wr1.y; + vy = z*wr1.x - x*wr1.z; + vz = x*wr1.y - y*wr1.x; + velByUnitImpulseN += body1.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + if (body2 != null && body2.movable) { +// velByUnitImpulseN += body2.invMass + _v.vCross2(wr2, impulseDirection).vTransformBy3(body2.invInertiaWorld).vCross(wr2).vDot(impulseDirection); + vx = wr2.y*impulseDirection.z - wr2.z*impulseDirection.y; + vy = wr2.z*impulseDirection.x - wr2.x*impulseDirection.z; + vz = wr2.x*impulseDirection.y - wr2.y*impulseDirection.x; + + m = body2.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr2.z - z*wr2.y; + vy = z*wr2.x - x*wr2.z; + vz = x*wr2.y - y*wr2.x; + velByUnitImpulseN += body2.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + } else { + satisfied = true; + } + } + + /** + * @param dt + */ + override public function apply(dt:Number):void { + if (satisfied) return; + // Расчитываем проекцию скорости относительной скорости + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 + var vel:Vector3 = body1.state.velocity; + var rot:Vector3 = body1.state.rotation; +// v1Cross2(state.rotation, wr1); + var vx:Number = vel.x + rot.y*wr1.z - rot.z*wr1.y; + var vy:Number = vel.y + rot.z*wr1.x - rot.x*wr1.z; + var vz:Number = vel.z + rot.x*wr1.y - rot.y*wr1.x; + // V2 = V2_c + w2%r2 + if (body2 != null) { + vel = body2.state.velocity; + rot = body2.state.rotation; +// _v2.vCross2(state.rotation, wr2); + vx -= vel.x + rot.y*wr2.z - rot.z*wr2.y; + vy -= vel.y + rot.z*wr2.x - rot.x*wr2.z; + vz -= vel.z + rot.x*wr2.y - rot.y*wr2.x; + } + var closingVel:Number = vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + if (closingVel > minClosingVel) return; + var impulse:Number = (minClosingVel - closingVel)/velByUnitImpulseN; + if (body1.movable) { + body1.applyRelPosWorldImpulse(wr1, impulseDirection, impulse); + } + if (body2 != null && body2.movable) { + body2.applyRelPosWorldImpulse(wr2, impulseDirection, -impulse); + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/constraints/Constraint.as b/0.0.9.3/src/alternativa/physics/constraints/Constraint.as new file mode 100644 index 0000000..8607442 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/constraints/Constraint.as @@ -0,0 +1,35 @@ +package alternativa.physics.constraints { + + import alternativa.physics.PhysicsScene; + import alternativa.physics.altphysics; + + use namespace altphysics; + + /** + * + */ + public class Constraint { + + altphysics var satisfied:Boolean; + altphysics var world:PhysicsScene; + + /** + * + */ + public function Constraint() { + } + + /** + * @param dt + */ + public function preProcess(dt:Number):void { + } + + /** + * @param dt + */ + public function apply(dt:Number):void { + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/constraints/MaxDistanceConstraint.as b/0.0.9.3/src/alternativa/physics/constraints/MaxDistanceConstraint.as new file mode 100644 index 0000000..d29f518 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/constraints/MaxDistanceConstraint.as @@ -0,0 +1,183 @@ +package alternativa.physics.constraints { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * + */ + public class MaxDistanceConstraint extends Constraint { + // Первое тело ограничения + altphysics var body1:Body; + // Второе тело ограничения. Может быть null. + altphysics var body2:Body; + // Первая точка крепления в системе координат первого тела + altphysics var r1:Vector3; + // Вторая точка привязки в системе координат второго тела. В случае отсутствия второго тела, координаты задаются в мировой системе. + altphysics var r2:Vector3; + // Максимально допустимое расстояние между точками крепления + altphysics var maxDistance:Number; + // Радиус-вектор первой точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr1:Vector3 = new Vector3(); + // Радиус-вектор второй точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr2:Vector3 = new Vector3(); + // Минимальная скорость сближения для удовлетворения ограничения + private var minClosingVel:Number; + // Изменение относительной скорости на единицу импульса + private var velByUnitImpulseN:Number; + // Направление корректирующего импульса + private var impulseDirection:Vector3 = new Vector3(); + + // Вспомогательные переменные +// private static var _v:Vector3 = new Vector3(); +// private static var _v1:Vector3 = new Vector3(); +// private static var _v2:Vector3 = new Vector3(); + + /** + * @param body1 + * @param body2 + * @param r1 + * @param r2 + * @param maxDistance + */ + public function MaxDistanceConstraint(body1:Body, body2:Body, r1:Vector3, r2:Vector3, maxDistance:Number) { + super(); + this.body1 = body1; + this.body2 = body2; + this.r1 = r1.vClone(); + this.r2 = r2.vClone(); + this.maxDistance = maxDistance; + } + + /** + * @param dt + */ + override public function preProcess(dt:Number):void { + // Вычислим расстояние между точками + var m:Matrix3 = body1.baseMatrix; + wr1.x = m.a*r1.x + m.b*r1.y + m.c*r1.z; + wr1.y = m.e*r1.x + m.f*r1.y + m.g*r1.z; + wr1.z = m.i*r1.x + m.j*r1.y + m.k*r1.z; + if (body2 != null) { + m = body2.baseMatrix; + wr2.x = m.a*r2.x + m.b*r2.y + m.c*r2.z; + wr2.y = m.e*r2.x + m.f*r2.y + m.g*r2.z; + wr2.z = m.i*r2.x + m.j*r2.y + m.k*r2.z; + } else { + wr2.x = r2.x; + wr2.y = r2.y; + wr2.z = r2.z; + } + var p1:Vector3 = body1.state.pos; + impulseDirection.x = wr2.x - wr1.x - p1.x; + impulseDirection.y = wr2.y - wr1.y - p1.y; + impulseDirection.z = wr2.z - wr1.z - p1.z; + if (body2 != null) { + var p2:Vector3 = body2.state.pos; + impulseDirection.x += p2.x; + impulseDirection.y += p2.y; + impulseDirection.z += p2.z; + } + + var len:Number = Math.sqrt(impulseDirection.x*impulseDirection.x + impulseDirection.y*impulseDirection.y + impulseDirection.z*impulseDirection.z); + var delta:Number = len - maxDistance; + if (delta > 0) { + satisfied = false; + if (len < 0.001) { + impulseDirection.x = 1; + } else { + len = 1/len; + impulseDirection.x *= len; + impulseDirection.y *= len; + impulseDirection.z *= len; + } + minClosingVel = delta/(world.penResolutionSteps*dt); + if (minClosingVel > world.maxPenResolutionSpeed) { + minClosingVel = world.maxPenResolutionSpeed; + } + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + var x:Number; + var y:Number; + var z:Number; + var vx:Number; + var vy:Number; + var vz:Number; + velByUnitImpulseN = 0; + if (body1.movable) { +// velByUnitImpulseN += body1.invMass + _v.vCross2(wr1, impulseDirection).vTransformBy3(body1.invInertiaWorld).vCross(wr1).vDot(impulseDirection); + vx = wr1.y*impulseDirection.z - wr1.z*impulseDirection.y; + vy = wr1.z*impulseDirection.x - wr1.x*impulseDirection.z; + vz = wr1.x*impulseDirection.y - wr1.y*impulseDirection.x; + + m = body1.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr1.z - z*wr1.y; + vy = z*wr1.x - x*wr1.z; + vz = x*wr1.y - y*wr1.x; + velByUnitImpulseN += body1.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + if (body2 != null && body2.movable) { +// velByUnitImpulseN += body2.invMass + _v.vCross2(wr2, impulseDirection).vTransformBy3(body2.invInertiaWorld).vCross(wr2).vDot(impulseDirection); + vx = wr2.y*impulseDirection.z - wr2.z*impulseDirection.y; + vy = wr2.z*impulseDirection.x - wr2.x*impulseDirection.z; + vz = wr2.x*impulseDirection.y - wr2.y*impulseDirection.x; + + m = body2.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr2.z - z*wr2.y; + vy = z*wr2.x - x*wr2.z; + vz = x*wr2.y - y*wr2.x; + velByUnitImpulseN += body2.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + } else { + satisfied = true; + } + } + + /** + * @param dt + */ + override public function apply(dt:Number):void { + if (satisfied) return; + // Расчитываем проекцию скорости относительной скорости + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 + var vel:Vector3 = body1.state.velocity; + var rot:Vector3 = body1.state.rotation; +// v1Cross2(state.rotation, wr1); + var vx:Number = vel.x + rot.y*wr1.z - rot.z*wr1.y; + var vy:Number = vel.y + rot.z*wr1.x - rot.x*wr1.z; + var vz:Number = vel.z + rot.x*wr1.y - rot.y*wr1.x; + // V2 = V2_c + w2%r2 + if (body2 != null) { + vel = body2.state.velocity; + rot = body2.state.rotation; +// _v2.vCross2(state.rotation, wr2); + vx -= vel.x + rot.y*wr2.z - rot.z*wr2.y; + vy -= vel.y + rot.z*wr2.x - rot.x*wr2.z; + vz -= vel.z + rot.x*wr2.y - rot.y*wr2.x; + } + var closingVel:Number = vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + if (closingVel > minClosingVel) return; + var impulse:Number = (minClosingVel - closingVel)/velByUnitImpulseN; + if (body1.movable) { + body1.applyRelPosWorldImpulse(wr1, impulseDirection, impulse); + } + if (body2 != null && body2.movable) { + body2.applyRelPosWorldImpulse(wr2, impulseDirection, -impulse); + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/primitives/.svn/all-wcprops b/0.0.9.3/src/alternativa/physics/primitives/.svn/all-wcprops new file mode 100644 index 0000000..be913a2 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/primitives/.svn/all-wcprops @@ -0,0 +1,35 @@ +K 25 +svn:wc:ra_dav:version-url +V 114 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/primitives +END +RigidPlane.as +K 25 +svn:wc:ra_dav:version-url +V 128 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/primitives/RigidPlane.as +END +RigidSphere.as +K 25 +svn:wc:ra_dav:version-url +V 129 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/primitives/RigidSphere.as +END +RigidBox.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/primitives/RigidBox.as +END +RigidCylinder.as +K 25 +svn:wc:ra_dav:version-url +V 131 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/primitives/RigidCylinder.as +END +RigidRect.as +K 25 +svn:wc:ra_dav:version-url +V 127 +/!svn/ver/36823/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/primitives/RigidRect.as +END diff --git a/0.0.9.3/src/alternativa/physics/primitives/.svn/entries b/0.0.9.3/src/alternativa/physics/primitives/.svn/entries new file mode 100644 index 0000000..04dc647 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/primitives/.svn/entries @@ -0,0 +1,88 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.3/src/alternativa/physics/primitives +http://svndev.alternativaplatform.com + + + +2009-11-24T08:44:15.941480Z +23758 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +RigidPlane.as +file + + + + +2010-10-28T04:32:44.000000Z +8faf607cf544d7388852668f5cd5672f +2009-11-24T08:44:15.941480Z +23758 +mike + +RigidSphere.as +file + + + + +2010-10-28T04:32:44.000000Z +774b0c23b084bfffb3381eb40e616deb +2009-11-24T08:44:15.941480Z +23758 +mike + +RigidBox.as +file + + + + +2010-10-28T04:32:44.000000Z +7b93f349a2818f22b76d9590cb05f84a +2009-11-24T08:44:15.941480Z +23758 +mike + +RigidCylinder.as +file + + + + +2010-10-28T04:32:44.000000Z +e9ce0eeb457428727cc10d2e53a0f164 +2009-11-24T08:44:15.941480Z +23758 +mike + +RigidRect.as +file + + + + +2010-10-28T04:32:44.000000Z +2eb6d0ebde8c2b46c4b0f810b1d64e1e +2009-11-24T08:44:15.941480Z +23758 +mike + diff --git a/0.0.9.3/src/alternativa/physics/primitives/.svn/format b/0.0.9.3/src/alternativa/physics/primitives/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/primitives/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.3/src/alternativa/physics/primitives/.svn/text-base/RigidBox.as.svn-base b/0.0.9.3/src/alternativa/physics/primitives/.svn/text-base/RigidBox.as.svn-base new file mode 100644 index 0000000..24e9b9f --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/primitives/.svn/text-base/RigidBox.as.svn-base @@ -0,0 +1,56 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * + */ + public class RigidBox extends Body { + + /** + * + * @param halfSize + * @param mass + */ + public function RigidBox(halfSize:Vector3, mass:Number) { + super(0, Matrix3.ZERO); + addCollisionPrimitive(new CollisionBox(halfSize, 1)); + setParams(halfSize, mass); + } + + /** + * @param halfSize + * @param mass + */ + public function setParams(halfSize:Vector3, mass:Number):void { + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + invInertia.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + invInertia.a = 3*invMass/(yy + zz); + invInertia.f = 3*invMass/(zz + xx); + invInertia.k = 3*invMass/(xx + yy); + } + var prim:CollisionBox = CollisionBox(collisionPrimitives.head.primitive); + prim.hs.vCopy(halfSize); + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/primitives/.svn/text-base/RigidCylinder.as.svn-base b/0.0.9.3/src/alternativa/physics/primitives/.svn/text-base/RigidCylinder.as.svn-base new file mode 100644 index 0000000..4ff65dd --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/primitives/.svn/text-base/RigidCylinder.as.svn-base @@ -0,0 +1,32 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.math.Matrix3; + + use namespace altphysics; + + public class RigidCylinder extends Body { + + altphysics var r:Number; + altphysics var h:Number; + + public function RigidCylinder(mass:Number, radius:Number, height:Number) { + super(0, Matrix3.ZERO); + setParams(mass, radius, height); + } + + public function setParams(mass:Number, radius:Number, height:Number):void { + r = radius; + h = height; + invInertiaWorld.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + invInertia.a = invInertia.f = mass*(h*h/12 + r*r/4); + invInertia.k = 0.5*mass*r*r; + invInertia.invert(); + } + } + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/primitives/.svn/text-base/RigidPlane.as.svn-base b/0.0.9.3/src/alternativa/physics/primitives/.svn/text-base/RigidPlane.as.svn-base new file mode 100644 index 0000000..d9ebf9b --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/primitives/.svn/text-base/RigidPlane.as.svn-base @@ -0,0 +1,23 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + public class RigidPlane extends Body { + + public var normal:Vector3 = new Vector3(); + public var offset:Number; + + public function RigidPlane() { + super(0, Matrix3.ZERO); + this.normal.vCopy(Vector3.Z_AXIS); + this.offset = 0; + movable = false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/primitives/.svn/text-base/RigidRect.as.svn-base b/0.0.9.3/src/alternativa/physics/primitives/.svn/text-base/RigidRect.as.svn-base new file mode 100644 index 0000000..4591931 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/primitives/.svn/text-base/RigidRect.as.svn-base @@ -0,0 +1,20 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + public class RigidRect extends Body { + + public function RigidRect(halfWidth:Number, halfLength:Number) { + super(0, Matrix3.ZERO); + movable = false; + addCollisionPrimitive(new CollisionRect(new Vector3(halfWidth, halfLength, 0), 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/primitives/.svn/text-base/RigidSphere.as.svn-base b/0.0.9.3/src/alternativa/physics/primitives/.svn/text-base/RigidSphere.as.svn-base new file mode 100644 index 0000000..8ad180c --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/primitives/.svn/text-base/RigidSphere.as.svn-base @@ -0,0 +1,29 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.math.Matrix3; + + use namespace altphysics; + + public class RigidSphere extends Body { + + public function RigidSphere(radius:Number, mass:Number) { + var m:Matrix3; + if (mass == Infinity) { + mass = 0; + m = Matrix3.ZERO; + } else { + mass = 1/mass; + var r2:Number = radius*radius; + m = new Matrix3(); + m.a = m.f = m.k = 2.5*mass/r2; + } + super(mass, m); + if (mass == 0) movable = false; + addCollisionPrimitive(new CollisionSphere(radius, 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/primitives/RigidBox.as b/0.0.9.3/src/alternativa/physics/primitives/RigidBox.as new file mode 100644 index 0000000..24e9b9f --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/primitives/RigidBox.as @@ -0,0 +1,56 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * + */ + public class RigidBox extends Body { + + /** + * + * @param halfSize + * @param mass + */ + public function RigidBox(halfSize:Vector3, mass:Number) { + super(0, Matrix3.ZERO); + addCollisionPrimitive(new CollisionBox(halfSize, 1)); + setParams(halfSize, mass); + } + + /** + * @param halfSize + * @param mass + */ + public function setParams(halfSize:Vector3, mass:Number):void { + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + invInertia.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + invInertia.a = 3*invMass/(yy + zz); + invInertia.f = 3*invMass/(zz + xx); + invInertia.k = 3*invMass/(xx + yy); + } + var prim:CollisionBox = CollisionBox(collisionPrimitives.head.primitive); + prim.hs.vCopy(halfSize); + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/primitives/RigidCylinder.as b/0.0.9.3/src/alternativa/physics/primitives/RigidCylinder.as new file mode 100644 index 0000000..4ff65dd --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/primitives/RigidCylinder.as @@ -0,0 +1,32 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.math.Matrix3; + + use namespace altphysics; + + public class RigidCylinder extends Body { + + altphysics var r:Number; + altphysics var h:Number; + + public function RigidCylinder(mass:Number, radius:Number, height:Number) { + super(0, Matrix3.ZERO); + setParams(mass, radius, height); + } + + public function setParams(mass:Number, radius:Number, height:Number):void { + r = radius; + h = height; + invInertiaWorld.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + invInertia.a = invInertia.f = mass*(h*h/12 + r*r/4); + invInertia.k = 0.5*mass*r*r; + invInertia.invert(); + } + } + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/primitives/RigidPlane.as b/0.0.9.3/src/alternativa/physics/primitives/RigidPlane.as new file mode 100644 index 0000000..d9ebf9b --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/primitives/RigidPlane.as @@ -0,0 +1,23 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + public class RigidPlane extends Body { + + public var normal:Vector3 = new Vector3(); + public var offset:Number; + + public function RigidPlane() { + super(0, Matrix3.ZERO); + this.normal.vCopy(Vector3.Z_AXIS); + this.offset = 0; + movable = false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/primitives/RigidRect.as b/0.0.9.3/src/alternativa/physics/primitives/RigidRect.as new file mode 100644 index 0000000..4591931 --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/primitives/RigidRect.as @@ -0,0 +1,20 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + public class RigidRect extends Body { + + public function RigidRect(halfWidth:Number, halfLength:Number) { + super(0, Matrix3.ZERO); + movable = false; + addCollisionPrimitive(new CollisionRect(new Vector3(halfWidth, halfLength, 0), 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.9.3/src/alternativa/physics/primitives/RigidSphere.as b/0.0.9.3/src/alternativa/physics/primitives/RigidSphere.as new file mode 100644 index 0000000..8ad180c --- /dev/null +++ b/0.0.9.3/src/alternativa/physics/primitives/RigidSphere.as @@ -0,0 +1,29 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.math.Matrix3; + + use namespace altphysics; + + public class RigidSphere extends Body { + + public function RigidSphere(radius:Number, mass:Number) { + var m:Matrix3; + if (mass == Infinity) { + mass = 0; + m = Matrix3.ZERO; + } else { + mass = 1/mass; + var r2:Number = radius*radius; + m = new Matrix3(); + m.a = m.f = m.k = 2.5*mass/r2; + } + super(mass, m); + if (mass == 0) movable = false; + addCollisionPrimitive(new CollisionSphere(radius, 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/.actionScriptProperties b/0.0.9.4/.actionScriptProperties new file mode 100644 index 0000000..f73adc8 --- /dev/null +++ b/0.0.9.4/.actionScriptProperties @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/0.0.9.4/.flexLibProperties b/0.0.9.4/.flexLibProperties new file mode 100644 index 0000000..0b65396 --- /dev/null +++ b/0.0.9.4/.flexLibProperties @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/0.0.9.4/.project b/0.0.9.4/.project new file mode 100644 index 0000000..1f1425f --- /dev/null +++ b/0.0.9.4/.project @@ -0,0 +1,19 @@ + + + AlternativaPhysics + + + + + + com.adobe.flexbuilder.project.flexbuilder + + + + + + org.maven.ide.eclipse.maven2Nature + com.adobe.flexbuilder.project.flexlibnature + com.adobe.flexbuilder.project.actionscriptnature + + diff --git a/0.0.9.4/.settings/.svn/all-wcprops b/0.0.9.4/.settings/.svn/all-wcprops new file mode 100644 index 0000000..c865a76 --- /dev/null +++ b/0.0.9.4/.settings/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 89 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/.settings +END +org.eclipse.core.resources.prefs +K 25 +svn:wc:ra_dav:version-url +V 122 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/.settings/org.eclipse.core.resources.prefs +END diff --git a/0.0.9.4/.settings/.svn/entries b/0.0.9.4/.settings/.svn/entries new file mode 100644 index 0000000..0e5df61 --- /dev/null +++ b/0.0.9.4/.settings/.svn/entries @@ -0,0 +1,40 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/.settings +http://svndev.alternativaplatform.com + + + +2009-04-01T12:25:29.638016Z +10301 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +org.eclipse.core.resources.prefs +file + + + + +2010-10-28T04:32:45.000000Z +4f70b476f3e5075e399505021df2f89b +2009-04-01T12:25:29.638016Z +10301 +mike + diff --git a/0.0.9.4/.settings/.svn/format b/0.0.9.4/.settings/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.4/.settings/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.4/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base b/0.0.9.4/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base new file mode 100644 index 0000000..b3d0d49 --- /dev/null +++ b/0.0.9.4/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base @@ -0,0 +1,3 @@ +#Fri Mar 20 00:19:33 YEKT 2009 +eclipse.preferences.version=1 +encoding/=UTF-8 diff --git a/0.0.9.4/.settings/org.eclipse.core.resources.prefs b/0.0.9.4/.settings/org.eclipse.core.resources.prefs new file mode 100644 index 0000000..b3d0d49 --- /dev/null +++ b/0.0.9.4/.settings/org.eclipse.core.resources.prefs @@ -0,0 +1,3 @@ +#Fri Mar 20 00:19:33 YEKT 2009 +eclipse.preferences.version=1 +encoding/=UTF-8 diff --git a/0.0.9.4/.svn/all-wcprops b/0.0.9.4/.svn/all-wcprops new file mode 100644 index 0000000..717e516 --- /dev/null +++ b/0.0.9.4/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 79 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4 +END +.flexLibProperties +K 25 +svn:wc:ra_dav:version-url +V 98 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/.flexLibProperties +END +.project +K 25 +svn:wc:ra_dav:version-url +V 88 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/.project +END +pom.xml +K 25 +svn:wc:ra_dav:version-url +V 87 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/pom.xml +END +.actionScriptProperties +K 25 +svn:wc:ra_dav:version-url +V 103 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/.actionScriptProperties +END diff --git a/0.0.9.4/.svn/dir-prop-base b/0.0.9.4/.svn/dir-prop-base new file mode 100644 index 0000000..c31ca28 --- /dev/null +++ b/0.0.9.4/.svn/dir-prop-base @@ -0,0 +1,7 @@ +K 13 +svn:mergeinfo +V 278 +/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized:19806-19860 +/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.7.0.LinkedList:20360-22217,22226-22252 +/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D:13892-14098 +END diff --git a/0.0.9.4/.svn/entries b/0.0.9.4/.svn/entries new file mode 100644 index 0000000..ae86bff --- /dev/null +++ b/0.0.9.4/.svn/entries @@ -0,0 +1,85 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4 +http://svndev.alternativaplatform.com + + + +2010-08-19T11:03:11.756670Z +39557 +mike +has-props + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +META-INF +dir + +.flexLibProperties +file + + + + +2010-10-28T04:32:45.000000Z +b170dccb732dcb12462ac421cdca9723 +2010-02-12T12:34:25.192477Z +28424 +mike + +.project +file + + + + +2010-10-28T04:32:45.000000Z +1e90cc68b52b93173b2b5de88eb5c3a3 +2009-04-20T17:15:55.260110Z +11541 +mike + +src +dir + +pom.xml +file + + + + +2010-10-28T04:32:45.000000Z +b9f3cb4f65eeff3193e82d68ff2ef681 +2010-08-19T11:03:11.756670Z +39557 +mike + +.actionScriptProperties +file + + + + +2010-10-28T04:32:45.000000Z +b8656f882fff2baedd57a92db86fcf8c +2010-01-14T07:05:07.823716Z +26358 +mike + +.settings +dir + diff --git a/0.0.9.4/.svn/format b/0.0.9.4/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.4/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.4/.svn/text-base/.actionScriptProperties.svn-base b/0.0.9.4/.svn/text-base/.actionScriptProperties.svn-base new file mode 100644 index 0000000..f73adc8 --- /dev/null +++ b/0.0.9.4/.svn/text-base/.actionScriptProperties.svn-base @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/0.0.9.4/.svn/text-base/.flexLibProperties.svn-base b/0.0.9.4/.svn/text-base/.flexLibProperties.svn-base new file mode 100644 index 0000000..0b65396 --- /dev/null +++ b/0.0.9.4/.svn/text-base/.flexLibProperties.svn-base @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/0.0.9.4/.svn/text-base/.project.svn-base b/0.0.9.4/.svn/text-base/.project.svn-base new file mode 100644 index 0000000..1f1425f --- /dev/null +++ b/0.0.9.4/.svn/text-base/.project.svn-base @@ -0,0 +1,19 @@ + + + AlternativaPhysics + + + + + + com.adobe.flexbuilder.project.flexbuilder + + + + + + org.maven.ide.eclipse.maven2Nature + com.adobe.flexbuilder.project.flexlibnature + com.adobe.flexbuilder.project.actionscriptnature + + diff --git a/0.0.9.4/.svn/text-base/pom.xml.svn-base b/0.0.9.4/.svn/text-base/pom.xml.svn-base new file mode 100644 index 0000000..250f16f --- /dev/null +++ b/0.0.9.4/.svn/text-base/pom.xml.svn-base @@ -0,0 +1,17 @@ + + 4.0.0 + platform.clients.fp10.libraries + AlternativaPhysics + swc + 0.0.9.4 + + platform.tools.maven + FlashBasePom + 2.0.3.0 + + + scm:svn:http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4 + + + + diff --git a/0.0.9.4/META-INF/.svn/all-wcprops b/0.0.9.4/META-INF/.svn/all-wcprops new file mode 100644 index 0000000..ae9fa45 --- /dev/null +++ b/0.0.9.4/META-INF/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 88 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/META-INF +END +MANIFEST.MF +K 25 +svn:wc:ra_dav:version-url +V 100 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/META-INF/MANIFEST.MF +END diff --git a/0.0.9.4/META-INF/.svn/entries b/0.0.9.4/META-INF/.svn/entries new file mode 100644 index 0000000..b4282e8 --- /dev/null +++ b/0.0.9.4/META-INF/.svn/entries @@ -0,0 +1,40 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/META-INF +http://svndev.alternativaplatform.com + + + +2010-03-06T18:33:59.318892Z +29409 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +MANIFEST.MF +file + + + + +2010-10-28T04:32:44.000000Z +7b64dceb50a6905850ff00a0bfbe77eb +2009-04-20T17:15:55.260110Z +11541 +mike + diff --git a/0.0.9.4/META-INF/.svn/format b/0.0.9.4/META-INF/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.4/META-INF/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.4/META-INF/.svn/text-base/MANIFEST.MF.svn-base b/0.0.9.4/META-INF/.svn/text-base/MANIFEST.MF.svn-base new file mode 100644 index 0000000..5066a55 --- /dev/null +++ b/0.0.9.4/META-INF/.svn/text-base/MANIFEST.MF.svn-base @@ -0,0 +1 @@ +Bundle-Name: platform.clients.fp10.libraries.AlternativaPhysics diff --git a/0.0.9.4/META-INF/MANIFEST.MF b/0.0.9.4/META-INF/MANIFEST.MF new file mode 100644 index 0000000..5066a55 --- /dev/null +++ b/0.0.9.4/META-INF/MANIFEST.MF @@ -0,0 +1 @@ +Bundle-Name: platform.clients.fp10.libraries.AlternativaPhysics diff --git a/0.0.9.4/pom.xml b/0.0.9.4/pom.xml new file mode 100644 index 0000000..250f16f --- /dev/null +++ b/0.0.9.4/pom.xml @@ -0,0 +1,17 @@ + + 4.0.0 + platform.clients.fp10.libraries + AlternativaPhysics + swc + 0.0.9.4 + + platform.tools.maven + FlashBasePom + 2.0.3.0 + + + scm:svn:http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4 + + + + diff --git a/0.0.9.4/src/.svn/all-wcprops b/0.0.9.4/src/.svn/all-wcprops new file mode 100644 index 0000000..fea6c27 --- /dev/null +++ b/0.0.9.4/src/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 83 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src +END diff --git a/0.0.9.4/src/.svn/entries b/0.0.9.4/src/.svn/entries new file mode 100644 index 0000000..6978e66 --- /dev/null +++ b/0.0.9.4/src/.svn/entries @@ -0,0 +1,31 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src +http://svndev.alternativaplatform.com + + + +2010-08-19T11:03:11.756670Z +39557 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +alternativa +dir + diff --git a/0.0.9.4/src/.svn/format b/0.0.9.4/src/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.4/src/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.4/src/alternativa/.svn/all-wcprops b/0.0.9.4/src/alternativa/.svn/all-wcprops new file mode 100644 index 0000000..cf76a57 --- /dev/null +++ b/0.0.9.4/src/alternativa/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 95 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa +END diff --git a/0.0.9.4/src/alternativa/.svn/entries b/0.0.9.4/src/alternativa/.svn/entries new file mode 100644 index 0000000..1ecdb4c --- /dev/null +++ b/0.0.9.4/src/alternativa/.svn/entries @@ -0,0 +1,34 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa +http://svndev.alternativaplatform.com + + + +2010-08-19T11:03:11.756670Z +39557 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +physics +dir + +math +dir + diff --git a/0.0.9.4/src/alternativa/.svn/format b/0.0.9.4/src/alternativa/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.4/src/alternativa/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.4/src/alternativa/math/.svn/all-wcprops b/0.0.9.4/src/alternativa/math/.svn/all-wcprops new file mode 100644 index 0000000..b51c8e3 --- /dev/null +++ b/0.0.9.4/src/alternativa/math/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 100 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/math +END +Quaternion.as +K 25 +svn:wc:ra_dav:version-url +V 114 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/math/Quaternion.as +END +Vector3.as +K 25 +svn:wc:ra_dav:version-url +V 111 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/math/Vector3.as +END +Matrix3.as +K 25 +svn:wc:ra_dav:version-url +V 111 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/math/Matrix3.as +END +Matrix4.as +K 25 +svn:wc:ra_dav:version-url +V 111 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/math/Matrix4.as +END diff --git a/0.0.9.4/src/alternativa/math/.svn/entries b/0.0.9.4/src/alternativa/math/.svn/entries new file mode 100644 index 0000000..9ee6be7 --- /dev/null +++ b/0.0.9.4/src/alternativa/math/.svn/entries @@ -0,0 +1,76 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/math +http://svndev.alternativaplatform.com + + + +2010-08-19T11:03:11.756670Z +39557 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Quaternion.as +file + + + + +2010-10-28T04:32:45.000000Z +7332c9620420bd10062d913decfb9501 +2010-02-12T12:34:25.192477Z +28424 +mike + +Vector3.as +file + + + + +2010-10-28T04:32:45.000000Z +a1571b527786783cef1a9f56db3f2af6 +2010-02-12T12:34:25.192477Z +28424 +mike + +Matrix3.as +file + + + + +2010-10-28T04:32:45.000000Z +0054bd91fe694474a28c8ec17697292b +2010-02-12T12:34:25.192477Z +28424 +mike + +Matrix4.as +file + + + + +2010-10-28T04:32:45.000000Z +8262d985e77059ee6d5f3be367ba08ee +2010-08-19T11:03:11.756670Z +39557 +mike + diff --git a/0.0.9.4/src/alternativa/math/.svn/format b/0.0.9.4/src/alternativa/math/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.4/src/alternativa/math/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.4/src/alternativa/math/.svn/text-base/Matrix3.as.svn-base b/0.0.9.4/src/alternativa/math/.svn/text-base/Matrix3.as.svn-base new file mode 100644 index 0000000..244cc98 --- /dev/null +++ b/0.0.9.4/src/alternativa/math/.svn/text-base/Matrix3.as.svn-base @@ -0,0 +1,390 @@ +package alternativa.math { + import flash.geom.Vector3D; + + /** + * + */ + public class Matrix3 { + + public static const ZERO:Matrix3 = new Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0); + public static const IDENTITY:Matrix3 = new Matrix3(); + + public var a:Number; + public var b:Number; + public var c:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix3(a:Number = 1, b:Number = 0, c:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1) { + this.a = a; + this.b = b; + this.c = c; + + this.e = e; + this.f = f; + this.g = g; + + this.i = i; + this.j = j; + this.k = k; + } + + /** + * + * @return + */ + public function toIdentity():Matrix3 { + a = f = k = 1; + b = c = e = g = i = j = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + var det:Number = 1/(-cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk); + + a = (ff*kk - gg*jj)*det; + b = (cc*jj - bb*kk)*det; + c = (bb*gg - cc*ff)*det; + e = (gg*ii - ee*kk)*det; + f = (aa*kk - cc*ii)*det; + g = (cc*ee - aa*gg)*det; + i = (ee*jj - ff*ii)*det; + j = (bb*ii - aa*jj)*det; + k = (aa*ff - bb*ee)*det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prependTransposed(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.b + cc*m.c; + b = aa*m.e + bb*m.f + cc*m.g; + c = aa*m.i + bb*m.j + cc*m.k; + e = ee*m.a + ff*m.b + gg*m.c; + f = ee*m.e + ff*m.f + gg*m.g; + g = ee*m.i + ff*m.j + gg*m.k; + i = ii*m.a + jj*m.b + kk*m.c; + j = ii*m.e + jj*m.f + kk*m.g; + k = ii*m.i + jj*m.j + kk*m.k; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix3):Matrix3 { + a += m.a; + b += m.b; + c += m.c; + e += m.e; + f += m.f; + g += m.g; + i += m.i; + j += m.j; + k += m.k; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix3):Matrix3 { + a -= m.a; + b -= m.b; + c -= m.c; + e -= m.e; + f -= m.f; + g -= m.g; + i -= m.i; + j -= m.j; + k -= m.k; + + return this; + } + + /** + * + * @return + */ + public function transpose():Matrix3 { + var tmp:Number = b; + b = e; + e = tmp; + tmp = c; + c = i; + i = tmp; + tmp = g; + g = j; + j = tmp; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector3To3D(vin:Vector3, vout:Vector3D):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * + * @param matrix + */ + public function createSkewSymmetric(v:Vector3):Matrix3 { + a = f = k = 0; + b = -v.z; + c = v.y; + e = v.z; + g = -v.x; + i = -v.y; + j = v.x; + return this; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix3):Matrix3 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @return + */ + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):Matrix3 { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + + return this; + } + + /** + * Метод формирует матрицу поворота на заданный угол относительно заданной оси. + * + * @param axis нормализованный вектор, задающий ось, относительно которой выполняется поворот + * @param angle угол поворота в радианах + */ + public function fromAxisAngle(axis:Vector3, angle:Number):void { + var c1:Number = Math.cos(angle); + var s:Number = Math.sin(angle); + var t:Number = 1 - c1; + var x:Number = axis.x; + var y:Number = axis.y; + var z:Number = axis.z; + + a = t*x*x + c1; + b = t*x*y - z*s; + c = t*x*z + y*s; + + e = t*x*y + z*s; + f = t*y*y + c1; + g = t*y*z - x*s; + + i = t*x*z - y*s; + j = t*y*z + x*s; + k = t*z*z + c1; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix3 { + return new Matrix3(a, b, c, e, f, g, i, j, k); + } + + /** + * + * @return + */ + public function toString():String { + return "[Matrix3 (" + a + ", " + b + ", " + c + "), (" + e + ", " + f + ", " + g + "), (" + i + ", " + j + ", " + k + ")]"; + } + + /** + * @param angles + */ + public function getEulerAngles(angles:Vector3):void { + if (-1 < i && i < 1) { + angles.x = Math.atan2(j, k); + angles.y = -Math.asin(i); + angles.z = Math.atan2(e, a); + } else { + angles.x = 0; + angles.y = (i <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-b, f); + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/math/.svn/text-base/Matrix4.as.svn-base b/0.0.9.4/src/alternativa/math/.svn/text-base/Matrix4.as.svn-base new file mode 100644 index 0000000..66c259c --- /dev/null +++ b/0.0.9.4/src/alternativa/math/.svn/text-base/Matrix4.as.svn-base @@ -0,0 +1,530 @@ +package alternativa.math { + + /** + * + */ + public class Matrix4 { + + public static const IDENTITY:Matrix4 = new Matrix4(); + + public var a:Number; + public var b:Number; + public var c:Number; + public var d:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + public var h:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + public var l:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix4(a:Number = 1, b:Number = 0, c:Number = 0, d:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, h:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1, l:Number = 0) { + this.a = a; + this.b = b; + this.c = c; + this.d = d; + + this.e = e; + this.f = f; + this.g = g; + this.h = h; + + this.i = i; + this.j = j; + this.k = k; + this.l = l; + } + + /** + * + * @return + */ + public function toIdentity():Matrix4 { + a = f = k = 1; + b = c = e = g = i = j = d = h = l = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + var det:Number = -cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk; + + a = (-gg*jj + ff*kk)/det; + b = (cc*jj - bb*kk)/det; + c = (-cc*ff + bb*gg)/det; + d = (dd*gg*jj - cc*hh*jj - dd*ff*kk + bb*hh*kk + cc*ff*ll - bb*gg*ll)/det; + e = (gg*ii - ee*kk)/det; + f = (-cc*ii + aa*kk)/det; + g = (cc*ee - aa*gg)/det; + h = (cc*hh*ii - dd*gg*ii + dd*ee*kk - aa*hh*kk - cc*ee*ll + aa*gg*ll)/det; + i = (-ff*ii + ee*jj)/det; + j = (bb*ii - aa*jj)/det; + k = (-bb*ee + aa*ff)/det; + l = (dd*ff*ii - bb*hh*ii - dd*ee*jj + aa*hh*jj + bb*ee*ll - aa*ff*ll)/det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + d = m.a*dd + m.b*hh + m.c*ll + m.d; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + h = m.e*dd + m.f*hh + m.g*ll + m.h; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + l = m.i*dd + m.j*hh + m.k*ll + m.l; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + d = aa*m.d + bb*m.h + cc*m.l + dd; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + h = ee*m.d + ff*m.h + gg*m.l + hh; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + l = ii*m.d + jj*m.h + kk*m.l + ll; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix4):Matrix4 { + a += m.a; + b += m.b; + c += m.c; + d += m.d; + e += m.e; + f += m.f; + g += m.g; + h += m.h; + i += m.i; + j += m.j; + k += m.k; + l += m.l; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix4):Matrix4 { + a -= m.a; + b -= m.b; + c -= m.c; + d -= m.d; + e -= m.e; + f -= m.f; + g -= m.g; + h -= m.h; + i -= m.i; + j -= m.j; + k -= m.k; + l -= m.l; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectors(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + * @param len + */ + public function transformVectorsN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverse(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverseN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(idx:int, axis:Vector3):void { + switch (idx) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + case 3: + axis.x = d; + axis.y = h; + axis.z = l; + return; + } + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix4):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = m.d; + e = m.e; + f = m.f; + g = m.g; + h = m.h; + i = m.i; + j = m.j; + k = m.k; + l = m.l; + + return this; + } + + public function setFromMatrix3(m:Matrix3, offset:Vector3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = offset.x; + e = m.e; + f = m.f; + g = m.g; + h = offset.y; + i = m.i; + j = m.j; + k = m.k; + l = offset.z; + + return this; + } + + public function setOrientationFromMatrix3(m:Matrix3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):Matrix4 { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + + return this; + } + + public function setMatrix(x:Number, y:Number, z:Number, rx:Number, ry:Number, rz:Number):Matrix4 { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + d = x; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + h = y; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + l = z; + + return this; + } + + /** + * @param angles + */ + public function getEulerAngles(angles:Vector3):void { + if (-1 < i && i < 1) { + angles.x = Math.atan2(j, k); + angles.y = -Math.asin(i); + angles.z = Math.atan2(e, a); + } else { + angles.x = 0; + angles.y = (i <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-b, f); + } + } + + /** + * Устанавливает координаты матрицы. + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + d = pos.x; + h = pos.y; + l = pos.z; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix4 { + return new Matrix4(a, b, c, d, e, f, g, h, i, j, k, l); + } + + /** + * + * @return + */ + public function toString():String { + return "[Matrix4 [" + a.toFixed(3) + " " + b.toFixed(3) + " " + c.toFixed(3) + " " + d.toFixed(3) + "] [" + e.toFixed(3) + " " + f.toFixed(3) + " " + g.toFixed(3) + " " + h.toFixed(3) + "] [" + i.toFixed(3) + " " + j.toFixed(3) + " " + k.toFixed(3) + " " + l.toFixed(3) + "]]"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/math/.svn/text-base/Quaternion.as.svn-base b/0.0.9.4/src/alternativa/math/.svn/text-base/Quaternion.as.svn-base new file mode 100644 index 0000000..ddc85f5 --- /dev/null +++ b/0.0.9.4/src/alternativa/math/.svn/text-base/Quaternion.as.svn-base @@ -0,0 +1,458 @@ +package alternativa.math { + import flash.geom.Vector3D; + + public class Quaternion { + + public var w:Number; + public var x:Number; + public var y:Number; + public var z:Number; + + public static function multiply(q1:Quaternion, q2:Quaternion, result:Quaternion):void { + result.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z; + result.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y; + result.y = q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z; + result.z = q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x; + } + + public static function createFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngle(axis, angle); + return q; + } + + public static function createFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngleComponents(x, y, z, angle); + return q; + } + + private static var _q:Quaternion = new Quaternion(); + + public function Quaternion(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0) { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + } + + public function reset(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0):Quaternion { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + return this; + } + + public function normalize():Quaternion { + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * Умножает на указанный кватернион слева: this * q + * + * @param q множитель + */ + public function prepend(q:Quaternion):Quaternion { + var ww:Number = w*q.w - x*q.x - y*q.y - z*q.z; + var xx:Number = w*q.x + x*q.w + y*q.z - z*q.y; + var yy:Number = w*q.y + y*q.w + z*q.x - x*q.z; + var zz:Number = w*q.z + z*q.w + x*q.y - y*q.x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Умножает на указанный кватернион справа: q * this + * + * @param q множитель + */ + public function append(q:Quaternion):Quaternion { + var ww:Number = q.w*w - q.x*x - q.y*y - q.z*z; + var xx:Number = q.w*x + q.x*w + q.y*z - q.z*y; + var yy:Number = q.w*y + q.y*w + q.z*x - q.x*z; + var zz:Number = q.w*z + q.z*w + q.x*y - q.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * + * @param vector + */ + public function rotateByVector(v:Vector3):Quaternion { + var ww:Number = -v.x*x - v.y*y - v.z*z; + var xx:Number = v.x*w + v.y*z - v.z*y; + var yy:Number = v.y*w + v.z*x - v.x*z; + var zz:Number = v.z*w + v.x*y - v.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Добавляет вращение, приданное вектором угловой скорости за указанное время. + * + * @param v + * @param scale + */ + public function addScaledVector(v:Vector3, scale:Number):Quaternion { + var vx:Number = v.x*scale; + var vy:Number = v.y*scale; + var vz:Number = v.z*scale; + var ww:Number = -x*vx - y*vy - z*vz; + var xx:Number = vx*w + vy*z - vz*y; + var yy:Number = vy*w + vz*x - vx*z; + var zz:Number = vz*w + vx*y - vy*x; + w += 0.5*ww; + x += 0.5*xx; + y += 0.5*yy; + z += 0.5*zz; + // inlined normalize + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param m + * @return + */ + public function toMatrix3(m:Matrix3):Quaternion { + var xx2:Number = 2.0*x*x; + var yy2:Number = 2.0*y*y; + var zz2:Number = 2.0*z*z; + var xy2:Number = 2.0*x*y; + var yz2:Number = 2.0*y*z; + var zx2:Number = 2.0*z*x; + var wx2:Number = 2.0*w*x; + var wy2:Number = 2.0*w*y; + var wz2:Number = 2.0*w*z; + + m.a = 1.0 - yy2 - zz2; + m.b = xy2 - wz2; + m.c = zx2 + wy2; + + m.e = xy2 + wz2; + m.f = 1.0 - xx2 - zz2; + m.g = yz2 - wx2; + + m.i = zx2 - wy2; + m.j = yz2 + wx2; + m.k = 1.0 - xx2 - yy2; + return this; + } + + /** + * + * @return + */ + public function length():Number { + return Math.sqrt(w*w + x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function lengthSqr():Number { + return w*w + x*x + y*y + z*z; + } + + /** + * + * @param axis + * @param angle + * @return + */ + public function setFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z); + x = axis.x*k; + y = axis.y*k; + z = axis.z*k; + return this; + } + + /** + * + * @param x + * @param y + * @param z + * @param angle + */ + public function setFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(x*x + y*y + z*z); + this.x = x*k; + this.y = y*k; + this.z = z*k; + return this; + } + + /** + * + * @param vector + */ + public function toAxisVector(v:Vector3 = null):Vector3 { + if (w < -1 || w > 1) { + normalize(); + } + if (v == null) { + v = new Vector3(); + } + if (w > -1 && w < 1) { + if (w == 0) { + v.x = x; + v.y = y; + v.z = z; + } else { + var angle:Number = 2*Math.acos(w); + var coeff:Number = 1/Math.sqrt(1 - w*w); + v.x = x*coeff*angle; + v.y = y*coeff*angle; + v.z = z*coeff*angle; + } + } else { + v.x = 0; + v.y = 0; + v.z = 0; + } + return v; + } + + /** + * + * @param rotations + */ + public function getEulerAngles(angles:Vector3):Vector3 { + var qi2:Number = 2*x*x; + var qj2:Number = 2*y*y; + var qk2:Number = 2*z*z; + var qij:Number = 2*x*y; + var qjk:Number = 2*y*z; + var qki:Number = 2*z*x; + var qri:Number = 2*w*x; + var qrj:Number = 2*w*y; + var qrk:Number = 2*w*z; + + var aa:Number = 1 - qj2 - qk2; + var bb:Number = qij - qrk; + var ee:Number = qij + qrk; + var ff:Number = 1 - qi2 - qk2; + var ii:Number = qki - qrj; + var jj:Number = qjk + qri; + var kk:Number = 1 - qi2 - qj2; + + if (-1 < ii && ii < 1) { + if (angles == null) { + angles = new Vector3(Math.atan2(jj, kk), -Math.asin(ii), Math.atan2(ee, aa)); + } else { + angles.x = Math.atan2(jj, kk); + angles.y = -Math.asin(ii); + angles.z = Math.atan2(ee, aa); + } + } else { + if (angles == null) { + angles = new Vector3(0, 0.5*((ii <= -1) ? Math.PI : -Math.PI), Math.atan2(-bb, ff)); + } else { + angles.x = 0; + angles.y = (ii <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-bb, ff); + } + } + return angles; + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setFromEulerAnglesXYZ(x:Number, y:Number, z:Number):void { + setFromAxisAngleComponents(1, 0, 0, x); + + _q.setFromAxisAngleComponents(0, 1, 0, y); + append(_q); + normalize(); + + _q.setFromAxisAngleComponents(0, 0, 1, z); + append(_q); + normalize(); + } + + /** + * + */ + public function conjugate():void { + x = -x; + y = -y; + z = -z; + } + + /** + * Выполняет линейную интерполяцию. + * + * @param q1 начало отрезка + * @param q2 конец отрезка + * @param t время, обычно задаётся в интервале [0, 1] + */ + public function nlerp(q1:Quaternion, q2:Quaternion, t:Number):Quaternion { + var d:Number = 1 - t; + w = q1.w*d + q2.w*t; + x = q1.x*d + q2.x*t; + y = q1.y*d + q2.y*t; + z = q1.z*d + q2.z*t; + // inlined normalize + d = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param q + * @return + */ + public function subtract(q:Quaternion):Quaternion { + w -= q.w; + x -= q.x; + y -= q.y; + z -= q.z; + return this; + } + + /** + * + * @param q1 + * @param q2 + * @return + */ + public function diff(q1:Quaternion, q2:Quaternion):Quaternion { + w = q2.w - q1.w; + x = q2.x - q1.x; + y = q2.y - q1.y; + z = q2.z - q1.z; + return this; + } + + /** + * + * @param q + * @return + */ + public function copy(q:Quaternion):Quaternion { + w = q.w; + x = q.x; + y = q.y; + z = q.z; + return this; + } + + /** + * @param reslult + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + result.w = w; + return result; + } + + /** + * + * @return + */ + public function clone():Quaternion { + return new Quaternion(w, x, y, z); + } + + /** + * + * @return + */ + public function toString():String { + return "[" + w + ", " + x + ", " + y + ", " + z + "]"; + } + + /** + * Выполняет сферическую интерполяцию между двумя заданными кватернионами по наименьшему расстоянию. + * + * @param a первый кватерион + * @param b второй кватернион + * @param t параметр интерполяции, обычно принадлежит отрезку [0, 1] + * @return this + */ + public function slerp(a:Quaternion, b:Quaternion, t:Number):Quaternion { + var flip:Number = 1; + // Так как одна и та же ориентация представляется двумя значениями q и -q, нужно сменить знак одного из кватернионов + // если скалярное произведение отрицательно. Иначе будет получено интерполированное значение по наибольшему расстоянию. + var cosine:Number = a.w*b.w + a.x*b.x + a.y*b.y + a.z*b.z; + if (cosine < 0) { + cosine = -cosine; + flip = -1; + } + + if ((1 - cosine) < 0.001) { + // Вблизи нуля используется линейная интерполяция + var k1:Number = 1 - t; + var k2:Number = t*flip; + w = a.w*k1 + b.w*k2; + x = a.x*k1 + b.x*k2; + y = a.y*k1 + b.y*k2; + z = a.z*k1 + b.z*k2; + normalize(); + } else { + var theta:Number = Math.acos(cosine); + var sine:Number = Math.sin(theta); + var beta:Number = Math.sin((1 - t)*theta)/sine; + var alpha:Number = Math.sin(t*theta)/sine*flip; + w = a.w*beta + b.w*alpha; + x = a.x*beta + b.x*alpha; + y = a.y*beta + b.y*alpha; + z = a.z*beta + b.z*alpha; + } + + return this; + } + + } +} diff --git a/0.0.9.4/src/alternativa/math/.svn/text-base/Vector3.as.svn-base b/0.0.9.4/src/alternativa/math/.svn/text-base/Vector3.as.svn-base new file mode 100644 index 0000000..89010a8 --- /dev/null +++ b/0.0.9.4/src/alternativa/math/.svn/text-base/Vector3.as.svn-base @@ -0,0 +1,334 @@ +package alternativa.math { + import flash.geom.Vector3D; + + /** + * + */ + public class Vector3 extends Vector3D { + + public static const ZERO:Vector3 = new Vector3(0, 0, 0); + public static const X_AXIS:Vector3 = new Vector3(1, 0, 0); + public static const Y_AXIS:Vector3 = new Vector3(0, 1, 0); + public static const Z_AXIS:Vector3 = new Vector3(0, 0, 1); + + /** + * + * @param x + * @param y + * @param z + */ + public function Vector3(x:Number = 0, y:Number = 0, z:Number = 0) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * + * @return + */ + public function vLength():Number { + return Math.sqrt(x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function vLengthSqr():Number { + return x*x + y*y + z*z; + } + + /** + * + * @param length + * @return + */ + public function vSetLength(length:Number):Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = length; + } else { + var k:Number = length/Math.sqrt(x*x + y*y + z*z); + x *= k; + y *= k; + z *= k; + } + return this; + } + + /** + * + * @return + */ + public function vNormalize():Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = 1; + } else { + d = Math.sqrt(d); + x /= d; + y /= d; + z /= d; + } + return this; + } + + /** + * Сложение координат. + * + * @param v точка, координаты которой прибавляются к собственным + */ + public function vAdd(v:Vector3):Vector3 { + x += v.x; + y += v.y; + z += v.z; + return this; + } + + /** + * + * @param k + * @param v + * @return + */ + public function vAddScaled(k:Number, v:Vector3):Vector3 { + x += k*v.x; + y += k*v.y; + z += k*v.z; + return this; + } + + /** + * Вычитание координат. + * + * @param v точка, координаты которой вычитаются из собственных + */ + public function vSubtract(v:Vector3):Vector3 { + x -= v.x; + y -= v.y; + z -= v.z; + return this; + } + + /** + * Вычисляет сумму векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + * @return this + */ + public function vSum(a:Vector3, b:Vector3):Vector3 { + x = a.x + b.x; + y = a.y + b.y; + z = a.z + b.z; + return this; + } + + /** + * Вычисление разности векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + */ + public function vDiff(a:Vector3, b:Vector3):Vector3 { + x = a.x - b.x; + y = a.y - b.y; + z = a.z - b.z; + return this; + } + + /** + * Умножение на скаляр. + * + * @param k число, на которое умножаются координаты + */ + public function vScale(k:Number):Vector3 { + x *= k; + y *= k; + z *= k; + return this; + } + + /** + * Инвертирование вектора. + */ + public function vReverse():Vector3 { + x = -x; + y = -y; + z = -z; + return this; + } + + /** + * + * @param v + * @return + */ + public function vDot(v:Vector3):Number { + return x*v.x + y*v.y + z*v.z; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross(v:Vector3):Vector3 { + var xx:Number = y*v.z - z*v.y; + var yy:Number = z*v.x - x*v.z; + var zz:Number = x*v.y - y*v.x; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross2(a:Vector3, b:Vector3):Vector3 { + x = a.y*b.z - a.z*b.y; + y = a.z*b.x - a.x*b.z; + z = a.x*b.y - a.y*b.x; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy3(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * + * @param matrix + */ + public function vTransformBy3Tr(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz + m.d; + y = m.e*xx + m.f*yy + m.g*zz + m.h; + z = m.i*xx + m.j*yy + m.k*zz + m.l; + return this; + } + + /** + * + * @param m + * @return + */ + public function vTransformInverseBy4(m:Matrix4):Vector3 { + var xx:Number = x - m.d; + var yy:Number = y - m.h; + var zz:Number = z - m.l; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * + * @param m + */ + public function vDeltaTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * Установка координат. + */ + public function vReset(x:Number = 0, y:Number = 0, z:Number = 0):Vector3 { + this.x = x; + this.y = y; + this.z = z; + return this; + } + + /** + * Копирование координат точки. + * + * @param v точка, координаты которой копируются + */ + public function vCopy(v:Vector3):Vector3 { + x = v.x; + y = v.y; + z = v.z; + return this; + } + + /** + * Клонирование точки. + * + * @return клонированная точка + */ + public function vClone():Vector3 { + return new Vector3(x, y, z); + } + + /** + * Копирует координаты вектора в экземпляр класса Vector3D. + * + * @param result экземпляр, в который копируются координаты + * @return переданный параметр + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + return result; + } + + /** + * @param source + * @return + */ + public function copyFromVector3D(source:Vector3D):Vector3 { + x = source.x; + y = source.y; + z = source.z; + return this; + } + + override public function toString():String { + return "Vector3(" + x + "," + y + ", " + z + ")"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/math/Matrix3.as b/0.0.9.4/src/alternativa/math/Matrix3.as new file mode 100644 index 0000000..244cc98 --- /dev/null +++ b/0.0.9.4/src/alternativa/math/Matrix3.as @@ -0,0 +1,390 @@ +package alternativa.math { + import flash.geom.Vector3D; + + /** + * + */ + public class Matrix3 { + + public static const ZERO:Matrix3 = new Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0); + public static const IDENTITY:Matrix3 = new Matrix3(); + + public var a:Number; + public var b:Number; + public var c:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix3(a:Number = 1, b:Number = 0, c:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1) { + this.a = a; + this.b = b; + this.c = c; + + this.e = e; + this.f = f; + this.g = g; + + this.i = i; + this.j = j; + this.k = k; + } + + /** + * + * @return + */ + public function toIdentity():Matrix3 { + a = f = k = 1; + b = c = e = g = i = j = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + var det:Number = 1/(-cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk); + + a = (ff*kk - gg*jj)*det; + b = (cc*jj - bb*kk)*det; + c = (bb*gg - cc*ff)*det; + e = (gg*ii - ee*kk)*det; + f = (aa*kk - cc*ii)*det; + g = (cc*ee - aa*gg)*det; + i = (ee*jj - ff*ii)*det; + j = (bb*ii - aa*jj)*det; + k = (aa*ff - bb*ee)*det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prependTransposed(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.b + cc*m.c; + b = aa*m.e + bb*m.f + cc*m.g; + c = aa*m.i + bb*m.j + cc*m.k; + e = ee*m.a + ff*m.b + gg*m.c; + f = ee*m.e + ff*m.f + gg*m.g; + g = ee*m.i + ff*m.j + gg*m.k; + i = ii*m.a + jj*m.b + kk*m.c; + j = ii*m.e + jj*m.f + kk*m.g; + k = ii*m.i + jj*m.j + kk*m.k; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix3):Matrix3 { + a += m.a; + b += m.b; + c += m.c; + e += m.e; + f += m.f; + g += m.g; + i += m.i; + j += m.j; + k += m.k; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix3):Matrix3 { + a -= m.a; + b -= m.b; + c -= m.c; + e -= m.e; + f -= m.f; + g -= m.g; + i -= m.i; + j -= m.j; + k -= m.k; + + return this; + } + + /** + * + * @return + */ + public function transpose():Matrix3 { + var tmp:Number = b; + b = e; + e = tmp; + tmp = c; + c = i; + i = tmp; + tmp = g; + g = j; + j = tmp; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector3To3D(vin:Vector3, vout:Vector3D):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * + * @param matrix + */ + public function createSkewSymmetric(v:Vector3):Matrix3 { + a = f = k = 0; + b = -v.z; + c = v.y; + e = v.z; + g = -v.x; + i = -v.y; + j = v.x; + return this; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix3):Matrix3 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @return + */ + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):Matrix3 { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + + return this; + } + + /** + * Метод формирует матрицу поворота на заданный угол относительно заданной оси. + * + * @param axis нормализованный вектор, задающий ось, относительно которой выполняется поворот + * @param angle угол поворота в радианах + */ + public function fromAxisAngle(axis:Vector3, angle:Number):void { + var c1:Number = Math.cos(angle); + var s:Number = Math.sin(angle); + var t:Number = 1 - c1; + var x:Number = axis.x; + var y:Number = axis.y; + var z:Number = axis.z; + + a = t*x*x + c1; + b = t*x*y - z*s; + c = t*x*z + y*s; + + e = t*x*y + z*s; + f = t*y*y + c1; + g = t*y*z - x*s; + + i = t*x*z - y*s; + j = t*y*z + x*s; + k = t*z*z + c1; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix3 { + return new Matrix3(a, b, c, e, f, g, i, j, k); + } + + /** + * + * @return + */ + public function toString():String { + return "[Matrix3 (" + a + ", " + b + ", " + c + "), (" + e + ", " + f + ", " + g + "), (" + i + ", " + j + ", " + k + ")]"; + } + + /** + * @param angles + */ + public function getEulerAngles(angles:Vector3):void { + if (-1 < i && i < 1) { + angles.x = Math.atan2(j, k); + angles.y = -Math.asin(i); + angles.z = Math.atan2(e, a); + } else { + angles.x = 0; + angles.y = (i <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-b, f); + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/math/Matrix4.as b/0.0.9.4/src/alternativa/math/Matrix4.as new file mode 100644 index 0000000..66c259c --- /dev/null +++ b/0.0.9.4/src/alternativa/math/Matrix4.as @@ -0,0 +1,530 @@ +package alternativa.math { + + /** + * + */ + public class Matrix4 { + + public static const IDENTITY:Matrix4 = new Matrix4(); + + public var a:Number; + public var b:Number; + public var c:Number; + public var d:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + public var h:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + public var l:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix4(a:Number = 1, b:Number = 0, c:Number = 0, d:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, h:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1, l:Number = 0) { + this.a = a; + this.b = b; + this.c = c; + this.d = d; + + this.e = e; + this.f = f; + this.g = g; + this.h = h; + + this.i = i; + this.j = j; + this.k = k; + this.l = l; + } + + /** + * + * @return + */ + public function toIdentity():Matrix4 { + a = f = k = 1; + b = c = e = g = i = j = d = h = l = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + var det:Number = -cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk; + + a = (-gg*jj + ff*kk)/det; + b = (cc*jj - bb*kk)/det; + c = (-cc*ff + bb*gg)/det; + d = (dd*gg*jj - cc*hh*jj - dd*ff*kk + bb*hh*kk + cc*ff*ll - bb*gg*ll)/det; + e = (gg*ii - ee*kk)/det; + f = (-cc*ii + aa*kk)/det; + g = (cc*ee - aa*gg)/det; + h = (cc*hh*ii - dd*gg*ii + dd*ee*kk - aa*hh*kk - cc*ee*ll + aa*gg*ll)/det; + i = (-ff*ii + ee*jj)/det; + j = (bb*ii - aa*jj)/det; + k = (-bb*ee + aa*ff)/det; + l = (dd*ff*ii - bb*hh*ii - dd*ee*jj + aa*hh*jj + bb*ee*ll - aa*ff*ll)/det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + d = m.a*dd + m.b*hh + m.c*ll + m.d; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + h = m.e*dd + m.f*hh + m.g*ll + m.h; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + l = m.i*dd + m.j*hh + m.k*ll + m.l; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + d = aa*m.d + bb*m.h + cc*m.l + dd; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + h = ee*m.d + ff*m.h + gg*m.l + hh; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + l = ii*m.d + jj*m.h + kk*m.l + ll; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix4):Matrix4 { + a += m.a; + b += m.b; + c += m.c; + d += m.d; + e += m.e; + f += m.f; + g += m.g; + h += m.h; + i += m.i; + j += m.j; + k += m.k; + l += m.l; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix4):Matrix4 { + a -= m.a; + b -= m.b; + c -= m.c; + d -= m.d; + e -= m.e; + f -= m.f; + g -= m.g; + h -= m.h; + i -= m.i; + j -= m.j; + k -= m.k; + l -= m.l; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectors(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + * @param len + */ + public function transformVectorsN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverse(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverseN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(idx:int, axis:Vector3):void { + switch (idx) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + case 3: + axis.x = d; + axis.y = h; + axis.z = l; + return; + } + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix4):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = m.d; + e = m.e; + f = m.f; + g = m.g; + h = m.h; + i = m.i; + j = m.j; + k = m.k; + l = m.l; + + return this; + } + + public function setFromMatrix3(m:Matrix3, offset:Vector3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = offset.x; + e = m.e; + f = m.f; + g = m.g; + h = offset.y; + i = m.i; + j = m.j; + k = m.k; + l = offset.z; + + return this; + } + + public function setOrientationFromMatrix3(m:Matrix3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):Matrix4 { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + + return this; + } + + public function setMatrix(x:Number, y:Number, z:Number, rx:Number, ry:Number, rz:Number):Matrix4 { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + d = x; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + h = y; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + l = z; + + return this; + } + + /** + * @param angles + */ + public function getEulerAngles(angles:Vector3):void { + if (-1 < i && i < 1) { + angles.x = Math.atan2(j, k); + angles.y = -Math.asin(i); + angles.z = Math.atan2(e, a); + } else { + angles.x = 0; + angles.y = (i <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-b, f); + } + } + + /** + * Устанавливает координаты матрицы. + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + d = pos.x; + h = pos.y; + l = pos.z; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix4 { + return new Matrix4(a, b, c, d, e, f, g, h, i, j, k, l); + } + + /** + * + * @return + */ + public function toString():String { + return "[Matrix4 [" + a.toFixed(3) + " " + b.toFixed(3) + " " + c.toFixed(3) + " " + d.toFixed(3) + "] [" + e.toFixed(3) + " " + f.toFixed(3) + " " + g.toFixed(3) + " " + h.toFixed(3) + "] [" + i.toFixed(3) + " " + j.toFixed(3) + " " + k.toFixed(3) + " " + l.toFixed(3) + "]]"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/math/Quaternion.as b/0.0.9.4/src/alternativa/math/Quaternion.as new file mode 100644 index 0000000..ddc85f5 --- /dev/null +++ b/0.0.9.4/src/alternativa/math/Quaternion.as @@ -0,0 +1,458 @@ +package alternativa.math { + import flash.geom.Vector3D; + + public class Quaternion { + + public var w:Number; + public var x:Number; + public var y:Number; + public var z:Number; + + public static function multiply(q1:Quaternion, q2:Quaternion, result:Quaternion):void { + result.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z; + result.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y; + result.y = q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z; + result.z = q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x; + } + + public static function createFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngle(axis, angle); + return q; + } + + public static function createFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngleComponents(x, y, z, angle); + return q; + } + + private static var _q:Quaternion = new Quaternion(); + + public function Quaternion(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0) { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + } + + public function reset(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0):Quaternion { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + return this; + } + + public function normalize():Quaternion { + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * Умножает на указанный кватернион слева: this * q + * + * @param q множитель + */ + public function prepend(q:Quaternion):Quaternion { + var ww:Number = w*q.w - x*q.x - y*q.y - z*q.z; + var xx:Number = w*q.x + x*q.w + y*q.z - z*q.y; + var yy:Number = w*q.y + y*q.w + z*q.x - x*q.z; + var zz:Number = w*q.z + z*q.w + x*q.y - y*q.x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Умножает на указанный кватернион справа: q * this + * + * @param q множитель + */ + public function append(q:Quaternion):Quaternion { + var ww:Number = q.w*w - q.x*x - q.y*y - q.z*z; + var xx:Number = q.w*x + q.x*w + q.y*z - q.z*y; + var yy:Number = q.w*y + q.y*w + q.z*x - q.x*z; + var zz:Number = q.w*z + q.z*w + q.x*y - q.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * + * @param vector + */ + public function rotateByVector(v:Vector3):Quaternion { + var ww:Number = -v.x*x - v.y*y - v.z*z; + var xx:Number = v.x*w + v.y*z - v.z*y; + var yy:Number = v.y*w + v.z*x - v.x*z; + var zz:Number = v.z*w + v.x*y - v.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Добавляет вращение, приданное вектором угловой скорости за указанное время. + * + * @param v + * @param scale + */ + public function addScaledVector(v:Vector3, scale:Number):Quaternion { + var vx:Number = v.x*scale; + var vy:Number = v.y*scale; + var vz:Number = v.z*scale; + var ww:Number = -x*vx - y*vy - z*vz; + var xx:Number = vx*w + vy*z - vz*y; + var yy:Number = vy*w + vz*x - vx*z; + var zz:Number = vz*w + vx*y - vy*x; + w += 0.5*ww; + x += 0.5*xx; + y += 0.5*yy; + z += 0.5*zz; + // inlined normalize + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param m + * @return + */ + public function toMatrix3(m:Matrix3):Quaternion { + var xx2:Number = 2.0*x*x; + var yy2:Number = 2.0*y*y; + var zz2:Number = 2.0*z*z; + var xy2:Number = 2.0*x*y; + var yz2:Number = 2.0*y*z; + var zx2:Number = 2.0*z*x; + var wx2:Number = 2.0*w*x; + var wy2:Number = 2.0*w*y; + var wz2:Number = 2.0*w*z; + + m.a = 1.0 - yy2 - zz2; + m.b = xy2 - wz2; + m.c = zx2 + wy2; + + m.e = xy2 + wz2; + m.f = 1.0 - xx2 - zz2; + m.g = yz2 - wx2; + + m.i = zx2 - wy2; + m.j = yz2 + wx2; + m.k = 1.0 - xx2 - yy2; + return this; + } + + /** + * + * @return + */ + public function length():Number { + return Math.sqrt(w*w + x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function lengthSqr():Number { + return w*w + x*x + y*y + z*z; + } + + /** + * + * @param axis + * @param angle + * @return + */ + public function setFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z); + x = axis.x*k; + y = axis.y*k; + z = axis.z*k; + return this; + } + + /** + * + * @param x + * @param y + * @param z + * @param angle + */ + public function setFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(x*x + y*y + z*z); + this.x = x*k; + this.y = y*k; + this.z = z*k; + return this; + } + + /** + * + * @param vector + */ + public function toAxisVector(v:Vector3 = null):Vector3 { + if (w < -1 || w > 1) { + normalize(); + } + if (v == null) { + v = new Vector3(); + } + if (w > -1 && w < 1) { + if (w == 0) { + v.x = x; + v.y = y; + v.z = z; + } else { + var angle:Number = 2*Math.acos(w); + var coeff:Number = 1/Math.sqrt(1 - w*w); + v.x = x*coeff*angle; + v.y = y*coeff*angle; + v.z = z*coeff*angle; + } + } else { + v.x = 0; + v.y = 0; + v.z = 0; + } + return v; + } + + /** + * + * @param rotations + */ + public function getEulerAngles(angles:Vector3):Vector3 { + var qi2:Number = 2*x*x; + var qj2:Number = 2*y*y; + var qk2:Number = 2*z*z; + var qij:Number = 2*x*y; + var qjk:Number = 2*y*z; + var qki:Number = 2*z*x; + var qri:Number = 2*w*x; + var qrj:Number = 2*w*y; + var qrk:Number = 2*w*z; + + var aa:Number = 1 - qj2 - qk2; + var bb:Number = qij - qrk; + var ee:Number = qij + qrk; + var ff:Number = 1 - qi2 - qk2; + var ii:Number = qki - qrj; + var jj:Number = qjk + qri; + var kk:Number = 1 - qi2 - qj2; + + if (-1 < ii && ii < 1) { + if (angles == null) { + angles = new Vector3(Math.atan2(jj, kk), -Math.asin(ii), Math.atan2(ee, aa)); + } else { + angles.x = Math.atan2(jj, kk); + angles.y = -Math.asin(ii); + angles.z = Math.atan2(ee, aa); + } + } else { + if (angles == null) { + angles = new Vector3(0, 0.5*((ii <= -1) ? Math.PI : -Math.PI), Math.atan2(-bb, ff)); + } else { + angles.x = 0; + angles.y = (ii <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-bb, ff); + } + } + return angles; + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setFromEulerAnglesXYZ(x:Number, y:Number, z:Number):void { + setFromAxisAngleComponents(1, 0, 0, x); + + _q.setFromAxisAngleComponents(0, 1, 0, y); + append(_q); + normalize(); + + _q.setFromAxisAngleComponents(0, 0, 1, z); + append(_q); + normalize(); + } + + /** + * + */ + public function conjugate():void { + x = -x; + y = -y; + z = -z; + } + + /** + * Выполняет линейную интерполяцию. + * + * @param q1 начало отрезка + * @param q2 конец отрезка + * @param t время, обычно задаётся в интервале [0, 1] + */ + public function nlerp(q1:Quaternion, q2:Quaternion, t:Number):Quaternion { + var d:Number = 1 - t; + w = q1.w*d + q2.w*t; + x = q1.x*d + q2.x*t; + y = q1.y*d + q2.y*t; + z = q1.z*d + q2.z*t; + // inlined normalize + d = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param q + * @return + */ + public function subtract(q:Quaternion):Quaternion { + w -= q.w; + x -= q.x; + y -= q.y; + z -= q.z; + return this; + } + + /** + * + * @param q1 + * @param q2 + * @return + */ + public function diff(q1:Quaternion, q2:Quaternion):Quaternion { + w = q2.w - q1.w; + x = q2.x - q1.x; + y = q2.y - q1.y; + z = q2.z - q1.z; + return this; + } + + /** + * + * @param q + * @return + */ + public function copy(q:Quaternion):Quaternion { + w = q.w; + x = q.x; + y = q.y; + z = q.z; + return this; + } + + /** + * @param reslult + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + result.w = w; + return result; + } + + /** + * + * @return + */ + public function clone():Quaternion { + return new Quaternion(w, x, y, z); + } + + /** + * + * @return + */ + public function toString():String { + return "[" + w + ", " + x + ", " + y + ", " + z + "]"; + } + + /** + * Выполняет сферическую интерполяцию между двумя заданными кватернионами по наименьшему расстоянию. + * + * @param a первый кватерион + * @param b второй кватернион + * @param t параметр интерполяции, обычно принадлежит отрезку [0, 1] + * @return this + */ + public function slerp(a:Quaternion, b:Quaternion, t:Number):Quaternion { + var flip:Number = 1; + // Так как одна и та же ориентация представляется двумя значениями q и -q, нужно сменить знак одного из кватернионов + // если скалярное произведение отрицательно. Иначе будет получено интерполированное значение по наибольшему расстоянию. + var cosine:Number = a.w*b.w + a.x*b.x + a.y*b.y + a.z*b.z; + if (cosine < 0) { + cosine = -cosine; + flip = -1; + } + + if ((1 - cosine) < 0.001) { + // Вблизи нуля используется линейная интерполяция + var k1:Number = 1 - t; + var k2:Number = t*flip; + w = a.w*k1 + b.w*k2; + x = a.x*k1 + b.x*k2; + y = a.y*k1 + b.y*k2; + z = a.z*k1 + b.z*k2; + normalize(); + } else { + var theta:Number = Math.acos(cosine); + var sine:Number = Math.sin(theta); + var beta:Number = Math.sin((1 - t)*theta)/sine; + var alpha:Number = Math.sin(t*theta)/sine*flip; + w = a.w*beta + b.w*alpha; + x = a.x*beta + b.x*alpha; + y = a.y*beta + b.y*alpha; + z = a.z*beta + b.z*alpha; + } + + return this; + } + + } +} diff --git a/0.0.9.4/src/alternativa/math/Vector3.as b/0.0.9.4/src/alternativa/math/Vector3.as new file mode 100644 index 0000000..89010a8 --- /dev/null +++ b/0.0.9.4/src/alternativa/math/Vector3.as @@ -0,0 +1,334 @@ +package alternativa.math { + import flash.geom.Vector3D; + + /** + * + */ + public class Vector3 extends Vector3D { + + public static const ZERO:Vector3 = new Vector3(0, 0, 0); + public static const X_AXIS:Vector3 = new Vector3(1, 0, 0); + public static const Y_AXIS:Vector3 = new Vector3(0, 1, 0); + public static const Z_AXIS:Vector3 = new Vector3(0, 0, 1); + + /** + * + * @param x + * @param y + * @param z + */ + public function Vector3(x:Number = 0, y:Number = 0, z:Number = 0) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * + * @return + */ + public function vLength():Number { + return Math.sqrt(x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function vLengthSqr():Number { + return x*x + y*y + z*z; + } + + /** + * + * @param length + * @return + */ + public function vSetLength(length:Number):Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = length; + } else { + var k:Number = length/Math.sqrt(x*x + y*y + z*z); + x *= k; + y *= k; + z *= k; + } + return this; + } + + /** + * + * @return + */ + public function vNormalize():Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = 1; + } else { + d = Math.sqrt(d); + x /= d; + y /= d; + z /= d; + } + return this; + } + + /** + * Сложение координат. + * + * @param v точка, координаты которой прибавляются к собственным + */ + public function vAdd(v:Vector3):Vector3 { + x += v.x; + y += v.y; + z += v.z; + return this; + } + + /** + * + * @param k + * @param v + * @return + */ + public function vAddScaled(k:Number, v:Vector3):Vector3 { + x += k*v.x; + y += k*v.y; + z += k*v.z; + return this; + } + + /** + * Вычитание координат. + * + * @param v точка, координаты которой вычитаются из собственных + */ + public function vSubtract(v:Vector3):Vector3 { + x -= v.x; + y -= v.y; + z -= v.z; + return this; + } + + /** + * Вычисляет сумму векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + * @return this + */ + public function vSum(a:Vector3, b:Vector3):Vector3 { + x = a.x + b.x; + y = a.y + b.y; + z = a.z + b.z; + return this; + } + + /** + * Вычисление разности векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + */ + public function vDiff(a:Vector3, b:Vector3):Vector3 { + x = a.x - b.x; + y = a.y - b.y; + z = a.z - b.z; + return this; + } + + /** + * Умножение на скаляр. + * + * @param k число, на которое умножаются координаты + */ + public function vScale(k:Number):Vector3 { + x *= k; + y *= k; + z *= k; + return this; + } + + /** + * Инвертирование вектора. + */ + public function vReverse():Vector3 { + x = -x; + y = -y; + z = -z; + return this; + } + + /** + * + * @param v + * @return + */ + public function vDot(v:Vector3):Number { + return x*v.x + y*v.y + z*v.z; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross(v:Vector3):Vector3 { + var xx:Number = y*v.z - z*v.y; + var yy:Number = z*v.x - x*v.z; + var zz:Number = x*v.y - y*v.x; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross2(a:Vector3, b:Vector3):Vector3 { + x = a.y*b.z - a.z*b.y; + y = a.z*b.x - a.x*b.z; + z = a.x*b.y - a.y*b.x; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy3(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * + * @param matrix + */ + public function vTransformBy3Tr(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz + m.d; + y = m.e*xx + m.f*yy + m.g*zz + m.h; + z = m.i*xx + m.j*yy + m.k*zz + m.l; + return this; + } + + /** + * + * @param m + * @return + */ + public function vTransformInverseBy4(m:Matrix4):Vector3 { + var xx:Number = x - m.d; + var yy:Number = y - m.h; + var zz:Number = z - m.l; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * + * @param m + */ + public function vDeltaTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * Установка координат. + */ + public function vReset(x:Number = 0, y:Number = 0, z:Number = 0):Vector3 { + this.x = x; + this.y = y; + this.z = z; + return this; + } + + /** + * Копирование координат точки. + * + * @param v точка, координаты которой копируются + */ + public function vCopy(v:Vector3):Vector3 { + x = v.x; + y = v.y; + z = v.z; + return this; + } + + /** + * Клонирование точки. + * + * @return клонированная точка + */ + public function vClone():Vector3 { + return new Vector3(x, y, z); + } + + /** + * Копирует координаты вектора в экземпляр класса Vector3D. + * + * @param result экземпляр, в который копируются координаты + * @return переданный параметр + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + return result; + } + + /** + * @param source + * @return + */ + public function copyFromVector3D(source:Vector3D):Vector3 { + x = source.x; + y = source.y; + z = source.z; + return this; + } + + override public function toString():String { + return "Vector3(" + x + "," + y + ", " + z + ")"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/.svn/all-wcprops b/0.0.9.4/src/alternativa/physics/.svn/all-wcprops new file mode 100644 index 0000000..bdd5518 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/.svn/all-wcprops @@ -0,0 +1,77 @@ +K 25 +svn:wc:ra_dav:version-url +V 103 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics +END +Body.as +K 25 +svn:wc:ra_dav:version-url +V 111 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/Body.as +END +BodyState.as +K 25 +svn:wc:ra_dav:version-url +V 116 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/BodyState.as +END +CollisionPrimitiveListItem.as +K 25 +svn:wc:ra_dav:version-url +V 133 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/CollisionPrimitiveListItem.as +END +CollisionPrimitiveList.as +K 25 +svn:wc:ra_dav:version-url +V 129 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/CollisionPrimitiveList.as +END +PhysicsScene.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/PhysicsScene.as +END +PhysicsUtils.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/PhysicsUtils.as +END +altphysics.as +K 25 +svn:wc:ra_dav:version-url +V 117 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/altphysics.as +END +ContactPoint.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/ContactPoint.as +END +BodyListItem.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/BodyListItem.as +END +BodyList.as +K 25 +svn:wc:ra_dav:version-url +V 115 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/BodyList.as +END +Contact.as +K 25 +svn:wc:ra_dav:version-url +V 114 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/Contact.as +END +BodyMaterial.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/BodyMaterial.as +END diff --git a/0.0.9.4/src/alternativa/physics/.svn/entries b/0.0.9.4/src/alternativa/physics/.svn/entries new file mode 100644 index 0000000..12cb7f0 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/.svn/entries @@ -0,0 +1,181 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics +http://svndev.alternativaplatform.com + + + +2010-07-03T11:40:52.545629Z +36684 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +CollisionPrimitiveList.as +file + + + + +2010-10-28T04:32:45.000000Z +5752b1f4909c5146720b58fbfffed468 +2009-11-01T12:35:52.270571Z +22955 +mike + +altphysics.as +file + + + + +2010-10-28T04:32:45.000000Z +04fbe40a27502dbbd0dba02a76b2df50 +2009-04-20T20:09:14.715403Z +11579 +mike + +collision +dir + +primitives +dir + +BodyList.as +file + + + + +2010-10-28T04:32:45.000000Z +08a90332660bd0273728006d1402dd25 +2009-11-01T12:35:52.270571Z +22955 +mike + +Contact.as +file + + + + +2010-10-28T04:32:45.000000Z +eb89f4f37bcf2d21eadc733f244fcf64 +2010-02-12T12:34:25.192477Z +28424 +mike + +Body.as +file + + + + +2010-10-28T04:32:45.000000Z +355d0fb70dbb4946d4090a7c94c0d58a +2010-02-12T12:34:25.192477Z +28424 +mike + +BodyState.as +file + + + + +2010-10-28T04:32:45.000000Z +5be68820e6f6185d62b3928c39dde285 +2009-11-24T08:44:15.941480Z +23758 +mike + +CollisionPrimitiveListItem.as +file + + + + +2010-10-28T04:32:45.000000Z +cec5dbfefef812d87481478455f367b1 +2009-10-19T07:17:33.113306Z +22253 +mike + +PhysicsScene.as +file + + + + +2010-10-28T04:32:45.000000Z +96c74784bb71c06141cc469eec57ccb8 +2010-02-19T13:09:00.412754Z +28731 +mike + +PhysicsUtils.as +file + + + + +2010-10-28T04:32:45.000000Z +dce0a94f8a861291a2a8942ed0ba54a5 +2009-11-24T08:44:15.941480Z +23758 +mike + +constraints +dir + +ContactPoint.as +file + + + + +2010-10-28T04:32:45.000000Z +14f23db94810328dedd7da2992812ced +2009-11-24T08:44:15.941480Z +23758 +mike + +BodyListItem.as +file + + + + +2010-10-28T04:32:45.000000Z +2fc2b598310b24aef3f8b90e14a8ba4f +2009-10-19T07:17:33.113306Z +22253 +mike + +BodyMaterial.as +file + + + + +2010-10-28T04:32:45.000000Z +9b700692ebb8017882f3e8cec095e1a9 +2009-10-19T07:17:33.113306Z +22253 +mike + diff --git a/0.0.9.4/src/alternativa/physics/.svn/format b/0.0.9.4/src/alternativa/physics/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.4/src/alternativa/physics/.svn/text-base/Body.as.svn-base b/0.0.9.4/src/alternativa/physics/.svn/text-base/Body.as.svn-base new file mode 100644 index 0000000..cd22c5b --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/.svn/text-base/Body.as.svn-base @@ -0,0 +1,405 @@ +package alternativa.physics { + + import alternativa.math.Matrix3; + import alternativa.math.Matrix4; + import alternativa.math.Quaternion; + import alternativa.math.Vector3; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.IBodyCollisionPredicate; + import alternativa.physics.collision.types.BoundBox; + + use namespace altphysics; + + /** + * + */ + public class Body { + + public static var linDamping:Number = 0.997; + public static var rotDamping:Number = 0.997; + + // Идентификатор тела, уникальный в пределах мира + public var id:int; + // Имя тела + public var name:String; + // Мир, в котором находится тело + public var world:PhysicsScene; + // Флаг подвижности тела + public var movable:Boolean = true; + // Флаг указывает, может ли тело быть заморожено + public var canFreeze:Boolean = false; + + public var freezeCounter:int; + public var frozen:Boolean = false; + // Ограничивающий бокс тела + public var aabb:BoundBox = new BoundBox(); + // Предикат, через который тело получает сообщения о столкновениях + public var postCollisionPredicate:IBodyCollisionPredicate; + + // Текущее состояние тела + public var state:BodyState = new BodyState(); + // Предыдущее состояние тела + public var prevState:BodyState = new BodyState(); + // Линейное ускорение тела на текущем шаге симуляции + public var accel:Vector3 = new Vector3(); + // Угловое ускорение тела на текущем шаге симуляции + public var angleAccel:Vector3 = new Vector3(); + // Физический материал тела + public var material:BodyMaterial = new BodyMaterial(); + // Обратная масса тела + public var invMass:Number = 1; + // Обратная матрица тензора инерции в локальных координатах + public var invInertia:Matrix3 = new Matrix3(); + // Обратная матрица тензора инерции в мировых координатах + public var invInertiaWorld:Matrix3 = new Matrix3(); + // Базисная матрица тела в мировых координатах + public var baseMatrix:Matrix3 = new Matrix3(); + + public const MAX_CONTACTS:int = 20; + public var contacts:Vector. = new Vector.(MAX_CONTACTS); + public var contactsNum:int; + + public var collisionPrimitives:CollisionPrimitiveList; + + // Аккумулятор сил + public var forceAccum:Vector3 = new Vector3(); + // Аккумулятор моментов + public var torqueAccum:Vector3 = new Vector3(); + + // Внутренние переменные для избежания создания экземпляров + private static var _r:Vector3 = new Vector3(); + private static var _f:Vector3 = new Vector3(); + + /** + * + * @param invMass + * @param invInertia + */ + public function Body(invMass:Number, invInertia:Matrix3) { + this.invMass = invMass; + this.invInertia.copy(invInertia); + } + + /** + * @param primitive + * @param localTransform + */ + public function addCollisionPrimitive(primitive:CollisionPrimitive, localTransform:Matrix4 = null):void { + if (primitive == null) { + throw new ArgumentError("Primitive cannot be null"); + } + if (collisionPrimitives == null) { + collisionPrimitives = new CollisionPrimitiveList(); + } + collisionPrimitives.append(primitive); + primitive.setBody(this, localTransform); + } + + /** + * + * @param primitive + */ + public function removeCollisionPrimitive(primitive:CollisionPrimitive):void { + if (collisionPrimitives == null) return; + primitive.setBody(null); + collisionPrimitives.remove(primitive); + if (collisionPrimitives.size == 0) { + collisionPrimitives = null; + } + } + + /** + * @param t + * @param result + */ + public function interpolate(t:Number, pos:Vector3, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.w = prevState.orientation.w*t1 + state.orientation.w*t; + orientation.x = prevState.orientation.x*t1 + state.orientation.x*t; + orientation.y = prevState.orientation.y*t1 + state.orientation.y*t; + orientation.z = prevState.orientation.z*t1 + state.orientation.z*t; + } + + /** + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + state.pos.vCopy(pos); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setPositionXYZ(x:Number, y:Number, z:Number):void { + state.pos.vReset(x, y, z); + } + + /** + * + * @param vel + */ + public function setVelocity(vel:Vector3):void { + state.velocity.vCopy(vel); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setVelocityXYZ(x:Number, y:Number, z:Number):void { + state.velocity.vReset(x, y, z); + } + + /** + * + * @param rot + */ + public function setRotation(rot:Vector3):void { + state.rotation.vCopy(rot); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setRotationXYZ(x:Number, y:Number, z:Number):void { + state.rotation.vReset(x, y, z); + } + + /** + * + * @param q + */ + public function setOrientation(q:Quaternion):void { + state.orientation.copy(q); + } + + /** + * @param r + * @param dir + * @param magnitude + */ + public function applyRelPosWorldImpulse(r:Vector3, dir:Vector3, magnitude:Number):void { + var d:Number = magnitude*invMass; + // Линейная часть + state.velocity.x += d*dir.x; + state.velocity.y += d*dir.y; + state.velocity.z += d*dir.z; + + // Вращательная часть + var x:Number = (r.y*dir.z - r.z*dir.y)*magnitude; + var y:Number = (r.z*dir.x - r.x*dir.z)*magnitude; + var z:Number = (r.x*dir.y - r.y*dir.x)*magnitude; + + state.rotation.x += invInertiaWorld.a*x + invInertiaWorld.b*y + invInertiaWorld.c*z; + state.rotation.y += invInertiaWorld.e*x + invInertiaWorld.f*y + invInertiaWorld.g*z; + state.rotation.z += invInertiaWorld.i*x + invInertiaWorld.j*y + invInertiaWorld.k*z; + } + + /** + * @param f + */ + public function addForce(f:Vector3):void { + forceAccum.vAdd(f); + } + + /** + * + * @param fx + * @param fy + * @param fz + */ + public function addForceXYZ(fx:Number, fy:Number, fz:Number):void { + forceAccum.x += fx; + forceAccum.y += fy; + forceAccum.z += fz; + } + + /** + * @param pos + * @param f + */ + public function addWorldForceXYZ(px:Number, py:Number, pz:Number, fx:Number, fy:Number, fz:Number):void { + forceAccum.x += fx; + forceAccum.y += fy; + forceAccum.z += fz; + + var pos:Vector3 = state.pos; + var rx:Number = px - pos.x; + var ry:Number = py - pos.y; + var rz:Number = pz - pos.z; + +// var x:Number = ry*fz - rz*fy; +// var y:Number = rz*fx - rx*fz; +// var z:Number = rx*fy - ry*fx; + + torqueAccum.x += ry*fz - rz*fy; + torqueAccum.y += rz*fx - rx*fz; + torqueAccum.z += rx*fy - ry*fx; + } + + /** + * @param pos + * @param f + */ + public function addWorldForce(pos:Vector3, force:Vector3):void { + forceAccum.vAdd(force); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(force)); + } + + /** + * @param pos + * @param f + */ + public function addWorldForceScaled(pos:Vector3, force:Vector3, scale:Number):void { + _f.x = scale*force.x; + _f.y = scale*force.y; + _f.z = scale*force.z; + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(_f)); + } + + /** + * @param pos + * @param f + */ + public function addLocalForce(pos:Vector3, force:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(pos, _r); + // Трансформируем вектор силы в мировую систему + baseMatrix.transformVector(force, _f); + // Добавляем силу и момент + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vCross(_f)); + } + + /** + * + * @param localPos + * @param worldForce + */ + public function addWorldForceAtLocalPoint(localPos:Vector3, worldForce:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(localPos, _r); + // Добавляем силу и момент + forceAccum.vAdd(worldForce); + torqueAccum.vAdd(_r.vCross(worldForce)); + } + + /** + * @param dt + */ + public function beforePhysicsStep(dt:Number):void { + } + + /** + * @param t + */ + public function addTorque(t:Vector3):void { + torqueAccum.vAdd(t); + } + + /** + * + */ + altphysics function clearAccumulators():void { + forceAccum.x = forceAccum.y = forceAccum.z = 0; + torqueAccum.x = torqueAccum.y = torqueAccum.z = 0; + } + + /** + * + */ + altphysics function calcAccelerations():void { + accel.x = forceAccum.x*invMass; + accel.y = forceAccum.y*invMass; + accel.z = forceAccum.z*invMass; + angleAccel.x = invInertiaWorld.a*torqueAccum.x + invInertiaWorld.b*torqueAccum.y + invInertiaWorld.c*torqueAccum.z; + angleAccel.y = invInertiaWorld.e*torqueAccum.x + invInertiaWorld.f*torqueAccum.y + invInertiaWorld.g*torqueAccum.z; + angleAccel.z = invInertiaWorld.i*torqueAccum.x + invInertiaWorld.j*torqueAccum.y + invInertiaWorld.k*torqueAccum.z; + } + + /** + * Вычисляет производные данные. + */ + public function calcDerivedData():void { + // Вычисление базисной матрицы и обратного тензора инерции в мировых координатах + state.orientation.toMatrix3(baseMatrix); + invInertiaWorld.copy(invInertia).append(baseMatrix).prependTransposed(baseMatrix); + if (collisionPrimitives != null) { + aabb.infinity(); + var item:CollisionPrimitiveListItem = collisionPrimitives.head; + while (item != null) { + var primitive:CollisionPrimitive = item.primitive; + primitive.transform.setFromMatrix3(baseMatrix, state.pos); + if (primitive.localTransform != null) { + primitive.transform.prepend(primitive.localTransform); + } + primitive.calculateAABB(); + aabb.addBoundBox(primitive.aabb); + item = item.next; + } + } + } + + /** + * + */ + altphysics function saveState():void { + prevState.copy(state); + } + + /** + * + */ + altphysics function restoreState():void { + state.copy(prevState); + } + + /** + * @param dt + */ + altphysics function integrateVelocity(dt:Number):void { + // v = v + a*t + state.velocity.x += accel.x*dt; + state.velocity.y += accel.y*dt; + state.velocity.z += accel.z*dt; + // rot = rot + eps*t + state.rotation.x += angleAccel.x*dt; + state.rotation.y += angleAccel.y*dt; + state.rotation.z += angleAccel.z*dt; + + state.velocity.x *= linDamping; + state.velocity.y *= linDamping; + state.velocity.z *= linDamping; + + state.rotation.x *= rotDamping; + state.rotation.y *= rotDamping; + state.rotation.z *= rotDamping; + } + + /** + * + */ + altphysics function integratePosition(dt:Number):void { + // pos = pos + v*t + state.pos.x += state.velocity.x*dt; + state.pos.y += state.velocity.y*dt; + state.pos.z += state.velocity.z*dt; + // q = q + 0.5*rot*q + state.orientation.addScaledVector(state.rotation, dt); + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/.svn/text-base/BodyList.as.svn-base b/0.0.9.4/src/alternativa/physics/.svn/text-base/BodyList.as.svn-base new file mode 100644 index 0000000..aa8f79f --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/.svn/text-base/BodyList.as.svn-base @@ -0,0 +1,76 @@ +package alternativa.physics { + + /** + * + */ + public class BodyList { + + public var head:BodyListItem; + public var tail:BodyListItem; + public var size:int; + + /** + * + */ + public function BodyList() { + } + + /** + * + * @param body + */ + public function append(body:Body):void { + var item:BodyListItem = BodyListItem.create(body); + if (head == null) { + head = tail = item; + } else { + tail.next = item; + item.prev = tail; + tail = item; + } + size++; + } + + /** + * + * @param body + */ + public function remove(body:Body):Boolean { + var item:BodyListItem = findItem(body); + if (item == null) return false; + if (item == head) { + if (size == 1) { + head = tail = null; + } else { + head = item.next; + head.prev = null; + } + } else { + if (item == tail) { + tail = item.prev; + tail.next = null; + } else { + item.prev.next = item.next; + item.next.prev = item.prev; + } + } + item.dispose(); + size--; + return true; + } + + /** + * + * @param body + * @return + */ + public function findItem(body:Body):BodyListItem { + var item:BodyListItem = head; + while (item != null && item.body != body) { + item = item.next; + } + return item; + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/.svn/text-base/BodyListItem.as.svn-base b/0.0.9.4/src/alternativa/physics/.svn/text-base/BodyListItem.as.svn-base new file mode 100644 index 0000000..382e11c --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/.svn/text-base/BodyListItem.as.svn-base @@ -0,0 +1,74 @@ +package alternativa.physics { + + /** + * + */ + public class BodyListItem { + + // Верхний элемент хранилища + private static var poolTop:BodyListItem; + + /** + * Создаёт новый элемент списка. + * + * @param primitive примитив, содержащийся в элементе + * @return новый элемент списка + */ + public static function create(body:Body):BodyListItem { + var item:BodyListItem; + if (poolTop == null) { + item = new BodyListItem(body); + } else { + item = poolTop; + poolTop = item.next; + item.next = null; + item.body = body; + } + return item; + } + + /** + * Очищает хранилище. + */ + public static function clearPool():void { + var item:BodyListItem = poolTop; + while (item != null) { + poolTop = item.next; + item.next = null; + item = poolTop; + } + } + + /** + * + */ + public var body:Body; + /** + * + */ + public var next:BodyListItem; + /** + * + */ + public var prev:BodyListItem; + + /** + * + * @param body + */ + public function BodyListItem(body:Body) { + this.body = body; + } + + /** + * + */ + public function dispose():void { + body = null; + prev = null; + next = poolTop; + poolTop = this; + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/.svn/text-base/BodyMaterial.as.svn-base b/0.0.9.4/src/alternativa/physics/.svn/text-base/BodyMaterial.as.svn-base new file mode 100644 index 0000000..7a7c47d --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/.svn/text-base/BodyMaterial.as.svn-base @@ -0,0 +1,11 @@ +package alternativa.physics { + + public class BodyMaterial { + + public var restitution:Number = 0; + public var friction:Number = 0.3; +// public var dynamicFriction:Number = 0.2; +// public var dynamicFriction:Number = 0.2; + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/.svn/text-base/BodyState.as.svn-base b/0.0.9.4/src/alternativa/physics/.svn/text-base/BodyState.as.svn-base new file mode 100644 index 0000000..3987388 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/.svn/text-base/BodyState.as.svn-base @@ -0,0 +1,39 @@ +package alternativa.physics { + import alternativa.math.Quaternion; + import alternativa.math.Vector3; + + /** + * Класс описывает состояние твёрдого тела. + */ + public class BodyState { + /** + * Положение тела. + */ + public var pos:Vector3 = new Vector3(); + /** + * Ориентация тела. + */ + public var orientation:Quaternion = new Quaternion(); + /** + * Скорость тела. + */ + public var velocity:Vector3 = new Vector3(); + /** + * Угловая скорость тела. + */ + public var rotation:Vector3 = new Vector3(); + + /** + * Копирует значение указанного объекта. + * + * @param state + */ + public function copy(state:BodyState):void { + pos.vCopy(state.pos); + orientation.copy(state.orientation); + velocity.vCopy(state.velocity); + rotation.vCopy(state.rotation); + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/.svn/text-base/CollisionPrimitiveList.as.svn-base b/0.0.9.4/src/alternativa/physics/.svn/text-base/CollisionPrimitiveList.as.svn-base new file mode 100644 index 0000000..5d5304f --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/.svn/text-base/CollisionPrimitiveList.as.svn-base @@ -0,0 +1,89 @@ +package alternativa.physics { + import alternativa.physics.collision.CollisionPrimitive; + + /** + * + */ + public class CollisionPrimitiveList { + + public var head:CollisionPrimitiveListItem; + public var tail:CollisionPrimitiveListItem; + public var size:int; + + /** + * + */ + public function CollisionPrimitiveList() { + } + + /** + * + * @param primitive + */ + public function append(primitive:CollisionPrimitive):void { + var item:CollisionPrimitiveListItem = CollisionPrimitiveListItem.create(primitive); + if (head == null) { + head = tail = item; + } else { + tail.next = item; + item.prev = tail; + tail = item; + } + size++; + } + + /** + * + * @param primitve + */ + public function remove(primitve:CollisionPrimitive):void { + var item:CollisionPrimitiveListItem = findItem(primitve); + if (item == null) return; + if (item == head) { + if (size == 1) { + head = tail = null; + } else { + head = item.next; + head.prev = null; + } + } else { + if (item == tail) { + tail = tail.prev; + tail.next = null; + } else { + item.prev.next = item.next; + item.next.prev = item.prev; + } + } + item.dispose(); + size--; + } + + /** + * + * @param primitive + * @return + */ + public function findItem(primitive:CollisionPrimitive):CollisionPrimitiveListItem { + var item:CollisionPrimitiveListItem = head; + while (item != null && item.primitive != primitive) { + item = item.next; + } + return item; + } + + /** + * + */ + public function clear():void { + while (head != null) { + var item:CollisionPrimitiveListItem = head; + head = head.next; + item.dispose(); + } + tail = null; + size = 0; + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/.svn/text-base/CollisionPrimitiveListItem.as.svn-base b/0.0.9.4/src/alternativa/physics/.svn/text-base/CollisionPrimitiveListItem.as.svn-base new file mode 100644 index 0000000..d204532 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/.svn/text-base/CollisionPrimitiveListItem.as.svn-base @@ -0,0 +1,76 @@ +package alternativa.physics { + import alternativa.physics.collision.CollisionPrimitive; + + /** + * Элемент списка примитивов. + */ + public class CollisionPrimitiveListItem { + + // Верхний элемент хранилища + private static var poolTop:CollisionPrimitiveListItem; + + /** + * Создаёт новый элемент списка. + * + * @param primitive примитив, содержащийся в элементе + * @return новый элемент списка + */ + public static function create(primitive:CollisionPrimitive):CollisionPrimitiveListItem { + var item:CollisionPrimitiveListItem; + if (poolTop == null) { + item = new CollisionPrimitiveListItem(primitive); + } else { + item = poolTop; + item.primitive = primitive; + poolTop = item.next; + item.next = null; + } + return item; + } + + /** + * Очищает хранилище. + */ + public static function clearPool():void { + var curr:CollisionPrimitiveListItem = poolTop; + while (curr != null) { + poolTop = curr.next; + curr.next = null; + curr = poolTop; + } + } + + /** + * Примитив, хранящийся в элементе списка. + */ + public var primitive:CollisionPrimitive; + /** + * Ссылка не следующий элемент списка. + */ + public var next:CollisionPrimitiveListItem; + /** + * Ссылка не предыдущий элемент списка. + */ + public var prev:CollisionPrimitiveListItem; + + /** + * Создаёт новый экземпляр. + * + * @param primitive примитив, хранящийся в элементе списка + */ + public function CollisionPrimitiveListItem(primitive:CollisionPrimitive) { + this.primitive = primitive; + } + + /** + * Очищает внутренние ссылки и помещает элемент в хранилище для дальнейшего использования. + */ + public function dispose():void { + primitive = null; + prev = null; + next = poolTop; + poolTop = this; + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/.svn/text-base/Contact.as.svn-base b/0.0.9.4/src/alternativa/physics/.svn/text-base/Contact.as.svn-base new file mode 100644 index 0000000..3f010ec --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/.svn/text-base/Contact.as.svn-base @@ -0,0 +1,69 @@ +package alternativa.physics { + + + import alternativa.math.Vector3; + + /** + * Описывает контакт одного или двух тел. + */ + public class Contact { + /** + * Первое тело контакта, не может быть равно null. + */ + public var body1:Body; + /** + * Второе тело контакта, может быть равно null. + */ + public var body2:Body; + /** + * Предрассчитанный взаимный коэффициент отскока. + */ + public var restitution:Number; + /** + * Предрассчитанный взаимный коэффициент трения. + */ + public var friction:Number; + /** + * Нормаль контакта. Направлена к первому телу. + */ + public var normal:Vector3 = new Vector3(); + /** + * Список точек контакта. + */ + public var points:Vector. = new Vector.(MAX_POINTS, true); + /** + * Количество точек контакта. + */ + public var pcount:int; + /** + * Максимальная глубина пересечения. + */ + public var maxPenetration:Number = 0; + /** + * Флаг показывает, разрешён контакт или нет. + */ + public var satisfied:Boolean; + /** + * Следующий контакт в списке. + */ + public var next:Contact; + /** + * Индекс контакта. Первый контакт в списке имеет индекс 0. + */ + public var index:int; + + // Максимальное количество точек контакта + private const MAX_POINTS:int = 8; + + /** + * + */ + public function Contact(index:int) { + this.index = index; + for (var i:int = 0; i < MAX_POINTS; i++) { + points[i] = new ContactPoint(); + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/.svn/text-base/ContactPoint.as.svn-base b/0.0.9.4/src/alternativa/physics/.svn/text-base/ContactPoint.as.svn-base new file mode 100644 index 0000000..395d2dc --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/.svn/text-base/ContactPoint.as.svn-base @@ -0,0 +1,55 @@ +package alternativa.physics { + import alternativa.math.Vector3; + + /** + * + */ + public class ContactPoint { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + + // Величины, расчитываемые перед началом фазы решения контактов + + // Требуемая проекция конечной скорости на нормаль для упругого контакта + public var normalVel:Number; + // Минимальная скорость разделения неупругого контакта + public var minSepVel:Number; + // Изменение проекции скорости на единицу нормального импульса + public var velByUnitImpulseN:Number; + + public var angularInertia1:Number; + public var angularInertia2:Number; + + // Радиус-вектор точки контакта относительно центра первого тела + public var r1:Vector3 = new Vector3(); + // Радиус-вектор точки контакта относительно центра второго тела + public var r2:Vector3 = new Vector3(); + + // Величины, накапливаемые во время фазы решения контактов + + // Накопленный импульс, применяемый для получения требуемой относительной скорости в точке контакта + public var accumImpulseN:Number; + // Накопленный импульс, применяемый для разделения тел в точке контакта. Не создаёт момента. +// public var accumSepImpulse:Number; + + public var satisfied:Boolean; + + /** + * + * @param cp + */ + public function copyFrom(cp:ContactPoint):void { + pos.vCopy(cp.pos); + penetration = cp.penetration; + feature1 = cp.feature1; + feature2 = cp.feature2; + r1.vCopy(cp.r1); + r2.vCopy(cp.r2); + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/.svn/text-base/PhysicsScene.as.svn-base b/0.0.9.4/src/alternativa/physics/.svn/text-base/PhysicsScene.as.svn-base new file mode 100644 index 0000000..abdc762 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/.svn/text-base/PhysicsScene.as.svn-base @@ -0,0 +1,743 @@ +package alternativa.physics { + + + import alternativa.physics.collision.ICollisionDetector; + import alternativa.physics.collision.KdTreeCollisionDetector; + import alternativa.physics.constraints.Constraint; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * Класс реализует физическую симуляцию поведения твёрдых тел. + */ + public class PhysicsScene { + + private static var lastBodyId:int; + + // Максимальное количество контактов + public const MAX_CONTACTS:int = 1000; + + ////////////////////////////////////////////////////////////////// + // Настроечные параметры симуляции. Могут быть изменены в любой + // момент времени без нарушения корректной работы симуляции. + ////////////////////////////////////////////////////////////////// + // Количество шагов, за которое пересекающиеся тела должны разделиться + public var penResolutionSteps:int = 10; + // Величина допустимой глубины пересечения + public var allowedPenetration:Number = 0.1; + // Максимальная скорость, добавляемая с целью разделения тел + public var maxPenResolutionSpeed:Number = 0.5; + // Количество итераций для обработки упругих контактов + public var collisionIterations:int = 5; + // Количество итераций для обработки неупругих контактов + public var contactIterations:int = 5; + // Флаг использования предсказания состояний + public var usePrediction:Boolean = false; + + public var freezeSteps:int = 10; + public var linSpeedFreezeLimit:Number = 1; + public var angSpeedFreezeLimit:Number = 0.01; + + // Переменные для процедуры разделения тел путём непосредственного их перемещенеия. + // !!!!!!!!!!!!!!!!!!!!!!!! + // !!! Экспериментально !!! + // !!!!!!!!!!!!!!!!!!!!!!!! + public var staticSeparationIterations:int = 10; + public var staticSeparationSteps:int = 10; + public var maxAngleMove:Number = 10; + public var useStaticSeparation:Boolean = false; + + // Вектор гравитации + public var _gravity:Vector3 = new Vector3(0, 0, -9.8); + // Модуль вектора гравитации + public var _gravityMagnitude:Number = 9.8; + + // Использующийся детектор столкновений + public var collisionDetector:ICollisionDetector; + + // Список тел, участвующих в симуляции + public var bodies:BodyList = new BodyList(); + + // Список контактов на текущем шаге симуляции + altphysics var contacts:Contact; + // Количество контактов на текущем шаге симуляции +// altphysics var contactsNum:int; + // Список ограничений + altphysics var constraints:Vector. = new Vector.(); + // Количество ограничений + altphysics var constraintsNum:int; + // Временная метка. Число прошедших шагов с начала симуляции. + public var timeStamp:int; + // Время с начала симуляции, мс + public var time:int; + // Первый неиспользованный контакт на текущем шаге симуляции + private var borderContact:Contact; + + // Временные переменные для избежания создания экземпляров + private var _r:Vector3 = new Vector3(); + private var _t:Vector3 = new Vector3(); + private var _v:Vector3 = new Vector3(); + private var _v1:Vector3 = new Vector3(); + private var _v2:Vector3 = new Vector3(); + + /** + * + */ + public function PhysicsScene() { + contacts = new Contact(0); + var contact:Contact = contacts; + for (var i:int = 1; i < MAX_CONTACTS; i++) { + contact.next = new Contact(i); + contact = contact.next; + } + collisionDetector = new KdTreeCollisionDetector(); + } + + /** + * Вектор гравитации. + */ + public function get gravity():Vector3 { + return _gravity.vClone(); + } + + /** + * @private + */ + public function set gravity(value:Vector3):void { + _gravity.vCopy(value); + _gravityMagnitude = _gravity.vLength(); + } + + /** + * Добавляет тело в симуляцию. + * + * @param body + */ + public function addBody(body:Body):void { + body.id = lastBodyId++; + body.world = this; + bodies.append(body); + } + + /** + * Удаляет тело из симуляции. + * @param body + * @return + */ + public function removeBody(body:Body):void { + if (bodies.remove(body)) { + body.world = null; + } + } + + /** + * Добавляет ограничение. + * @param c + */ + public function addConstraint(c:Constraint):void { + constraints[constraintsNum++] = c; + c.world = this; + } + + /** + * Удаляет ограничение. + * @param c + */ + public function removeConstraint(c:Constraint):Boolean { + var idx:int = constraints.indexOf(c); + if (idx < 0) return false; + constraints.splice(idx, 1) + constraintsNum--; + c.world = null; + return true; + } + + /** + * Применяет к телам действующте на них силы. Аккумуляторы сил и моментов тел очищаются после завершения шага + * симуляции, поэтому на момент вызова метода могут уже содержать некоторые значения. + * + * @param dt промежуток времени, в течении которого действуют силы + */ + private function applyForces(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + body.beforePhysicsStep(dt); + body.calcAccelerations(); + // Ускорение свободного падения применяется только к подвижным телам во избежание некорректного изменения + // фиктивной скорости неподвижных тел. + if (body.movable && !body.frozen) { + body.accel.x += _gravity.x; + body.accel.y += _gravity.y; + body.accel.z += _gravity.z; + } + item = item.next; + } + } + + /** + * Определяет все столкновения на текущем шаге симуляции и заполняет список получившихся контактов. + * + * @param dt длительность шага симуляции + */ + private function detectCollisions(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + if (!body.frozen) { + body.contactsNum = 0; + body.saveState(); + // При включённом режиме предсказания состояние тел интегрируется на один шаг вперёд + if (usePrediction) { + body.integrateVelocity(dt); + body.integratePosition(dt); + } + body.calcDerivedData(); + } + item = item.next; + } + + borderContact = collisionDetector.getAllContacts(contacts); + + // Расчёт относительных векторов точки контакта вынесен сюда из-за необходимости учитывать + // положение тел в предсказанном состоянии + var contact:Contact = contacts; + while (contact != borderContact) { + var b1:Body = contact.body1; + var b2:Body = contact.body2; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + var bPos:Vector3 = b1.state.pos; + cp.r1.x = cp.pos.x - bPos.x; + cp.r1.y = cp.pos.y - bPos.y; + cp.r1.z = cp.pos.z - bPos.z; + if (b2 != null) { + bPos = b2.state.pos; + cp.r2.x = cp.pos.x - bPos.x; + cp.r2.y = cp.pos.y - bPos.y; + cp.r2.z = cp.pos.z - bPos.z; + } + } + contact = contact.next; + } + + // Восстановление состояния тел + if (usePrediction) { + item = bodies.head; + while (item != null) { + body = item.body; + if (!body.frozen) { + body.restoreState(); + body.calcDerivedData(); + } + item = item.next; + } + } + } + + /** + * Подготваливает полученные из детектора столкновений контакты, расчитывая значения, не меняющиеся + * в ходе шага симуляции. + */ + private function preProcessContacts(dt:Number):void { + var contact:Contact = contacts; + while (contact != borderContact) { + var b1:Body = contact.body1; + var b2:Body = contact.body2; + // Столкнувшиеся тела размораживаются + if (b1.frozen) { + b1.frozen = false; + b1.freezeCounter = 0; + } + if (b2 != null && b2.frozen) { + b2.frozen = false; + b2.freezeCounter = 0; + } + contact.restitution = b1.material.restitution; + if (b2 != null && b2.material.restitution < contact.restitution) contact.restitution = b2.material.restitution; + contact.friction = b1.material.friction; + if (b2 != null && b2.material.friction < contact.friction) contact.friction = b2.material.friction; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.accumImpulseN = 0; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + cp.velByUnitImpulseN = 0; + if (b1.movable) { + cp.angularInertia1 = _v.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1).vDot(contact.normal); + cp.velByUnitImpulseN += b1.invMass + cp.angularInertia1; + } + if (b2 != null && b2.movable) { + cp.angularInertia2 = _v.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2).vDot(contact.normal); + cp.velByUnitImpulseN += b2.invMass + cp.angularInertia2; + } + // Расчёт требуемой конечной скорости для упругого контакта + calcSepVelocity(b1, b2, cp, _v); + cp.normalVel = _v.vDot(contact.normal); + if (cp.normalVel < 0) cp.normalVel = - contact.restitution*cp.normalVel; + // Скорость разделения неупругого контакта + cp.minSepVel = cp.penetration > allowedPenetration ? (cp.penetration - allowedPenetration)/(penResolutionSteps*dt) : 0; + if (cp.minSepVel > maxPenResolutionSpeed) cp.minSepVel = maxPenResolutionSpeed; + } + contact = contact.next; + } + for (var i:int = 0; i < constraintsNum; i++) { + var constraint:Constraint = constraints[i]; + constraint.preProcess(dt); + } + } + + /** + * + * @param dt + * @param forceInelastic + */ + private function processContacts(dt:Number, forceInelastic:Boolean):void { + var iterNum:int = forceInelastic ? contactIterations : collisionIterations; + var i:int; + var forwardLoop:Boolean = false; + for (var iter:int = 0; iter < iterNum; iter++) { + forwardLoop = !forwardLoop; + var contact:Contact = contacts; + while (contact != borderContact) { + resolveContact(contact, forceInelastic, forwardLoop); + contact = contact.next; + } + // Ограничения + for (i = 0; i < constraintsNum; i++) { + var constraint:Constraint = constraints[i]; + constraint.apply(dt); + } + } + + // Разделение контактов путём непосредственного изменения координат и ориентации. Экспериментально. +// if (forceInelastic && useStaticSeparation) performStaticSeparation(); + } + + /** + * + */ + private function resolveContact(contactInfo:Contact, forceInelastic:Boolean, forwardLoop:Boolean):void { + var b1:Body = contactInfo.body1; + var b2:Body = contactInfo.body2; + var normal:Vector3 = contactInfo.normal; + var i:int; + if (forwardLoop) { + for (i = 0; i < contactInfo.pcount; i++) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } else { + for (i = contactInfo.pcount - 1; i >= 0; i--) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } + } + + /** + * + * @param idx + * @param b1 + * @param b2 + * @param colInfo + * @param normal + * @param forceInelastic + */ + private function resolveContactPoint(idx:int, b1:Body, b2:Body, contact:Contact, normal:Vector3, forceInelastic:Boolean):void { + var cp:ContactPoint = contact.points[idx]; + if (!forceInelastic) { + cp.satisfied = true; + } + + var newVel:Number = 0; + calcSepVelocity(b1, b2, cp, _v); + var cnormal:Vector3 = contact.normal; + var sepVel:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + if (forceInelastic) { + var minSpeVel:Number = useStaticSeparation ? 0 : cp.minSepVel; + if (sepVel < minSpeVel) { + cp.satisfied = false; + } else if (cp.satisfied) return; + newVel = minSpeVel; + } else { + newVel = cp.normalVel; + } + var deltaVel:Number = newVel - sepVel; + var impulse:Number = deltaVel/cp.velByUnitImpulseN; + var accumImpulse:Number = cp.accumImpulseN + impulse; + if (accumImpulse < 0) { + accumImpulse = 0; + } + var deltaImpulse:Number = accumImpulse - cp.accumImpulseN; + cp.accumImpulseN = accumImpulse; + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, normal, deltaImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, normal, -deltaImpulse); + } + + // Учёт силы трения + calcSepVelocity(b1, b2, cp, _v); + // Расчитываем изменение касательной скорости на единицу касательного импульса + var tanSpeedByUnitImpulse:Number = 0; + +// _v.vAddScaled(-_v.vDot(contact.normal), contact.normal); + var dot:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + _v.x -= dot*cnormal.x; + _v.y -= dot*cnormal.y; + _v.z -= dot*cnormal.z; + + var tanSpeed:Number = _v.vLength(); + if (tanSpeed < 0.001) return; + +// _t.vCopy(_v).vNormalize().vReverse(); + _t.x = -_v.x; + _t.y = -_v.y; + _t.z = -_v.z; + _t.vNormalize(); + + var r:Vector3; + var m:Matrix3; + var xx:Number; + var yy:Number; + var zz:Number; + // dV = b.invMass + ((invI * (r % t)) % r) * t + if (b1.movable) { +// _v.vCross2(cp.r1, _t).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1); + r = cp.r1; + m = b1.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b1.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + if (b2 != null && b2.movable) { +// _v.vCross2(cp.r2, _t).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2); + + r = cp.r2; + m = b2.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b2.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + + var tanImpulse:Number = tanSpeed/tanSpeedByUnitImpulse; + var max:Number = contact.friction*cp.accumImpulseN; + if (max < 0) { + if (tanImpulse < max) tanImpulse = max; + } else { + if (tanImpulse > max) tanImpulse = max; + } + + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, _t, tanImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, _t, -tanImpulse); + } + } + + /** + * + * @param cp + * @param normal + * @return + */ + private function calcSepVelocity(body1:Body, body2:Body, cp:ContactPoint, result:Vector3):void { + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 +// result.vCopy(body1.state.velocity).vAdd(_v1.vCross2(body1.state.rotation, cp.r1)); + var rot:Vector3 = body1.state.rotation; + var v:Vector3 = cp.r1; + var x:Number = rot.y*v.z - rot.z*v.y; + var y:Number = rot.z*v.x - rot.x*v.z; + var z:Number = rot.x*v.y - rot.y*v.x; + v = body1.state.velocity; + result.x = v.x + x; + result.y = v.y + y; + result.z = v.z + z; + // V2 = V2_c + w2%r2 + if (body2 != null) { +// result.vSubtract(body2.state.velocity).vSubtract(_v2.vCross2(body2.state.rotation, cp.r2)); + rot = body2.state.rotation; + v = cp.r2; + x = rot.y*v.z - rot.z*v.y; + y = rot.z*v.x - rot.x*v.z; + z = rot.x*v.y - rot.y*v.x; + v = body2.state.velocity; + result.x -= v.x + x; + result.y -= v.y + y; + result.z -= v.z + z; + } + } + + /** + * + * @param dt + */ + private function intergateVelocities(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + item.body.integrateVelocity(dt); + item = item.next; + } + } + + /** + * + * @param dt + */ + private function integratePositions(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + if (body.movable && !body.frozen) { + body.integratePosition(dt); + } + item = item.next; + } + } + + /** + * + */ + private function performStaticSeparation():void { + var iterNum:int = staticSeparationIterations; +// iterNum = 100; + + // 1. В начале каждой итерации для всех контактов сбрасывается флаг satisfied с одновременным поиском наихудшего контакта, т.е. имеющего наибольшую величину пересечения тел. + // 2. Если найденный контакт имеет величину пересечения меньше предельно допустимой процедура прерывается, т.к. разделение контактов не требуется. + // 3. Если процедура продолжается, то выполняется разделение найденного контакта и установка его флага satisfied в true, после чего запускается внутренний цикл, состоящий + // из contactsNum - 1 итераций. На каждой итерации ищется наихудший контакт среди оставшихся (satisfied == false). Если такой контакт найден, то выполняется его разделение + // и переход к следующей итерации внутреннего цикла, иначе внутренний цикл прерывается и выполняется переход к следующей итерации внешнего цикла (шаг 1.). +// for (var iter:int = 0; iter < iterNum; iter++) { +// // Ищем контакт с максимальной величиной пересечения тел, одновременно сбрасывая флаги satisfied в false +// var worstContact:Contact = contacts[0]; +// var i:int; +// for (i = 1; i < contactsNum; i++) { +// var contact:Contact = contacts[i]; +// contact.satisfied = false; +// if (contact.maxPenetration > worstContact.maxPenetration) worstContact = contact; +// } +// if (worstContact.maxPenetration <= allowedPenetration) return; +// +// resolveInterpenetration(worstContact); +// // Внутренний цикл по оставшимся контактам +// for (i = 1; i < contactsNum; i++) { +// worstContact = getWorstContact(); +// if (worstContact == null) break; +// resolveInterpenetration(worstContact); +// } +// } + } + + /** + * + */ +// private function getWorstContact():Contact { +// var maxPen:Number = 0; +// var worst:Contact = null; +// for (var i:int = 0; i < contactsNum; i++) { +// var c:Contact = contacts[i]; +// if (!c.satisfied && c.maxPenetration > maxPen) { +// worst = c; +// maxPen = c.maxPenetration; +// } +// } +// return maxPen > allowedPenetration ? worst : null; +// } + + /** + * Разделяет указанный контакт, выполняя staticSeparationSteps итераций по списку точек контакта. + * 1. В начале каждой итерации ищется точка с наибольшим пересечением, одновременно сбрасываются флаги satisfied у точек. + * 2. Для найденной точки выполняется процедура разделения. + * 3. После выполняется pcount - 1 итерация по списку точек, каждый раз ищется наихудшая среди имеющих satisfied == false, для которой выполняется процедура разделения. + * Если наихудшая точка не найдена, то выполнется переход к следующей итерации внешнего цикла (шаг 1). + * + * В конце процедуры разделения обновляются значения пересечений для остальных точек контакта, а также для всех прочих контактов, относящихся к телам текущего. + * + * @param contact контакт для разделения + */ + private function resolveInterpenetration(contact:Contact):void { + contact.satisfied = true; + + for (var step:int = 0; step < staticSeparationSteps; step++) { + var worstCp:ContactPoint = contact.points[0]; + var cp:ContactPoint; + var i:int; + for (i = 1; i < contact.pcount; i++) { + cp = contact.points[i]; + cp.satisfied = false; + if (cp.penetration > worstCp.penetration) worstCp = cp; + } + if (worstCp.penetration <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + // Разделяем оставшиеся точки + var maxPen:Number = 0; + for (i = 1; i < contact.pcount; i++) { + // Поиск наихудшей точки + for (var j:int = 0; j < contact.pcount; j++) { + cp = contact.points[j]; + if (cp.satisfied) continue; + if (cp.penetration > maxPen) { + maxPen = cp.penetration; + worstCp = cp; + } + } + if (maxPen <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + } + } + } + + /** + * + * @param cp + * @param contact + */ + private function separateContactPoint(cp:ContactPoint, contact:Contact):void { +// cp.satisfied = true; +// +// var b1:Body = contact.body1; +// var b2:Body = contact.body2; +// var totalMove:Number = cp.penetration - allowedPenetration; +// var moveCoeff:Number = totalMove/cp.velByUnitImpulseN; +// var linMove1:Number; +// var angleMove1:Number; +// if (b1.movable) { +// linMove1 = b1.invMass*moveCoeff; +// angleMove1 = cp.angularInertia1*moveCoeff; +// if (angleMove1 > maxAngleMove) { +// linMove1 += angleMove1 - maxAngleMove; +// angleMove1 -= maxAngleMove; +// } +// b1.state.pos.vAddScaled(linMove1, contact.normal); +// _v1.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vScale(angleMove1); +// b1.state.orientation.addScaledVector(_v1, 1); +// } +// var linMove2:Number; +// var angleMove2:Number; +// if (b2 != null && b2.movable) { +// linMove2 = b2.invMass*moveCoeff; +// angleMove2 = cp.angularInertia2*moveCoeff; +// if (angleMove2 > maxAngleMove) { +// linMove2 += angleMove2 - maxAngleMove; +// angleMove2 -= maxAngleMove; +// } +// b2.state.pos.vAddScaled(-linMove2, contact.normal); +// _v2.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vScale(angleMove2); +// _v2.vReverse(); +// b2.state.orientation.addScaledVector(_v2, 1); +// } +// cp.penetration = allowedPenetration; +// // Обновляем пересечения в других точках +// var i:int; +// for (i = 0; i < contact.pcount; i++) { +// var cp1:ContactPoint = contact.points[i]; +// if (cp1 == cp) continue; +// var angularMove:Number; +// if (b1.movable) { +// angularMove = _v.vCross2(_v1, cp1.r1).vDot(contact.normal); +// cp1.penetration -= linMove1 + angularMove; +// } +// if (b2 != null && b2.movable) { +// angularMove = _v.vCross2(_v2, cp1.r2).vDot(contact.normal); +// cp1.penetration -= linMove2 - angularMove; +// } +// // Обновление максимального значения для контакта +// if (cp1.penetration > contact.maxPenetration) contact.maxPenetration = cp1.penetration; +// } +// // Обновляем пересечения для других контактов +// var c:Contact; +// var j:int; +// if (b1.movable) { +// for (i = 0; i < b1.contactsNum; i++) { +// c = b1.contacts[i]; +// if (c == contact) continue; +// for (j = 0; j < c.pcount; j++) { +// cp1 = c.points[j]; +// if (b1 == c.body1) cp1.penetration -= linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r1).vDot(c.normal); +// else cp1.penetration += linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r2).vDot(c.normal); +// if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; +// } +// } +// } +// if (b2 != null && b2.movable) { +// for (i = 0; i < b2.contactsNum; i++) { +// c = b2.contacts[i]; +// if (c == contact) continue; +// for (j = 0; j < c.pcount; j++) { +// cp1 = c.points[j]; +// if (b2 == c.body1) cp1.penetration -= linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r1).vDot(c.normal); +// else cp1.penetration += linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r2).vDot(c.normal); +// if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; +// } +// } +// } + } + + /** + * + */ + private function postPhysics():void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + body.clearAccumulators(); + body.calcDerivedData(); + if (body.canFreeze) { + if (body.state.velocity.vLength() < linSpeedFreezeLimit && body.state.rotation.vLength() < angSpeedFreezeLimit) { + if (!body.frozen) { + body.freezeCounter++; + if (body.freezeCounter >= freezeSteps) body.frozen = true; + } + } else { + body.freezeCounter = 0; + body.frozen = false; + } + } + item = item.next; + } + } + + /** + * + * @param delta + */ + public function update(delta:int):void { + timeStamp++; + time += delta; + var dt:Number = 0.001*delta; + applyForces(dt); + detectCollisions(dt); + preProcessContacts(dt); + processContacts(dt, false); + intergateVelocities(dt); + processContacts(dt, true); + integratePositions(dt); + postPhysics(); + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/.svn/text-base/PhysicsUtils.as.svn-base b/0.0.9.4/src/alternativa/physics/.svn/text-base/PhysicsUtils.as.svn-base new file mode 100644 index 0000000..5a2e46f --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/.svn/text-base/PhysicsUtils.as.svn-base @@ -0,0 +1,49 @@ +package alternativa.physics { + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + public class PhysicsUtils { + + /** + * @param mass + * @param halfSize + * @param result + */ + public static function getBoxInvInertia(mass:Number, halfSize:Vector3, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + result.a = 3/(mass*(yy + zz)); + result.f = 3/(mass*(zz + xx)); + result.k = 3/(mass*(xx + yy)); + } + + /** + * @param mass + * @param r + * @param h + * @param result + */ + public static function getCylinderInvInertia(mass:Number, r:Number, h:Number, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + + result.a = result.f = 1/(mass*(h*h/12 + r*r/4)); + result.k = 2/(mass*r*r); + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base b/0.0.9.4/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base new file mode 100644 index 0000000..2715307 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base @@ -0,0 +1,3 @@ +package alternativa.physics { + public namespace altphysics = "http://alternativaplatform.com/en/altphysics"; +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/Body.as b/0.0.9.4/src/alternativa/physics/Body.as new file mode 100644 index 0000000..cd22c5b --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/Body.as @@ -0,0 +1,405 @@ +package alternativa.physics { + + import alternativa.math.Matrix3; + import alternativa.math.Matrix4; + import alternativa.math.Quaternion; + import alternativa.math.Vector3; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.IBodyCollisionPredicate; + import alternativa.physics.collision.types.BoundBox; + + use namespace altphysics; + + /** + * + */ + public class Body { + + public static var linDamping:Number = 0.997; + public static var rotDamping:Number = 0.997; + + // Идентификатор тела, уникальный в пределах мира + public var id:int; + // Имя тела + public var name:String; + // Мир, в котором находится тело + public var world:PhysicsScene; + // Флаг подвижности тела + public var movable:Boolean = true; + // Флаг указывает, может ли тело быть заморожено + public var canFreeze:Boolean = false; + + public var freezeCounter:int; + public var frozen:Boolean = false; + // Ограничивающий бокс тела + public var aabb:BoundBox = new BoundBox(); + // Предикат, через который тело получает сообщения о столкновениях + public var postCollisionPredicate:IBodyCollisionPredicate; + + // Текущее состояние тела + public var state:BodyState = new BodyState(); + // Предыдущее состояние тела + public var prevState:BodyState = new BodyState(); + // Линейное ускорение тела на текущем шаге симуляции + public var accel:Vector3 = new Vector3(); + // Угловое ускорение тела на текущем шаге симуляции + public var angleAccel:Vector3 = new Vector3(); + // Физический материал тела + public var material:BodyMaterial = new BodyMaterial(); + // Обратная масса тела + public var invMass:Number = 1; + // Обратная матрица тензора инерции в локальных координатах + public var invInertia:Matrix3 = new Matrix3(); + // Обратная матрица тензора инерции в мировых координатах + public var invInertiaWorld:Matrix3 = new Matrix3(); + // Базисная матрица тела в мировых координатах + public var baseMatrix:Matrix3 = new Matrix3(); + + public const MAX_CONTACTS:int = 20; + public var contacts:Vector. = new Vector.(MAX_CONTACTS); + public var contactsNum:int; + + public var collisionPrimitives:CollisionPrimitiveList; + + // Аккумулятор сил + public var forceAccum:Vector3 = new Vector3(); + // Аккумулятор моментов + public var torqueAccum:Vector3 = new Vector3(); + + // Внутренние переменные для избежания создания экземпляров + private static var _r:Vector3 = new Vector3(); + private static var _f:Vector3 = new Vector3(); + + /** + * + * @param invMass + * @param invInertia + */ + public function Body(invMass:Number, invInertia:Matrix3) { + this.invMass = invMass; + this.invInertia.copy(invInertia); + } + + /** + * @param primitive + * @param localTransform + */ + public function addCollisionPrimitive(primitive:CollisionPrimitive, localTransform:Matrix4 = null):void { + if (primitive == null) { + throw new ArgumentError("Primitive cannot be null"); + } + if (collisionPrimitives == null) { + collisionPrimitives = new CollisionPrimitiveList(); + } + collisionPrimitives.append(primitive); + primitive.setBody(this, localTransform); + } + + /** + * + * @param primitive + */ + public function removeCollisionPrimitive(primitive:CollisionPrimitive):void { + if (collisionPrimitives == null) return; + primitive.setBody(null); + collisionPrimitives.remove(primitive); + if (collisionPrimitives.size == 0) { + collisionPrimitives = null; + } + } + + /** + * @param t + * @param result + */ + public function interpolate(t:Number, pos:Vector3, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.w = prevState.orientation.w*t1 + state.orientation.w*t; + orientation.x = prevState.orientation.x*t1 + state.orientation.x*t; + orientation.y = prevState.orientation.y*t1 + state.orientation.y*t; + orientation.z = prevState.orientation.z*t1 + state.orientation.z*t; + } + + /** + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + state.pos.vCopy(pos); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setPositionXYZ(x:Number, y:Number, z:Number):void { + state.pos.vReset(x, y, z); + } + + /** + * + * @param vel + */ + public function setVelocity(vel:Vector3):void { + state.velocity.vCopy(vel); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setVelocityXYZ(x:Number, y:Number, z:Number):void { + state.velocity.vReset(x, y, z); + } + + /** + * + * @param rot + */ + public function setRotation(rot:Vector3):void { + state.rotation.vCopy(rot); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setRotationXYZ(x:Number, y:Number, z:Number):void { + state.rotation.vReset(x, y, z); + } + + /** + * + * @param q + */ + public function setOrientation(q:Quaternion):void { + state.orientation.copy(q); + } + + /** + * @param r + * @param dir + * @param magnitude + */ + public function applyRelPosWorldImpulse(r:Vector3, dir:Vector3, magnitude:Number):void { + var d:Number = magnitude*invMass; + // Линейная часть + state.velocity.x += d*dir.x; + state.velocity.y += d*dir.y; + state.velocity.z += d*dir.z; + + // Вращательная часть + var x:Number = (r.y*dir.z - r.z*dir.y)*magnitude; + var y:Number = (r.z*dir.x - r.x*dir.z)*magnitude; + var z:Number = (r.x*dir.y - r.y*dir.x)*magnitude; + + state.rotation.x += invInertiaWorld.a*x + invInertiaWorld.b*y + invInertiaWorld.c*z; + state.rotation.y += invInertiaWorld.e*x + invInertiaWorld.f*y + invInertiaWorld.g*z; + state.rotation.z += invInertiaWorld.i*x + invInertiaWorld.j*y + invInertiaWorld.k*z; + } + + /** + * @param f + */ + public function addForce(f:Vector3):void { + forceAccum.vAdd(f); + } + + /** + * + * @param fx + * @param fy + * @param fz + */ + public function addForceXYZ(fx:Number, fy:Number, fz:Number):void { + forceAccum.x += fx; + forceAccum.y += fy; + forceAccum.z += fz; + } + + /** + * @param pos + * @param f + */ + public function addWorldForceXYZ(px:Number, py:Number, pz:Number, fx:Number, fy:Number, fz:Number):void { + forceAccum.x += fx; + forceAccum.y += fy; + forceAccum.z += fz; + + var pos:Vector3 = state.pos; + var rx:Number = px - pos.x; + var ry:Number = py - pos.y; + var rz:Number = pz - pos.z; + +// var x:Number = ry*fz - rz*fy; +// var y:Number = rz*fx - rx*fz; +// var z:Number = rx*fy - ry*fx; + + torqueAccum.x += ry*fz - rz*fy; + torqueAccum.y += rz*fx - rx*fz; + torqueAccum.z += rx*fy - ry*fx; + } + + /** + * @param pos + * @param f + */ + public function addWorldForce(pos:Vector3, force:Vector3):void { + forceAccum.vAdd(force); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(force)); + } + + /** + * @param pos + * @param f + */ + public function addWorldForceScaled(pos:Vector3, force:Vector3, scale:Number):void { + _f.x = scale*force.x; + _f.y = scale*force.y; + _f.z = scale*force.z; + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(_f)); + } + + /** + * @param pos + * @param f + */ + public function addLocalForce(pos:Vector3, force:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(pos, _r); + // Трансформируем вектор силы в мировую систему + baseMatrix.transformVector(force, _f); + // Добавляем силу и момент + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vCross(_f)); + } + + /** + * + * @param localPos + * @param worldForce + */ + public function addWorldForceAtLocalPoint(localPos:Vector3, worldForce:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(localPos, _r); + // Добавляем силу и момент + forceAccum.vAdd(worldForce); + torqueAccum.vAdd(_r.vCross(worldForce)); + } + + /** + * @param dt + */ + public function beforePhysicsStep(dt:Number):void { + } + + /** + * @param t + */ + public function addTorque(t:Vector3):void { + torqueAccum.vAdd(t); + } + + /** + * + */ + altphysics function clearAccumulators():void { + forceAccum.x = forceAccum.y = forceAccum.z = 0; + torqueAccum.x = torqueAccum.y = torqueAccum.z = 0; + } + + /** + * + */ + altphysics function calcAccelerations():void { + accel.x = forceAccum.x*invMass; + accel.y = forceAccum.y*invMass; + accel.z = forceAccum.z*invMass; + angleAccel.x = invInertiaWorld.a*torqueAccum.x + invInertiaWorld.b*torqueAccum.y + invInertiaWorld.c*torqueAccum.z; + angleAccel.y = invInertiaWorld.e*torqueAccum.x + invInertiaWorld.f*torqueAccum.y + invInertiaWorld.g*torqueAccum.z; + angleAccel.z = invInertiaWorld.i*torqueAccum.x + invInertiaWorld.j*torqueAccum.y + invInertiaWorld.k*torqueAccum.z; + } + + /** + * Вычисляет производные данные. + */ + public function calcDerivedData():void { + // Вычисление базисной матрицы и обратного тензора инерции в мировых координатах + state.orientation.toMatrix3(baseMatrix); + invInertiaWorld.copy(invInertia).append(baseMatrix).prependTransposed(baseMatrix); + if (collisionPrimitives != null) { + aabb.infinity(); + var item:CollisionPrimitiveListItem = collisionPrimitives.head; + while (item != null) { + var primitive:CollisionPrimitive = item.primitive; + primitive.transform.setFromMatrix3(baseMatrix, state.pos); + if (primitive.localTransform != null) { + primitive.transform.prepend(primitive.localTransform); + } + primitive.calculateAABB(); + aabb.addBoundBox(primitive.aabb); + item = item.next; + } + } + } + + /** + * + */ + altphysics function saveState():void { + prevState.copy(state); + } + + /** + * + */ + altphysics function restoreState():void { + state.copy(prevState); + } + + /** + * @param dt + */ + altphysics function integrateVelocity(dt:Number):void { + // v = v + a*t + state.velocity.x += accel.x*dt; + state.velocity.y += accel.y*dt; + state.velocity.z += accel.z*dt; + // rot = rot + eps*t + state.rotation.x += angleAccel.x*dt; + state.rotation.y += angleAccel.y*dt; + state.rotation.z += angleAccel.z*dt; + + state.velocity.x *= linDamping; + state.velocity.y *= linDamping; + state.velocity.z *= linDamping; + + state.rotation.x *= rotDamping; + state.rotation.y *= rotDamping; + state.rotation.z *= rotDamping; + } + + /** + * + */ + altphysics function integratePosition(dt:Number):void { + // pos = pos + v*t + state.pos.x += state.velocity.x*dt; + state.pos.y += state.velocity.y*dt; + state.pos.z += state.velocity.z*dt; + // q = q + 0.5*rot*q + state.orientation.addScaledVector(state.rotation, dt); + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/BodyList.as b/0.0.9.4/src/alternativa/physics/BodyList.as new file mode 100644 index 0000000..aa8f79f --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/BodyList.as @@ -0,0 +1,76 @@ +package alternativa.physics { + + /** + * + */ + public class BodyList { + + public var head:BodyListItem; + public var tail:BodyListItem; + public var size:int; + + /** + * + */ + public function BodyList() { + } + + /** + * + * @param body + */ + public function append(body:Body):void { + var item:BodyListItem = BodyListItem.create(body); + if (head == null) { + head = tail = item; + } else { + tail.next = item; + item.prev = tail; + tail = item; + } + size++; + } + + /** + * + * @param body + */ + public function remove(body:Body):Boolean { + var item:BodyListItem = findItem(body); + if (item == null) return false; + if (item == head) { + if (size == 1) { + head = tail = null; + } else { + head = item.next; + head.prev = null; + } + } else { + if (item == tail) { + tail = item.prev; + tail.next = null; + } else { + item.prev.next = item.next; + item.next.prev = item.prev; + } + } + item.dispose(); + size--; + return true; + } + + /** + * + * @param body + * @return + */ + public function findItem(body:Body):BodyListItem { + var item:BodyListItem = head; + while (item != null && item.body != body) { + item = item.next; + } + return item; + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/BodyListItem.as b/0.0.9.4/src/alternativa/physics/BodyListItem.as new file mode 100644 index 0000000..382e11c --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/BodyListItem.as @@ -0,0 +1,74 @@ +package alternativa.physics { + + /** + * + */ + public class BodyListItem { + + // Верхний элемент хранилища + private static var poolTop:BodyListItem; + + /** + * Создаёт новый элемент списка. + * + * @param primitive примитив, содержащийся в элементе + * @return новый элемент списка + */ + public static function create(body:Body):BodyListItem { + var item:BodyListItem; + if (poolTop == null) { + item = new BodyListItem(body); + } else { + item = poolTop; + poolTop = item.next; + item.next = null; + item.body = body; + } + return item; + } + + /** + * Очищает хранилище. + */ + public static function clearPool():void { + var item:BodyListItem = poolTop; + while (item != null) { + poolTop = item.next; + item.next = null; + item = poolTop; + } + } + + /** + * + */ + public var body:Body; + /** + * + */ + public var next:BodyListItem; + /** + * + */ + public var prev:BodyListItem; + + /** + * + * @param body + */ + public function BodyListItem(body:Body) { + this.body = body; + } + + /** + * + */ + public function dispose():void { + body = null; + prev = null; + next = poolTop; + poolTop = this; + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/BodyMaterial.as b/0.0.9.4/src/alternativa/physics/BodyMaterial.as new file mode 100644 index 0000000..7a7c47d --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/BodyMaterial.as @@ -0,0 +1,11 @@ +package alternativa.physics { + + public class BodyMaterial { + + public var restitution:Number = 0; + public var friction:Number = 0.3; +// public var dynamicFriction:Number = 0.2; +// public var dynamicFriction:Number = 0.2; + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/BodyState.as b/0.0.9.4/src/alternativa/physics/BodyState.as new file mode 100644 index 0000000..3987388 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/BodyState.as @@ -0,0 +1,39 @@ +package alternativa.physics { + import alternativa.math.Quaternion; + import alternativa.math.Vector3; + + /** + * Класс описывает состояние твёрдого тела. + */ + public class BodyState { + /** + * Положение тела. + */ + public var pos:Vector3 = new Vector3(); + /** + * Ориентация тела. + */ + public var orientation:Quaternion = new Quaternion(); + /** + * Скорость тела. + */ + public var velocity:Vector3 = new Vector3(); + /** + * Угловая скорость тела. + */ + public var rotation:Vector3 = new Vector3(); + + /** + * Копирует значение указанного объекта. + * + * @param state + */ + public function copy(state:BodyState):void { + pos.vCopy(state.pos); + orientation.copy(state.orientation); + velocity.vCopy(state.velocity); + rotation.vCopy(state.rotation); + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/CollisionPrimitiveList.as b/0.0.9.4/src/alternativa/physics/CollisionPrimitiveList.as new file mode 100644 index 0000000..5d5304f --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/CollisionPrimitiveList.as @@ -0,0 +1,89 @@ +package alternativa.physics { + import alternativa.physics.collision.CollisionPrimitive; + + /** + * + */ + public class CollisionPrimitiveList { + + public var head:CollisionPrimitiveListItem; + public var tail:CollisionPrimitiveListItem; + public var size:int; + + /** + * + */ + public function CollisionPrimitiveList() { + } + + /** + * + * @param primitive + */ + public function append(primitive:CollisionPrimitive):void { + var item:CollisionPrimitiveListItem = CollisionPrimitiveListItem.create(primitive); + if (head == null) { + head = tail = item; + } else { + tail.next = item; + item.prev = tail; + tail = item; + } + size++; + } + + /** + * + * @param primitve + */ + public function remove(primitve:CollisionPrimitive):void { + var item:CollisionPrimitiveListItem = findItem(primitve); + if (item == null) return; + if (item == head) { + if (size == 1) { + head = tail = null; + } else { + head = item.next; + head.prev = null; + } + } else { + if (item == tail) { + tail = tail.prev; + tail.next = null; + } else { + item.prev.next = item.next; + item.next.prev = item.prev; + } + } + item.dispose(); + size--; + } + + /** + * + * @param primitive + * @return + */ + public function findItem(primitive:CollisionPrimitive):CollisionPrimitiveListItem { + var item:CollisionPrimitiveListItem = head; + while (item != null && item.primitive != primitive) { + item = item.next; + } + return item; + } + + /** + * + */ + public function clear():void { + while (head != null) { + var item:CollisionPrimitiveListItem = head; + head = head.next; + item.dispose(); + } + tail = null; + size = 0; + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/CollisionPrimitiveListItem.as b/0.0.9.4/src/alternativa/physics/CollisionPrimitiveListItem.as new file mode 100644 index 0000000..d204532 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/CollisionPrimitiveListItem.as @@ -0,0 +1,76 @@ +package alternativa.physics { + import alternativa.physics.collision.CollisionPrimitive; + + /** + * Элемент списка примитивов. + */ + public class CollisionPrimitiveListItem { + + // Верхний элемент хранилища + private static var poolTop:CollisionPrimitiveListItem; + + /** + * Создаёт новый элемент списка. + * + * @param primitive примитив, содержащийся в элементе + * @return новый элемент списка + */ + public static function create(primitive:CollisionPrimitive):CollisionPrimitiveListItem { + var item:CollisionPrimitiveListItem; + if (poolTop == null) { + item = new CollisionPrimitiveListItem(primitive); + } else { + item = poolTop; + item.primitive = primitive; + poolTop = item.next; + item.next = null; + } + return item; + } + + /** + * Очищает хранилище. + */ + public static function clearPool():void { + var curr:CollisionPrimitiveListItem = poolTop; + while (curr != null) { + poolTop = curr.next; + curr.next = null; + curr = poolTop; + } + } + + /** + * Примитив, хранящийся в элементе списка. + */ + public var primitive:CollisionPrimitive; + /** + * Ссылка не следующий элемент списка. + */ + public var next:CollisionPrimitiveListItem; + /** + * Ссылка не предыдущий элемент списка. + */ + public var prev:CollisionPrimitiveListItem; + + /** + * Создаёт новый экземпляр. + * + * @param primitive примитив, хранящийся в элементе списка + */ + public function CollisionPrimitiveListItem(primitive:CollisionPrimitive) { + this.primitive = primitive; + } + + /** + * Очищает внутренние ссылки и помещает элемент в хранилище для дальнейшего использования. + */ + public function dispose():void { + primitive = null; + prev = null; + next = poolTop; + poolTop = this; + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/Contact.as b/0.0.9.4/src/alternativa/physics/Contact.as new file mode 100644 index 0000000..3f010ec --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/Contact.as @@ -0,0 +1,69 @@ +package alternativa.physics { + + + import alternativa.math.Vector3; + + /** + * Описывает контакт одного или двух тел. + */ + public class Contact { + /** + * Первое тело контакта, не может быть равно null. + */ + public var body1:Body; + /** + * Второе тело контакта, может быть равно null. + */ + public var body2:Body; + /** + * Предрассчитанный взаимный коэффициент отскока. + */ + public var restitution:Number; + /** + * Предрассчитанный взаимный коэффициент трения. + */ + public var friction:Number; + /** + * Нормаль контакта. Направлена к первому телу. + */ + public var normal:Vector3 = new Vector3(); + /** + * Список точек контакта. + */ + public var points:Vector. = new Vector.(MAX_POINTS, true); + /** + * Количество точек контакта. + */ + public var pcount:int; + /** + * Максимальная глубина пересечения. + */ + public var maxPenetration:Number = 0; + /** + * Флаг показывает, разрешён контакт или нет. + */ + public var satisfied:Boolean; + /** + * Следующий контакт в списке. + */ + public var next:Contact; + /** + * Индекс контакта. Первый контакт в списке имеет индекс 0. + */ + public var index:int; + + // Максимальное количество точек контакта + private const MAX_POINTS:int = 8; + + /** + * + */ + public function Contact(index:int) { + this.index = index; + for (var i:int = 0; i < MAX_POINTS; i++) { + points[i] = new ContactPoint(); + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/ContactPoint.as b/0.0.9.4/src/alternativa/physics/ContactPoint.as new file mode 100644 index 0000000..395d2dc --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/ContactPoint.as @@ -0,0 +1,55 @@ +package alternativa.physics { + import alternativa.math.Vector3; + + /** + * + */ + public class ContactPoint { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + + // Величины, расчитываемые перед началом фазы решения контактов + + // Требуемая проекция конечной скорости на нормаль для упругого контакта + public var normalVel:Number; + // Минимальная скорость разделения неупругого контакта + public var minSepVel:Number; + // Изменение проекции скорости на единицу нормального импульса + public var velByUnitImpulseN:Number; + + public var angularInertia1:Number; + public var angularInertia2:Number; + + // Радиус-вектор точки контакта относительно центра первого тела + public var r1:Vector3 = new Vector3(); + // Радиус-вектор точки контакта относительно центра второго тела + public var r2:Vector3 = new Vector3(); + + // Величины, накапливаемые во время фазы решения контактов + + // Накопленный импульс, применяемый для получения требуемой относительной скорости в точке контакта + public var accumImpulseN:Number; + // Накопленный импульс, применяемый для разделения тел в точке контакта. Не создаёт момента. +// public var accumSepImpulse:Number; + + public var satisfied:Boolean; + + /** + * + * @param cp + */ + public function copyFrom(cp:ContactPoint):void { + pos.vCopy(cp.pos); + penetration = cp.penetration; + feature1 = cp.feature1; + feature2 = cp.feature2; + r1.vCopy(cp.r1); + r2.vCopy(cp.r2); + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/PhysicsScene.as b/0.0.9.4/src/alternativa/physics/PhysicsScene.as new file mode 100644 index 0000000..abdc762 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/PhysicsScene.as @@ -0,0 +1,743 @@ +package alternativa.physics { + + + import alternativa.physics.collision.ICollisionDetector; + import alternativa.physics.collision.KdTreeCollisionDetector; + import alternativa.physics.constraints.Constraint; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * Класс реализует физическую симуляцию поведения твёрдых тел. + */ + public class PhysicsScene { + + private static var lastBodyId:int; + + // Максимальное количество контактов + public const MAX_CONTACTS:int = 1000; + + ////////////////////////////////////////////////////////////////// + // Настроечные параметры симуляции. Могут быть изменены в любой + // момент времени без нарушения корректной работы симуляции. + ////////////////////////////////////////////////////////////////// + // Количество шагов, за которое пересекающиеся тела должны разделиться + public var penResolutionSteps:int = 10; + // Величина допустимой глубины пересечения + public var allowedPenetration:Number = 0.1; + // Максимальная скорость, добавляемая с целью разделения тел + public var maxPenResolutionSpeed:Number = 0.5; + // Количество итераций для обработки упругих контактов + public var collisionIterations:int = 5; + // Количество итераций для обработки неупругих контактов + public var contactIterations:int = 5; + // Флаг использования предсказания состояний + public var usePrediction:Boolean = false; + + public var freezeSteps:int = 10; + public var linSpeedFreezeLimit:Number = 1; + public var angSpeedFreezeLimit:Number = 0.01; + + // Переменные для процедуры разделения тел путём непосредственного их перемещенеия. + // !!!!!!!!!!!!!!!!!!!!!!!! + // !!! Экспериментально !!! + // !!!!!!!!!!!!!!!!!!!!!!!! + public var staticSeparationIterations:int = 10; + public var staticSeparationSteps:int = 10; + public var maxAngleMove:Number = 10; + public var useStaticSeparation:Boolean = false; + + // Вектор гравитации + public var _gravity:Vector3 = new Vector3(0, 0, -9.8); + // Модуль вектора гравитации + public var _gravityMagnitude:Number = 9.8; + + // Использующийся детектор столкновений + public var collisionDetector:ICollisionDetector; + + // Список тел, участвующих в симуляции + public var bodies:BodyList = new BodyList(); + + // Список контактов на текущем шаге симуляции + altphysics var contacts:Contact; + // Количество контактов на текущем шаге симуляции +// altphysics var contactsNum:int; + // Список ограничений + altphysics var constraints:Vector. = new Vector.(); + // Количество ограничений + altphysics var constraintsNum:int; + // Временная метка. Число прошедших шагов с начала симуляции. + public var timeStamp:int; + // Время с начала симуляции, мс + public var time:int; + // Первый неиспользованный контакт на текущем шаге симуляции + private var borderContact:Contact; + + // Временные переменные для избежания создания экземпляров + private var _r:Vector3 = new Vector3(); + private var _t:Vector3 = new Vector3(); + private var _v:Vector3 = new Vector3(); + private var _v1:Vector3 = new Vector3(); + private var _v2:Vector3 = new Vector3(); + + /** + * + */ + public function PhysicsScene() { + contacts = new Contact(0); + var contact:Contact = contacts; + for (var i:int = 1; i < MAX_CONTACTS; i++) { + contact.next = new Contact(i); + contact = contact.next; + } + collisionDetector = new KdTreeCollisionDetector(); + } + + /** + * Вектор гравитации. + */ + public function get gravity():Vector3 { + return _gravity.vClone(); + } + + /** + * @private + */ + public function set gravity(value:Vector3):void { + _gravity.vCopy(value); + _gravityMagnitude = _gravity.vLength(); + } + + /** + * Добавляет тело в симуляцию. + * + * @param body + */ + public function addBody(body:Body):void { + body.id = lastBodyId++; + body.world = this; + bodies.append(body); + } + + /** + * Удаляет тело из симуляции. + * @param body + * @return + */ + public function removeBody(body:Body):void { + if (bodies.remove(body)) { + body.world = null; + } + } + + /** + * Добавляет ограничение. + * @param c + */ + public function addConstraint(c:Constraint):void { + constraints[constraintsNum++] = c; + c.world = this; + } + + /** + * Удаляет ограничение. + * @param c + */ + public function removeConstraint(c:Constraint):Boolean { + var idx:int = constraints.indexOf(c); + if (idx < 0) return false; + constraints.splice(idx, 1) + constraintsNum--; + c.world = null; + return true; + } + + /** + * Применяет к телам действующте на них силы. Аккумуляторы сил и моментов тел очищаются после завершения шага + * симуляции, поэтому на момент вызова метода могут уже содержать некоторые значения. + * + * @param dt промежуток времени, в течении которого действуют силы + */ + private function applyForces(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + body.beforePhysicsStep(dt); + body.calcAccelerations(); + // Ускорение свободного падения применяется только к подвижным телам во избежание некорректного изменения + // фиктивной скорости неподвижных тел. + if (body.movable && !body.frozen) { + body.accel.x += _gravity.x; + body.accel.y += _gravity.y; + body.accel.z += _gravity.z; + } + item = item.next; + } + } + + /** + * Определяет все столкновения на текущем шаге симуляции и заполняет список получившихся контактов. + * + * @param dt длительность шага симуляции + */ + private function detectCollisions(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + if (!body.frozen) { + body.contactsNum = 0; + body.saveState(); + // При включённом режиме предсказания состояние тел интегрируется на один шаг вперёд + if (usePrediction) { + body.integrateVelocity(dt); + body.integratePosition(dt); + } + body.calcDerivedData(); + } + item = item.next; + } + + borderContact = collisionDetector.getAllContacts(contacts); + + // Расчёт относительных векторов точки контакта вынесен сюда из-за необходимости учитывать + // положение тел в предсказанном состоянии + var contact:Contact = contacts; + while (contact != borderContact) { + var b1:Body = contact.body1; + var b2:Body = contact.body2; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + var bPos:Vector3 = b1.state.pos; + cp.r1.x = cp.pos.x - bPos.x; + cp.r1.y = cp.pos.y - bPos.y; + cp.r1.z = cp.pos.z - bPos.z; + if (b2 != null) { + bPos = b2.state.pos; + cp.r2.x = cp.pos.x - bPos.x; + cp.r2.y = cp.pos.y - bPos.y; + cp.r2.z = cp.pos.z - bPos.z; + } + } + contact = contact.next; + } + + // Восстановление состояния тел + if (usePrediction) { + item = bodies.head; + while (item != null) { + body = item.body; + if (!body.frozen) { + body.restoreState(); + body.calcDerivedData(); + } + item = item.next; + } + } + } + + /** + * Подготваливает полученные из детектора столкновений контакты, расчитывая значения, не меняющиеся + * в ходе шага симуляции. + */ + private function preProcessContacts(dt:Number):void { + var contact:Contact = contacts; + while (contact != borderContact) { + var b1:Body = contact.body1; + var b2:Body = contact.body2; + // Столкнувшиеся тела размораживаются + if (b1.frozen) { + b1.frozen = false; + b1.freezeCounter = 0; + } + if (b2 != null && b2.frozen) { + b2.frozen = false; + b2.freezeCounter = 0; + } + contact.restitution = b1.material.restitution; + if (b2 != null && b2.material.restitution < contact.restitution) contact.restitution = b2.material.restitution; + contact.friction = b1.material.friction; + if (b2 != null && b2.material.friction < contact.friction) contact.friction = b2.material.friction; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.accumImpulseN = 0; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + cp.velByUnitImpulseN = 0; + if (b1.movable) { + cp.angularInertia1 = _v.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1).vDot(contact.normal); + cp.velByUnitImpulseN += b1.invMass + cp.angularInertia1; + } + if (b2 != null && b2.movable) { + cp.angularInertia2 = _v.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2).vDot(contact.normal); + cp.velByUnitImpulseN += b2.invMass + cp.angularInertia2; + } + // Расчёт требуемой конечной скорости для упругого контакта + calcSepVelocity(b1, b2, cp, _v); + cp.normalVel = _v.vDot(contact.normal); + if (cp.normalVel < 0) cp.normalVel = - contact.restitution*cp.normalVel; + // Скорость разделения неупругого контакта + cp.minSepVel = cp.penetration > allowedPenetration ? (cp.penetration - allowedPenetration)/(penResolutionSteps*dt) : 0; + if (cp.minSepVel > maxPenResolutionSpeed) cp.minSepVel = maxPenResolutionSpeed; + } + contact = contact.next; + } + for (var i:int = 0; i < constraintsNum; i++) { + var constraint:Constraint = constraints[i]; + constraint.preProcess(dt); + } + } + + /** + * + * @param dt + * @param forceInelastic + */ + private function processContacts(dt:Number, forceInelastic:Boolean):void { + var iterNum:int = forceInelastic ? contactIterations : collisionIterations; + var i:int; + var forwardLoop:Boolean = false; + for (var iter:int = 0; iter < iterNum; iter++) { + forwardLoop = !forwardLoop; + var contact:Contact = contacts; + while (contact != borderContact) { + resolveContact(contact, forceInelastic, forwardLoop); + contact = contact.next; + } + // Ограничения + for (i = 0; i < constraintsNum; i++) { + var constraint:Constraint = constraints[i]; + constraint.apply(dt); + } + } + + // Разделение контактов путём непосредственного изменения координат и ориентации. Экспериментально. +// if (forceInelastic && useStaticSeparation) performStaticSeparation(); + } + + /** + * + */ + private function resolveContact(contactInfo:Contact, forceInelastic:Boolean, forwardLoop:Boolean):void { + var b1:Body = contactInfo.body1; + var b2:Body = contactInfo.body2; + var normal:Vector3 = contactInfo.normal; + var i:int; + if (forwardLoop) { + for (i = 0; i < contactInfo.pcount; i++) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } else { + for (i = contactInfo.pcount - 1; i >= 0; i--) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } + } + + /** + * + * @param idx + * @param b1 + * @param b2 + * @param colInfo + * @param normal + * @param forceInelastic + */ + private function resolveContactPoint(idx:int, b1:Body, b2:Body, contact:Contact, normal:Vector3, forceInelastic:Boolean):void { + var cp:ContactPoint = contact.points[idx]; + if (!forceInelastic) { + cp.satisfied = true; + } + + var newVel:Number = 0; + calcSepVelocity(b1, b2, cp, _v); + var cnormal:Vector3 = contact.normal; + var sepVel:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + if (forceInelastic) { + var minSpeVel:Number = useStaticSeparation ? 0 : cp.minSepVel; + if (sepVel < minSpeVel) { + cp.satisfied = false; + } else if (cp.satisfied) return; + newVel = minSpeVel; + } else { + newVel = cp.normalVel; + } + var deltaVel:Number = newVel - sepVel; + var impulse:Number = deltaVel/cp.velByUnitImpulseN; + var accumImpulse:Number = cp.accumImpulseN + impulse; + if (accumImpulse < 0) { + accumImpulse = 0; + } + var deltaImpulse:Number = accumImpulse - cp.accumImpulseN; + cp.accumImpulseN = accumImpulse; + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, normal, deltaImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, normal, -deltaImpulse); + } + + // Учёт силы трения + calcSepVelocity(b1, b2, cp, _v); + // Расчитываем изменение касательной скорости на единицу касательного импульса + var tanSpeedByUnitImpulse:Number = 0; + +// _v.vAddScaled(-_v.vDot(contact.normal), contact.normal); + var dot:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + _v.x -= dot*cnormal.x; + _v.y -= dot*cnormal.y; + _v.z -= dot*cnormal.z; + + var tanSpeed:Number = _v.vLength(); + if (tanSpeed < 0.001) return; + +// _t.vCopy(_v).vNormalize().vReverse(); + _t.x = -_v.x; + _t.y = -_v.y; + _t.z = -_v.z; + _t.vNormalize(); + + var r:Vector3; + var m:Matrix3; + var xx:Number; + var yy:Number; + var zz:Number; + // dV = b.invMass + ((invI * (r % t)) % r) * t + if (b1.movable) { +// _v.vCross2(cp.r1, _t).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1); + r = cp.r1; + m = b1.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b1.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + if (b2 != null && b2.movable) { +// _v.vCross2(cp.r2, _t).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2); + + r = cp.r2; + m = b2.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b2.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + + var tanImpulse:Number = tanSpeed/tanSpeedByUnitImpulse; + var max:Number = contact.friction*cp.accumImpulseN; + if (max < 0) { + if (tanImpulse < max) tanImpulse = max; + } else { + if (tanImpulse > max) tanImpulse = max; + } + + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, _t, tanImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, _t, -tanImpulse); + } + } + + /** + * + * @param cp + * @param normal + * @return + */ + private function calcSepVelocity(body1:Body, body2:Body, cp:ContactPoint, result:Vector3):void { + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 +// result.vCopy(body1.state.velocity).vAdd(_v1.vCross2(body1.state.rotation, cp.r1)); + var rot:Vector3 = body1.state.rotation; + var v:Vector3 = cp.r1; + var x:Number = rot.y*v.z - rot.z*v.y; + var y:Number = rot.z*v.x - rot.x*v.z; + var z:Number = rot.x*v.y - rot.y*v.x; + v = body1.state.velocity; + result.x = v.x + x; + result.y = v.y + y; + result.z = v.z + z; + // V2 = V2_c + w2%r2 + if (body2 != null) { +// result.vSubtract(body2.state.velocity).vSubtract(_v2.vCross2(body2.state.rotation, cp.r2)); + rot = body2.state.rotation; + v = cp.r2; + x = rot.y*v.z - rot.z*v.y; + y = rot.z*v.x - rot.x*v.z; + z = rot.x*v.y - rot.y*v.x; + v = body2.state.velocity; + result.x -= v.x + x; + result.y -= v.y + y; + result.z -= v.z + z; + } + } + + /** + * + * @param dt + */ + private function intergateVelocities(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + item.body.integrateVelocity(dt); + item = item.next; + } + } + + /** + * + * @param dt + */ + private function integratePositions(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + if (body.movable && !body.frozen) { + body.integratePosition(dt); + } + item = item.next; + } + } + + /** + * + */ + private function performStaticSeparation():void { + var iterNum:int = staticSeparationIterations; +// iterNum = 100; + + // 1. В начале каждой итерации для всех контактов сбрасывается флаг satisfied с одновременным поиском наихудшего контакта, т.е. имеющего наибольшую величину пересечения тел. + // 2. Если найденный контакт имеет величину пересечения меньше предельно допустимой процедура прерывается, т.к. разделение контактов не требуется. + // 3. Если процедура продолжается, то выполняется разделение найденного контакта и установка его флага satisfied в true, после чего запускается внутренний цикл, состоящий + // из contactsNum - 1 итераций. На каждой итерации ищется наихудший контакт среди оставшихся (satisfied == false). Если такой контакт найден, то выполняется его разделение + // и переход к следующей итерации внутреннего цикла, иначе внутренний цикл прерывается и выполняется переход к следующей итерации внешнего цикла (шаг 1.). +// for (var iter:int = 0; iter < iterNum; iter++) { +// // Ищем контакт с максимальной величиной пересечения тел, одновременно сбрасывая флаги satisfied в false +// var worstContact:Contact = contacts[0]; +// var i:int; +// for (i = 1; i < contactsNum; i++) { +// var contact:Contact = contacts[i]; +// contact.satisfied = false; +// if (contact.maxPenetration > worstContact.maxPenetration) worstContact = contact; +// } +// if (worstContact.maxPenetration <= allowedPenetration) return; +// +// resolveInterpenetration(worstContact); +// // Внутренний цикл по оставшимся контактам +// for (i = 1; i < contactsNum; i++) { +// worstContact = getWorstContact(); +// if (worstContact == null) break; +// resolveInterpenetration(worstContact); +// } +// } + } + + /** + * + */ +// private function getWorstContact():Contact { +// var maxPen:Number = 0; +// var worst:Contact = null; +// for (var i:int = 0; i < contactsNum; i++) { +// var c:Contact = contacts[i]; +// if (!c.satisfied && c.maxPenetration > maxPen) { +// worst = c; +// maxPen = c.maxPenetration; +// } +// } +// return maxPen > allowedPenetration ? worst : null; +// } + + /** + * Разделяет указанный контакт, выполняя staticSeparationSteps итераций по списку точек контакта. + * 1. В начале каждой итерации ищется точка с наибольшим пересечением, одновременно сбрасываются флаги satisfied у точек. + * 2. Для найденной точки выполняется процедура разделения. + * 3. После выполняется pcount - 1 итерация по списку точек, каждый раз ищется наихудшая среди имеющих satisfied == false, для которой выполняется процедура разделения. + * Если наихудшая точка не найдена, то выполнется переход к следующей итерации внешнего цикла (шаг 1). + * + * В конце процедуры разделения обновляются значения пересечений для остальных точек контакта, а также для всех прочих контактов, относящихся к телам текущего. + * + * @param contact контакт для разделения + */ + private function resolveInterpenetration(contact:Contact):void { + contact.satisfied = true; + + for (var step:int = 0; step < staticSeparationSteps; step++) { + var worstCp:ContactPoint = contact.points[0]; + var cp:ContactPoint; + var i:int; + for (i = 1; i < contact.pcount; i++) { + cp = contact.points[i]; + cp.satisfied = false; + if (cp.penetration > worstCp.penetration) worstCp = cp; + } + if (worstCp.penetration <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + // Разделяем оставшиеся точки + var maxPen:Number = 0; + for (i = 1; i < contact.pcount; i++) { + // Поиск наихудшей точки + for (var j:int = 0; j < contact.pcount; j++) { + cp = contact.points[j]; + if (cp.satisfied) continue; + if (cp.penetration > maxPen) { + maxPen = cp.penetration; + worstCp = cp; + } + } + if (maxPen <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + } + } + } + + /** + * + * @param cp + * @param contact + */ + private function separateContactPoint(cp:ContactPoint, contact:Contact):void { +// cp.satisfied = true; +// +// var b1:Body = contact.body1; +// var b2:Body = contact.body2; +// var totalMove:Number = cp.penetration - allowedPenetration; +// var moveCoeff:Number = totalMove/cp.velByUnitImpulseN; +// var linMove1:Number; +// var angleMove1:Number; +// if (b1.movable) { +// linMove1 = b1.invMass*moveCoeff; +// angleMove1 = cp.angularInertia1*moveCoeff; +// if (angleMove1 > maxAngleMove) { +// linMove1 += angleMove1 - maxAngleMove; +// angleMove1 -= maxAngleMove; +// } +// b1.state.pos.vAddScaled(linMove1, contact.normal); +// _v1.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vScale(angleMove1); +// b1.state.orientation.addScaledVector(_v1, 1); +// } +// var linMove2:Number; +// var angleMove2:Number; +// if (b2 != null && b2.movable) { +// linMove2 = b2.invMass*moveCoeff; +// angleMove2 = cp.angularInertia2*moveCoeff; +// if (angleMove2 > maxAngleMove) { +// linMove2 += angleMove2 - maxAngleMove; +// angleMove2 -= maxAngleMove; +// } +// b2.state.pos.vAddScaled(-linMove2, contact.normal); +// _v2.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vScale(angleMove2); +// _v2.vReverse(); +// b2.state.orientation.addScaledVector(_v2, 1); +// } +// cp.penetration = allowedPenetration; +// // Обновляем пересечения в других точках +// var i:int; +// for (i = 0; i < contact.pcount; i++) { +// var cp1:ContactPoint = contact.points[i]; +// if (cp1 == cp) continue; +// var angularMove:Number; +// if (b1.movable) { +// angularMove = _v.vCross2(_v1, cp1.r1).vDot(contact.normal); +// cp1.penetration -= linMove1 + angularMove; +// } +// if (b2 != null && b2.movable) { +// angularMove = _v.vCross2(_v2, cp1.r2).vDot(contact.normal); +// cp1.penetration -= linMove2 - angularMove; +// } +// // Обновление максимального значения для контакта +// if (cp1.penetration > contact.maxPenetration) contact.maxPenetration = cp1.penetration; +// } +// // Обновляем пересечения для других контактов +// var c:Contact; +// var j:int; +// if (b1.movable) { +// for (i = 0; i < b1.contactsNum; i++) { +// c = b1.contacts[i]; +// if (c == contact) continue; +// for (j = 0; j < c.pcount; j++) { +// cp1 = c.points[j]; +// if (b1 == c.body1) cp1.penetration -= linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r1).vDot(c.normal); +// else cp1.penetration += linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r2).vDot(c.normal); +// if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; +// } +// } +// } +// if (b2 != null && b2.movable) { +// for (i = 0; i < b2.contactsNum; i++) { +// c = b2.contacts[i]; +// if (c == contact) continue; +// for (j = 0; j < c.pcount; j++) { +// cp1 = c.points[j]; +// if (b2 == c.body1) cp1.penetration -= linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r1).vDot(c.normal); +// else cp1.penetration += linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r2).vDot(c.normal); +// if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; +// } +// } +// } + } + + /** + * + */ + private function postPhysics():void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + body.clearAccumulators(); + body.calcDerivedData(); + if (body.canFreeze) { + if (body.state.velocity.vLength() < linSpeedFreezeLimit && body.state.rotation.vLength() < angSpeedFreezeLimit) { + if (!body.frozen) { + body.freezeCounter++; + if (body.freezeCounter >= freezeSteps) body.frozen = true; + } + } else { + body.freezeCounter = 0; + body.frozen = false; + } + } + item = item.next; + } + } + + /** + * + * @param delta + */ + public function update(delta:int):void { + timeStamp++; + time += delta; + var dt:Number = 0.001*delta; + applyForces(dt); + detectCollisions(dt); + preProcessContacts(dt); + processContacts(dt, false); + intergateVelocities(dt); + processContacts(dt, true); + integratePositions(dt); + postPhysics(); + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/PhysicsUtils.as b/0.0.9.4/src/alternativa/physics/PhysicsUtils.as new file mode 100644 index 0000000..5a2e46f --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/PhysicsUtils.as @@ -0,0 +1,49 @@ +package alternativa.physics { + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + public class PhysicsUtils { + + /** + * @param mass + * @param halfSize + * @param result + */ + public static function getBoxInvInertia(mass:Number, halfSize:Vector3, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + result.a = 3/(mass*(yy + zz)); + result.f = 3/(mass*(zz + xx)); + result.k = 3/(mass*(xx + yy)); + } + + /** + * @param mass + * @param r + * @param h + * @param result + */ + public static function getCylinderInvInertia(mass:Number, r:Number, h:Number, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + + result.a = result.f = 1/(mass*(h*h/12 + r*r/4)); + result.k = 2/(mass*r*r); + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/altphysics.as b/0.0.9.4/src/alternativa/physics/altphysics.as new file mode 100644 index 0000000..2715307 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/altphysics.as @@ -0,0 +1,3 @@ +package alternativa.physics { + public namespace altphysics = "http://alternativaplatform.com/en/altphysics"; +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/collision/.svn/all-wcprops b/0.0.9.4/src/alternativa/physics/collision/.svn/all-wcprops new file mode 100644 index 0000000..26f1738 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/.svn/all-wcprops @@ -0,0 +1,65 @@ +K 25 +svn:wc:ra_dav:version-url +V 113 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/collision +END +ICollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 135 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/collision/ICollisionDetector.as +END +CollisionKdNode.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/collision/CollisionKdNode.as +END +CollisionKdTree2D.as +K 25 +svn:wc:ra_dav:version-url +V 134 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/collision/CollisionKdTree2D.as +END +IRayCollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 139 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/collision/IRayCollisionPredicate.as +END +IBodyCollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/collision/IBodyCollisionPredicate.as +END +CollisionPrimitive.as +K 25 +svn:wc:ra_dav:version-url +V 135 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/collision/CollisionPrimitive.as +END +KdTreeCollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/collision/KdTreeCollisionDetector.as +END +ICollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 136 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/collision/ICollisionPredicate.as +END +ICollider.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/collision/ICollider.as +END +CollisionKdTree.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/collision/CollisionKdTree.as +END diff --git a/0.0.9.4/src/alternativa/physics/collision/.svn/entries b/0.0.9.4/src/alternativa/physics/collision/.svn/entries new file mode 100644 index 0000000..896ef59 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/.svn/entries @@ -0,0 +1,157 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/collision +http://svndev.alternativaplatform.com + + + +2010-07-03T11:40:52.545629Z +36684 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +colliders +dir + +ICollisionDetector.as +file + + + + +2010-10-28T04:32:45.000000Z +9f76503bdaf92b4170b05b6e66c45de1 +2010-02-12T12:34:25.192477Z +28424 +mike + +CollisionKdNode.as +file + + + + +2010-10-28T04:32:45.000000Z +a6fab491cd9bd45a5472bf4ffc321e08 +2010-02-12T12:34:25.192477Z +28424 +mike + +CollisionKdTree2D.as +file + + + + +2010-10-28T04:32:45.000000Z +82bb636d8b49cdfbc22694dd8b01e71d +2010-02-12T12:34:25.192477Z +28424 +mike + +IRayCollisionPredicate.as +file + + + + +2010-10-28T04:32:45.000000Z +32f9b1eaeacfcb40e7ae1927f52b8ab8 +2009-10-19T07:17:33.113306Z +22253 +mike + +types +dir + +IBodyCollisionPredicate.as +file + + + + +2010-10-28T04:32:45.000000Z +618f907a588979a8d06d4f4bcf6618a0 +2009-10-19T07:17:33.113306Z +22253 +mike + +CollisionPrimitive.as +file + + + + +2010-10-28T04:32:45.000000Z +8501ce34a3effa2380c472cc9b823afa +2009-11-24T08:44:15.941480Z +23758 +mike + +KdTreeCollisionDetector.as +file + + + + +2010-10-28T04:32:45.000000Z +ea670ad5d0f72f6a6df5163d65f204f9 +2010-02-12T12:34:25.192477Z +28424 +mike + +ICollisionPredicate.as +file + + + + +2010-10-28T04:32:45.000000Z +65f3250d36556fffc457c740fb7d38b3 +2009-10-19T07:17:33.113306Z +22253 +mike + +ICollider.as +file + + + + +2010-10-28T04:32:45.000000Z +128a7d7479e13caeccecb45161b27b22 +2009-10-19T07:17:33.113306Z +22253 +mike + +primitives +dir + +CollisionKdTree.as +file + + + + +2010-10-28T04:32:45.000000Z +8e815c1f1e527b0884b3f0124257a603 +2010-02-12T12:34:25.192477Z +28424 +mike + diff --git a/0.0.9.4/src/alternativa/physics/collision/.svn/format b/0.0.9.4/src/alternativa/physics/collision/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.4/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base b/0.0.9.4/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base new file mode 100644 index 0000000..f450f1e --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base @@ -0,0 +1,18 @@ +package alternativa.physics.collision { + + + import alternativa.physics.collision.types.BoundBox; + + public class CollisionKdNode { + public var indices:Vector.; + public var splitIndices:Vector.; + public var boundBox:BoundBox; + public var parent:CollisionKdNode; + public var splitTree:CollisionKdTree2D; + + public var axis:int = -1; // 0 - x, 1 - y, 2 - z + public var coord:Number; + public var positiveNode:CollisionKdNode; + public var negativeNode:CollisionKdNode; + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base b/0.0.9.4/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base new file mode 100644 index 0000000..d3dc307 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base @@ -0,0 +1,261 @@ +package alternativa.physics.collision { + + + + import alternativa.physics.collision.types.BoundBox; + + /** + * @author mike + */ + public class CollisionKdTree { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + public var rootNode:CollisionKdNode; + public var staticChildren:Vector.; + public var numStaticChildren:int; + public var staticBoundBoxes:Vector. = new Vector.(); + + private var splitAxis:int; + private var splitCoord:Number; + private var splitCost:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + + /** + * @param boundBox + */ + public function createTree(collisionPrimitives:Vector., boundBox:BoundBox = null):void { + staticChildren = collisionPrimitives.concat(); + numStaticChildren = staticChildren.length; + // Создаём корневую ноду + rootNode = new CollisionKdNode(); + rootNode.indices = new Vector.(); + // Расчитываем баунды объектов и рутовой ноды + var rootNodeBoundBox:BoundBox = rootNode.boundBox = (boundBox != null) ? boundBox : new BoundBox(); + for (var i:int = 0; i < numStaticChildren; ++i) { + var child:CollisionPrimitive = staticChildren[i]; + var childBoundBox:BoundBox = staticBoundBoxes[i] = child.calculateAABB(); + rootNodeBoundBox.addBoundBox(childBoundBox); + rootNode.indices[i] = i; + } + staticBoundBoxes.length = numStaticChildren; + // Разделяем рутовую ноду + splitNode(rootNode); + + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + var indices:Vector. = node.indices; + var numPrimitives:int = indices.length; + if (numPrimitives <= minPrimitivesPerNode) return; + + // Подготовка баунда с погрешностями + var nodeBoundBox:BoundBox = node.boundBox; + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + // Собираем опорные координаты + var i:int; + var j:int; + var numSplitCoordsX:int = 0 + var numSplitCoordsY:int = 0; + var numSplitCoordsZ:int = 0; + for (i = 0; i < numPrimitives; ++i) { + var boundBox:BoundBox = staticBoundBoxes[indices[i]]; + + if (boundBox.maxX - boundBox.minX <= doubleThreshold) { + if (boundBox.minX <= nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.minX; + else if (boundBox.maxX >= nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.maxX; + else splitCoordsX[numSplitCoordsX++] = (boundBox.minX + boundBox.maxX)*0.5; + } else { + if (boundBox.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = boundBox.minX; + if (boundBox.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = boundBox.maxX; + } + + if (boundBox.maxY - boundBox.minY <= doubleThreshold) { + if (boundBox.minY <= nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.minY; + else if (boundBox.maxY >= nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.maxY; + else splitCoordsY[numSplitCoordsY++] = (boundBox.minY + boundBox.maxY)*0.5; + } else { + if (boundBox.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = boundBox.minY; + if (boundBox.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = boundBox.maxY; + } + + if (boundBox.maxZ - boundBox.minZ <= doubleThreshold) { + if (boundBox.minZ <= nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.minZ; + else if (boundBox.maxZ >= nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.maxZ; + else splitCoordsZ[numSplitCoordsZ++] = (boundBox.minZ + boundBox.maxZ)*0.5; + } else { + if (boundBox.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.minZ; + if (boundBox.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numPrimitives; ++i) { + boundBox = staticBoundBoxes[indices[i]]; + var min:Number = axisX ? boundBox.minX : (axisY ? boundBox.minY : boundBox.minZ); + var max:Number = axisX ? boundBox.maxX : (axisY ? boundBox.maxY : boundBox.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(indices[i]); + indices[i] = -1; + } else { + if (node.splitIndices == null) node.splitIndices = new Vector.(); + node.splitIndices.push(indices[i]); + indices[i] = -1; + } + } else { + if (min >= coordMin) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(indices[i]); + indices[i] = -1; + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numPrimitives; ++i) { + if (indices[i] >= 0) indices[j++] = indices[i]; + } + if (j > 0) indices.length = j; + else node.indices = null; + + if (node.splitIndices != null) { + node.splitTree = new CollisionKdTree2D(this, node); + node.splitTree.createTree(); + } + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + /** + * + */ + public function traceTree():void { + traceNode("", rootNode); + } + + /** + * @param str + * @param node + */ + private function traceNode(str:String, node:CollisionKdNode):void { + if (node == null) return; + trace(str, node.axis == -1 ? "end" : ((node.axis == 0) ? "X" : ((node.axis == 1) ? "Y" : "Z")), "splitCoord=" + splitCoord, "bound", node.boundBox, "objs:", node.indices); + traceNode(str + "-", node.negativeNode); + traceNode(str + "+", node.positiveNode); + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base b/0.0.9.4/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base new file mode 100644 index 0000000..2621517 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base @@ -0,0 +1,226 @@ +package alternativa.physics.collision { + + + import alternativa.physics.collision.types.BoundBox; + + /** + * + */ + public class CollisionKdTree2D { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + + public var parentTree:CollisionKdTree; + public var parentNode:CollisionKdNode; + public var rootNode:CollisionKdNode; + + private var splitAxis:int; + private var splitCost:Number; + private var splitCoord:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + /** + * + * @param parentTree + * @param parentNode + */ + public function CollisionKdTree2D(parentTree:CollisionKdTree, parentNode:CollisionKdNode) { + this.parentTree = parentTree; + this.parentNode = parentNode; + } + + /** + * + */ + public function createTree():void { + rootNode = new CollisionKdNode(); + rootNode.boundBox = parentNode.boundBox.clone(); + rootNode.indices = new Vector.(); + var numObjects:int = parentNode.splitIndices.length; + for (var i:int = 0; i < numObjects; ++i) rootNode.indices[i] = parentNode.splitIndices[i]; + + splitNode(rootNode); + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + if (node.indices.length <= minPrimitivesPerNode) return; + + var objects:Vector. = node.indices; + var i:int; + var j:int; + + var nodeBoundBox:BoundBox = node.boundBox; + + // Подготовка баунда с погрешностями + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + // Собираем опорные координаты + var numSplitCoordsX:int; + var numSplitCoordsY:int; + var numSplitCoordsZ:int; + var numObjects:int = objects.length; + for (i = 0; i < numObjects; ++i) { + var bb:BoundBox = staticBoundBoxes[objects[i]]; + + if (parentNode.axis != 0) { + if (bb.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = bb.minX; + if (bb.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = bb.maxX; + } + + if (parentNode.axis != 1) { + if (bb.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = bb.minY; + if (bb.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = bb.maxY; + } + + if (parentNode.axis != 2) { + if (bb.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = bb.minZ; + if (bb.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = bb.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + if (parentNode.axis != 0) checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + if (parentNode.axis != 1) checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + if (parentNode.axis != 2) checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numObjects; ++i) { + bb = staticBoundBoxes[objects[i]]; + var min:Number = axisX ? bb.minX : (axisY ? bb.minY : bb.minZ); + var max:Number = axisX ? bb.maxX : (axisY ? bb.maxY : bb.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(objects[i]); + objects[i] = -1; + } + } else { + if (min >= coordMin) { + if (max > coordMax) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(objects[i]); + objects[i] = -1; + } + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numObjects; ++i) { + if (objects[i] >= 0) objects[j++] = objects[i]; + } + if (j > 0) objects.length = j; + else node.indices = null; + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/collision/.svn/text-base/CollisionPrimitive.as.svn-base b/0.0.9.4/src/alternativa/physics/collision/.svn/text-base/CollisionPrimitive.as.svn-base new file mode 100644 index 0000000..e03cfee --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/.svn/text-base/CollisionPrimitive.as.svn-base @@ -0,0 +1,140 @@ +package alternativa.physics.collision { + + import alternativa.physics.Body; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * Базовый класс для примитивов, использующихся детектором столкновений. + */ + public class CollisionPrimitive { + // Константы типов примитива + public static const BOX:int = 1; + public static const PLANE:int = 2; + public static const SPHERE:int = 3; + public static const RECT:int = 4; + public static const TRIANGLE:int = 5; + + // Тип примитива + public var type:int; + // Группы примитива. Каждая группа определяется установленным битом. Столкновения проверяются только для примитивов, + // имеющих хотя бы одну общую группу. + public var collisionGroup:int; + // Предикат примитива, вызывающийся детектором при нахождении столкновения. В зависимости от возвращённого + // предикатом результата столкновение либо регистрируется, либо игнорируется. + // Nullable + public var postCollisionPredicate:ICollisionPredicate; + // Тело, владеющее примитивом. Поле сделано открытым для быстрого доступа на чтение. Установка значения должна выполняться вызовом метода setBody(). + // Nullable + public var body:Body; + // Трансформация примитива в системе координат тела, если оно указано. Не допускается масштабирование матрицы. + public var localTransform:Matrix4; + // Полная трансформация примитива. Не допускается масштабирование матрицы. + public var transform:Matrix4 = new Matrix4(); + // AABB в мировой системе координат. Расчитывается системой вызовом функции calculateBoundBox(). + public var aabb:BoundBox = new BoundBox(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param type тип примитива + * @param collisionGroup группа примитива + */ + public function CollisionPrimitive(type:int, collisionGroup:int) { + this.type = type; + this.collisionGroup = collisionGroup; + } + + /** + * Устанавливает тело, владеющее примитивом. + * + * @param body тело, которое владеет примитивом + * @param localTransform трансформация примитива в системе координат тела. Указание значения null равносильно + * заданию единичной матрицы, однако в первом случае не будет дополнительного умножения матриц при вычислении полной трансформации примитива. + */ + public function setBody(body:Body, localTransform:Matrix4 = null):void { + if (this.body == body) return; + this.body = body; + if (body != null) { + if (localTransform != null) { + if (this.localTransform == null) { + this.localTransform = new Matrix4(); + } + this.localTransform.copy(localTransform); + } else { + this.localTransform = null; + } + } + } + + /** + * Рассчитывает AABB примитива. Наследники должны переопределять этот метод, реализуя в нём корректный рассчёт. + * + * @return ссылка на свой AABB + */ + public function calculateAABB():BoundBox { + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + return -1; + } + + /** + * Клонирует примитив. Переопределять не рекомендуется. Вместо этого переопределяются методы createPrimitive() и copyFrom(). + * + * @return клон примитива + */ + public function clone():CollisionPrimitive { + var p:CollisionPrimitive = createPrimitive(); + return p.copyFrom(this); + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + if (source == null) { + throw new ArgumentError("Parameter source cannot be null"); + } + type = source.type; + transform.copy(source.transform); + collisionGroup = source.collisionGroup; + setBody(source.body, source.localTransform); + aabb.copyFrom(source.aabb); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + public function toString():String { + return "[CollisionPrimitive type=" + type + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + protected function createPrimitive():CollisionPrimitive { + return new CollisionPrimitive(type, collisionGroup); + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base b/0.0.9.4/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base new file mode 100644 index 0000000..4ace681 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.Body; + + public interface IBodyCollisionPredicate { + function considerBodies(body1:Body, body2:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base b/0.0.9.4/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base new file mode 100644 index 0000000..e6aac2a --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base @@ -0,0 +1,29 @@ +package alternativa.physics.collision { + + import alternativa.physics.Contact; + + /** + * Интерфейс определителя столкновений между двумя примитивами. + */ + public interface ICollider { + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base b/0.0.9.4/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base new file mode 100644 index 0000000..916f67b --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base @@ -0,0 +1,64 @@ +package alternativa.physics.collision { + import alternativa.physics.Contact; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.math.Vector3; + + /** + * Интерфейс детектора столкновений. + */ + public interface ICollisionDetector { + + /** + * Получает все столкновения в текущей конфигурации физической геометрии. + * + * @param contacts список контактов, в кторые будет записана информация о столкновении + * @return количество найденных столкновений + */ + function getAllContacts(contacts:Contact):Contact; + + /** + * Тестирует луч на пересечение с физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param direction направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRay(origin:Vector3, direction:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * + * @param origin + * @param direction + * @param collisionGroup + * @param maxTime + * @param predicate + * @param result + * @return + */ + function intersectRayWithStatic(origin:Vector3, direction:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @return + */ + function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base b/0.0.9.4/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base new file mode 100644 index 0000000..33d30ca --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base @@ -0,0 +1,8 @@ +package alternativa.physics.collision { + + public interface ICollisionPredicate { + + function considerCollision(primitive:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base b/0.0.9.4/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base new file mode 100644 index 0000000..b319f62 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.Body; + + public interface IRayCollisionPredicate { + function considerBody(body:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base b/0.0.9.4/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base new file mode 100644 index 0000000..9ca4c2a --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base @@ -0,0 +1,486 @@ +package alternativa.physics.collision { + + + 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.math.Vector3; + + use namespace altphysics; + + /** + * Детектор, хранящий статическую геометрию в kD-дереве и использующий дерево для ускорения тестов на пересечения. + */ + public class KdTreeCollisionDetector implements ICollisionDetector { + + altphysics var tree:CollisionKdTree; + altphysics var dynamicPrimitives:Vector.; + 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.(); + + 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.):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. = tree.staticChildren; +// var indices:Vector. = 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., indices:Vector., 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; + } \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/collision/CollisionKdNode.as b/0.0.9.4/src/alternativa/physics/collision/CollisionKdNode.as new file mode 100644 index 0000000..f450f1e --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/CollisionKdNode.as @@ -0,0 +1,18 @@ +package alternativa.physics.collision { + + + import alternativa.physics.collision.types.BoundBox; + + public class CollisionKdNode { + public var indices:Vector.; + public var splitIndices:Vector.; + public var boundBox:BoundBox; + public var parent:CollisionKdNode; + public var splitTree:CollisionKdTree2D; + + public var axis:int = -1; // 0 - x, 1 - y, 2 - z + public var coord:Number; + public var positiveNode:CollisionKdNode; + public var negativeNode:CollisionKdNode; + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/collision/CollisionKdTree.as b/0.0.9.4/src/alternativa/physics/collision/CollisionKdTree.as new file mode 100644 index 0000000..d3dc307 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/CollisionKdTree.as @@ -0,0 +1,261 @@ +package alternativa.physics.collision { + + + + import alternativa.physics.collision.types.BoundBox; + + /** + * @author mike + */ + public class CollisionKdTree { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + public var rootNode:CollisionKdNode; + public var staticChildren:Vector.; + public var numStaticChildren:int; + public var staticBoundBoxes:Vector. = new Vector.(); + + private var splitAxis:int; + private var splitCoord:Number; + private var splitCost:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + + /** + * @param boundBox + */ + public function createTree(collisionPrimitives:Vector., boundBox:BoundBox = null):void { + staticChildren = collisionPrimitives.concat(); + numStaticChildren = staticChildren.length; + // Создаём корневую ноду + rootNode = new CollisionKdNode(); + rootNode.indices = new Vector.(); + // Расчитываем баунды объектов и рутовой ноды + var rootNodeBoundBox:BoundBox = rootNode.boundBox = (boundBox != null) ? boundBox : new BoundBox(); + for (var i:int = 0; i < numStaticChildren; ++i) { + var child:CollisionPrimitive = staticChildren[i]; + var childBoundBox:BoundBox = staticBoundBoxes[i] = child.calculateAABB(); + rootNodeBoundBox.addBoundBox(childBoundBox); + rootNode.indices[i] = i; + } + staticBoundBoxes.length = numStaticChildren; + // Разделяем рутовую ноду + splitNode(rootNode); + + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + var indices:Vector. = node.indices; + var numPrimitives:int = indices.length; + if (numPrimitives <= minPrimitivesPerNode) return; + + // Подготовка баунда с погрешностями + var nodeBoundBox:BoundBox = node.boundBox; + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + // Собираем опорные координаты + var i:int; + var j:int; + var numSplitCoordsX:int = 0 + var numSplitCoordsY:int = 0; + var numSplitCoordsZ:int = 0; + for (i = 0; i < numPrimitives; ++i) { + var boundBox:BoundBox = staticBoundBoxes[indices[i]]; + + if (boundBox.maxX - boundBox.minX <= doubleThreshold) { + if (boundBox.minX <= nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.minX; + else if (boundBox.maxX >= nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.maxX; + else splitCoordsX[numSplitCoordsX++] = (boundBox.minX + boundBox.maxX)*0.5; + } else { + if (boundBox.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = boundBox.minX; + if (boundBox.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = boundBox.maxX; + } + + if (boundBox.maxY - boundBox.minY <= doubleThreshold) { + if (boundBox.minY <= nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.minY; + else if (boundBox.maxY >= nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.maxY; + else splitCoordsY[numSplitCoordsY++] = (boundBox.minY + boundBox.maxY)*0.5; + } else { + if (boundBox.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = boundBox.minY; + if (boundBox.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = boundBox.maxY; + } + + if (boundBox.maxZ - boundBox.minZ <= doubleThreshold) { + if (boundBox.minZ <= nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.minZ; + else if (boundBox.maxZ >= nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.maxZ; + else splitCoordsZ[numSplitCoordsZ++] = (boundBox.minZ + boundBox.maxZ)*0.5; + } else { + if (boundBox.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.minZ; + if (boundBox.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numPrimitives; ++i) { + boundBox = staticBoundBoxes[indices[i]]; + var min:Number = axisX ? boundBox.minX : (axisY ? boundBox.minY : boundBox.minZ); + var max:Number = axisX ? boundBox.maxX : (axisY ? boundBox.maxY : boundBox.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(indices[i]); + indices[i] = -1; + } else { + if (node.splitIndices == null) node.splitIndices = new Vector.(); + node.splitIndices.push(indices[i]); + indices[i] = -1; + } + } else { + if (min >= coordMin) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(indices[i]); + indices[i] = -1; + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numPrimitives; ++i) { + if (indices[i] >= 0) indices[j++] = indices[i]; + } + if (j > 0) indices.length = j; + else node.indices = null; + + if (node.splitIndices != null) { + node.splitTree = new CollisionKdTree2D(this, node); + node.splitTree.createTree(); + } + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + /** + * + */ + public function traceTree():void { + traceNode("", rootNode); + } + + /** + * @param str + * @param node + */ + private function traceNode(str:String, node:CollisionKdNode):void { + if (node == null) return; + trace(str, node.axis == -1 ? "end" : ((node.axis == 0) ? "X" : ((node.axis == 1) ? "Y" : "Z")), "splitCoord=" + splitCoord, "bound", node.boundBox, "objs:", node.indices); + traceNode(str + "-", node.negativeNode); + traceNode(str + "+", node.positiveNode); + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/collision/CollisionKdTree2D.as b/0.0.9.4/src/alternativa/physics/collision/CollisionKdTree2D.as new file mode 100644 index 0000000..2621517 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/CollisionKdTree2D.as @@ -0,0 +1,226 @@ +package alternativa.physics.collision { + + + import alternativa.physics.collision.types.BoundBox; + + /** + * + */ + public class CollisionKdTree2D { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + + public var parentTree:CollisionKdTree; + public var parentNode:CollisionKdNode; + public var rootNode:CollisionKdNode; + + private var splitAxis:int; + private var splitCost:Number; + private var splitCoord:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + /** + * + * @param parentTree + * @param parentNode + */ + public function CollisionKdTree2D(parentTree:CollisionKdTree, parentNode:CollisionKdNode) { + this.parentTree = parentTree; + this.parentNode = parentNode; + } + + /** + * + */ + public function createTree():void { + rootNode = new CollisionKdNode(); + rootNode.boundBox = parentNode.boundBox.clone(); + rootNode.indices = new Vector.(); + var numObjects:int = parentNode.splitIndices.length; + for (var i:int = 0; i < numObjects; ++i) rootNode.indices[i] = parentNode.splitIndices[i]; + + splitNode(rootNode); + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + if (node.indices.length <= minPrimitivesPerNode) return; + + var objects:Vector. = node.indices; + var i:int; + var j:int; + + var nodeBoundBox:BoundBox = node.boundBox; + + // Подготовка баунда с погрешностями + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + // Собираем опорные координаты + var numSplitCoordsX:int; + var numSplitCoordsY:int; + var numSplitCoordsZ:int; + var numObjects:int = objects.length; + for (i = 0; i < numObjects; ++i) { + var bb:BoundBox = staticBoundBoxes[objects[i]]; + + if (parentNode.axis != 0) { + if (bb.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = bb.minX; + if (bb.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = bb.maxX; + } + + if (parentNode.axis != 1) { + if (bb.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = bb.minY; + if (bb.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = bb.maxY; + } + + if (parentNode.axis != 2) { + if (bb.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = bb.minZ; + if (bb.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = bb.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + if (parentNode.axis != 0) checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + if (parentNode.axis != 1) checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + if (parentNode.axis != 2) checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numObjects; ++i) { + bb = staticBoundBoxes[objects[i]]; + var min:Number = axisX ? bb.minX : (axisY ? bb.minY : bb.minZ); + var max:Number = axisX ? bb.maxX : (axisY ? bb.maxY : bb.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(objects[i]); + objects[i] = -1; + } + } else { + if (min >= coordMin) { + if (max > coordMax) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(objects[i]); + objects[i] = -1; + } + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numObjects; ++i) { + if (objects[i] >= 0) objects[j++] = objects[i]; + } + if (j > 0) objects.length = j; + else node.indices = null; + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/collision/CollisionPrimitive.as b/0.0.9.4/src/alternativa/physics/collision/CollisionPrimitive.as new file mode 100644 index 0000000..e03cfee --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/CollisionPrimitive.as @@ -0,0 +1,140 @@ +package alternativa.physics.collision { + + import alternativa.physics.Body; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * Базовый класс для примитивов, использующихся детектором столкновений. + */ + public class CollisionPrimitive { + // Константы типов примитива + public static const BOX:int = 1; + public static const PLANE:int = 2; + public static const SPHERE:int = 3; + public static const RECT:int = 4; + public static const TRIANGLE:int = 5; + + // Тип примитива + public var type:int; + // Группы примитива. Каждая группа определяется установленным битом. Столкновения проверяются только для примитивов, + // имеющих хотя бы одну общую группу. + public var collisionGroup:int; + // Предикат примитива, вызывающийся детектором при нахождении столкновения. В зависимости от возвращённого + // предикатом результата столкновение либо регистрируется, либо игнорируется. + // Nullable + public var postCollisionPredicate:ICollisionPredicate; + // Тело, владеющее примитивом. Поле сделано открытым для быстрого доступа на чтение. Установка значения должна выполняться вызовом метода setBody(). + // Nullable + public var body:Body; + // Трансформация примитива в системе координат тела, если оно указано. Не допускается масштабирование матрицы. + public var localTransform:Matrix4; + // Полная трансформация примитива. Не допускается масштабирование матрицы. + public var transform:Matrix4 = new Matrix4(); + // AABB в мировой системе координат. Расчитывается системой вызовом функции calculateBoundBox(). + public var aabb:BoundBox = new BoundBox(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param type тип примитива + * @param collisionGroup группа примитива + */ + public function CollisionPrimitive(type:int, collisionGroup:int) { + this.type = type; + this.collisionGroup = collisionGroup; + } + + /** + * Устанавливает тело, владеющее примитивом. + * + * @param body тело, которое владеет примитивом + * @param localTransform трансформация примитива в системе координат тела. Указание значения null равносильно + * заданию единичной матрицы, однако в первом случае не будет дополнительного умножения матриц при вычислении полной трансформации примитива. + */ + public function setBody(body:Body, localTransform:Matrix4 = null):void { + if (this.body == body) return; + this.body = body; + if (body != null) { + if (localTransform != null) { + if (this.localTransform == null) { + this.localTransform = new Matrix4(); + } + this.localTransform.copy(localTransform); + } else { + this.localTransform = null; + } + } + } + + /** + * Рассчитывает AABB примитива. Наследники должны переопределять этот метод, реализуя в нём корректный рассчёт. + * + * @return ссылка на свой AABB + */ + public function calculateAABB():BoundBox { + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + return -1; + } + + /** + * Клонирует примитив. Переопределять не рекомендуется. Вместо этого переопределяются методы createPrimitive() и copyFrom(). + * + * @return клон примитива + */ + public function clone():CollisionPrimitive { + var p:CollisionPrimitive = createPrimitive(); + return p.copyFrom(this); + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + if (source == null) { + throw new ArgumentError("Parameter source cannot be null"); + } + type = source.type; + transform.copy(source.transform); + collisionGroup = source.collisionGroup; + setBody(source.body, source.localTransform); + aabb.copyFrom(source.aabb); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + public function toString():String { + return "[CollisionPrimitive type=" + type + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + protected function createPrimitive():CollisionPrimitive { + return new CollisionPrimitive(type, collisionGroup); + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/collision/IBodyCollisionPredicate.as b/0.0.9.4/src/alternativa/physics/collision/IBodyCollisionPredicate.as new file mode 100644 index 0000000..4ace681 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/IBodyCollisionPredicate.as @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.Body; + + public interface IBodyCollisionPredicate { + function considerBodies(body1:Body, body2:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/collision/ICollider.as b/0.0.9.4/src/alternativa/physics/collision/ICollider.as new file mode 100644 index 0000000..e6aac2a --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/ICollider.as @@ -0,0 +1,29 @@ +package alternativa.physics.collision { + + import alternativa.physics.Contact; + + /** + * Интерфейс определителя столкновений между двумя примитивами. + */ + public interface ICollider { + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/collision/ICollisionDetector.as b/0.0.9.4/src/alternativa/physics/collision/ICollisionDetector.as new file mode 100644 index 0000000..916f67b --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/ICollisionDetector.as @@ -0,0 +1,64 @@ +package alternativa.physics.collision { + import alternativa.physics.Contact; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.math.Vector3; + + /** + * Интерфейс детектора столкновений. + */ + public interface ICollisionDetector { + + /** + * Получает все столкновения в текущей конфигурации физической геометрии. + * + * @param contacts список контактов, в кторые будет записана информация о столкновении + * @return количество найденных столкновений + */ + function getAllContacts(contacts:Contact):Contact; + + /** + * Тестирует луч на пересечение с физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param direction направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRay(origin:Vector3, direction:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * + * @param origin + * @param direction + * @param collisionGroup + * @param maxTime + * @param predicate + * @param result + * @return + */ + function intersectRayWithStatic(origin:Vector3, direction:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @return + */ + function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/collision/ICollisionPredicate.as b/0.0.9.4/src/alternativa/physics/collision/ICollisionPredicate.as new file mode 100644 index 0000000..33d30ca --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/ICollisionPredicate.as @@ -0,0 +1,8 @@ +package alternativa.physics.collision { + + public interface ICollisionPredicate { + + function considerCollision(primitive:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/collision/IRayCollisionPredicate.as b/0.0.9.4/src/alternativa/physics/collision/IRayCollisionPredicate.as new file mode 100644 index 0000000..b319f62 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/IRayCollisionPredicate.as @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.Body; + + public interface IRayCollisionPredicate { + function considerBody(body:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/collision/KdTreeCollisionDetector.as b/0.0.9.4/src/alternativa/physics/collision/KdTreeCollisionDetector.as new file mode 100644 index 0000000..9ca4c2a --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/KdTreeCollisionDetector.as @@ -0,0 +1,486 @@ +package alternativa.physics.collision { + + + 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.math.Vector3; + + use namespace altphysics; + + /** + * Детектор, хранящий статическую геометрию в kD-дереве и использующий дерево для ускорения тестов на пересечения. + */ + public class KdTreeCollisionDetector implements ICollisionDetector { + + altphysics var tree:CollisionKdTree; + altphysics var dynamicPrimitives:Vector.; + 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.(); + + 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.):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. = tree.staticChildren; +// var indices:Vector. = 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., indices:Vector., 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; + } \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/collision/colliders/.svn/all-wcprops b/0.0.9.4/src/alternativa/physics/collision/colliders/.svn/all-wcprops new file mode 100644 index 0000000..9928192 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/colliders/.svn/all-wcprops @@ -0,0 +1,53 @@ +K 25 +svn:wc:ra_dav:version-url +V 123 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/collision/colliders +END +BoxSphereCollider.as +K 25 +svn:wc:ra_dav:version-url +V 144 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/collision/colliders/BoxSphereCollider.as +END +BoxBoxCollider.as +K 25 +svn:wc:ra_dav:version-url +V 141 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/collision/colliders/BoxBoxCollider.as +END +BoxRectCollider.as +K 25 +svn:wc:ra_dav:version-url +V 142 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/collision/colliders/BoxRectCollider.as +END +SpherePlaneCollider.as +K 25 +svn:wc:ra_dav:version-url +V 146 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/collision/colliders/SpherePlaneCollider.as +END +BoxCollider.as +K 25 +svn:wc:ra_dav:version-url +V 138 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/collision/colliders/BoxCollider.as +END +BoxPlaneCollider.as +K 25 +svn:wc:ra_dav:version-url +V 143 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/collision/colliders/BoxPlaneCollider.as +END +SphereSphereCollider.as +K 25 +svn:wc:ra_dav:version-url +V 147 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/collision/colliders/SphereSphereCollider.as +END +BoxTriangleCollider.as +K 25 +svn:wc:ra_dav:version-url +V 146 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/collision/colliders/BoxTriangleCollider.as +END diff --git a/0.0.9.4/src/alternativa/physics/collision/colliders/.svn/entries b/0.0.9.4/src/alternativa/physics/collision/colliders/.svn/entries new file mode 100644 index 0000000..25263ca --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/colliders/.svn/entries @@ -0,0 +1,124 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/collision/colliders +http://svndev.alternativaplatform.com + + + +2010-03-06T16:51:19.781174Z +29404 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +BoxSphereCollider.as +file + + + + +2010-10-28T04:32:45.000000Z +25170bcb9a1eec2a0941f4b50a06a554 +2009-12-08T15:53:11.094448Z +24506 +mike + +BoxBoxCollider.as +file + + + + +2010-10-28T04:32:45.000000Z +15da13b1383e9c1892edc205c8c055b0 +2010-02-12T12:34:25.192477Z +28424 +mike + +BoxRectCollider.as +file + + + + +2010-10-28T04:32:45.000000Z +4a35f94d3ef1af0a5b42e1a100580f6d +2010-03-06T16:51:19.781174Z +29404 +mike + +SpherePlaneCollider.as +file + + + + +2010-10-28T04:32:45.000000Z +67621ed47d7e90b72498acf2802c4e2d +2009-11-24T08:44:15.941480Z +23758 +mike + +BoxCollider.as +file + + + + +2010-10-28T04:32:45.000000Z +52e72ed30a24a3a43ce16a19930546b5 +2010-02-12T12:34:25.192477Z +28424 +mike + +BoxPlaneCollider.as +file + + + + +2010-10-28T04:32:45.000000Z +204c7b02a9a1913269c558658bad4262 +2010-02-12T12:34:25.192477Z +28424 +mike + +SphereSphereCollider.as +file + + + + +2010-10-28T04:32:45.000000Z +301c7102f91a7f3ede83e35ca202db0b +2009-11-24T08:44:15.941480Z +23758 +mike + +BoxTriangleCollider.as +file + + + + +2010-10-28T04:32:45.000000Z +448502271cac4f09491ca128b4bb485a +2010-03-06T16:51:19.781174Z +29404 +mike + diff --git a/0.0.9.4/src/alternativa/physics/collision/colliders/.svn/format b/0.0.9.4/src/alternativa/physics/collision/colliders/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/colliders/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.4/src/alternativa/physics/collision/colliders/.svn/text-base/BoxBoxCollider.as.svn-base b/0.0.9.4/src/alternativa/physics/collision/colliders/.svn/text-base/BoxBoxCollider.as.svn-base new file mode 100644 index 0000000..2d3f04c --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/colliders/.svn/text-base/BoxBoxCollider.as.svn-base @@ -0,0 +1,589 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * Расчитывает точки контакта двух боксов. Нормаль контакта направляется в сторону бокса с меньшим ID. + */ + public class BoxBoxCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + private var vectorToBox1:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var bestAxisIndex:int; + private var minOverlap:Number; + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + private var tmpPoints:Vector. = new Vector.(8, true); + private var pcount:int; + + /** + * + */ + public function BoxBoxCollider() { + for (var i:int = 0; i < 8; i++) { + tmpPoints[i] = new ContactPoint(); + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + + if (bestAxisIndex < 6) { + // Контакт грань-(грань|ребро|вершина) + if (!findFaceContactPoints(box1, box2, vectorToBox1, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 6; + findEdgesIntersection(box1, box2, vectorToBox1, int(bestAxisIndex/3), bestAxisIndex%3, contact); + } + contact.body1 = box1.body; + contact.body2 = box2.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + // Вектор из центра второго бокса в центр первого + vectorToBox1.x = transform1.d - transform2.d; + vectorToBox1.y = transform1.h - transform2.h; + vectorToBox1.z = transform1.l - transform2.l; + + // Проверка пересечения по основным осям + axis10.x = transform1.a; + axis10.y = transform1.e; + axis10.z = transform1.i; + if (!testMainAxis(box1, box2, axis10, 0, vectorToBox1)) return false; + axis11.x = transform1.b; + axis11.y = transform1.f; + axis11.z = transform1.j; + if (!testMainAxis(box1, box2, axis11, 1, vectorToBox1)) return false; + axis12.x = transform1.c; + axis12.y = transform1.g; + axis12.z = transform1.k; + if (!testMainAxis(box1, box2, axis12, 2, vectorToBox1)) return false; + + axis20.x = transform2.a; + axis20.y = transform2.e; + axis20.z = transform2.i; + if (!testMainAxis(box1, box2, axis20, 3, vectorToBox1)) return false; + axis21.x = transform2.b; + axis21.y = transform2.f; + axis21.z = transform2.j; + if (!testMainAxis(box1, box2, axis21, 4, vectorToBox1)) return false; + axis22.x = transform2.c; + axis22.y = transform2.g; + axis22.z = transform2.k; + if (!testMainAxis(box1, box2, axis22, 5, vectorToBox1)) return false; + + // Проверка производных осей + if (!testDerivedAxis(box1, box2, axis10, axis20, 6, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis21, 7, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis22, 8, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis11, axis20, 9, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis21, 10, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis22, 11, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis12, axis20, 12, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis21, 13, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis22, 14, vectorToBox1)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта грани одного бокса с гранью/ребром/вершиной другого. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param faceAxisIdx индекс оси первого бокса, перпендикулярной грани, с которой произошло столкновение + * @param contactInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var swapNormal:Boolean = false; + if (faceAxisIdx > 2) { + // Столкновение с гранью второго бокса. Для дальнейших расчётов боксы меняются местами, + // но нормаль контакта всё равно должна быть направлена в сторону первоначального box1 + var tmpBox:CollisionBox = box1; + box1 = box2; + box2 = tmpBox; + vectorToBox1.x = -vectorToBox1.x; + vectorToBox1.y = -vectorToBox1.y; + vectorToBox1.z = -vectorToBox1.z; + faceAxisIdx -= 3; + swapNormal = true; + } + + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + var colAxis:Vector3 = contact.normal; + + transform1.getAxis(faceAxisIdx, colAxis); + var negativeFace:Boolean = colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z > 0; + + var code:int = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box1.excludedFaces) != 0) return false; + + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + // Ищем ось второго бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + transform2.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisDot = dot; + incidentAxisIdx = axisIdx; + } + } + // Получаем список вершин грани второго бокса, переводим их в систему координат первого бокса и выполняем обрезку + // по грани первого бокса. Таким образом получается список потенциальных точек контакта. + transform2.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box2.hs, incidentAxisIdx, incidentAxisDot < 0, points1); + + // TODO: Вычислить результирующую матрицу, затем преобразовать векторы за один заход + transform2.transformVectorsN(points1, points2, 4); + transform1.transformVectorsInverseN(points2, points1, 4); + + var pnum:int = clip(box1.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points1[i]; + if ((pen = getPointBoxPenetration(box1.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + var cp:ContactPoint = tmpPoints[pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = transform1.a*v.x + transform1.b*v.y + transform1.c*v.z + transform1.d; + cpPos.y = transform1.e*v.x + transform1.f*v.y + transform1.g*v.z + transform1.h; + cpPos.z = transform1.i*v.x + transform1.j*v.y + transform1.k*v.z + transform1.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + + cp.penetration = pen; + } + } + if (swapNormal) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (pcount > 4) { + reducePoints(); + } + for (i = 0; i < pcount; i++) { + cp = contact.points[i]; + cp.copyFrom(tmpPoints[i]); + } + contact.pcount = pcount; + return true; + } + + /** + * + * @param contactInfo + */ + private function reducePoints():void { + var i:int; + var minIdx:int; + var cp1:ContactPoint; + var cp2:ContactPoint; + while (pcount > 4) { + var minLen:Number = 1e10; + var p1:Vector3 = (tmpPoints[int(pcount - 1)] as ContactPoint).pos; + var p2:Vector3; + for (i = 0; i < pcount; i++) { + p2 = (tmpPoints[i] as ContactPoint).pos; + var dx:Number = p2.x - p1.x; + var dy:Number = p2.y - p1.y; + var dz:Number = p2.z - p1.z; + var len:Number = dx*dx + dy*dy + dz*dz; + if (len < minLen) { + minLen = len; + minIdx = i; + } + p1 = p2; + } + var ii:int = minIdx == 0 ? (pcount - 1) : (minIdx - 1); + cp1 = tmpPoints[ii]; + cp2 = tmpPoints[minIdx]; + p1 = cp1.pos; + p2 = cp2.pos; + p2.x = 0.5*(p1.x + p2.x); + p2.y = 0.5*(p1.y + p2.y); + p2.z = 0.5*(p1.z + p2.z); + cp2.penetration = 0.5*(cp1.penetration + cp2.penetration); + if (minIdx > 0) { + for (i = minIdx; i < pcount; i++) { + tmpPoints[int(i - 1)] = tmpPoints[i]; + } + tmpPoints[int(pcount - 1)] = cp1; + } + pcount--; + } + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clip(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param contactInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):void { + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + transform1.getAxis(axisIdx1, axis10); + transform2.getAxis(axisIdx2, axis20); + + var colAxis:Vector3 = contact.normal; + + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону первого бокса + if (colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + + var tx:Number = box1.hs.x; + var ty:Number = box1.hs.y; + var tz:Number = box1.hs.z; + + var x2:Number = box2.hs.x; + var y2:Number = box2.hs.y; + var z2:Number = box2.hs.z; + // x + if (axisIdx1 == 0) { + tx = 0; + halfLen1 = box1.hs.x; + } else { + if (colAxis.x*transform1.a + colAxis.y*transform1.e + colAxis.z*transform1.i > 0) { + tx = -tx; + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = box2.hs.x; + } else { + if (colAxis.x*transform2.a + colAxis.y*transform2.e + colAxis.z*transform2.i < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + ty = 0; + halfLen1 = box1.hs.y; + } else { + if (colAxis.x*transform1.b + colAxis.y*transform1.f + colAxis.z*transform1.j > 0) { + ty = -ty; + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = box2.hs.y; + } else { + if (colAxis.x*transform2.b + colAxis.y*transform2.f + colAxis.z*transform2.j < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + tz = 0; + halfLen1 = box1.hs.z; + } else { + if (colAxis.x*transform1.c + colAxis.y*transform1.g + colAxis.z*transform1.k > 0) { + tz = -tz; + } + } + if (axisIdx2 == 2) { + z2 = 0; + halfLen2 = box2.hs.z; + } else { + if (colAxis.x*transform2.c + colAxis.y*transform2.g + colAxis.z*transform2.k < 0) { + z2 = -z2; + } + } + // Получаем глобальные координаты средних точек рёбер + var x1:Number = transform1.a*tx + transform1.b*ty + transform1.c*tz + transform1.d; + var y1:Number = transform1.e*tx + transform1.f*ty + transform1.g*tz + transform1.h; + var z1:Number = transform1.i*tx + transform1.j*ty + transform1.k*tz + transform1.l; + tx = x2; + ty = y2; + tz = z2; + x2 = transform2.a*tx + transform2.b*ty + transform2.c*tz + transform2.d; + y2 = transform2.e*tx + transform2.f*ty + transform2.g*tz + transform2.h; + z2 = transform2.i*tx + transform2.j*ty + transform2.k*tz + transform2.l; + + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + tx = x2 - x1; + ty = y2 - y1; + tz = z2 - z1; + var c1:Number = axis10.x*tx + axis10.y*ty + axis10.z*tz; + var c2:Number = axis20.x*tx + axis20.y*ty + axis20.z*tz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cp.pos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cp.pos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение боксов вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box1 + * @param box2 + * @param axis + * @param axisIndex + * @param toBox1 + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box1 + * @param box2 + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox1 + * @return + */ + private function testDerivedAxis(box1:CollisionBox, box2:CollisionBox, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия двух боксов вдоль заданной оси. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param axis ось + * @param vectorToBox1 вектор, соединяющий центр второго бокса с центром первого + * @return величина перекрытия боксов вдоль оси + */ + public function overlapOnAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, vectorToBox1:Vector3):Number { + var m:Matrix4 = box1.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box1.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box1.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box1.hs.z; + if (d < 0) d = -d; + projection += d; + + m = box2.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box2.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box2.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box2.hs.z; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox1.x*axis.x + vectorToBox1.y*axis.y + vectorToBox1.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} + import alternativa.math.Vector3; + + class CollisionPointTmp { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + } diff --git a/0.0.9.4/src/alternativa/physics/collision/colliders/.svn/text-base/BoxCollider.as.svn-base b/0.0.9.4/src/alternativa/physics/collision/colliders/.svn/text-base/BoxCollider.as.svn-base new file mode 100644 index 0000000..2429bb1 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/colliders/.svn/text-base/BoxCollider.as.svn-base @@ -0,0 +1,427 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + /** + * + */ + public class BoxCollider implements ICollider { + + /** + * + */ + public function BoxCollider() { + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + return false; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + protected function getFaceVertsByAxis(hs:Vector3, axisIdx:int, negativeFace:Boolean, result:Vector.):void { + var v:Vector3; + switch (axisIdx) { + case 0: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + } + break; + case 1: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + } + break; + case 2: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + } else { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } + break; + } + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/collision/colliders/.svn/text-base/BoxPlaneCollider.as.svn-base b/0.0.9.4/src/alternativa/physics/collision/colliders/.svn/text-base/BoxPlaneCollider.as.svn-base new file mode 100644 index 0000000..3bebdc3 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/colliders/.svn/text-base/BoxPlaneCollider.as.svn-base @@ -0,0 +1,88 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + public class BoxPlaneCollider implements ICollider { + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var normal:Vector3 = new Vector3(); + + public function BoxPlaneCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var box:RigidBox = body1 as RigidBox; +// var plane:RigidPlane; +// if (box == null) { +// box = body2 as RigidBox; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные координаты вершин бокса +// var sx:Number = box.halfSize.x; +// var sy:Number = box.halfSize.y; +// var sz:Number = box.halfSize.z; +// (verts1[0] as Vector3).reset(-sx, -sy, -sz); +// (verts1[1] as Vector3).reset(sx, -sy, -sz); +// (verts1[2] as Vector3).reset(sx, sy, -sz); +// (verts1[3] as Vector3).reset(-sx, sy, -sz); +// (verts1[4] as Vector3).reset(-sx, -sy, sz); +// (verts1[5] as Vector3).reset(sx, -sy, sz); +// (verts1[6] as Vector3).reset(sx, sy, sz); +// (verts1[7] as Vector3).reset(-sx, sy, sz); +// +// box.transform.transformVectors(verts1, verts2); +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// // Проверяем наличие столкновений с каждой вершиной +// collisionInfo.pcount = 0; +// for (var i:int = 0; i < 8; i++) { +// // Вершина добавляется в список точек столкновения, если лежит под плоскостью +// var dist:Number = (verts2[i] as Vector3).dot(normal); +// if (dist < offset) { +// var cp:ContactPoint; +// if (collisionInfo.pcount == collisionInfo.points.length) { +// cp = new ContactPoint(); +// collisionInfo.points[collisionInfo.pcount] = cp; +// } else { +// cp = collisionInfo.points[collisionInfo.pcount]; +// } +// cp.pos.copy(verts2[i]); +// cp.r1.diff(cp.pos, box.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); +// cp.penetration = offset - dist; +// collisionInfo.pcount++; +// } +// } +// if (collisionInfo.pcount > 0) { +// collisionInfo.body1 = box; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// return true; +// } + return false; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/collision/colliders/.svn/text-base/BoxRectCollider.as.svn-base b/0.0.9.4/src/alternativa/physics/collision/colliders/.svn/text-base/BoxRectCollider.as.svn-base new file mode 100644 index 0000000..d6ba302 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/colliders/.svn/text-base/BoxRectCollider.as.svn-base @@ -0,0 +1,575 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * + */ + public class BoxRectCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + + private var vectorToBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxRectCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, rect, vectorToBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + if (!findEdgesIntersection(box, rect, vectorToBox, int(bestAxisIndex/2), bestAxisIndex%2, contact)) { + return false; + } + } + contact.body1 = box.body; + contact.body2 = rect.body; + + // Хак для танков, чтобы исключить утыкания танков в стыки статических примитивов + if (rect.transform.k > 0.99999) { + contact.normal.x = 0; + contact.normal.y = 0; + contact.normal.z = 1; + } + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + // Вектор из центра прямоугольника в центр бокса + vectorToBox.x = boxTransform.d - rectTransform.d; + vectorToBox.y = boxTransform.h - rectTransform.h; + vectorToBox.z = boxTransform.l - rectTransform.l; + + // Проверка пересечения по нормали прямоугольника + rectTransform.getAxis(2, axis22); + if (!testMainAxis(box, rect, axis22, 0, vectorToBox)) return false; + + // Проверка пересечения по основным осям бокса + boxTransform.getAxis(0, axis10); + if (!testMainAxis(box, rect, axis10, 1, vectorToBox)) return false; + boxTransform.getAxis(1, axis11); + if (!testMainAxis(box, rect, axis11, 2, vectorToBox)) return false; + boxTransform.getAxis(2, axis12); + if (!testMainAxis(box, rect, axis12, 3, vectorToBox)) return false; + + // Получаем направляющие рёбер прямоугольника + rectTransform.getAxis(0, axis20); + rectTransform.getAxis(1, axis21); + + // Проверка производных осей + if (!testDerivedAxis(box, rect, axis10, axis20, 4, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis10, axis21, 5, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis11, axis20, 6, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis11, axis21, 7, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis12, axis20, 8, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis12, axis21, 9, vectorToBox)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта бокса с прямоугольником. + * + * @param box бокс + * @param rect прямоугольник + * @param vectorToBox вектор, направленный из центра прямоугольника в центр бокса + * @param faceAxisIdx индекс оси, идентифицирующей полскость столкновения (грань бокса или полскость прямоугольника) + * @param colInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var pnum:int; + var i:int; + var v:Vector3; + var cp:ContactPoint; + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + var colAxis:Vector3 = contact.normal; + + var negativeFace:Boolean; + var code:int; + + if (faceAxisIdx == 0) { + // Столкновение с плоскостью прямоугольника + + // Проверим положение бокса относительно плоскости прямоугольника + colAxis.x = rectTransform.c; + colAxis.y = rectTransform.g; + colAxis.z = rectTransform.k; + +// var offset:Number = colAxis.x*rectTransform.d + colAxis.y*rectTransform.h + colAxis.z*rectTransform.l; +// if (bbPos.vDot(colAxis) < offset) return false; + + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisIdx = axisIdx; + incidentAxisDot = dot; + } + } + negativeFace = incidentAxisDot > 0; + + code = 1 << (incidentAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + + // Получаем список вершин грани бокса, переводим их в систему координат прямоугольника и выполняем обрезку + // по прямоугольнику. Таким образом получается список потенциальных точек контакта. + boxTransform.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box.hs, incidentAxisIdx, negativeFace, points1); + boxTransform.transformVectorsN(points1, points2, 4); + rectTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByRect(rect.hs); + // Проверяем каждую потенциальную точку на принадлежность нижней полуплоскости прямоугольника и добавляем такие точки в список контактов + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if (v.z < epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = -v.z; + var cpPos:Vector3 = cp.pos; + cpPos.x = rectTransform.a*v.x + rectTransform.b*v.y + rectTransform.c*v.z + rectTransform.d; + cpPos.y = rectTransform.e*v.x + rectTransform.f*v.y + rectTransform.g*v.z + rectTransform.h; + cpPos.z = rectTransform.i*v.x + rectTransform.j*v.y + rectTransform.k*v.z + rectTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } else { + // Столкновение с гранью бокса + faceAxisIdx--; + boxTransform.getAxis(faceAxisIdx, colAxis); + negativeFace = colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z > 0; + + code = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) { + return false; + } + + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (rectTransform.c*colAxis.x + rectTransform.g*colAxis.y + rectTransform.k*colAxis.z < 0) return false; + + getFaceVertsByAxis(rect.hs, 2, false, points1); + rectTransform.transformVectorsN(points1, points2, 4); + boxTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if ((pen = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = pen; + cpPos = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } + return true; + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * + * @param hs + * @return + */ + private function clipByRect(hs:Vector3):int { + var pnum:int = 4; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box первый бокс + * @param rect второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param colInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + boxTransform.getAxis(axisIdx1, axis10); + rectTransform.getAxis(axisIdx2, axis20); + var colAxis:Vector3 = contact.normal; + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону бокса + if (colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + var vx:Number = box.hs.x; + var vy:Number = box.hs.y; + var vz:Number = box.hs.z; + var x2:Number = rect.hs.x; + var y2:Number = rect.hs.y; + var z2:Number = rect.hs.z; + // x + if (axisIdx1 == 0) { + vx = 0; + halfLen1 = box.hs.x; + } else { + if (boxTransform.a*colAxis.x + boxTransform.e*colAxis.y + boxTransform.i*colAxis.z > 0) { + vx = -vx; + if ((box.excludedFaces & 2) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 1) != 0) { + return false; + } + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = rect.hs.x; + } else { + if (rectTransform.a*colAxis.x + rectTransform.e*colAxis.y + rectTransform.i*colAxis.z < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + vy = 0; + halfLen1 = box.hs.y; + } else { + if (boxTransform.b*colAxis.x + boxTransform.f*colAxis.y + boxTransform.j*colAxis.z > 0) { + vy = -vy; + if ((box.excludedFaces & 8) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 4) != 0) { + return false; + } + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = rect.hs.y; + } else { + if (rectTransform.b*colAxis.x + rectTransform.f*colAxis.y + rectTransform.j*colAxis.z < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + vz = 0; + halfLen1 = box.hs.z; + } else { + if (boxTransform.c*colAxis.x + boxTransform.g*colAxis.y + boxTransform.k*colAxis.z > 0) { + vz = -vz; + if ((box.excludedFaces & 32) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 16) != 0) { + return false; + } + } + } + // Получаем глобальные координаты средних точек рёбер + + var x1:Number = boxTransform.a*vx + boxTransform.b*vy + boxTransform.c*vz + boxTransform.d; + var y1:Number = boxTransform.e*vx + boxTransform.f*vy + boxTransform.g*vz + boxTransform.h; + var z1:Number = boxTransform.i*vx + boxTransform.j*vy + boxTransform.k*vz + boxTransform.l; + vx = x2; + vy = y2; + vz = z2; + x2 = rectTransform.a*vx + rectTransform.b*vy + rectTransform.c*vz + rectTransform.d; + y2 = rectTransform.e*vx + rectTransform.f*vy + rectTransform.g*vz + rectTransform.h; + z2 = rectTransform.i*vx + rectTransform.j*vy + rectTransform.k*vz + rectTransform.l; + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + vx = x2 - x1; + vy = y2 - y1; + vz = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cpPos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cpPos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cpPos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var v:Vector3 = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + return true; + } + + /** + * Проверяет пересечение вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box + * @param rect + * @param axis + * @param axisIndex + * @param vectorToBox + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param rect + * @param axis1 + * @param axis2 + * @param axisIndex + * @param vectorToBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, rect:CollisionRect, axis1:Vector3, axis2:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия вдоль заданной оси. + * + * @param box бокс + * @param rect прямоугольник + * @param axis ось + * @param vectorToBox вектор, соединяющий центр прямоугольника с центром бокса + * @return величина перекрытия вдоль оси + */ + public function overlapOnAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, vectorToBox:Vector3):Number { + var m:Matrix4 = box.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box.hs.z; + if (d < 0) d = -d; + projection += d; + + m = rect.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*rect.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*rect.hs.y; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox.x*axis.x + vectorToBox.y*axis.y + vectorToBox.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} diff --git a/0.0.9.4/src/alternativa/physics/collision/colliders/.svn/text-base/BoxSphereCollider.as.svn-base b/0.0.9.4/src/alternativa/physics/collision/colliders/.svn/text-base/BoxSphereCollider.as.svn-base new file mode 100644 index 0000000..da5e36d --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/colliders/.svn/text-base/BoxSphereCollider.as.svn-base @@ -0,0 +1,168 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.math.Vector3; + use namespace altphysics; + /** + * + */ + public class BoxSphereCollider implements ICollider { + + private var center:Vector3 = new Vector3(); + private var closestPt:Vector3 = new Vector3(); + + private var bPos:Vector3 = new Vector3(); + private var sPos:Vector3 = new Vector3(); + + /** + * + */ + public function BoxSphereCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + // Зафиксированно столкновение + contact.body1 = sphere.body; + contact.body2 = box.body; + contact.normal.vCopy(closestPt).vTransformBy4(box.transform).vSubtract(sPos).vNormalize().vReverse(); + contact.pcount = 1; + + var cp:ContactPoint = contact.points[0]; + cp.penetration = sphere.r - Math.sqrt(distSqr); + cp.pos.vCopy(contact.normal).vScale(-sphere.r).vAdd(sPos); + cp.r1.vDiff(cp.pos, sPos); + cp.r2.vDiff(cp.pos, bPos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + + return true; + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/collision/colliders/.svn/text-base/BoxTriangleCollider.as.svn-base b/0.0.9.4/src/alternativa/physics/collision/colliders/.svn/text-base/BoxTriangleCollider.as.svn-base new file mode 100644 index 0000000..935525d --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/colliders/.svn/text-base/BoxTriangleCollider.as.svn-base @@ -0,0 +1,725 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionTriangle; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * + */ + public class BoxTriangleCollider extends BoxCollider { + + public var epsilon:Number = 0.001; + + private var bestAxisIndex:int; + private var minOverlap:Number; + private var toBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var colNormal:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxTriangleCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, tri, toBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + if (!findEdgesIntersection(box, tri, toBox, bestAxisIndex%3, int(bestAxisIndex/3), contact)) return false; + } + + contact.body1 = box.body; + contact.body2 = tri.body; + + // Хак для танков, чтобы исключить утыкания танков в стыки статических примитивов + if (tri.transform.k > 0.99999) { + contact.normal.x = 0; + contact.normal.y = 0; + contact.normal.z = 1; + } + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + toBox.x = boxTransform.d - triTransform.d; + toBox.y = boxTransform.h - triTransform.h; + toBox.z = boxTransform.l - triTransform.l; + + minOverlap = 1e308; + + // Сначала проверяется нормаль треугольника + axis.x = triTransform.c; + axis.y = triTransform.g; + axis.z = triTransform.k; + if (!testMainAxis(box, tri, axis, 0, toBox)) return false; + + // Проверка основных осей бокса + axis10.x = boxTransform.a; + axis10.y = boxTransform.e; + axis10.z = boxTransform.i; + if (!testMainAxis(box, tri, axis10, 1, toBox)) return false; + axis11.x = boxTransform.b; + axis11.y = boxTransform.f; + axis11.z = boxTransform.j; + if (!testMainAxis(box, tri, axis11, 2, toBox)) return false; + axis12.x = boxTransform.c; + axis12.y = boxTransform.g; + axis12.z = boxTransform.k; + if (!testMainAxis(box, tri, axis12, 3, toBox)) return false; + + // Проверка производных осей + // TODO: заменить вычисления векторных произведений инлайнами + var v:Vector3 = tri.e0; + axis20.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis20.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis20.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis20, 4, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis20, 5, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis20, 6, toBox)) return false; + + v = tri.e1; + axis21.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis21.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis21.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis21, 7, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis21, 8, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis21, 9, toBox)) return false; + + v = tri.e2; + axis22.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis22.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis22.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis22, 10, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis22, 11, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis22, 12, toBox)) return false; + + return true; + } + + /** + * Тестирует пересечение примитивов вдоль заданной оси. + * + * @param box бокс + * @param tri треугольник + * @param axis ось + * @param axisIndex индекс оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return true, если примитивы имеют перекрытие проекций вдоль указанной оси, иначе false + */ + private function testMainAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, axisIndex:int, toBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param tri + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, tri:CollisionTriangle, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Рассчитывает величину перекрытия проекций бокса и треугольника на заданную ось. + * + * @param box бокс + * @param tri треугольник + * @param axis единичный направляющий вектор оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return величина перекрытия. Положительное значение указывает на наличие перекрытия, нулевое или отрицательное значение указывает на отсутствие перекрытия. + */ + private function overlapOnAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, toBox:Vector3):Number { + var t:Matrix4 = box.transform; + var projection:Number = (t.a*axis.x + t.e*axis.y + t.i*axis.z)*box.hs.x; + if (projection < 0) projection = -projection; + var d:Number = (t.b*axis.x + t.f*axis.y + t.j*axis.z)*box.hs.y; + projection += d < 0 ? -d : d; + d = (t.c*axis.x + t.g*axis.y + t.k*axis.z)*box.hs.z; + projection += d < 0 ? -d : d; + + var vectorProjection:Number = toBox.x*axis.x + toBox.y*axis.y + toBox.z*axis.z; + // Для оптимизации ось преобразуется в систему треугольника, а не его вершины в мировую систему + t = tri.transform; + var ax:Number = t.a*axis.x + t.e*axis.y + t.i*axis.z; + var ay:Number = t.b*axis.x + t.f*axis.y + t.j*axis.z; + var az:Number = t.c*axis.x + t.g*axis.y + t.k*axis.z; + var max:Number = 0; + if (vectorProjection < 0) { + vectorProjection = -vectorProjection; + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d < max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d < max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d < max) max = d; + max = -max; + } else { + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d > max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d > max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d > max) max = d; + } + + return projection + max - vectorProjection; + } + + /** + * Определяет точки контакта бокса и треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function findFaceContactPoints(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIndex:int, contact:Contact):Boolean { + if (faceAxisIndex == 0) { + // Столкновение с плоскостью треугольника + return getBoxToTriContact(box, tri, toBox, contact); + } else { + // Столкновение с гранью бокса + return getTriToBoxContact(box, tri, toBox, faceAxisIndex, contact); + } + } + + /** + * Определяет точки контакта бокса с плоскостью треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getBoxToTriContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + colNormal.x = triTransform.c; + colNormal.y = triTransform.g; + colNormal.z = triTransform.k; + + var over:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + if (!over) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var incAxisDot:Number = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colNormal.x + axis.y*colNormal.y + axis.z*colNormal.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incAxisDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Обрезка грани + var negativeFace:Boolean = incAxisDot > 0; + + var code:int = 1 << (incFaceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + getFaceVertsByAxis(box.hs, incFaceAxisIdx, negativeFace, points1); + boxTransform.transformVectorsN(points1, points2, 4); + triTransform.transformVectorsInverseN(points2, points1, 4); + var pnum:int = clipByTriangle(tri); + // Среди конечного списка точек определяются лежащие под плоскостью треугольника + var cp:ContactPoint; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points2[i]; + if ((over && v.z < 0) || (!over && v.z > 0)) { + cp = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z + triTransform.d; + cpPos.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z + triTransform.h; + cpPos.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z + triTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = over ? -v.z : v.z; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Оперделяет точки контакта треугольника с гранью бокса. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getTriToBoxContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + faceAxisIdx--; + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + boxTransform.getAxis(faceAxisIdx, colNormal); + var negativeFace:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + + var code:int = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + if (!negativeFace) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + var v:Vector3 = points1[0]; + v.x = tri.v0.x; + v.y = tri.v0.y; + v.z = tri.v0.z; + + v = points1[1]; + v.x = tri.v1.x; + v.y = tri.v1.y; + v.z = tri.v1.z; + + v = points1[2]; + v.x = tri.v2.x; + v.y = tri.v2.y; + v.z = tri.v2.z; + + triTransform.transformVectorsN(points1, points2, 3); + boxTransform.transformVectorsInverseN(points2, points1, 3); + + var pnum:int = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность боксу и добавляем такие точки в список контактов + var penetration:Number; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + v = points1[i]; + penetration = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace); + if (penetration > -epsilon) { + var cp:ContactPoint = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = penetration; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Вычисляет величину проникновения точки в бокс. + * + * @param hs вектор половинных размеров бокса + * @param p точка в системе координат бокса + * @param axisIndex индекс оси + * @param negativeFace если true, проверяется нижняя грань + * @return величина проникновения точки в бокс + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, negativeFace:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (negativeFace) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (negativeFace) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (negativeFace) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, вершины которой заданы в списке points1. Результат сохраняется в этом же списке. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 3; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Обрезает грань прямоугольника сторонами треугольника. Обрезка выполняется в системе координат и в проекции на + * плоскость треугольника. Входные вершины грани в количестве четырёх штук должны находиться в списке points1. + * Конечные вершины сохраняются в списке points2. + * + * @param tri треугольник + * @return количество врешин в конечном списке + */ + private function clipByTriangle(tri:CollisionTriangle):int { + var vnum:int = 4; + vnum = clipByLine(tri.v0, tri.e0, points1, vnum, points2); + if (vnum == 0) return 0; + vnum = clipByLine(tri.v1, tri.e1, points2, vnum, points1); + if (vnum == 0) return 0; + return clipByLine(tri.v2, tri.e2, points1, vnum, points2);; + } + + /** + * Обрезает полигон указанной прямой. + * + * @param linePoint точка на прямой + * @param lineDir единичный направляющий вектор прямой + * @param verticesIn список вершин исходного полигона + * @param vnum количество вершин исходного полигона + * @param verticesOut список, куда будут записаны вершины конечного полигона + * @return количество вершин конечного полигона + */ + private function clipByLine(linePoint:Vector3, lineDir:Vector3, verticesIn:Vector., vnum:int, verticesOut:Vector.):int { + var nx:Number = -lineDir.y; + var ny:Number = lineDir.x; + var offset:Number = linePoint.x*nx + linePoint.y*ny; + var v1:Vector3 = verticesIn[int(vnum - 1)]; + var offset1:Number = v1.x*nx + v1.y*ny; + var t:Number; + var v:Vector3; + var num:int = 0; + for (var i:int = 0; i < vnum; i++) { + var v2:Vector3 = verticesIn[i]; + var offset2:Number = v2.x*nx + v2.y*ny; + if (offset1 < offset) { + // Первая точка ребра во внешней полуплоскости + if (offset2 > offset) { + // Вторая точка ребра во внутренней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } else { + // Первая точка ребра во внутренней полуплоскости. Добавляем её в конечный список. + v = verticesOut[num]; + v.x = v1.x; + v.y = v1.y; + v.z = v1.z; + num++; + if (offset2 < offset) { + // Вторая точка ребра во внешней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } + v1 = v2; + offset1 = offset2; + } + return num; + } + + /** + * + * @param box + * @param tri + * @param toBox + * @param boxAxisIdx + * @param triAxisIdx + * @param contact + */ + private function findEdgesIntersection(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, boxAxisIdx:int, triAxisIdx:int, contact:Contact):Boolean { + // Определение точки и направляющего вектора ребра треугольника + var tmpx1:Number; + var tmpy1:Number; + var tmpz1:Number; + var tmpx2:Number; + var tmpy2:Number; + var tmpz2:Number; + switch (triAxisIdx) { + case 0: + tmpx1 = tri.e0.x; + tmpy1 = tri.e0.y; + tmpz1 = tri.e0.z; + tmpx2 = tri.v0.x; + tmpy2 = tri.v0.y; + tmpz2 = tri.v0.z; + break; + case 1: + tmpx1 = tri.e1.x; + tmpy1 = tri.e1.y; + tmpz1 = tri.e1.z; + tmpx2 = tri.v1.x; + tmpy2 = tri.v1.y; + tmpz2 = tri.v1.z; + break; + case 2: + tmpx1 = tri.e2.x; + tmpy1 = tri.e2.y; + tmpz1 = tri.e2.z; + tmpx2 = tri.v2.x; + tmpy2 = tri.v2.y; + tmpz2 = tri.v2.z; + break; + } + var triTransform:Matrix4 = tri.transform; + axis20.x = triTransform.a*tmpx1 + triTransform.b*tmpy1 + triTransform.c*tmpz1; + axis20.y = triTransform.e*tmpx1 + triTransform.f*tmpy1 + triTransform.g*tmpz1; + axis20.z = triTransform.i*tmpx1 + triTransform.j*tmpy1 + triTransform.k*tmpz1; + var x2:Number = triTransform.a*tmpx2 + triTransform.b*tmpy2 + triTransform.c*tmpz2 + triTransform.d; + var y2:Number = triTransform.e*tmpx2 + triTransform.f*tmpy2 + triTransform.g*tmpz2 + triTransform.h; + var z2:Number = triTransform.i*tmpx2 + triTransform.j*tmpy2 + triTransform.k*tmpz2 + triTransform.l; + + // Определение нормали контакта, точки и направляющего вектора ребра бокса + var boxTransform:Matrix4 = box.transform; + boxTransform.getAxis(boxAxisIdx, axis10); + + // Нормаль контакта + var v:Vector3 = contact.normal; + v.x = axis10.y*axis20.z - axis10.z*axis20.y; + v.y = axis10.z*axis20.x - axis10.x*axis20.z; + v.z = axis10.x*axis20.y - axis10.y*axis20.x; + k = 1/Math.sqrt(v.x*v.x + v.y*v.y + v.z*v.z); + v.x *= k; + v.y *= k; + v.z *= k; + // Разворот нормали в сторону бокса + if (v.x*toBox.x + v.y*toBox.y + v.z*toBox.z < 0) { + v.x = -v.x; + v.y = -v.y; + v.z = -v.z; + } + + var boxHalfLen:Number; + tmpx1 = box.hs.x; + tmpy1 = box.hs.y; + tmpz1 = box.hs.z; + // X + if (boxAxisIdx == 0) { + tmpx1 = 0; + boxHalfLen = box.hs.x; + } else { + if (boxTransform.a*v.x + boxTransform.e*v.y + boxTransform.i*v.z > 0) { + tmpx1 = -tmpx1; + if ((box.excludedFaces & 2) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 1) != 0) { + return false; + } + } + } + // Y + if (boxAxisIdx == 1) { + tmpy1 = 0; + boxHalfLen = box.hs.y; + } else { + if (boxTransform.b*v.x + boxTransform.f*v.y + boxTransform.j*v.z > 0) { + tmpy1 = -tmpy1; + if ((box.excludedFaces & 8) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 4) != 0) { + return false; + } + } + } + // Z + if (boxAxisIdx == 2) { + tmpz1 = 0; + boxHalfLen = box.hs.z; + } else { + if (boxTransform.c*v.x + boxTransform.g*v.y + boxTransform.k*v.z > 0) { + tmpz1 = -tmpz1; + if ((box.excludedFaces & 32) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 16) != 0) { + return false; + } + } + } + var x1:Number = boxTransform.a*tmpx1 + boxTransform.b*tmpy1 + boxTransform.c*tmpz1 + boxTransform.d; + var y1:Number = boxTransform.e*tmpx1 + boxTransform.f*tmpy1 + boxTransform.g*tmpz1 + boxTransform.h; + var z1:Number = boxTransform.i*tmpx1 + boxTransform.j*tmpy1 + boxTransform.k*tmpz1 + boxTransform.l; + + // Находим точку пересечения рёбер, решая систему уравнений + // axis10 - направляющий вектор ребра бокса + // x1, y1, z1 - средняя точка ребра бокса + // axis20 - направляющий вектор ребра треугольника + // x2, y2, z2 - начальная точка ребра треугольника + var k:Number = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + var vx:Number = x2 - x1; + var vy:Number = y2 - y1; + var vz:Number = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + + // Запись данных о контакте + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + v = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + v.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + v.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + v.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = v.x - boxTransform.d; + r.y = v.y - boxTransform.h; + r.z = v.z - boxTransform.l; + r = cp.r2; + r.x = v.x - triTransform.d; + r.y = v.y - triTransform.h; + r.z = v.z - triTransform.l; + return true; + } + + } +} diff --git a/0.0.9.4/src/alternativa/physics/collision/colliders/.svn/text-base/SpherePlaneCollider.as.svn-base b/0.0.9.4/src/alternativa/physics/collision/colliders/.svn/text-base/SpherePlaneCollider.as.svn-base new file mode 100644 index 0000000..30cd55f --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/colliders/.svn/text-base/SpherePlaneCollider.as.svn-base @@ -0,0 +1,68 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + /** + * + */ + public class SpherePlaneCollider implements ICollider { + + private var normal:Vector3 = new Vector3(); + + /** + * + */ + public function SpherePlaneCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var sphere:RigidSphere = body1 as RigidSphere; +// var plane:RigidPlane; +// if (sphere == null) { +// sphere = body2 as RigidSphere; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// +// var dist:Number = sphere.state.pos.dot(normal) - offset; +// if (dist > sphere.r) return false; +// +// collisionInfo.body1 = sphere; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// collisionInfo.pcount = 1; +// +// var cp:ContactPoint = collisionInfo.points[0]; +// cp.penetration = sphere.r - dist; +// cp.pos.copy(normal).reverse().scale(sphere.r).add(sphere.state.pos); +// cp.r1.diff(cp.pos, sphere.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/collision/colliders/.svn/text-base/SphereSphereCollider.as.svn-base b/0.0.9.4/src/alternativa/physics/collision/colliders/.svn/text-base/SphereSphereCollider.as.svn-base new file mode 100644 index 0000000..3332f3c --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/colliders/.svn/text-base/SphereSphereCollider.as.svn-base @@ -0,0 +1,70 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.math.Vector3; + use namespace altphysics; + + public class SphereSphereCollider implements ICollider { + + private var p1:Vector3 = new Vector3(); + private var p2:Vector3 = new Vector3(); + + public function SphereSphereCollider() { + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var s1:CollisionSphere; + var s2:CollisionSphere; + if (prim1.body != null) { + s1 = prim1 as CollisionSphere; + s2 = prim2 as CollisionSphere; + } else { + s1 = prim2 as CollisionSphere; + s2 = prim1 as CollisionSphere; + } + + s1.transform.getAxis(3, p1); + s2.transform.getAxis(3, p2); + var dx:Number = p1.x - p2.x; + var dy:Number = p1.y - p2.y; + var dz:Number = p1.z - p2.z; + var len:Number = dx*dx + dy*dy + dz*dz; + var sum:Number = s1.r + s2.r; + if (len > sum*sum) return false; + len = Math.sqrt(len); + dx /= len; + dy /= len; + dz /= len; + + contact.body1 = s1.body; + contact.body2 = s2.body; + contact.normal.x = dx; + contact.normal.y = dy; + contact.normal.z = dz; + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = sum - len; + cp.pos.x = p1.x - dx*s1.r; + cp.pos.y = p1.y - dy*s1.r; + cp.pos.z = p1.z - dz*s1.r; + cp.r1.vDiff(cp.pos, p1); + cp.r2.vDiff(cp.pos, p2); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/collision/colliders/BoxBoxCollider.as b/0.0.9.4/src/alternativa/physics/collision/colliders/BoxBoxCollider.as new file mode 100644 index 0000000..2d3f04c --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/colliders/BoxBoxCollider.as @@ -0,0 +1,589 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * Расчитывает точки контакта двух боксов. Нормаль контакта направляется в сторону бокса с меньшим ID. + */ + public class BoxBoxCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + private var vectorToBox1:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var bestAxisIndex:int; + private var minOverlap:Number; + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + private var tmpPoints:Vector. = new Vector.(8, true); + private var pcount:int; + + /** + * + */ + public function BoxBoxCollider() { + for (var i:int = 0; i < 8; i++) { + tmpPoints[i] = new ContactPoint(); + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + + if (bestAxisIndex < 6) { + // Контакт грань-(грань|ребро|вершина) + if (!findFaceContactPoints(box1, box2, vectorToBox1, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 6; + findEdgesIntersection(box1, box2, vectorToBox1, int(bestAxisIndex/3), bestAxisIndex%3, contact); + } + contact.body1 = box1.body; + contact.body2 = box2.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + // Вектор из центра второго бокса в центр первого + vectorToBox1.x = transform1.d - transform2.d; + vectorToBox1.y = transform1.h - transform2.h; + vectorToBox1.z = transform1.l - transform2.l; + + // Проверка пересечения по основным осям + axis10.x = transform1.a; + axis10.y = transform1.e; + axis10.z = transform1.i; + if (!testMainAxis(box1, box2, axis10, 0, vectorToBox1)) return false; + axis11.x = transform1.b; + axis11.y = transform1.f; + axis11.z = transform1.j; + if (!testMainAxis(box1, box2, axis11, 1, vectorToBox1)) return false; + axis12.x = transform1.c; + axis12.y = transform1.g; + axis12.z = transform1.k; + if (!testMainAxis(box1, box2, axis12, 2, vectorToBox1)) return false; + + axis20.x = transform2.a; + axis20.y = transform2.e; + axis20.z = transform2.i; + if (!testMainAxis(box1, box2, axis20, 3, vectorToBox1)) return false; + axis21.x = transform2.b; + axis21.y = transform2.f; + axis21.z = transform2.j; + if (!testMainAxis(box1, box2, axis21, 4, vectorToBox1)) return false; + axis22.x = transform2.c; + axis22.y = transform2.g; + axis22.z = transform2.k; + if (!testMainAxis(box1, box2, axis22, 5, vectorToBox1)) return false; + + // Проверка производных осей + if (!testDerivedAxis(box1, box2, axis10, axis20, 6, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis21, 7, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis22, 8, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis11, axis20, 9, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis21, 10, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis22, 11, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis12, axis20, 12, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis21, 13, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis22, 14, vectorToBox1)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта грани одного бокса с гранью/ребром/вершиной другого. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param faceAxisIdx индекс оси первого бокса, перпендикулярной грани, с которой произошло столкновение + * @param contactInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var swapNormal:Boolean = false; + if (faceAxisIdx > 2) { + // Столкновение с гранью второго бокса. Для дальнейших расчётов боксы меняются местами, + // но нормаль контакта всё равно должна быть направлена в сторону первоначального box1 + var tmpBox:CollisionBox = box1; + box1 = box2; + box2 = tmpBox; + vectorToBox1.x = -vectorToBox1.x; + vectorToBox1.y = -vectorToBox1.y; + vectorToBox1.z = -vectorToBox1.z; + faceAxisIdx -= 3; + swapNormal = true; + } + + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + var colAxis:Vector3 = contact.normal; + + transform1.getAxis(faceAxisIdx, colAxis); + var negativeFace:Boolean = colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z > 0; + + var code:int = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box1.excludedFaces) != 0) return false; + + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + // Ищем ось второго бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + transform2.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisDot = dot; + incidentAxisIdx = axisIdx; + } + } + // Получаем список вершин грани второго бокса, переводим их в систему координат первого бокса и выполняем обрезку + // по грани первого бокса. Таким образом получается список потенциальных точек контакта. + transform2.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box2.hs, incidentAxisIdx, incidentAxisDot < 0, points1); + + // TODO: Вычислить результирующую матрицу, затем преобразовать векторы за один заход + transform2.transformVectorsN(points1, points2, 4); + transform1.transformVectorsInverseN(points2, points1, 4); + + var pnum:int = clip(box1.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points1[i]; + if ((pen = getPointBoxPenetration(box1.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + var cp:ContactPoint = tmpPoints[pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = transform1.a*v.x + transform1.b*v.y + transform1.c*v.z + transform1.d; + cpPos.y = transform1.e*v.x + transform1.f*v.y + transform1.g*v.z + transform1.h; + cpPos.z = transform1.i*v.x + transform1.j*v.y + transform1.k*v.z + transform1.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + + cp.penetration = pen; + } + } + if (swapNormal) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (pcount > 4) { + reducePoints(); + } + for (i = 0; i < pcount; i++) { + cp = contact.points[i]; + cp.copyFrom(tmpPoints[i]); + } + contact.pcount = pcount; + return true; + } + + /** + * + * @param contactInfo + */ + private function reducePoints():void { + var i:int; + var minIdx:int; + var cp1:ContactPoint; + var cp2:ContactPoint; + while (pcount > 4) { + var minLen:Number = 1e10; + var p1:Vector3 = (tmpPoints[int(pcount - 1)] as ContactPoint).pos; + var p2:Vector3; + for (i = 0; i < pcount; i++) { + p2 = (tmpPoints[i] as ContactPoint).pos; + var dx:Number = p2.x - p1.x; + var dy:Number = p2.y - p1.y; + var dz:Number = p2.z - p1.z; + var len:Number = dx*dx + dy*dy + dz*dz; + if (len < minLen) { + minLen = len; + minIdx = i; + } + p1 = p2; + } + var ii:int = minIdx == 0 ? (pcount - 1) : (minIdx - 1); + cp1 = tmpPoints[ii]; + cp2 = tmpPoints[minIdx]; + p1 = cp1.pos; + p2 = cp2.pos; + p2.x = 0.5*(p1.x + p2.x); + p2.y = 0.5*(p1.y + p2.y); + p2.z = 0.5*(p1.z + p2.z); + cp2.penetration = 0.5*(cp1.penetration + cp2.penetration); + if (minIdx > 0) { + for (i = minIdx; i < pcount; i++) { + tmpPoints[int(i - 1)] = tmpPoints[i]; + } + tmpPoints[int(pcount - 1)] = cp1; + } + pcount--; + } + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clip(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param contactInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):void { + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + transform1.getAxis(axisIdx1, axis10); + transform2.getAxis(axisIdx2, axis20); + + var colAxis:Vector3 = contact.normal; + + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону первого бокса + if (colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + + var tx:Number = box1.hs.x; + var ty:Number = box1.hs.y; + var tz:Number = box1.hs.z; + + var x2:Number = box2.hs.x; + var y2:Number = box2.hs.y; + var z2:Number = box2.hs.z; + // x + if (axisIdx1 == 0) { + tx = 0; + halfLen1 = box1.hs.x; + } else { + if (colAxis.x*transform1.a + colAxis.y*transform1.e + colAxis.z*transform1.i > 0) { + tx = -tx; + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = box2.hs.x; + } else { + if (colAxis.x*transform2.a + colAxis.y*transform2.e + colAxis.z*transform2.i < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + ty = 0; + halfLen1 = box1.hs.y; + } else { + if (colAxis.x*transform1.b + colAxis.y*transform1.f + colAxis.z*transform1.j > 0) { + ty = -ty; + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = box2.hs.y; + } else { + if (colAxis.x*transform2.b + colAxis.y*transform2.f + colAxis.z*transform2.j < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + tz = 0; + halfLen1 = box1.hs.z; + } else { + if (colAxis.x*transform1.c + colAxis.y*transform1.g + colAxis.z*transform1.k > 0) { + tz = -tz; + } + } + if (axisIdx2 == 2) { + z2 = 0; + halfLen2 = box2.hs.z; + } else { + if (colAxis.x*transform2.c + colAxis.y*transform2.g + colAxis.z*transform2.k < 0) { + z2 = -z2; + } + } + // Получаем глобальные координаты средних точек рёбер + var x1:Number = transform1.a*tx + transform1.b*ty + transform1.c*tz + transform1.d; + var y1:Number = transform1.e*tx + transform1.f*ty + transform1.g*tz + transform1.h; + var z1:Number = transform1.i*tx + transform1.j*ty + transform1.k*tz + transform1.l; + tx = x2; + ty = y2; + tz = z2; + x2 = transform2.a*tx + transform2.b*ty + transform2.c*tz + transform2.d; + y2 = transform2.e*tx + transform2.f*ty + transform2.g*tz + transform2.h; + z2 = transform2.i*tx + transform2.j*ty + transform2.k*tz + transform2.l; + + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + tx = x2 - x1; + ty = y2 - y1; + tz = z2 - z1; + var c1:Number = axis10.x*tx + axis10.y*ty + axis10.z*tz; + var c2:Number = axis20.x*tx + axis20.y*ty + axis20.z*tz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cp.pos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cp.pos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение боксов вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box1 + * @param box2 + * @param axis + * @param axisIndex + * @param toBox1 + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box1 + * @param box2 + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox1 + * @return + */ + private function testDerivedAxis(box1:CollisionBox, box2:CollisionBox, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия двух боксов вдоль заданной оси. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param axis ось + * @param vectorToBox1 вектор, соединяющий центр второго бокса с центром первого + * @return величина перекрытия боксов вдоль оси + */ + public function overlapOnAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, vectorToBox1:Vector3):Number { + var m:Matrix4 = box1.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box1.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box1.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box1.hs.z; + if (d < 0) d = -d; + projection += d; + + m = box2.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box2.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box2.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box2.hs.z; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox1.x*axis.x + vectorToBox1.y*axis.y + vectorToBox1.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} + import alternativa.math.Vector3; + + class CollisionPointTmp { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + } diff --git a/0.0.9.4/src/alternativa/physics/collision/colliders/BoxCollider.as b/0.0.9.4/src/alternativa/physics/collision/colliders/BoxCollider.as new file mode 100644 index 0000000..2429bb1 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/colliders/BoxCollider.as @@ -0,0 +1,427 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + /** + * + */ + public class BoxCollider implements ICollider { + + /** + * + */ + public function BoxCollider() { + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + return false; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + protected function getFaceVertsByAxis(hs:Vector3, axisIdx:int, negativeFace:Boolean, result:Vector.):void { + var v:Vector3; + switch (axisIdx) { + case 0: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + } + break; + case 1: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + } + break; + case 2: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + } else { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } + break; + } + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/collision/colliders/BoxPlaneCollider.as b/0.0.9.4/src/alternativa/physics/collision/colliders/BoxPlaneCollider.as new file mode 100644 index 0000000..3bebdc3 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/colliders/BoxPlaneCollider.as @@ -0,0 +1,88 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + public class BoxPlaneCollider implements ICollider { + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var normal:Vector3 = new Vector3(); + + public function BoxPlaneCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var box:RigidBox = body1 as RigidBox; +// var plane:RigidPlane; +// if (box == null) { +// box = body2 as RigidBox; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные координаты вершин бокса +// var sx:Number = box.halfSize.x; +// var sy:Number = box.halfSize.y; +// var sz:Number = box.halfSize.z; +// (verts1[0] as Vector3).reset(-sx, -sy, -sz); +// (verts1[1] as Vector3).reset(sx, -sy, -sz); +// (verts1[2] as Vector3).reset(sx, sy, -sz); +// (verts1[3] as Vector3).reset(-sx, sy, -sz); +// (verts1[4] as Vector3).reset(-sx, -sy, sz); +// (verts1[5] as Vector3).reset(sx, -sy, sz); +// (verts1[6] as Vector3).reset(sx, sy, sz); +// (verts1[7] as Vector3).reset(-sx, sy, sz); +// +// box.transform.transformVectors(verts1, verts2); +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// // Проверяем наличие столкновений с каждой вершиной +// collisionInfo.pcount = 0; +// for (var i:int = 0; i < 8; i++) { +// // Вершина добавляется в список точек столкновения, если лежит под плоскостью +// var dist:Number = (verts2[i] as Vector3).dot(normal); +// if (dist < offset) { +// var cp:ContactPoint; +// if (collisionInfo.pcount == collisionInfo.points.length) { +// cp = new ContactPoint(); +// collisionInfo.points[collisionInfo.pcount] = cp; +// } else { +// cp = collisionInfo.points[collisionInfo.pcount]; +// } +// cp.pos.copy(verts2[i]); +// cp.r1.diff(cp.pos, box.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); +// cp.penetration = offset - dist; +// collisionInfo.pcount++; +// } +// } +// if (collisionInfo.pcount > 0) { +// collisionInfo.body1 = box; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// return true; +// } + return false; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/collision/colliders/BoxRectCollider.as b/0.0.9.4/src/alternativa/physics/collision/colliders/BoxRectCollider.as new file mode 100644 index 0000000..d6ba302 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/colliders/BoxRectCollider.as @@ -0,0 +1,575 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * + */ + public class BoxRectCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + + private var vectorToBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxRectCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, rect, vectorToBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + if (!findEdgesIntersection(box, rect, vectorToBox, int(bestAxisIndex/2), bestAxisIndex%2, contact)) { + return false; + } + } + contact.body1 = box.body; + contact.body2 = rect.body; + + // Хак для танков, чтобы исключить утыкания танков в стыки статических примитивов + if (rect.transform.k > 0.99999) { + contact.normal.x = 0; + contact.normal.y = 0; + contact.normal.z = 1; + } + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + // Вектор из центра прямоугольника в центр бокса + vectorToBox.x = boxTransform.d - rectTransform.d; + vectorToBox.y = boxTransform.h - rectTransform.h; + vectorToBox.z = boxTransform.l - rectTransform.l; + + // Проверка пересечения по нормали прямоугольника + rectTransform.getAxis(2, axis22); + if (!testMainAxis(box, rect, axis22, 0, vectorToBox)) return false; + + // Проверка пересечения по основным осям бокса + boxTransform.getAxis(0, axis10); + if (!testMainAxis(box, rect, axis10, 1, vectorToBox)) return false; + boxTransform.getAxis(1, axis11); + if (!testMainAxis(box, rect, axis11, 2, vectorToBox)) return false; + boxTransform.getAxis(2, axis12); + if (!testMainAxis(box, rect, axis12, 3, vectorToBox)) return false; + + // Получаем направляющие рёбер прямоугольника + rectTransform.getAxis(0, axis20); + rectTransform.getAxis(1, axis21); + + // Проверка производных осей + if (!testDerivedAxis(box, rect, axis10, axis20, 4, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis10, axis21, 5, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis11, axis20, 6, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis11, axis21, 7, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis12, axis20, 8, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis12, axis21, 9, vectorToBox)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта бокса с прямоугольником. + * + * @param box бокс + * @param rect прямоугольник + * @param vectorToBox вектор, направленный из центра прямоугольника в центр бокса + * @param faceAxisIdx индекс оси, идентифицирующей полскость столкновения (грань бокса или полскость прямоугольника) + * @param colInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var pnum:int; + var i:int; + var v:Vector3; + var cp:ContactPoint; + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + var colAxis:Vector3 = contact.normal; + + var negativeFace:Boolean; + var code:int; + + if (faceAxisIdx == 0) { + // Столкновение с плоскостью прямоугольника + + // Проверим положение бокса относительно плоскости прямоугольника + colAxis.x = rectTransform.c; + colAxis.y = rectTransform.g; + colAxis.z = rectTransform.k; + +// var offset:Number = colAxis.x*rectTransform.d + colAxis.y*rectTransform.h + colAxis.z*rectTransform.l; +// if (bbPos.vDot(colAxis) < offset) return false; + + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisIdx = axisIdx; + incidentAxisDot = dot; + } + } + negativeFace = incidentAxisDot > 0; + + code = 1 << (incidentAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + + // Получаем список вершин грани бокса, переводим их в систему координат прямоугольника и выполняем обрезку + // по прямоугольнику. Таким образом получается список потенциальных точек контакта. + boxTransform.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box.hs, incidentAxisIdx, negativeFace, points1); + boxTransform.transformVectorsN(points1, points2, 4); + rectTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByRect(rect.hs); + // Проверяем каждую потенциальную точку на принадлежность нижней полуплоскости прямоугольника и добавляем такие точки в список контактов + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if (v.z < epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = -v.z; + var cpPos:Vector3 = cp.pos; + cpPos.x = rectTransform.a*v.x + rectTransform.b*v.y + rectTransform.c*v.z + rectTransform.d; + cpPos.y = rectTransform.e*v.x + rectTransform.f*v.y + rectTransform.g*v.z + rectTransform.h; + cpPos.z = rectTransform.i*v.x + rectTransform.j*v.y + rectTransform.k*v.z + rectTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } else { + // Столкновение с гранью бокса + faceAxisIdx--; + boxTransform.getAxis(faceAxisIdx, colAxis); + negativeFace = colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z > 0; + + code = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) { + return false; + } + + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (rectTransform.c*colAxis.x + rectTransform.g*colAxis.y + rectTransform.k*colAxis.z < 0) return false; + + getFaceVertsByAxis(rect.hs, 2, false, points1); + rectTransform.transformVectorsN(points1, points2, 4); + boxTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if ((pen = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = pen; + cpPos = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } + return true; + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * + * @param hs + * @return + */ + private function clipByRect(hs:Vector3):int { + var pnum:int = 4; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box первый бокс + * @param rect второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param colInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + boxTransform.getAxis(axisIdx1, axis10); + rectTransform.getAxis(axisIdx2, axis20); + var colAxis:Vector3 = contact.normal; + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону бокса + if (colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + var vx:Number = box.hs.x; + var vy:Number = box.hs.y; + var vz:Number = box.hs.z; + var x2:Number = rect.hs.x; + var y2:Number = rect.hs.y; + var z2:Number = rect.hs.z; + // x + if (axisIdx1 == 0) { + vx = 0; + halfLen1 = box.hs.x; + } else { + if (boxTransform.a*colAxis.x + boxTransform.e*colAxis.y + boxTransform.i*colAxis.z > 0) { + vx = -vx; + if ((box.excludedFaces & 2) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 1) != 0) { + return false; + } + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = rect.hs.x; + } else { + if (rectTransform.a*colAxis.x + rectTransform.e*colAxis.y + rectTransform.i*colAxis.z < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + vy = 0; + halfLen1 = box.hs.y; + } else { + if (boxTransform.b*colAxis.x + boxTransform.f*colAxis.y + boxTransform.j*colAxis.z > 0) { + vy = -vy; + if ((box.excludedFaces & 8) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 4) != 0) { + return false; + } + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = rect.hs.y; + } else { + if (rectTransform.b*colAxis.x + rectTransform.f*colAxis.y + rectTransform.j*colAxis.z < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + vz = 0; + halfLen1 = box.hs.z; + } else { + if (boxTransform.c*colAxis.x + boxTransform.g*colAxis.y + boxTransform.k*colAxis.z > 0) { + vz = -vz; + if ((box.excludedFaces & 32) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 16) != 0) { + return false; + } + } + } + // Получаем глобальные координаты средних точек рёбер + + var x1:Number = boxTransform.a*vx + boxTransform.b*vy + boxTransform.c*vz + boxTransform.d; + var y1:Number = boxTransform.e*vx + boxTransform.f*vy + boxTransform.g*vz + boxTransform.h; + var z1:Number = boxTransform.i*vx + boxTransform.j*vy + boxTransform.k*vz + boxTransform.l; + vx = x2; + vy = y2; + vz = z2; + x2 = rectTransform.a*vx + rectTransform.b*vy + rectTransform.c*vz + rectTransform.d; + y2 = rectTransform.e*vx + rectTransform.f*vy + rectTransform.g*vz + rectTransform.h; + z2 = rectTransform.i*vx + rectTransform.j*vy + rectTransform.k*vz + rectTransform.l; + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + vx = x2 - x1; + vy = y2 - y1; + vz = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cpPos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cpPos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cpPos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var v:Vector3 = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + return true; + } + + /** + * Проверяет пересечение вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box + * @param rect + * @param axis + * @param axisIndex + * @param vectorToBox + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param rect + * @param axis1 + * @param axis2 + * @param axisIndex + * @param vectorToBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, rect:CollisionRect, axis1:Vector3, axis2:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия вдоль заданной оси. + * + * @param box бокс + * @param rect прямоугольник + * @param axis ось + * @param vectorToBox вектор, соединяющий центр прямоугольника с центром бокса + * @return величина перекрытия вдоль оси + */ + public function overlapOnAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, vectorToBox:Vector3):Number { + var m:Matrix4 = box.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box.hs.z; + if (d < 0) d = -d; + projection += d; + + m = rect.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*rect.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*rect.hs.y; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox.x*axis.x + vectorToBox.y*axis.y + vectorToBox.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} diff --git a/0.0.9.4/src/alternativa/physics/collision/colliders/BoxSphereCollider.as b/0.0.9.4/src/alternativa/physics/collision/colliders/BoxSphereCollider.as new file mode 100644 index 0000000..da5e36d --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/colliders/BoxSphereCollider.as @@ -0,0 +1,168 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.math.Vector3; + use namespace altphysics; + /** + * + */ + public class BoxSphereCollider implements ICollider { + + private var center:Vector3 = new Vector3(); + private var closestPt:Vector3 = new Vector3(); + + private var bPos:Vector3 = new Vector3(); + private var sPos:Vector3 = new Vector3(); + + /** + * + */ + public function BoxSphereCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + // Зафиксированно столкновение + contact.body1 = sphere.body; + contact.body2 = box.body; + contact.normal.vCopy(closestPt).vTransformBy4(box.transform).vSubtract(sPos).vNormalize().vReverse(); + contact.pcount = 1; + + var cp:ContactPoint = contact.points[0]; + cp.penetration = sphere.r - Math.sqrt(distSqr); + cp.pos.vCopy(contact.normal).vScale(-sphere.r).vAdd(sPos); + cp.r1.vDiff(cp.pos, sPos); + cp.r2.vDiff(cp.pos, bPos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + + return true; + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/collision/colliders/BoxTriangleCollider.as b/0.0.9.4/src/alternativa/physics/collision/colliders/BoxTriangleCollider.as new file mode 100644 index 0000000..935525d --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/colliders/BoxTriangleCollider.as @@ -0,0 +1,725 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionTriangle; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * + */ + public class BoxTriangleCollider extends BoxCollider { + + public var epsilon:Number = 0.001; + + private var bestAxisIndex:int; + private var minOverlap:Number; + private var toBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var colNormal:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxTriangleCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, tri, toBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + if (!findEdgesIntersection(box, tri, toBox, bestAxisIndex%3, int(bestAxisIndex/3), contact)) return false; + } + + contact.body1 = box.body; + contact.body2 = tri.body; + + // Хак для танков, чтобы исключить утыкания танков в стыки статических примитивов + if (tri.transform.k > 0.99999) { + contact.normal.x = 0; + contact.normal.y = 0; + contact.normal.z = 1; + } + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + toBox.x = boxTransform.d - triTransform.d; + toBox.y = boxTransform.h - triTransform.h; + toBox.z = boxTransform.l - triTransform.l; + + minOverlap = 1e308; + + // Сначала проверяется нормаль треугольника + axis.x = triTransform.c; + axis.y = triTransform.g; + axis.z = triTransform.k; + if (!testMainAxis(box, tri, axis, 0, toBox)) return false; + + // Проверка основных осей бокса + axis10.x = boxTransform.a; + axis10.y = boxTransform.e; + axis10.z = boxTransform.i; + if (!testMainAxis(box, tri, axis10, 1, toBox)) return false; + axis11.x = boxTransform.b; + axis11.y = boxTransform.f; + axis11.z = boxTransform.j; + if (!testMainAxis(box, tri, axis11, 2, toBox)) return false; + axis12.x = boxTransform.c; + axis12.y = boxTransform.g; + axis12.z = boxTransform.k; + if (!testMainAxis(box, tri, axis12, 3, toBox)) return false; + + // Проверка производных осей + // TODO: заменить вычисления векторных произведений инлайнами + var v:Vector3 = tri.e0; + axis20.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis20.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis20.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis20, 4, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis20, 5, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis20, 6, toBox)) return false; + + v = tri.e1; + axis21.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis21.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis21.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis21, 7, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis21, 8, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis21, 9, toBox)) return false; + + v = tri.e2; + axis22.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis22.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis22.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis22, 10, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis22, 11, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis22, 12, toBox)) return false; + + return true; + } + + /** + * Тестирует пересечение примитивов вдоль заданной оси. + * + * @param box бокс + * @param tri треугольник + * @param axis ось + * @param axisIndex индекс оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return true, если примитивы имеют перекрытие проекций вдоль указанной оси, иначе false + */ + private function testMainAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, axisIndex:int, toBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param tri + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, tri:CollisionTriangle, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Рассчитывает величину перекрытия проекций бокса и треугольника на заданную ось. + * + * @param box бокс + * @param tri треугольник + * @param axis единичный направляющий вектор оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return величина перекрытия. Положительное значение указывает на наличие перекрытия, нулевое или отрицательное значение указывает на отсутствие перекрытия. + */ + private function overlapOnAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, toBox:Vector3):Number { + var t:Matrix4 = box.transform; + var projection:Number = (t.a*axis.x + t.e*axis.y + t.i*axis.z)*box.hs.x; + if (projection < 0) projection = -projection; + var d:Number = (t.b*axis.x + t.f*axis.y + t.j*axis.z)*box.hs.y; + projection += d < 0 ? -d : d; + d = (t.c*axis.x + t.g*axis.y + t.k*axis.z)*box.hs.z; + projection += d < 0 ? -d : d; + + var vectorProjection:Number = toBox.x*axis.x + toBox.y*axis.y + toBox.z*axis.z; + // Для оптимизации ось преобразуется в систему треугольника, а не его вершины в мировую систему + t = tri.transform; + var ax:Number = t.a*axis.x + t.e*axis.y + t.i*axis.z; + var ay:Number = t.b*axis.x + t.f*axis.y + t.j*axis.z; + var az:Number = t.c*axis.x + t.g*axis.y + t.k*axis.z; + var max:Number = 0; + if (vectorProjection < 0) { + vectorProjection = -vectorProjection; + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d < max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d < max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d < max) max = d; + max = -max; + } else { + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d > max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d > max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d > max) max = d; + } + + return projection + max - vectorProjection; + } + + /** + * Определяет точки контакта бокса и треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function findFaceContactPoints(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIndex:int, contact:Contact):Boolean { + if (faceAxisIndex == 0) { + // Столкновение с плоскостью треугольника + return getBoxToTriContact(box, tri, toBox, contact); + } else { + // Столкновение с гранью бокса + return getTriToBoxContact(box, tri, toBox, faceAxisIndex, contact); + } + } + + /** + * Определяет точки контакта бокса с плоскостью треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getBoxToTriContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + colNormal.x = triTransform.c; + colNormal.y = triTransform.g; + colNormal.z = triTransform.k; + + var over:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + if (!over) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var incAxisDot:Number = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colNormal.x + axis.y*colNormal.y + axis.z*colNormal.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incAxisDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Обрезка грани + var negativeFace:Boolean = incAxisDot > 0; + + var code:int = 1 << (incFaceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + getFaceVertsByAxis(box.hs, incFaceAxisIdx, negativeFace, points1); + boxTransform.transformVectorsN(points1, points2, 4); + triTransform.transformVectorsInverseN(points2, points1, 4); + var pnum:int = clipByTriangle(tri); + // Среди конечного списка точек определяются лежащие под плоскостью треугольника + var cp:ContactPoint; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points2[i]; + if ((over && v.z < 0) || (!over && v.z > 0)) { + cp = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z + triTransform.d; + cpPos.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z + triTransform.h; + cpPos.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z + triTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = over ? -v.z : v.z; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Оперделяет точки контакта треугольника с гранью бокса. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getTriToBoxContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + faceAxisIdx--; + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + boxTransform.getAxis(faceAxisIdx, colNormal); + var negativeFace:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + + var code:int = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + if (!negativeFace) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + var v:Vector3 = points1[0]; + v.x = tri.v0.x; + v.y = tri.v0.y; + v.z = tri.v0.z; + + v = points1[1]; + v.x = tri.v1.x; + v.y = tri.v1.y; + v.z = tri.v1.z; + + v = points1[2]; + v.x = tri.v2.x; + v.y = tri.v2.y; + v.z = tri.v2.z; + + triTransform.transformVectorsN(points1, points2, 3); + boxTransform.transformVectorsInverseN(points2, points1, 3); + + var pnum:int = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность боксу и добавляем такие точки в список контактов + var penetration:Number; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + v = points1[i]; + penetration = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace); + if (penetration > -epsilon) { + var cp:ContactPoint = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = penetration; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Вычисляет величину проникновения точки в бокс. + * + * @param hs вектор половинных размеров бокса + * @param p точка в системе координат бокса + * @param axisIndex индекс оси + * @param negativeFace если true, проверяется нижняя грань + * @return величина проникновения точки в бокс + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, negativeFace:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (negativeFace) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (negativeFace) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (negativeFace) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, вершины которой заданы в списке points1. Результат сохраняется в этом же списке. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 3; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Обрезает грань прямоугольника сторонами треугольника. Обрезка выполняется в системе координат и в проекции на + * плоскость треугольника. Входные вершины грани в количестве четырёх штук должны находиться в списке points1. + * Конечные вершины сохраняются в списке points2. + * + * @param tri треугольник + * @return количество врешин в конечном списке + */ + private function clipByTriangle(tri:CollisionTriangle):int { + var vnum:int = 4; + vnum = clipByLine(tri.v0, tri.e0, points1, vnum, points2); + if (vnum == 0) return 0; + vnum = clipByLine(tri.v1, tri.e1, points2, vnum, points1); + if (vnum == 0) return 0; + return clipByLine(tri.v2, tri.e2, points1, vnum, points2);; + } + + /** + * Обрезает полигон указанной прямой. + * + * @param linePoint точка на прямой + * @param lineDir единичный направляющий вектор прямой + * @param verticesIn список вершин исходного полигона + * @param vnum количество вершин исходного полигона + * @param verticesOut список, куда будут записаны вершины конечного полигона + * @return количество вершин конечного полигона + */ + private function clipByLine(linePoint:Vector3, lineDir:Vector3, verticesIn:Vector., vnum:int, verticesOut:Vector.):int { + var nx:Number = -lineDir.y; + var ny:Number = lineDir.x; + var offset:Number = linePoint.x*nx + linePoint.y*ny; + var v1:Vector3 = verticesIn[int(vnum - 1)]; + var offset1:Number = v1.x*nx + v1.y*ny; + var t:Number; + var v:Vector3; + var num:int = 0; + for (var i:int = 0; i < vnum; i++) { + var v2:Vector3 = verticesIn[i]; + var offset2:Number = v2.x*nx + v2.y*ny; + if (offset1 < offset) { + // Первая точка ребра во внешней полуплоскости + if (offset2 > offset) { + // Вторая точка ребра во внутренней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } else { + // Первая точка ребра во внутренней полуплоскости. Добавляем её в конечный список. + v = verticesOut[num]; + v.x = v1.x; + v.y = v1.y; + v.z = v1.z; + num++; + if (offset2 < offset) { + // Вторая точка ребра во внешней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } + v1 = v2; + offset1 = offset2; + } + return num; + } + + /** + * + * @param box + * @param tri + * @param toBox + * @param boxAxisIdx + * @param triAxisIdx + * @param contact + */ + private function findEdgesIntersection(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, boxAxisIdx:int, triAxisIdx:int, contact:Contact):Boolean { + // Определение точки и направляющего вектора ребра треугольника + var tmpx1:Number; + var tmpy1:Number; + var tmpz1:Number; + var tmpx2:Number; + var tmpy2:Number; + var tmpz2:Number; + switch (triAxisIdx) { + case 0: + tmpx1 = tri.e0.x; + tmpy1 = tri.e0.y; + tmpz1 = tri.e0.z; + tmpx2 = tri.v0.x; + tmpy2 = tri.v0.y; + tmpz2 = tri.v0.z; + break; + case 1: + tmpx1 = tri.e1.x; + tmpy1 = tri.e1.y; + tmpz1 = tri.e1.z; + tmpx2 = tri.v1.x; + tmpy2 = tri.v1.y; + tmpz2 = tri.v1.z; + break; + case 2: + tmpx1 = tri.e2.x; + tmpy1 = tri.e2.y; + tmpz1 = tri.e2.z; + tmpx2 = tri.v2.x; + tmpy2 = tri.v2.y; + tmpz2 = tri.v2.z; + break; + } + var triTransform:Matrix4 = tri.transform; + axis20.x = triTransform.a*tmpx1 + triTransform.b*tmpy1 + triTransform.c*tmpz1; + axis20.y = triTransform.e*tmpx1 + triTransform.f*tmpy1 + triTransform.g*tmpz1; + axis20.z = triTransform.i*tmpx1 + triTransform.j*tmpy1 + triTransform.k*tmpz1; + var x2:Number = triTransform.a*tmpx2 + triTransform.b*tmpy2 + triTransform.c*tmpz2 + triTransform.d; + var y2:Number = triTransform.e*tmpx2 + triTransform.f*tmpy2 + triTransform.g*tmpz2 + triTransform.h; + var z2:Number = triTransform.i*tmpx2 + triTransform.j*tmpy2 + triTransform.k*tmpz2 + triTransform.l; + + // Определение нормали контакта, точки и направляющего вектора ребра бокса + var boxTransform:Matrix4 = box.transform; + boxTransform.getAxis(boxAxisIdx, axis10); + + // Нормаль контакта + var v:Vector3 = contact.normal; + v.x = axis10.y*axis20.z - axis10.z*axis20.y; + v.y = axis10.z*axis20.x - axis10.x*axis20.z; + v.z = axis10.x*axis20.y - axis10.y*axis20.x; + k = 1/Math.sqrt(v.x*v.x + v.y*v.y + v.z*v.z); + v.x *= k; + v.y *= k; + v.z *= k; + // Разворот нормали в сторону бокса + if (v.x*toBox.x + v.y*toBox.y + v.z*toBox.z < 0) { + v.x = -v.x; + v.y = -v.y; + v.z = -v.z; + } + + var boxHalfLen:Number; + tmpx1 = box.hs.x; + tmpy1 = box.hs.y; + tmpz1 = box.hs.z; + // X + if (boxAxisIdx == 0) { + tmpx1 = 0; + boxHalfLen = box.hs.x; + } else { + if (boxTransform.a*v.x + boxTransform.e*v.y + boxTransform.i*v.z > 0) { + tmpx1 = -tmpx1; + if ((box.excludedFaces & 2) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 1) != 0) { + return false; + } + } + } + // Y + if (boxAxisIdx == 1) { + tmpy1 = 0; + boxHalfLen = box.hs.y; + } else { + if (boxTransform.b*v.x + boxTransform.f*v.y + boxTransform.j*v.z > 0) { + tmpy1 = -tmpy1; + if ((box.excludedFaces & 8) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 4) != 0) { + return false; + } + } + } + // Z + if (boxAxisIdx == 2) { + tmpz1 = 0; + boxHalfLen = box.hs.z; + } else { + if (boxTransform.c*v.x + boxTransform.g*v.y + boxTransform.k*v.z > 0) { + tmpz1 = -tmpz1; + if ((box.excludedFaces & 32) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 16) != 0) { + return false; + } + } + } + var x1:Number = boxTransform.a*tmpx1 + boxTransform.b*tmpy1 + boxTransform.c*tmpz1 + boxTransform.d; + var y1:Number = boxTransform.e*tmpx1 + boxTransform.f*tmpy1 + boxTransform.g*tmpz1 + boxTransform.h; + var z1:Number = boxTransform.i*tmpx1 + boxTransform.j*tmpy1 + boxTransform.k*tmpz1 + boxTransform.l; + + // Находим точку пересечения рёбер, решая систему уравнений + // axis10 - направляющий вектор ребра бокса + // x1, y1, z1 - средняя точка ребра бокса + // axis20 - направляющий вектор ребра треугольника + // x2, y2, z2 - начальная точка ребра треугольника + var k:Number = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + var vx:Number = x2 - x1; + var vy:Number = y2 - y1; + var vz:Number = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + + // Запись данных о контакте + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + v = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + v.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + v.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + v.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = v.x - boxTransform.d; + r.y = v.y - boxTransform.h; + r.z = v.z - boxTransform.l; + r = cp.r2; + r.x = v.x - triTransform.d; + r.y = v.y - triTransform.h; + r.z = v.z - triTransform.l; + return true; + } + + } +} diff --git a/0.0.9.4/src/alternativa/physics/collision/colliders/SpherePlaneCollider.as b/0.0.9.4/src/alternativa/physics/collision/colliders/SpherePlaneCollider.as new file mode 100644 index 0000000..30cd55f --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/colliders/SpherePlaneCollider.as @@ -0,0 +1,68 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + /** + * + */ + public class SpherePlaneCollider implements ICollider { + + private var normal:Vector3 = new Vector3(); + + /** + * + */ + public function SpherePlaneCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var sphere:RigidSphere = body1 as RigidSphere; +// var plane:RigidPlane; +// if (sphere == null) { +// sphere = body2 as RigidSphere; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// +// var dist:Number = sphere.state.pos.dot(normal) - offset; +// if (dist > sphere.r) return false; +// +// collisionInfo.body1 = sphere; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// collisionInfo.pcount = 1; +// +// var cp:ContactPoint = collisionInfo.points[0]; +// cp.penetration = sphere.r - dist; +// cp.pos.copy(normal).reverse().scale(sphere.r).add(sphere.state.pos); +// cp.r1.diff(cp.pos, sphere.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/collision/colliders/SphereSphereCollider.as b/0.0.9.4/src/alternativa/physics/collision/colliders/SphereSphereCollider.as new file mode 100644 index 0000000..3332f3c --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/colliders/SphereSphereCollider.as @@ -0,0 +1,70 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.math.Vector3; + use namespace altphysics; + + public class SphereSphereCollider implements ICollider { + + private var p1:Vector3 = new Vector3(); + private var p2:Vector3 = new Vector3(); + + public function SphereSphereCollider() { + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var s1:CollisionSphere; + var s2:CollisionSphere; + if (prim1.body != null) { + s1 = prim1 as CollisionSphere; + s2 = prim2 as CollisionSphere; + } else { + s1 = prim2 as CollisionSphere; + s2 = prim1 as CollisionSphere; + } + + s1.transform.getAxis(3, p1); + s2.transform.getAxis(3, p2); + var dx:Number = p1.x - p2.x; + var dy:Number = p1.y - p2.y; + var dz:Number = p1.z - p2.z; + var len:Number = dx*dx + dy*dy + dz*dz; + var sum:Number = s1.r + s2.r; + if (len > sum*sum) return false; + len = Math.sqrt(len); + dx /= len; + dy /= len; + dz /= len; + + contact.body1 = s1.body; + contact.body2 = s2.body; + contact.normal.x = dx; + contact.normal.y = dy; + contact.normal.z = dz; + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = sum - len; + cp.pos.x = p1.x - dx*s1.r; + cp.pos.y = p1.y - dy*s1.r; + cp.pos.z = p1.z - dz*s1.r; + cp.r1.vDiff(cp.pos, p1); + cp.r2.vDiff(cp.pos, p2); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/collision/primitives/.svn/all-wcprops b/0.0.9.4/src/alternativa/physics/collision/primitives/.svn/all-wcprops new file mode 100644 index 0000000..c46c7b8 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/primitives/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 124 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/collision/primitives +END +CollisionTriangle.as +K 25 +svn:wc:ra_dav:version-url +V 145 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/collision/primitives/CollisionTriangle.as +END +CollisionSphere.as +K 25 +svn:wc:ra_dav:version-url +V 143 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/collision/primitives/CollisionSphere.as +END +CollisionBox.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/collision/primitives/CollisionBox.as +END +CollisionRect.as +K 25 +svn:wc:ra_dav:version-url +V 141 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/collision/primitives/CollisionRect.as +END diff --git a/0.0.9.4/src/alternativa/physics/collision/primitives/.svn/entries b/0.0.9.4/src/alternativa/physics/collision/primitives/.svn/entries new file mode 100644 index 0000000..5f1be62 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/primitives/.svn/entries @@ -0,0 +1,76 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/collision/primitives +http://svndev.alternativaplatform.com + + + +2010-07-03T11:40:52.545629Z +36684 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +CollisionTriangle.as +file + + + + +2010-10-28T04:32:45.000000Z +23ebb96cda309378ce15ead4cf865446 +2009-11-24T08:44:15.941480Z +23758 +mike + +CollisionSphere.as +file + + + + +2010-10-28T04:32:45.000000Z +335b6b72039123d6d54fee4d6b17f74e +2009-11-24T08:44:15.941480Z +23758 +mike + +CollisionBox.as +file + + + + +2010-10-28T04:32:45.000000Z +d1552d6d5cc026233dd769249d679970 +2010-07-03T11:40:52.545629Z +36684 +mike + +CollisionRect.as +file + + + + +2010-10-28T04:32:45.000000Z +91b913679dca5ae4baabafb09c5aa051 +2009-11-24T08:44:15.941480Z +23758 +mike + diff --git a/0.0.9.4/src/alternativa/physics/collision/primitives/.svn/format b/0.0.9.4/src/alternativa/physics/collision/primitives/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/primitives/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.4/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base b/0.0.9.4/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base new file mode 100644 index 0000000..844c65d --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base @@ -0,0 +1,215 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * Ориентированный бокс. + */ + public class CollisionBox extends CollisionPrimitive { + + // Половинные размеры вдоль каждой из осей + public var hs:Vector3 = new Vector3(); + + public var excludedFaces:int; + + /** + * @param hs + * @param collisionGroup + */ + public function CollisionBox(hs:Vector3, collisionGroup:int) { + super(BOX, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * @return - + */ + override public function calculateAABB():BoundBox { + var t:Matrix4 = transform; + + var xx:Number; + var yy:Number; + var zz:Number; + + xx = t.a < 0 ? -t.a : t.a; + yy = t.b < 0 ? -t.b : t.b; + zz = t.c < 0 ? -t.c : t.c; + aabb.maxX = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minX = -aabb.maxX; + + xx = t.e < 0 ? -t.e : t.e; + yy = t.f < 0 ? -t.f : t.f; + zz = t.g < 0 ? -t.g : t.g; + aabb.maxY = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minY = -aabb.maxY; + + xx = t.i < 0 ? -t.i : t.i; + yy = t.j < 0 ? -t.j : t.j; + zz = t.k < 0 ? -t.k : t.k; + aabb.maxZ = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return - + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var box:CollisionBox = source as CollisionBox; + if (box == null) return this; + super.copyFrom(box); + hs.vCopy(box.hs); + return this; + } + + /** + * @return - + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionBox(hs, collisionGroup); + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + var tMin:Number = -1; + var tMax:Number = 1e308; + var t1:Number; + var t2:Number; + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + + // Проверка, не находится ли начало луча внутри бокса. В этом случае точкой пересечения будет начало луча, + // а нормалью будет нормированный вектор, противоположный вектору луча. + if (ox >= -hs.x && ox <= hs.x && oy >= -hs.y && oy <= hs.y && oz >= -hs.z && oz <= hs.z) { + normal.x = -vector.x; + normal.y = -vector.y; + normal.z = -vector.z; + normal.vNormalize(); + return 0; + } + + // X + if (vx < epsilon && vx > -epsilon) { + if (ox < -hs.x || ox > hs.x) return -1; + } else { + t1 = (-hs.x - ox)/vx; + t2 = (hs.x - ox)/vx; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.x = -1; + normal.y = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.x = 1; + normal.y = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Y + if (vy < epsilon && vy > -epsilon) { + if (oy < -hs.y || oy > hs.y) return -1; + } else { + t1 = (-hs.y - oy)/vy; + t2 = (hs.y - oy)/vy; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.y = -1; + normal.x = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.y = 1; + normal.x = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Z + if (vz < epsilon && vz > -epsilon) { + if (oz < -hs.z || oz > hs.z) return -1; + } else { + t1 = (-hs.z - oz)/vz; + t2 = (hs.z - oz)/vz; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.z = -1; + normal.x = normal.y = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.z = 1; + normal.x = normal.y = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Перевод нормали в мировую систему координат + // Inlined normal.vDeltaTransformBy4(transform); + vx = normal.x; + vy = normal.y; + vz = normal.z; + normal.x = transform.a*vx + transform.b*vy + transform.c*vz; + normal.y = transform.e*vx + transform.f*vy + transform.g*vz; + normal.z = transform.i*vx + transform.j*vy + transform.k*vz; + + return tMin; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionBox hs=" + hs + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base b/0.0.9.4/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base new file mode 100644 index 0000000..7a33b79 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base @@ -0,0 +1,143 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * Ориентированный прямоугольник. Задаётся половинами размеров вдоль осей X (ширина) и Y (длина) локальной системы + * координат примитива. Таким образом, прямоугольник лежит в плоскости XY, его стороны параллельны осям этой + * плоскости, а нормаль направлена вдоль локальной оси Z. + * + * Прямоугольник может быть одно- или двусторонним. В случае одностороннего прямоугольника, + * столкновения с внутренней стороны не регистрируются. + */ + public class CollisionRect extends CollisionPrimitive { + // Половинные размеры прямоугольника вдоль осей X и Y. Нормаль направлена вдоль оси Z. + public var hs:Vector3 = new Vector3(); + // Флаг указывает, является примитив одно- или двусторонним + public var twoSided:Boolean = true; + + // Малое значение. Используется для указания фиктивной высоты примитива и в функции определения пересечения с сегментом. + private static const EPSILON:Number = 0.005; + + /** + * Создаёт новый экземпляр примитива. + * + * @param hs половинные размерв прямоугольника вдоль осей X и Y. Значение z игнорируется. + * @param collisionGroup группа примитива + */ + public function CollisionRect(hs:Vector3, collisionGroup:int) { + super(RECT, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * Расчитывает ограничивающий бокс прямоугольника. Для избежания проблем высота примитива принимается равной + * не нулю, а малому значению. + * + * @return + */ + override public function calculateAABB():BoundBox { + // Баунд бокс прямоугольника имеет минимальную высоту, отличную от нуля во избежание проблем с построением kd-дерева + var t:Matrix4 = transform; + var xx:Number = t.a < 0 ? -t.a : t.a; + var yy:Number = t.b < 0 ? -t.b : t.b; + var zz:Number = t.c < 0 ? -t.c : t.c; + aabb.maxX = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minX = -aabb.maxX; + + xx = t.e < 0 ? -t.e : t.e; + yy = t.f < 0 ? -t.f : t.f; + zz = t.g < 0 ? -t.g : t.g; + aabb.maxY = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minY = -aabb.maxY; + + xx = t.i < 0 ? -t.i : t.i; + yy = t.j < 0 ? -t.j : t.j; + zz = t.k < 0 ? -t.k : t.k; + aabb.maxZ = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var rect:CollisionRect = source as CollisionRect; + if (rect == null) return this; + super.copyFrom(rect); + hs.vCopy(rect.hs); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionRect hs=" + hs + "]"; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionRect(hs, collisionGroup); + } + + /** + * @param origin + * @param vector + * @param collisionGroup + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + + // Проверка параллельности сегмента и плоскости примитива + if (vz > -threshold && vz < threshold) return -1; + var t:Number = -oz/vz; + if (t < 0) return -1; + // Проверка вхождения точки пересечения в прямоугольник + ox += vx*t; + oy += vy*t; + oz = 0; + if (ox < (-hs.x - threshold) || ox > (hs.x + threshold) || oy < (-hs.y - threshold) || oy > (hs.y + threshold)) return -1; + + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + return t; + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base b/0.0.9.4/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base new file mode 100644 index 0000000..b912a5b --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base @@ -0,0 +1,81 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Vector3; + use namespace altphysics; + + + /** + * Сфера. + */ + public class CollisionSphere extends CollisionPrimitive { + + // Радиус сферы + public var r:Number = 0; + + /** + * + * @param r + * @param collisionGroup + */ + public function CollisionSphere(r:Number, collisionGroup:int) { + super(SPHERE, collisionGroup); + this.r = r; + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + aabb.maxX = transform.d + r; + aabb.minX = transform.d - r; + + aabb.maxY = transform.h + r; + aabb.minY = transform.h - r; + + aabb.maxZ = transform.l + r; + aabb.minZ = transform.l - r; + + return aabb; + } + + /** + * @param origin + * @param vector + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var px:Number = origin.x - transform.d; + var py:Number = origin.y - transform.h; + var pz:Number = origin.z - transform.l; + var k:Number = vector.x*px + vector.y*py + vector.z*pz; + if (k > 0) return -1; + var a:Number = vector.x*vector.x + vector.y*vector.y + vector.z*vector.z; + var D:Number = k*k - a*(px*px + py*py + pz*pz - r*r); + if (D < 0) return -1; + return -(k + Math.sqrt(D))/a; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var sphere:CollisionSphere = source as CollisionSphere; + if (sphere == null) return this; + super.copyFrom(sphere); + r = sphere.r; + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionSphere(r, collisionGroup); + } + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base b/0.0.9.4/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base new file mode 100644 index 0000000..bdc3166 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base @@ -0,0 +1,244 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * Примитив треугольник. + */ + public class CollisionTriangle extends CollisionPrimitive { + + public var v0:Vector3 = new Vector3(); + public var v1:Vector3 = new Vector3(); + public var v2:Vector3 = new Vector3(); + + public var e0:Vector3 = new Vector3(); + public var e1:Vector3 = new Vector3(); + public var e2:Vector3 = new Vector3(); + +// public var len0:Number; +// public var len1:Number; +// public var len2:Number; +// + /** + * + * @param v0 + * @param v1 + * @param v2 + * @param collisionGroup + */ + public function CollisionTriangle(v0:Vector3, v1:Vector3, v2:Vector3, collisionGroup:int) { + super(TRIANGLE, collisionGroup); + initVertices(v0, v1, v2); + } + + /** + * Рассчитывает AABB примитива. + * + * @return ссылка на свой AABB + */ + override public function calculateAABB():BoundBox { + var epsilon:Number = 0.005; + var a:Number; + var b:Number; + var eps_c:Number = epsilon*transform.c; + var eps_g:Number = epsilon*transform.g; + var eps_k:Number = epsilon*transform.k; + + // Вершина 0 + // Ось X + a = v0.x*transform.a + v0.y*transform.b; + aabb.minX = aabb.maxX = a + eps_c; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v0.x*transform.e + v0.y*transform.f; + aabb.minY = aabb.maxY = a + eps_g; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v0.x*transform.i + v0.y*transform.j; + aabb.minZ = aabb.maxZ = a + eps_k; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 1 + // Ось X + a = v1.x*transform.a + v1.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v1.x*transform.e + v1.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v1.x*transform.i + v1.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 2 + // Ось X + a = v2.x*transform.a + v2.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v2.x*transform.e + v2.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v2.x*transform.i + v2.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + aabb.minX += transform.d; + aabb.maxX += transform.d; + + aabb.minY += transform.h; + aabb.maxY += transform.h; + + aabb.minZ += transform.l; + aabb.maxZ += transform.l; + + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + // Луч трансформируется в систему координат примитива, затем проверяется пересечение + + var vz:Number = vector.x*transform.c + vector.y*transform.g + vector.z*transform.k; + // Если луч параллелен плоскости птимитива, то пересечения нет + if (vz < epsilon && vz > -epsilon) return -1; + + var tx:Number = origin.x - transform.d; + var ty:Number = origin.y - transform.h; + var tz:Number = origin.z - transform.l; + + var oz:Number = tx*transform.c + ty*transform.g + tz*transform.k; + var t:Number = -oz/vz; + if (t < 0) return -1; + + var ox:Number = tx*transform.a + ty*transform.e + tz*transform.i; + var oy:Number = tx*transform.b + ty*transform.f + tz*transform.j; + + tx = ox + t*(vector.x*transform.a + vector.y*transform.e + vector.z*transform.i); + ty = oy + t*(vector.x*transform.b + vector.y*transform.f + vector.z*transform.j); + tz = oz + t*vz; + + // Проверка вхождения точки в треугольник + if ((e0.x*(ty - v0.y) - e0.y*(tx - v0.x) < 0) || (e1.x*(ty - v1.y) - e1.y*(tx - v1.x) < 0) || (e2.x*(ty - v2.y) - e2.y*(tx - v2.x) < 0)) return -1; + + // Запись нормали + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + + return t; + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + super.copyFrom(source); + var tri:CollisionTriangle = source as CollisionTriangle; + if (tri != null) { + v0.vCopy(tri.v0); + v1.vCopy(tri.v1); + v2.vCopy(tri.v2); + + e0.vCopy(tri.e0); + e1.vCopy(tri.e1); + e2.vCopy(tri.e2); + +// len0 = tri.len0; +// len1 = tri.len1; +// len2 = tri.len2; + } + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionTriangle v0=" + v0 + ", v1=" + v1 + ", v2=" + v2 + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionTriangle(v0, v1, v2, collisionGroup); + } + + /** + * + * @param v0 + * @param v1 + * @param v2 + */ + private function initVertices(v0:Vector3, v1:Vector3, v2:Vector3):void { + this.v0.vCopy(v0); + this.v1.vCopy(v1); + this.v2.vCopy(v2); + + e0.vDiff(v1, v0); +// len0 = e0.vLength(); + e0.vNormalize(); + + e1.vDiff(v2, v1); +// len1 = e1.vLength(); + e1.vNormalize(); + + e2.vDiff(v0, v2); +// len2 = e2.vLength(); + e2.vNormalize(); + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/collision/primitives/CollisionBox.as b/0.0.9.4/src/alternativa/physics/collision/primitives/CollisionBox.as new file mode 100644 index 0000000..844c65d --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/primitives/CollisionBox.as @@ -0,0 +1,215 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * Ориентированный бокс. + */ + public class CollisionBox extends CollisionPrimitive { + + // Половинные размеры вдоль каждой из осей + public var hs:Vector3 = new Vector3(); + + public var excludedFaces:int; + + /** + * @param hs + * @param collisionGroup + */ + public function CollisionBox(hs:Vector3, collisionGroup:int) { + super(BOX, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * @return - + */ + override public function calculateAABB():BoundBox { + var t:Matrix4 = transform; + + var xx:Number; + var yy:Number; + var zz:Number; + + xx = t.a < 0 ? -t.a : t.a; + yy = t.b < 0 ? -t.b : t.b; + zz = t.c < 0 ? -t.c : t.c; + aabb.maxX = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minX = -aabb.maxX; + + xx = t.e < 0 ? -t.e : t.e; + yy = t.f < 0 ? -t.f : t.f; + zz = t.g < 0 ? -t.g : t.g; + aabb.maxY = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minY = -aabb.maxY; + + xx = t.i < 0 ? -t.i : t.i; + yy = t.j < 0 ? -t.j : t.j; + zz = t.k < 0 ? -t.k : t.k; + aabb.maxZ = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return - + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var box:CollisionBox = source as CollisionBox; + if (box == null) return this; + super.copyFrom(box); + hs.vCopy(box.hs); + return this; + } + + /** + * @return - + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionBox(hs, collisionGroup); + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + var tMin:Number = -1; + var tMax:Number = 1e308; + var t1:Number; + var t2:Number; + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + + // Проверка, не находится ли начало луча внутри бокса. В этом случае точкой пересечения будет начало луча, + // а нормалью будет нормированный вектор, противоположный вектору луча. + if (ox >= -hs.x && ox <= hs.x && oy >= -hs.y && oy <= hs.y && oz >= -hs.z && oz <= hs.z) { + normal.x = -vector.x; + normal.y = -vector.y; + normal.z = -vector.z; + normal.vNormalize(); + return 0; + } + + // X + if (vx < epsilon && vx > -epsilon) { + if (ox < -hs.x || ox > hs.x) return -1; + } else { + t1 = (-hs.x - ox)/vx; + t2 = (hs.x - ox)/vx; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.x = -1; + normal.y = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.x = 1; + normal.y = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Y + if (vy < epsilon && vy > -epsilon) { + if (oy < -hs.y || oy > hs.y) return -1; + } else { + t1 = (-hs.y - oy)/vy; + t2 = (hs.y - oy)/vy; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.y = -1; + normal.x = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.y = 1; + normal.x = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Z + if (vz < epsilon && vz > -epsilon) { + if (oz < -hs.z || oz > hs.z) return -1; + } else { + t1 = (-hs.z - oz)/vz; + t2 = (hs.z - oz)/vz; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.z = -1; + normal.x = normal.y = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.z = 1; + normal.x = normal.y = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Перевод нормали в мировую систему координат + // Inlined normal.vDeltaTransformBy4(transform); + vx = normal.x; + vy = normal.y; + vz = normal.z; + normal.x = transform.a*vx + transform.b*vy + transform.c*vz; + normal.y = transform.e*vx + transform.f*vy + transform.g*vz; + normal.z = transform.i*vx + transform.j*vy + transform.k*vz; + + return tMin; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionBox hs=" + hs + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/collision/primitives/CollisionRect.as b/0.0.9.4/src/alternativa/physics/collision/primitives/CollisionRect.as new file mode 100644 index 0000000..7a33b79 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/primitives/CollisionRect.as @@ -0,0 +1,143 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * Ориентированный прямоугольник. Задаётся половинами размеров вдоль осей X (ширина) и Y (длина) локальной системы + * координат примитива. Таким образом, прямоугольник лежит в плоскости XY, его стороны параллельны осям этой + * плоскости, а нормаль направлена вдоль локальной оси Z. + * + * Прямоугольник может быть одно- или двусторонним. В случае одностороннего прямоугольника, + * столкновения с внутренней стороны не регистрируются. + */ + public class CollisionRect extends CollisionPrimitive { + // Половинные размеры прямоугольника вдоль осей X и Y. Нормаль направлена вдоль оси Z. + public var hs:Vector3 = new Vector3(); + // Флаг указывает, является примитив одно- или двусторонним + public var twoSided:Boolean = true; + + // Малое значение. Используется для указания фиктивной высоты примитива и в функции определения пересечения с сегментом. + private static const EPSILON:Number = 0.005; + + /** + * Создаёт новый экземпляр примитива. + * + * @param hs половинные размерв прямоугольника вдоль осей X и Y. Значение z игнорируется. + * @param collisionGroup группа примитива + */ + public function CollisionRect(hs:Vector3, collisionGroup:int) { + super(RECT, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * Расчитывает ограничивающий бокс прямоугольника. Для избежания проблем высота примитива принимается равной + * не нулю, а малому значению. + * + * @return + */ + override public function calculateAABB():BoundBox { + // Баунд бокс прямоугольника имеет минимальную высоту, отличную от нуля во избежание проблем с построением kd-дерева + var t:Matrix4 = transform; + var xx:Number = t.a < 0 ? -t.a : t.a; + var yy:Number = t.b < 0 ? -t.b : t.b; + var zz:Number = t.c < 0 ? -t.c : t.c; + aabb.maxX = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minX = -aabb.maxX; + + xx = t.e < 0 ? -t.e : t.e; + yy = t.f < 0 ? -t.f : t.f; + zz = t.g < 0 ? -t.g : t.g; + aabb.maxY = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minY = -aabb.maxY; + + xx = t.i < 0 ? -t.i : t.i; + yy = t.j < 0 ? -t.j : t.j; + zz = t.k < 0 ? -t.k : t.k; + aabb.maxZ = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var rect:CollisionRect = source as CollisionRect; + if (rect == null) return this; + super.copyFrom(rect); + hs.vCopy(rect.hs); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionRect hs=" + hs + "]"; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionRect(hs, collisionGroup); + } + + /** + * @param origin + * @param vector + * @param collisionGroup + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + + // Проверка параллельности сегмента и плоскости примитива + if (vz > -threshold && vz < threshold) return -1; + var t:Number = -oz/vz; + if (t < 0) return -1; + // Проверка вхождения точки пересечения в прямоугольник + ox += vx*t; + oy += vy*t; + oz = 0; + if (ox < (-hs.x - threshold) || ox > (hs.x + threshold) || oy < (-hs.y - threshold) || oy > (hs.y + threshold)) return -1; + + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + return t; + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/collision/primitives/CollisionSphere.as b/0.0.9.4/src/alternativa/physics/collision/primitives/CollisionSphere.as new file mode 100644 index 0000000..b912a5b --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/primitives/CollisionSphere.as @@ -0,0 +1,81 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Vector3; + use namespace altphysics; + + + /** + * Сфера. + */ + public class CollisionSphere extends CollisionPrimitive { + + // Радиус сферы + public var r:Number = 0; + + /** + * + * @param r + * @param collisionGroup + */ + public function CollisionSphere(r:Number, collisionGroup:int) { + super(SPHERE, collisionGroup); + this.r = r; + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + aabb.maxX = transform.d + r; + aabb.minX = transform.d - r; + + aabb.maxY = transform.h + r; + aabb.minY = transform.h - r; + + aabb.maxZ = transform.l + r; + aabb.minZ = transform.l - r; + + return aabb; + } + + /** + * @param origin + * @param vector + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var px:Number = origin.x - transform.d; + var py:Number = origin.y - transform.h; + var pz:Number = origin.z - transform.l; + var k:Number = vector.x*px + vector.y*py + vector.z*pz; + if (k > 0) return -1; + var a:Number = vector.x*vector.x + vector.y*vector.y + vector.z*vector.z; + var D:Number = k*k - a*(px*px + py*py + pz*pz - r*r); + if (D < 0) return -1; + return -(k + Math.sqrt(D))/a; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var sphere:CollisionSphere = source as CollisionSphere; + if (sphere == null) return this; + super.copyFrom(sphere); + r = sphere.r; + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionSphere(r, collisionGroup); + } + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/collision/primitives/CollisionTriangle.as b/0.0.9.4/src/alternativa/physics/collision/primitives/CollisionTriangle.as new file mode 100644 index 0000000..bdc3166 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/primitives/CollisionTriangle.as @@ -0,0 +1,244 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * Примитив треугольник. + */ + public class CollisionTriangle extends CollisionPrimitive { + + public var v0:Vector3 = new Vector3(); + public var v1:Vector3 = new Vector3(); + public var v2:Vector3 = new Vector3(); + + public var e0:Vector3 = new Vector3(); + public var e1:Vector3 = new Vector3(); + public var e2:Vector3 = new Vector3(); + +// public var len0:Number; +// public var len1:Number; +// public var len2:Number; +// + /** + * + * @param v0 + * @param v1 + * @param v2 + * @param collisionGroup + */ + public function CollisionTriangle(v0:Vector3, v1:Vector3, v2:Vector3, collisionGroup:int) { + super(TRIANGLE, collisionGroup); + initVertices(v0, v1, v2); + } + + /** + * Рассчитывает AABB примитива. + * + * @return ссылка на свой AABB + */ + override public function calculateAABB():BoundBox { + var epsilon:Number = 0.005; + var a:Number; + var b:Number; + var eps_c:Number = epsilon*transform.c; + var eps_g:Number = epsilon*transform.g; + var eps_k:Number = epsilon*transform.k; + + // Вершина 0 + // Ось X + a = v0.x*transform.a + v0.y*transform.b; + aabb.minX = aabb.maxX = a + eps_c; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v0.x*transform.e + v0.y*transform.f; + aabb.minY = aabb.maxY = a + eps_g; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v0.x*transform.i + v0.y*transform.j; + aabb.minZ = aabb.maxZ = a + eps_k; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 1 + // Ось X + a = v1.x*transform.a + v1.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v1.x*transform.e + v1.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v1.x*transform.i + v1.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 2 + // Ось X + a = v2.x*transform.a + v2.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v2.x*transform.e + v2.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v2.x*transform.i + v2.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + aabb.minX += transform.d; + aabb.maxX += transform.d; + + aabb.minY += transform.h; + aabb.maxY += transform.h; + + aabb.minZ += transform.l; + aabb.maxZ += transform.l; + + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + // Луч трансформируется в систему координат примитива, затем проверяется пересечение + + var vz:Number = vector.x*transform.c + vector.y*transform.g + vector.z*transform.k; + // Если луч параллелен плоскости птимитива, то пересечения нет + if (vz < epsilon && vz > -epsilon) return -1; + + var tx:Number = origin.x - transform.d; + var ty:Number = origin.y - transform.h; + var tz:Number = origin.z - transform.l; + + var oz:Number = tx*transform.c + ty*transform.g + tz*transform.k; + var t:Number = -oz/vz; + if (t < 0) return -1; + + var ox:Number = tx*transform.a + ty*transform.e + tz*transform.i; + var oy:Number = tx*transform.b + ty*transform.f + tz*transform.j; + + tx = ox + t*(vector.x*transform.a + vector.y*transform.e + vector.z*transform.i); + ty = oy + t*(vector.x*transform.b + vector.y*transform.f + vector.z*transform.j); + tz = oz + t*vz; + + // Проверка вхождения точки в треугольник + if ((e0.x*(ty - v0.y) - e0.y*(tx - v0.x) < 0) || (e1.x*(ty - v1.y) - e1.y*(tx - v1.x) < 0) || (e2.x*(ty - v2.y) - e2.y*(tx - v2.x) < 0)) return -1; + + // Запись нормали + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + + return t; + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + super.copyFrom(source); + var tri:CollisionTriangle = source as CollisionTriangle; + if (tri != null) { + v0.vCopy(tri.v0); + v1.vCopy(tri.v1); + v2.vCopy(tri.v2); + + e0.vCopy(tri.e0); + e1.vCopy(tri.e1); + e2.vCopy(tri.e2); + +// len0 = tri.len0; +// len1 = tri.len1; +// len2 = tri.len2; + } + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionTriangle v0=" + v0 + ", v1=" + v1 + ", v2=" + v2 + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionTriangle(v0, v1, v2, collisionGroup); + } + + /** + * + * @param v0 + * @param v1 + * @param v2 + */ + private function initVertices(v0:Vector3, v1:Vector3, v2:Vector3):void { + this.v0.vCopy(v0); + this.v1.vCopy(v1); + this.v2.vCopy(v2); + + e0.vDiff(v1, v0); +// len0 = e0.vLength(); + e0.vNormalize(); + + e1.vDiff(v2, v1); +// len1 = e1.vLength(); + e1.vNormalize(); + + e2.vDiff(v0, v2); +// len2 = e2.vLength(); + e2.vNormalize(); + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/collision/types/.svn/all-wcprops b/0.0.9.4/src/alternativa/physics/collision/types/.svn/all-wcprops new file mode 100644 index 0000000..0eecd9b --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/types/.svn/all-wcprops @@ -0,0 +1,23 @@ +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/collision/types +END +BoundBox.as +K 25 +svn:wc:ra_dav:version-url +V 131 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/collision/types/BoundBox.as +END +RayIntersection.as +K 25 +svn:wc:ra_dav:version-url +V 138 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/collision/types/RayIntersection.as +END +Ray.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/collision/types/Ray.as +END diff --git a/0.0.9.4/src/alternativa/physics/collision/types/.svn/entries b/0.0.9.4/src/alternativa/physics/collision/types/.svn/entries new file mode 100644 index 0000000..6503040 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/types/.svn/entries @@ -0,0 +1,64 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/collision/types +http://svndev.alternativaplatform.com + + + +2009-11-24T08:44:15.941480Z +23758 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +BoundBox.as +file + + + + +2010-10-28T04:32:45.000000Z +d92d4ab3dde698feb674fb9488f59678 +2009-09-09T14:02:48.134744Z +19633 +mike + +RayIntersection.as +file + + + + +2010-10-28T04:32:45.000000Z +88c0836ead77b8eb0841f11159dfb836 +2009-11-24T08:44:15.941480Z +23758 +mike + +Ray.as +file + + + + +2010-10-28T04:32:45.000000Z +c65cc8bfae377f8f0520b5c79c8b3ab9 +2009-11-24T08:44:15.941480Z +23758 +mike + diff --git a/0.0.9.4/src/alternativa/physics/collision/types/.svn/format b/0.0.9.4/src/alternativa/physics/collision/types/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/types/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.4/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base b/0.0.9.4/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base new file mode 100644 index 0000000..b4d5278 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base @@ -0,0 +1,75 @@ +package alternativa.physics.collision.types { + + /** + * Ограничивающий параллелепипед. + */ + public class BoundBox { + + public var minX:Number = 1e308; + public var minY:Number = 1e308; + public var minZ:Number = 1e308; + public var maxX:Number = -1e308; + public var maxY:Number = -1e308; + public var maxZ:Number = -1e308; + + public function setSize(minX:Number, minY:Number, minZ:Number, maxX:Number, maxY:Number, maxZ:Number):void { + this.minX = minX; + this.minY = minY; + this.minZ = minZ; + this.maxX = maxX; + this.maxY = maxY; + this.maxZ = maxZ; + } + + public function addBoundBox(boundBox:BoundBox):void { + minX = (boundBox.minX < minX) ? boundBox.minX : minX; + minY = (boundBox.minY < minY) ? boundBox.minY : minY; + minZ = (boundBox.minZ < minZ) ? boundBox.minZ : minZ; + maxX = (boundBox.maxX > maxX) ? boundBox.maxX : maxX; + maxY = (boundBox.maxY > maxY) ? boundBox.maxY : maxY; + maxZ = (boundBox.maxZ > maxZ) ? boundBox.maxZ : maxZ; + } + + public function addPoint(x:Number, y:Number, z:Number):void { + if (x < minX) minX = x; + if (x > maxX) maxX = x; + if (y < minY) minY = y; + if (y > maxY) maxY = y; + if (z < minZ) minZ = z; + if (z > maxZ) maxZ = z; + } + + public function infinity():void { + minX = 1e308; + minY = 1e308; + minZ = 1e308; + maxX = -1e308; + maxY = -1e308; + maxZ = -1e308; + } + + public function intersects(bb:BoundBox, epsilon:Number):Boolean { + return !(minX > bb.maxX + epsilon || maxX < bb.minX - epsilon || minY > bb.maxY + epsilon || maxY < bb.minY - epsilon || minZ > bb.maxZ + epsilon || maxZ < bb.minZ - epsilon); + } + + public function copyFrom(boundBox:BoundBox):void { + minX = boundBox.minX; + minY = boundBox.minY; + minZ = boundBox.minZ; + maxX = boundBox.maxX; + maxY = boundBox.maxY; + maxZ = boundBox.maxZ; + } + + public function clone():BoundBox { + var clone:BoundBox = new BoundBox(); + clone.copyFrom(this); + return clone; + } + + public function toString():String { + return "BoundBox [" + minX + ", " + minY + ", " + minZ + " : " + maxX + ", " + maxY + ", " + maxZ + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base b/0.0.9.4/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base new file mode 100644 index 0000000..fb2ffbc --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base @@ -0,0 +1,13 @@ +package alternativa.physics.collision.types { + import alternativa.math.Vector3; + + /** + * Структура, описывающая луч и сегмент прямой в пространстве. + */ + public class Ray { + // Начало луча. Начальная точка сегмента на прямой луча. + public var origin:Vector3 = new Vector3(); + // Направляющий вектор луча. Длина вектора задаёт длину сегмента на луче. Конечная точка сегмента вычисляется как origin + dir. + public var dir:Vector3 = new Vector3(); + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base b/0.0.9.4/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base new file mode 100644 index 0000000..21e9290 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base @@ -0,0 +1,25 @@ +package alternativa.physics.collision.types { + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.math.Vector3; + + /** + * Структура описывает точку пересечения луча с физической геометрией. + */ + public class RayIntersection { + // Физический примитив, с которым пересекается луч + public var primitive:CollisionPrimitive; + // Положенеи точки пересечения + public var pos:Vector3 = new Vector3(); + // Нормаль поверхности физического примитива в точке пересечения + public var normal:Vector3 = new Vector3(); + // Время пересечения (|pos - ray.origin|/|ray.dir|) + public var t:Number = 0; + + public function copy(source:RayIntersection):void { + primitive = source.primitive; + pos.vCopy(source.pos); + normal.vCopy(source.normal); + t = source.t; + } + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/collision/types/BoundBox.as b/0.0.9.4/src/alternativa/physics/collision/types/BoundBox.as new file mode 100644 index 0000000..b4d5278 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/types/BoundBox.as @@ -0,0 +1,75 @@ +package alternativa.physics.collision.types { + + /** + * Ограничивающий параллелепипед. + */ + public class BoundBox { + + public var minX:Number = 1e308; + public var minY:Number = 1e308; + public var minZ:Number = 1e308; + public var maxX:Number = -1e308; + public var maxY:Number = -1e308; + public var maxZ:Number = -1e308; + + public function setSize(minX:Number, minY:Number, minZ:Number, maxX:Number, maxY:Number, maxZ:Number):void { + this.minX = minX; + this.minY = minY; + this.minZ = minZ; + this.maxX = maxX; + this.maxY = maxY; + this.maxZ = maxZ; + } + + public function addBoundBox(boundBox:BoundBox):void { + minX = (boundBox.minX < minX) ? boundBox.minX : minX; + minY = (boundBox.minY < minY) ? boundBox.minY : minY; + minZ = (boundBox.minZ < minZ) ? boundBox.minZ : minZ; + maxX = (boundBox.maxX > maxX) ? boundBox.maxX : maxX; + maxY = (boundBox.maxY > maxY) ? boundBox.maxY : maxY; + maxZ = (boundBox.maxZ > maxZ) ? boundBox.maxZ : maxZ; + } + + public function addPoint(x:Number, y:Number, z:Number):void { + if (x < minX) minX = x; + if (x > maxX) maxX = x; + if (y < minY) minY = y; + if (y > maxY) maxY = y; + if (z < minZ) minZ = z; + if (z > maxZ) maxZ = z; + } + + public function infinity():void { + minX = 1e308; + minY = 1e308; + minZ = 1e308; + maxX = -1e308; + maxY = -1e308; + maxZ = -1e308; + } + + public function intersects(bb:BoundBox, epsilon:Number):Boolean { + return !(minX > bb.maxX + epsilon || maxX < bb.minX - epsilon || minY > bb.maxY + epsilon || maxY < bb.minY - epsilon || minZ > bb.maxZ + epsilon || maxZ < bb.minZ - epsilon); + } + + public function copyFrom(boundBox:BoundBox):void { + minX = boundBox.minX; + minY = boundBox.minY; + minZ = boundBox.minZ; + maxX = boundBox.maxX; + maxY = boundBox.maxY; + maxZ = boundBox.maxZ; + } + + public function clone():BoundBox { + var clone:BoundBox = new BoundBox(); + clone.copyFrom(this); + return clone; + } + + public function toString():String { + return "BoundBox [" + minX + ", " + minY + ", " + minZ + " : " + maxX + ", " + maxY + ", " + maxZ + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/collision/types/Ray.as b/0.0.9.4/src/alternativa/physics/collision/types/Ray.as new file mode 100644 index 0000000..fb2ffbc --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/types/Ray.as @@ -0,0 +1,13 @@ +package alternativa.physics.collision.types { + import alternativa.math.Vector3; + + /** + * Структура, описывающая луч и сегмент прямой в пространстве. + */ + public class Ray { + // Начало луча. Начальная точка сегмента на прямой луча. + public var origin:Vector3 = new Vector3(); + // Направляющий вектор луча. Длина вектора задаёт длину сегмента на луче. Конечная точка сегмента вычисляется как origin + dir. + public var dir:Vector3 = new Vector3(); + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/collision/types/RayIntersection.as b/0.0.9.4/src/alternativa/physics/collision/types/RayIntersection.as new file mode 100644 index 0000000..21e9290 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/collision/types/RayIntersection.as @@ -0,0 +1,25 @@ +package alternativa.physics.collision.types { + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.math.Vector3; + + /** + * Структура описывает точку пересечения луча с физической геометрией. + */ + public class RayIntersection { + // Физический примитив, с которым пересекается луч + public var primitive:CollisionPrimitive; + // Положенеи точки пересечения + public var pos:Vector3 = new Vector3(); + // Нормаль поверхности физического примитива в точке пересечения + public var normal:Vector3 = new Vector3(); + // Время пересечения (|pos - ray.origin|/|ray.dir|) + public var t:Number = 0; + + public function copy(source:RayIntersection):void { + primitive = source.primitive; + pos.vCopy(source.pos); + normal.vCopy(source.normal); + t = source.t; + } + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/constraints/.svn/all-wcprops b/0.0.9.4/src/alternativa/physics/constraints/.svn/all-wcprops new file mode 100644 index 0000000..12a6299 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/constraints/.svn/all-wcprops @@ -0,0 +1,17 @@ +K 25 +svn:wc:ra_dav:version-url +V 115 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/constraints +END +Constraint.as +K 25 +svn:wc:ra_dav:version-url +V 129 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/constraints/Constraint.as +END +MaxDistanceConstraint.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/constraints/MaxDistanceConstraint.as +END diff --git a/0.0.9.4/src/alternativa/physics/constraints/.svn/entries b/0.0.9.4/src/alternativa/physics/constraints/.svn/entries new file mode 100644 index 0000000..2d31d54 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/constraints/.svn/entries @@ -0,0 +1,52 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/constraints +http://svndev.alternativaplatform.com + + + +2010-01-20T17:53:18.555251Z +26849 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Constraint.as +file + + + + +2010-10-28T04:32:45.000000Z +02bdbd40eda515549956d6393551a63b +2010-01-20T17:53:18.555251Z +26849 +mike + +MaxDistanceConstraint.as +file + + + + +2010-10-28T04:32:45.000000Z +dbf792986b9dd7e2a1c720cf4a578f42 +2009-11-24T08:44:15.941480Z +23758 +mike + diff --git a/0.0.9.4/src/alternativa/physics/constraints/.svn/format b/0.0.9.4/src/alternativa/physics/constraints/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/constraints/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.4/src/alternativa/physics/constraints/.svn/text-base/Constraint.as.svn-base b/0.0.9.4/src/alternativa/physics/constraints/.svn/text-base/Constraint.as.svn-base new file mode 100644 index 0000000..8607442 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/constraints/.svn/text-base/Constraint.as.svn-base @@ -0,0 +1,35 @@ +package alternativa.physics.constraints { + + import alternativa.physics.PhysicsScene; + import alternativa.physics.altphysics; + + use namespace altphysics; + + /** + * + */ + public class Constraint { + + altphysics var satisfied:Boolean; + altphysics var world:PhysicsScene; + + /** + * + */ + public function Constraint() { + } + + /** + * @param dt + */ + public function preProcess(dt:Number):void { + } + + /** + * @param dt + */ + public function apply(dt:Number):void { + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base b/0.0.9.4/src/alternativa/physics/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base new file mode 100644 index 0000000..d29f518 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base @@ -0,0 +1,183 @@ +package alternativa.physics.constraints { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * + */ + public class MaxDistanceConstraint extends Constraint { + // Первое тело ограничения + altphysics var body1:Body; + // Второе тело ограничения. Может быть null. + altphysics var body2:Body; + // Первая точка крепления в системе координат первого тела + altphysics var r1:Vector3; + // Вторая точка привязки в системе координат второго тела. В случае отсутствия второго тела, координаты задаются в мировой системе. + altphysics var r2:Vector3; + // Максимально допустимое расстояние между точками крепления + altphysics var maxDistance:Number; + // Радиус-вектор первой точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr1:Vector3 = new Vector3(); + // Радиус-вектор второй точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr2:Vector3 = new Vector3(); + // Минимальная скорость сближения для удовлетворения ограничения + private var minClosingVel:Number; + // Изменение относительной скорости на единицу импульса + private var velByUnitImpulseN:Number; + // Направление корректирующего импульса + private var impulseDirection:Vector3 = new Vector3(); + + // Вспомогательные переменные +// private static var _v:Vector3 = new Vector3(); +// private static var _v1:Vector3 = new Vector3(); +// private static var _v2:Vector3 = new Vector3(); + + /** + * @param body1 + * @param body2 + * @param r1 + * @param r2 + * @param maxDistance + */ + public function MaxDistanceConstraint(body1:Body, body2:Body, r1:Vector3, r2:Vector3, maxDistance:Number) { + super(); + this.body1 = body1; + this.body2 = body2; + this.r1 = r1.vClone(); + this.r2 = r2.vClone(); + this.maxDistance = maxDistance; + } + + /** + * @param dt + */ + override public function preProcess(dt:Number):void { + // Вычислим расстояние между точками + var m:Matrix3 = body1.baseMatrix; + wr1.x = m.a*r1.x + m.b*r1.y + m.c*r1.z; + wr1.y = m.e*r1.x + m.f*r1.y + m.g*r1.z; + wr1.z = m.i*r1.x + m.j*r1.y + m.k*r1.z; + if (body2 != null) { + m = body2.baseMatrix; + wr2.x = m.a*r2.x + m.b*r2.y + m.c*r2.z; + wr2.y = m.e*r2.x + m.f*r2.y + m.g*r2.z; + wr2.z = m.i*r2.x + m.j*r2.y + m.k*r2.z; + } else { + wr2.x = r2.x; + wr2.y = r2.y; + wr2.z = r2.z; + } + var p1:Vector3 = body1.state.pos; + impulseDirection.x = wr2.x - wr1.x - p1.x; + impulseDirection.y = wr2.y - wr1.y - p1.y; + impulseDirection.z = wr2.z - wr1.z - p1.z; + if (body2 != null) { + var p2:Vector3 = body2.state.pos; + impulseDirection.x += p2.x; + impulseDirection.y += p2.y; + impulseDirection.z += p2.z; + } + + var len:Number = Math.sqrt(impulseDirection.x*impulseDirection.x + impulseDirection.y*impulseDirection.y + impulseDirection.z*impulseDirection.z); + var delta:Number = len - maxDistance; + if (delta > 0) { + satisfied = false; + if (len < 0.001) { + impulseDirection.x = 1; + } else { + len = 1/len; + impulseDirection.x *= len; + impulseDirection.y *= len; + impulseDirection.z *= len; + } + minClosingVel = delta/(world.penResolutionSteps*dt); + if (minClosingVel > world.maxPenResolutionSpeed) { + minClosingVel = world.maxPenResolutionSpeed; + } + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + var x:Number; + var y:Number; + var z:Number; + var vx:Number; + var vy:Number; + var vz:Number; + velByUnitImpulseN = 0; + if (body1.movable) { +// velByUnitImpulseN += body1.invMass + _v.vCross2(wr1, impulseDirection).vTransformBy3(body1.invInertiaWorld).vCross(wr1).vDot(impulseDirection); + vx = wr1.y*impulseDirection.z - wr1.z*impulseDirection.y; + vy = wr1.z*impulseDirection.x - wr1.x*impulseDirection.z; + vz = wr1.x*impulseDirection.y - wr1.y*impulseDirection.x; + + m = body1.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr1.z - z*wr1.y; + vy = z*wr1.x - x*wr1.z; + vz = x*wr1.y - y*wr1.x; + velByUnitImpulseN += body1.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + if (body2 != null && body2.movable) { +// velByUnitImpulseN += body2.invMass + _v.vCross2(wr2, impulseDirection).vTransformBy3(body2.invInertiaWorld).vCross(wr2).vDot(impulseDirection); + vx = wr2.y*impulseDirection.z - wr2.z*impulseDirection.y; + vy = wr2.z*impulseDirection.x - wr2.x*impulseDirection.z; + vz = wr2.x*impulseDirection.y - wr2.y*impulseDirection.x; + + m = body2.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr2.z - z*wr2.y; + vy = z*wr2.x - x*wr2.z; + vz = x*wr2.y - y*wr2.x; + velByUnitImpulseN += body2.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + } else { + satisfied = true; + } + } + + /** + * @param dt + */ + override public function apply(dt:Number):void { + if (satisfied) return; + // Расчитываем проекцию скорости относительной скорости + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 + var vel:Vector3 = body1.state.velocity; + var rot:Vector3 = body1.state.rotation; +// v1Cross2(state.rotation, wr1); + var vx:Number = vel.x + rot.y*wr1.z - rot.z*wr1.y; + var vy:Number = vel.y + rot.z*wr1.x - rot.x*wr1.z; + var vz:Number = vel.z + rot.x*wr1.y - rot.y*wr1.x; + // V2 = V2_c + w2%r2 + if (body2 != null) { + vel = body2.state.velocity; + rot = body2.state.rotation; +// _v2.vCross2(state.rotation, wr2); + vx -= vel.x + rot.y*wr2.z - rot.z*wr2.y; + vy -= vel.y + rot.z*wr2.x - rot.x*wr2.z; + vz -= vel.z + rot.x*wr2.y - rot.y*wr2.x; + } + var closingVel:Number = vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + if (closingVel > minClosingVel) return; + var impulse:Number = (minClosingVel - closingVel)/velByUnitImpulseN; + if (body1.movable) { + body1.applyRelPosWorldImpulse(wr1, impulseDirection, impulse); + } + if (body2 != null && body2.movable) { + body2.applyRelPosWorldImpulse(wr2, impulseDirection, -impulse); + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/constraints/Constraint.as b/0.0.9.4/src/alternativa/physics/constraints/Constraint.as new file mode 100644 index 0000000..8607442 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/constraints/Constraint.as @@ -0,0 +1,35 @@ +package alternativa.physics.constraints { + + import alternativa.physics.PhysicsScene; + import alternativa.physics.altphysics; + + use namespace altphysics; + + /** + * + */ + public class Constraint { + + altphysics var satisfied:Boolean; + altphysics var world:PhysicsScene; + + /** + * + */ + public function Constraint() { + } + + /** + * @param dt + */ + public function preProcess(dt:Number):void { + } + + /** + * @param dt + */ + public function apply(dt:Number):void { + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/constraints/MaxDistanceConstraint.as b/0.0.9.4/src/alternativa/physics/constraints/MaxDistanceConstraint.as new file mode 100644 index 0000000..d29f518 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/constraints/MaxDistanceConstraint.as @@ -0,0 +1,183 @@ +package alternativa.physics.constraints { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * + */ + public class MaxDistanceConstraint extends Constraint { + // Первое тело ограничения + altphysics var body1:Body; + // Второе тело ограничения. Может быть null. + altphysics var body2:Body; + // Первая точка крепления в системе координат первого тела + altphysics var r1:Vector3; + // Вторая точка привязки в системе координат второго тела. В случае отсутствия второго тела, координаты задаются в мировой системе. + altphysics var r2:Vector3; + // Максимально допустимое расстояние между точками крепления + altphysics var maxDistance:Number; + // Радиус-вектор первой точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr1:Vector3 = new Vector3(); + // Радиус-вектор второй точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr2:Vector3 = new Vector3(); + // Минимальная скорость сближения для удовлетворения ограничения + private var minClosingVel:Number; + // Изменение относительной скорости на единицу импульса + private var velByUnitImpulseN:Number; + // Направление корректирующего импульса + private var impulseDirection:Vector3 = new Vector3(); + + // Вспомогательные переменные +// private static var _v:Vector3 = new Vector3(); +// private static var _v1:Vector3 = new Vector3(); +// private static var _v2:Vector3 = new Vector3(); + + /** + * @param body1 + * @param body2 + * @param r1 + * @param r2 + * @param maxDistance + */ + public function MaxDistanceConstraint(body1:Body, body2:Body, r1:Vector3, r2:Vector3, maxDistance:Number) { + super(); + this.body1 = body1; + this.body2 = body2; + this.r1 = r1.vClone(); + this.r2 = r2.vClone(); + this.maxDistance = maxDistance; + } + + /** + * @param dt + */ + override public function preProcess(dt:Number):void { + // Вычислим расстояние между точками + var m:Matrix3 = body1.baseMatrix; + wr1.x = m.a*r1.x + m.b*r1.y + m.c*r1.z; + wr1.y = m.e*r1.x + m.f*r1.y + m.g*r1.z; + wr1.z = m.i*r1.x + m.j*r1.y + m.k*r1.z; + if (body2 != null) { + m = body2.baseMatrix; + wr2.x = m.a*r2.x + m.b*r2.y + m.c*r2.z; + wr2.y = m.e*r2.x + m.f*r2.y + m.g*r2.z; + wr2.z = m.i*r2.x + m.j*r2.y + m.k*r2.z; + } else { + wr2.x = r2.x; + wr2.y = r2.y; + wr2.z = r2.z; + } + var p1:Vector3 = body1.state.pos; + impulseDirection.x = wr2.x - wr1.x - p1.x; + impulseDirection.y = wr2.y - wr1.y - p1.y; + impulseDirection.z = wr2.z - wr1.z - p1.z; + if (body2 != null) { + var p2:Vector3 = body2.state.pos; + impulseDirection.x += p2.x; + impulseDirection.y += p2.y; + impulseDirection.z += p2.z; + } + + var len:Number = Math.sqrt(impulseDirection.x*impulseDirection.x + impulseDirection.y*impulseDirection.y + impulseDirection.z*impulseDirection.z); + var delta:Number = len - maxDistance; + if (delta > 0) { + satisfied = false; + if (len < 0.001) { + impulseDirection.x = 1; + } else { + len = 1/len; + impulseDirection.x *= len; + impulseDirection.y *= len; + impulseDirection.z *= len; + } + minClosingVel = delta/(world.penResolutionSteps*dt); + if (minClosingVel > world.maxPenResolutionSpeed) { + minClosingVel = world.maxPenResolutionSpeed; + } + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + var x:Number; + var y:Number; + var z:Number; + var vx:Number; + var vy:Number; + var vz:Number; + velByUnitImpulseN = 0; + if (body1.movable) { +// velByUnitImpulseN += body1.invMass + _v.vCross2(wr1, impulseDirection).vTransformBy3(body1.invInertiaWorld).vCross(wr1).vDot(impulseDirection); + vx = wr1.y*impulseDirection.z - wr1.z*impulseDirection.y; + vy = wr1.z*impulseDirection.x - wr1.x*impulseDirection.z; + vz = wr1.x*impulseDirection.y - wr1.y*impulseDirection.x; + + m = body1.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr1.z - z*wr1.y; + vy = z*wr1.x - x*wr1.z; + vz = x*wr1.y - y*wr1.x; + velByUnitImpulseN += body1.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + if (body2 != null && body2.movable) { +// velByUnitImpulseN += body2.invMass + _v.vCross2(wr2, impulseDirection).vTransformBy3(body2.invInertiaWorld).vCross(wr2).vDot(impulseDirection); + vx = wr2.y*impulseDirection.z - wr2.z*impulseDirection.y; + vy = wr2.z*impulseDirection.x - wr2.x*impulseDirection.z; + vz = wr2.x*impulseDirection.y - wr2.y*impulseDirection.x; + + m = body2.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr2.z - z*wr2.y; + vy = z*wr2.x - x*wr2.z; + vz = x*wr2.y - y*wr2.x; + velByUnitImpulseN += body2.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + } else { + satisfied = true; + } + } + + /** + * @param dt + */ + override public function apply(dt:Number):void { + if (satisfied) return; + // Расчитываем проекцию скорости относительной скорости + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 + var vel:Vector3 = body1.state.velocity; + var rot:Vector3 = body1.state.rotation; +// v1Cross2(state.rotation, wr1); + var vx:Number = vel.x + rot.y*wr1.z - rot.z*wr1.y; + var vy:Number = vel.y + rot.z*wr1.x - rot.x*wr1.z; + var vz:Number = vel.z + rot.x*wr1.y - rot.y*wr1.x; + // V2 = V2_c + w2%r2 + if (body2 != null) { + vel = body2.state.velocity; + rot = body2.state.rotation; +// _v2.vCross2(state.rotation, wr2); + vx -= vel.x + rot.y*wr2.z - rot.z*wr2.y; + vy -= vel.y + rot.z*wr2.x - rot.x*wr2.z; + vz -= vel.z + rot.x*wr2.y - rot.y*wr2.x; + } + var closingVel:Number = vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + if (closingVel > minClosingVel) return; + var impulse:Number = (minClosingVel - closingVel)/velByUnitImpulseN; + if (body1.movable) { + body1.applyRelPosWorldImpulse(wr1, impulseDirection, impulse); + } + if (body2 != null && body2.movable) { + body2.applyRelPosWorldImpulse(wr2, impulseDirection, -impulse); + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/primitives/.svn/all-wcprops b/0.0.9.4/src/alternativa/physics/primitives/.svn/all-wcprops new file mode 100644 index 0000000..0a541f6 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/primitives/.svn/all-wcprops @@ -0,0 +1,35 @@ +K 25 +svn:wc:ra_dav:version-url +V 114 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/primitives +END +RigidPlane.as +K 25 +svn:wc:ra_dav:version-url +V 128 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/primitives/RigidPlane.as +END +RigidSphere.as +K 25 +svn:wc:ra_dav:version-url +V 129 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/primitives/RigidSphere.as +END +RigidBox.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/primitives/RigidBox.as +END +RigidCylinder.as +K 25 +svn:wc:ra_dav:version-url +V 131 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/primitives/RigidCylinder.as +END +RigidRect.as +K 25 +svn:wc:ra_dav:version-url +V 127 +/!svn/ver/39557/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/primitives/RigidRect.as +END diff --git a/0.0.9.4/src/alternativa/physics/primitives/.svn/entries b/0.0.9.4/src/alternativa/physics/primitives/.svn/entries new file mode 100644 index 0000000..f8afa3e --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/primitives/.svn/entries @@ -0,0 +1,88 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.4/src/alternativa/physics/primitives +http://svndev.alternativaplatform.com + + + +2009-11-24T08:44:15.941480Z +23758 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +RigidPlane.as +file + + + + +2010-10-28T04:32:45.000000Z +8faf607cf544d7388852668f5cd5672f +2009-11-24T08:44:15.941480Z +23758 +mike + +RigidSphere.as +file + + + + +2010-10-28T04:32:45.000000Z +774b0c23b084bfffb3381eb40e616deb +2009-11-24T08:44:15.941480Z +23758 +mike + +RigidBox.as +file + + + + +2010-10-28T04:32:45.000000Z +7b93f349a2818f22b76d9590cb05f84a +2009-11-24T08:44:15.941480Z +23758 +mike + +RigidCylinder.as +file + + + + +2010-10-28T04:32:45.000000Z +e9ce0eeb457428727cc10d2e53a0f164 +2009-11-24T08:44:15.941480Z +23758 +mike + +RigidRect.as +file + + + + +2010-10-28T04:32:45.000000Z +2eb6d0ebde8c2b46c4b0f810b1d64e1e +2009-11-24T08:44:15.941480Z +23758 +mike + diff --git a/0.0.9.4/src/alternativa/physics/primitives/.svn/format b/0.0.9.4/src/alternativa/physics/primitives/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/primitives/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.4/src/alternativa/physics/primitives/.svn/text-base/RigidBox.as.svn-base b/0.0.9.4/src/alternativa/physics/primitives/.svn/text-base/RigidBox.as.svn-base new file mode 100644 index 0000000..24e9b9f --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/primitives/.svn/text-base/RigidBox.as.svn-base @@ -0,0 +1,56 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * + */ + public class RigidBox extends Body { + + /** + * + * @param halfSize + * @param mass + */ + public function RigidBox(halfSize:Vector3, mass:Number) { + super(0, Matrix3.ZERO); + addCollisionPrimitive(new CollisionBox(halfSize, 1)); + setParams(halfSize, mass); + } + + /** + * @param halfSize + * @param mass + */ + public function setParams(halfSize:Vector3, mass:Number):void { + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + invInertia.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + invInertia.a = 3*invMass/(yy + zz); + invInertia.f = 3*invMass/(zz + xx); + invInertia.k = 3*invMass/(xx + yy); + } + var prim:CollisionBox = CollisionBox(collisionPrimitives.head.primitive); + prim.hs.vCopy(halfSize); + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/primitives/.svn/text-base/RigidCylinder.as.svn-base b/0.0.9.4/src/alternativa/physics/primitives/.svn/text-base/RigidCylinder.as.svn-base new file mode 100644 index 0000000..4ff65dd --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/primitives/.svn/text-base/RigidCylinder.as.svn-base @@ -0,0 +1,32 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.math.Matrix3; + + use namespace altphysics; + + public class RigidCylinder extends Body { + + altphysics var r:Number; + altphysics var h:Number; + + public function RigidCylinder(mass:Number, radius:Number, height:Number) { + super(0, Matrix3.ZERO); + setParams(mass, radius, height); + } + + public function setParams(mass:Number, radius:Number, height:Number):void { + r = radius; + h = height; + invInertiaWorld.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + invInertia.a = invInertia.f = mass*(h*h/12 + r*r/4); + invInertia.k = 0.5*mass*r*r; + invInertia.invert(); + } + } + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/primitives/.svn/text-base/RigidPlane.as.svn-base b/0.0.9.4/src/alternativa/physics/primitives/.svn/text-base/RigidPlane.as.svn-base new file mode 100644 index 0000000..d9ebf9b --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/primitives/.svn/text-base/RigidPlane.as.svn-base @@ -0,0 +1,23 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + public class RigidPlane extends Body { + + public var normal:Vector3 = new Vector3(); + public var offset:Number; + + public function RigidPlane() { + super(0, Matrix3.ZERO); + this.normal.vCopy(Vector3.Z_AXIS); + this.offset = 0; + movable = false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/primitives/.svn/text-base/RigidRect.as.svn-base b/0.0.9.4/src/alternativa/physics/primitives/.svn/text-base/RigidRect.as.svn-base new file mode 100644 index 0000000..4591931 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/primitives/.svn/text-base/RigidRect.as.svn-base @@ -0,0 +1,20 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + public class RigidRect extends Body { + + public function RigidRect(halfWidth:Number, halfLength:Number) { + super(0, Matrix3.ZERO); + movable = false; + addCollisionPrimitive(new CollisionRect(new Vector3(halfWidth, halfLength, 0), 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/primitives/.svn/text-base/RigidSphere.as.svn-base b/0.0.9.4/src/alternativa/physics/primitives/.svn/text-base/RigidSphere.as.svn-base new file mode 100644 index 0000000..8ad180c --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/primitives/.svn/text-base/RigidSphere.as.svn-base @@ -0,0 +1,29 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.math.Matrix3; + + use namespace altphysics; + + public class RigidSphere extends Body { + + public function RigidSphere(radius:Number, mass:Number) { + var m:Matrix3; + if (mass == Infinity) { + mass = 0; + m = Matrix3.ZERO; + } else { + mass = 1/mass; + var r2:Number = radius*radius; + m = new Matrix3(); + m.a = m.f = m.k = 2.5*mass/r2; + } + super(mass, m); + if (mass == 0) movable = false; + addCollisionPrimitive(new CollisionSphere(radius, 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/primitives/RigidBox.as b/0.0.9.4/src/alternativa/physics/primitives/RigidBox.as new file mode 100644 index 0000000..24e9b9f --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/primitives/RigidBox.as @@ -0,0 +1,56 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * + */ + public class RigidBox extends Body { + + /** + * + * @param halfSize + * @param mass + */ + public function RigidBox(halfSize:Vector3, mass:Number) { + super(0, Matrix3.ZERO); + addCollisionPrimitive(new CollisionBox(halfSize, 1)); + setParams(halfSize, mass); + } + + /** + * @param halfSize + * @param mass + */ + public function setParams(halfSize:Vector3, mass:Number):void { + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + invInertia.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + invInertia.a = 3*invMass/(yy + zz); + invInertia.f = 3*invMass/(zz + xx); + invInertia.k = 3*invMass/(xx + yy); + } + var prim:CollisionBox = CollisionBox(collisionPrimitives.head.primitive); + prim.hs.vCopy(halfSize); + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/primitives/RigidCylinder.as b/0.0.9.4/src/alternativa/physics/primitives/RigidCylinder.as new file mode 100644 index 0000000..4ff65dd --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/primitives/RigidCylinder.as @@ -0,0 +1,32 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.math.Matrix3; + + use namespace altphysics; + + public class RigidCylinder extends Body { + + altphysics var r:Number; + altphysics var h:Number; + + public function RigidCylinder(mass:Number, radius:Number, height:Number) { + super(0, Matrix3.ZERO); + setParams(mass, radius, height); + } + + public function setParams(mass:Number, radius:Number, height:Number):void { + r = radius; + h = height; + invInertiaWorld.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + invInertia.a = invInertia.f = mass*(h*h/12 + r*r/4); + invInertia.k = 0.5*mass*r*r; + invInertia.invert(); + } + } + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/primitives/RigidPlane.as b/0.0.9.4/src/alternativa/physics/primitives/RigidPlane.as new file mode 100644 index 0000000..d9ebf9b --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/primitives/RigidPlane.as @@ -0,0 +1,23 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + public class RigidPlane extends Body { + + public var normal:Vector3 = new Vector3(); + public var offset:Number; + + public function RigidPlane() { + super(0, Matrix3.ZERO); + this.normal.vCopy(Vector3.Z_AXIS); + this.offset = 0; + movable = false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/primitives/RigidRect.as b/0.0.9.4/src/alternativa/physics/primitives/RigidRect.as new file mode 100644 index 0000000..4591931 --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/primitives/RigidRect.as @@ -0,0 +1,20 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + public class RigidRect extends Body { + + public function RigidRect(halfWidth:Number, halfLength:Number) { + super(0, Matrix3.ZERO); + movable = false; + addCollisionPrimitive(new CollisionRect(new Vector3(halfWidth, halfLength, 0), 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.9.4/src/alternativa/physics/primitives/RigidSphere.as b/0.0.9.4/src/alternativa/physics/primitives/RigidSphere.as new file mode 100644 index 0000000..8ad180c --- /dev/null +++ b/0.0.9.4/src/alternativa/physics/primitives/RigidSphere.as @@ -0,0 +1,29 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.math.Matrix3; + + use namespace altphysics; + + public class RigidSphere extends Body { + + public function RigidSphere(radius:Number, mass:Number) { + var m:Matrix3; + if (mass == Infinity) { + mass = 0; + m = Matrix3.ZERO; + } else { + mass = 1/mass; + var r2:Number = radius*radius; + m = new Matrix3(); + m.a = m.f = m.k = 2.5*mass/r2; + } + super(mass, m); + if (mass == 0) movable = false; + addCollisionPrimitive(new CollisionSphere(radius, 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/.actionScriptProperties b/0.0.9.5/.actionScriptProperties new file mode 100644 index 0000000..f73adc8 --- /dev/null +++ b/0.0.9.5/.actionScriptProperties @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/0.0.9.5/.flexLibProperties b/0.0.9.5/.flexLibProperties new file mode 100644 index 0000000..0b65396 --- /dev/null +++ b/0.0.9.5/.flexLibProperties @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/0.0.9.5/.project b/0.0.9.5/.project new file mode 100644 index 0000000..1f1425f --- /dev/null +++ b/0.0.9.5/.project @@ -0,0 +1,19 @@ + + + AlternativaPhysics + + + + + + com.adobe.flexbuilder.project.flexbuilder + + + + + + org.maven.ide.eclipse.maven2Nature + com.adobe.flexbuilder.project.flexlibnature + com.adobe.flexbuilder.project.actionscriptnature + + diff --git a/0.0.9.5/.settings/.svn/all-wcprops b/0.0.9.5/.settings/.svn/all-wcprops new file mode 100644 index 0000000..8abcd32 --- /dev/null +++ b/0.0.9.5/.settings/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 89 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/.settings +END +org.eclipse.core.resources.prefs +K 25 +svn:wc:ra_dav:version-url +V 122 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/.settings/org.eclipse.core.resources.prefs +END diff --git a/0.0.9.5/.settings/.svn/entries b/0.0.9.5/.settings/.svn/entries new file mode 100644 index 0000000..b1b33b0 --- /dev/null +++ b/0.0.9.5/.settings/.svn/entries @@ -0,0 +1,40 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/.settings +http://svndev.alternativaplatform.com + + + +2009-04-01T12:25:29.638016Z +10301 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +org.eclipse.core.resources.prefs +file + + + + +2010-10-28T04:32:45.000000Z +4f70b476f3e5075e399505021df2f89b +2009-04-01T12:25:29.638016Z +10301 +mike + diff --git a/0.0.9.5/.settings/.svn/format b/0.0.9.5/.settings/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.5/.settings/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.5/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base b/0.0.9.5/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base new file mode 100644 index 0000000..b3d0d49 --- /dev/null +++ b/0.0.9.5/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base @@ -0,0 +1,3 @@ +#Fri Mar 20 00:19:33 YEKT 2009 +eclipse.preferences.version=1 +encoding/=UTF-8 diff --git a/0.0.9.5/.settings/org.eclipse.core.resources.prefs b/0.0.9.5/.settings/org.eclipse.core.resources.prefs new file mode 100644 index 0000000..b3d0d49 --- /dev/null +++ b/0.0.9.5/.settings/org.eclipse.core.resources.prefs @@ -0,0 +1,3 @@ +#Fri Mar 20 00:19:33 YEKT 2009 +eclipse.preferences.version=1 +encoding/=UTF-8 diff --git a/0.0.9.5/.svn/all-wcprops b/0.0.9.5/.svn/all-wcprops new file mode 100644 index 0000000..7750be6 --- /dev/null +++ b/0.0.9.5/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 79 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5 +END +.flexLibProperties +K 25 +svn:wc:ra_dav:version-url +V 98 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/.flexLibProperties +END +.project +K 25 +svn:wc:ra_dav:version-url +V 88 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/.project +END +pom.xml +K 25 +svn:wc:ra_dav:version-url +V 87 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/pom.xml +END +.actionScriptProperties +K 25 +svn:wc:ra_dav:version-url +V 103 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/.actionScriptProperties +END diff --git a/0.0.9.5/.svn/dir-prop-base b/0.0.9.5/.svn/dir-prop-base new file mode 100644 index 0000000..c31ca28 --- /dev/null +++ b/0.0.9.5/.svn/dir-prop-base @@ -0,0 +1,7 @@ +K 13 +svn:mergeinfo +V 278 +/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized:19806-19860 +/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.7.0.LinkedList:20360-22217,22226-22252 +/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D:13892-14098 +END diff --git a/0.0.9.5/.svn/entries b/0.0.9.5/.svn/entries new file mode 100644 index 0000000..e59c41c --- /dev/null +++ b/0.0.9.5/.svn/entries @@ -0,0 +1,85 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5 +http://svndev.alternativaplatform.com + + + +2010-08-20T07:58:00.614935Z +39732 +mike +has-props + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +META-INF +dir + +.flexLibProperties +file + + + + +2010-10-28T04:32:45.000000Z +b170dccb732dcb12462ac421cdca9723 +2010-02-12T12:34:25.192477Z +28424 +mike + +.project +file + + + + +2010-10-28T04:32:45.000000Z +1e90cc68b52b93173b2b5de88eb5c3a3 +2009-04-20T17:15:55.260110Z +11541 +mike + +src +dir + +pom.xml +file + + + + +2010-10-28T04:32:45.000000Z +93ef7a401e390d4cace6f0b1bc1aa381 +2010-08-20T07:58:00.614935Z +39732 +mike + +.actionScriptProperties +file + + + + +2010-10-28T04:32:45.000000Z +b8656f882fff2baedd57a92db86fcf8c +2010-01-14T07:05:07.823716Z +26358 +mike + +.settings +dir + diff --git a/0.0.9.5/.svn/format b/0.0.9.5/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.5/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.5/.svn/text-base/.actionScriptProperties.svn-base b/0.0.9.5/.svn/text-base/.actionScriptProperties.svn-base new file mode 100644 index 0000000..f73adc8 --- /dev/null +++ b/0.0.9.5/.svn/text-base/.actionScriptProperties.svn-base @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/0.0.9.5/.svn/text-base/.flexLibProperties.svn-base b/0.0.9.5/.svn/text-base/.flexLibProperties.svn-base new file mode 100644 index 0000000..0b65396 --- /dev/null +++ b/0.0.9.5/.svn/text-base/.flexLibProperties.svn-base @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/0.0.9.5/.svn/text-base/.project.svn-base b/0.0.9.5/.svn/text-base/.project.svn-base new file mode 100644 index 0000000..1f1425f --- /dev/null +++ b/0.0.9.5/.svn/text-base/.project.svn-base @@ -0,0 +1,19 @@ + + + AlternativaPhysics + + + + + + com.adobe.flexbuilder.project.flexbuilder + + + + + + org.maven.ide.eclipse.maven2Nature + com.adobe.flexbuilder.project.flexlibnature + com.adobe.flexbuilder.project.actionscriptnature + + diff --git a/0.0.9.5/.svn/text-base/pom.xml.svn-base b/0.0.9.5/.svn/text-base/pom.xml.svn-base new file mode 100644 index 0000000..6b1afb7 --- /dev/null +++ b/0.0.9.5/.svn/text-base/pom.xml.svn-base @@ -0,0 +1,17 @@ + + 4.0.0 + platform.clients.fp10.libraries + AlternativaPhysics + swc + 0.0.9.5 + + platform.tools.maven + FlashBasePom + 2.0.3.0 + + + scm:svn:http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5 + + + + diff --git a/0.0.9.5/META-INF/.svn/all-wcprops b/0.0.9.5/META-INF/.svn/all-wcprops new file mode 100644 index 0000000..49a1514 --- /dev/null +++ b/0.0.9.5/META-INF/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 88 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/META-INF +END +MANIFEST.MF +K 25 +svn:wc:ra_dav:version-url +V 100 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/META-INF/MANIFEST.MF +END diff --git a/0.0.9.5/META-INF/.svn/entries b/0.0.9.5/META-INF/.svn/entries new file mode 100644 index 0000000..1d7ee20 --- /dev/null +++ b/0.0.9.5/META-INF/.svn/entries @@ -0,0 +1,40 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/META-INF +http://svndev.alternativaplatform.com + + + +2010-03-06T18:33:59.318892Z +29409 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +MANIFEST.MF +file + + + + +2010-10-28T04:32:45.000000Z +7b64dceb50a6905850ff00a0bfbe77eb +2009-04-20T17:15:55.260110Z +11541 +mike + diff --git a/0.0.9.5/META-INF/.svn/format b/0.0.9.5/META-INF/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.5/META-INF/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.5/META-INF/.svn/text-base/MANIFEST.MF.svn-base b/0.0.9.5/META-INF/.svn/text-base/MANIFEST.MF.svn-base new file mode 100644 index 0000000..5066a55 --- /dev/null +++ b/0.0.9.5/META-INF/.svn/text-base/MANIFEST.MF.svn-base @@ -0,0 +1 @@ +Bundle-Name: platform.clients.fp10.libraries.AlternativaPhysics diff --git a/0.0.9.5/META-INF/MANIFEST.MF b/0.0.9.5/META-INF/MANIFEST.MF new file mode 100644 index 0000000..5066a55 --- /dev/null +++ b/0.0.9.5/META-INF/MANIFEST.MF @@ -0,0 +1 @@ +Bundle-Name: platform.clients.fp10.libraries.AlternativaPhysics diff --git a/0.0.9.5/pom.xml b/0.0.9.5/pom.xml new file mode 100644 index 0000000..6b1afb7 --- /dev/null +++ b/0.0.9.5/pom.xml @@ -0,0 +1,17 @@ + + 4.0.0 + platform.clients.fp10.libraries + AlternativaPhysics + swc + 0.0.9.5 + + platform.tools.maven + FlashBasePom + 2.0.3.0 + + + scm:svn:http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5 + + + + diff --git a/0.0.9.5/src/.svn/all-wcprops b/0.0.9.5/src/.svn/all-wcprops new file mode 100644 index 0000000..98754dd --- /dev/null +++ b/0.0.9.5/src/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 83 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src +END diff --git a/0.0.9.5/src/.svn/entries b/0.0.9.5/src/.svn/entries new file mode 100644 index 0000000..ba45e9b --- /dev/null +++ b/0.0.9.5/src/.svn/entries @@ -0,0 +1,31 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src +http://svndev.alternativaplatform.com + + + +2010-08-20T07:58:00.614935Z +39732 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +alternativa +dir + diff --git a/0.0.9.5/src/.svn/format b/0.0.9.5/src/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.5/src/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.5/src/alternativa/.svn/all-wcprops b/0.0.9.5/src/alternativa/.svn/all-wcprops new file mode 100644 index 0000000..dab3bc8 --- /dev/null +++ b/0.0.9.5/src/alternativa/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 95 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa +END diff --git a/0.0.9.5/src/alternativa/.svn/entries b/0.0.9.5/src/alternativa/.svn/entries new file mode 100644 index 0000000..6c59359 --- /dev/null +++ b/0.0.9.5/src/alternativa/.svn/entries @@ -0,0 +1,34 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa +http://svndev.alternativaplatform.com + + + +2010-08-20T07:58:00.614935Z +39732 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +physics +dir + +math +dir + diff --git a/0.0.9.5/src/alternativa/.svn/format b/0.0.9.5/src/alternativa/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.5/src/alternativa/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.5/src/alternativa/math/.svn/all-wcprops b/0.0.9.5/src/alternativa/math/.svn/all-wcprops new file mode 100644 index 0000000..7b06e5f --- /dev/null +++ b/0.0.9.5/src/alternativa/math/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 100 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/math +END +Quaternion.as +K 25 +svn:wc:ra_dav:version-url +V 114 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/math/Quaternion.as +END +Vector3.as +K 25 +svn:wc:ra_dav:version-url +V 111 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/math/Vector3.as +END +Matrix3.as +K 25 +svn:wc:ra_dav:version-url +V 111 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/math/Matrix3.as +END +Matrix4.as +K 25 +svn:wc:ra_dav:version-url +V 111 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/math/Matrix4.as +END diff --git a/0.0.9.5/src/alternativa/math/.svn/entries b/0.0.9.5/src/alternativa/math/.svn/entries new file mode 100644 index 0000000..a07e8d7 --- /dev/null +++ b/0.0.9.5/src/alternativa/math/.svn/entries @@ -0,0 +1,76 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/math +http://svndev.alternativaplatform.com + + + +2010-08-20T07:58:00.614935Z +39732 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Quaternion.as +file + + + + +2010-10-28T04:32:45.000000Z +7332c9620420bd10062d913decfb9501 +2010-02-12T12:34:25.192477Z +28424 +mike + +Vector3.as +file + + + + +2010-10-28T04:32:45.000000Z +a1571b527786783cef1a9f56db3f2af6 +2010-02-12T12:34:25.192477Z +28424 +mike + +Matrix3.as +file + + + + +2010-10-28T04:32:45.000000Z +0054bd91fe694474a28c8ec17697292b +2010-02-12T12:34:25.192477Z +28424 +mike + +Matrix4.as +file + + + + +2010-10-28T04:32:45.000000Z +8262d985e77059ee6d5f3be367ba08ee +2010-08-20T07:58:00.614935Z +39732 +mike + diff --git a/0.0.9.5/src/alternativa/math/.svn/format b/0.0.9.5/src/alternativa/math/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.5/src/alternativa/math/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.5/src/alternativa/math/.svn/text-base/Matrix3.as.svn-base b/0.0.9.5/src/alternativa/math/.svn/text-base/Matrix3.as.svn-base new file mode 100644 index 0000000..244cc98 --- /dev/null +++ b/0.0.9.5/src/alternativa/math/.svn/text-base/Matrix3.as.svn-base @@ -0,0 +1,390 @@ +package alternativa.math { + import flash.geom.Vector3D; + + /** + * + */ + public class Matrix3 { + + public static const ZERO:Matrix3 = new Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0); + public static const IDENTITY:Matrix3 = new Matrix3(); + + public var a:Number; + public var b:Number; + public var c:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix3(a:Number = 1, b:Number = 0, c:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1) { + this.a = a; + this.b = b; + this.c = c; + + this.e = e; + this.f = f; + this.g = g; + + this.i = i; + this.j = j; + this.k = k; + } + + /** + * + * @return + */ + public function toIdentity():Matrix3 { + a = f = k = 1; + b = c = e = g = i = j = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + var det:Number = 1/(-cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk); + + a = (ff*kk - gg*jj)*det; + b = (cc*jj - bb*kk)*det; + c = (bb*gg - cc*ff)*det; + e = (gg*ii - ee*kk)*det; + f = (aa*kk - cc*ii)*det; + g = (cc*ee - aa*gg)*det; + i = (ee*jj - ff*ii)*det; + j = (bb*ii - aa*jj)*det; + k = (aa*ff - bb*ee)*det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prependTransposed(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.b + cc*m.c; + b = aa*m.e + bb*m.f + cc*m.g; + c = aa*m.i + bb*m.j + cc*m.k; + e = ee*m.a + ff*m.b + gg*m.c; + f = ee*m.e + ff*m.f + gg*m.g; + g = ee*m.i + ff*m.j + gg*m.k; + i = ii*m.a + jj*m.b + kk*m.c; + j = ii*m.e + jj*m.f + kk*m.g; + k = ii*m.i + jj*m.j + kk*m.k; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix3):Matrix3 { + a += m.a; + b += m.b; + c += m.c; + e += m.e; + f += m.f; + g += m.g; + i += m.i; + j += m.j; + k += m.k; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix3):Matrix3 { + a -= m.a; + b -= m.b; + c -= m.c; + e -= m.e; + f -= m.f; + g -= m.g; + i -= m.i; + j -= m.j; + k -= m.k; + + return this; + } + + /** + * + * @return + */ + public function transpose():Matrix3 { + var tmp:Number = b; + b = e; + e = tmp; + tmp = c; + c = i; + i = tmp; + tmp = g; + g = j; + j = tmp; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector3To3D(vin:Vector3, vout:Vector3D):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * + * @param matrix + */ + public function createSkewSymmetric(v:Vector3):Matrix3 { + a = f = k = 0; + b = -v.z; + c = v.y; + e = v.z; + g = -v.x; + i = -v.y; + j = v.x; + return this; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix3):Matrix3 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @return + */ + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):Matrix3 { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + + return this; + } + + /** + * Метод формирует матрицу поворота на заданный угол относительно заданной оси. + * + * @param axis нормализованный вектор, задающий ось, относительно которой выполняется поворот + * @param angle угол поворота в радианах + */ + public function fromAxisAngle(axis:Vector3, angle:Number):void { + var c1:Number = Math.cos(angle); + var s:Number = Math.sin(angle); + var t:Number = 1 - c1; + var x:Number = axis.x; + var y:Number = axis.y; + var z:Number = axis.z; + + a = t*x*x + c1; + b = t*x*y - z*s; + c = t*x*z + y*s; + + e = t*x*y + z*s; + f = t*y*y + c1; + g = t*y*z - x*s; + + i = t*x*z - y*s; + j = t*y*z + x*s; + k = t*z*z + c1; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix3 { + return new Matrix3(a, b, c, e, f, g, i, j, k); + } + + /** + * + * @return + */ + public function toString():String { + return "[Matrix3 (" + a + ", " + b + ", " + c + "), (" + e + ", " + f + ", " + g + "), (" + i + ", " + j + ", " + k + ")]"; + } + + /** + * @param angles + */ + public function getEulerAngles(angles:Vector3):void { + if (-1 < i && i < 1) { + angles.x = Math.atan2(j, k); + angles.y = -Math.asin(i); + angles.z = Math.atan2(e, a); + } else { + angles.x = 0; + angles.y = (i <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-b, f); + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/math/.svn/text-base/Matrix4.as.svn-base b/0.0.9.5/src/alternativa/math/.svn/text-base/Matrix4.as.svn-base new file mode 100644 index 0000000..66c259c --- /dev/null +++ b/0.0.9.5/src/alternativa/math/.svn/text-base/Matrix4.as.svn-base @@ -0,0 +1,530 @@ +package alternativa.math { + + /** + * + */ + public class Matrix4 { + + public static const IDENTITY:Matrix4 = new Matrix4(); + + public var a:Number; + public var b:Number; + public var c:Number; + public var d:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + public var h:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + public var l:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix4(a:Number = 1, b:Number = 0, c:Number = 0, d:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, h:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1, l:Number = 0) { + this.a = a; + this.b = b; + this.c = c; + this.d = d; + + this.e = e; + this.f = f; + this.g = g; + this.h = h; + + this.i = i; + this.j = j; + this.k = k; + this.l = l; + } + + /** + * + * @return + */ + public function toIdentity():Matrix4 { + a = f = k = 1; + b = c = e = g = i = j = d = h = l = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + var det:Number = -cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk; + + a = (-gg*jj + ff*kk)/det; + b = (cc*jj - bb*kk)/det; + c = (-cc*ff + bb*gg)/det; + d = (dd*gg*jj - cc*hh*jj - dd*ff*kk + bb*hh*kk + cc*ff*ll - bb*gg*ll)/det; + e = (gg*ii - ee*kk)/det; + f = (-cc*ii + aa*kk)/det; + g = (cc*ee - aa*gg)/det; + h = (cc*hh*ii - dd*gg*ii + dd*ee*kk - aa*hh*kk - cc*ee*ll + aa*gg*ll)/det; + i = (-ff*ii + ee*jj)/det; + j = (bb*ii - aa*jj)/det; + k = (-bb*ee + aa*ff)/det; + l = (dd*ff*ii - bb*hh*ii - dd*ee*jj + aa*hh*jj + bb*ee*ll - aa*ff*ll)/det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + d = m.a*dd + m.b*hh + m.c*ll + m.d; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + h = m.e*dd + m.f*hh + m.g*ll + m.h; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + l = m.i*dd + m.j*hh + m.k*ll + m.l; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + d = aa*m.d + bb*m.h + cc*m.l + dd; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + h = ee*m.d + ff*m.h + gg*m.l + hh; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + l = ii*m.d + jj*m.h + kk*m.l + ll; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix4):Matrix4 { + a += m.a; + b += m.b; + c += m.c; + d += m.d; + e += m.e; + f += m.f; + g += m.g; + h += m.h; + i += m.i; + j += m.j; + k += m.k; + l += m.l; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix4):Matrix4 { + a -= m.a; + b -= m.b; + c -= m.c; + d -= m.d; + e -= m.e; + f -= m.f; + g -= m.g; + h -= m.h; + i -= m.i; + j -= m.j; + k -= m.k; + l -= m.l; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectors(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + * @param len + */ + public function transformVectorsN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverse(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverseN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(idx:int, axis:Vector3):void { + switch (idx) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + case 3: + axis.x = d; + axis.y = h; + axis.z = l; + return; + } + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix4):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = m.d; + e = m.e; + f = m.f; + g = m.g; + h = m.h; + i = m.i; + j = m.j; + k = m.k; + l = m.l; + + return this; + } + + public function setFromMatrix3(m:Matrix3, offset:Vector3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = offset.x; + e = m.e; + f = m.f; + g = m.g; + h = offset.y; + i = m.i; + j = m.j; + k = m.k; + l = offset.z; + + return this; + } + + public function setOrientationFromMatrix3(m:Matrix3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):Matrix4 { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + + return this; + } + + public function setMatrix(x:Number, y:Number, z:Number, rx:Number, ry:Number, rz:Number):Matrix4 { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + d = x; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + h = y; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + l = z; + + return this; + } + + /** + * @param angles + */ + public function getEulerAngles(angles:Vector3):void { + if (-1 < i && i < 1) { + angles.x = Math.atan2(j, k); + angles.y = -Math.asin(i); + angles.z = Math.atan2(e, a); + } else { + angles.x = 0; + angles.y = (i <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-b, f); + } + } + + /** + * Устанавливает координаты матрицы. + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + d = pos.x; + h = pos.y; + l = pos.z; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix4 { + return new Matrix4(a, b, c, d, e, f, g, h, i, j, k, l); + } + + /** + * + * @return + */ + public function toString():String { + return "[Matrix4 [" + a.toFixed(3) + " " + b.toFixed(3) + " " + c.toFixed(3) + " " + d.toFixed(3) + "] [" + e.toFixed(3) + " " + f.toFixed(3) + " " + g.toFixed(3) + " " + h.toFixed(3) + "] [" + i.toFixed(3) + " " + j.toFixed(3) + " " + k.toFixed(3) + " " + l.toFixed(3) + "]]"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/math/.svn/text-base/Quaternion.as.svn-base b/0.0.9.5/src/alternativa/math/.svn/text-base/Quaternion.as.svn-base new file mode 100644 index 0000000..ddc85f5 --- /dev/null +++ b/0.0.9.5/src/alternativa/math/.svn/text-base/Quaternion.as.svn-base @@ -0,0 +1,458 @@ +package alternativa.math { + import flash.geom.Vector3D; + + public class Quaternion { + + public var w:Number; + public var x:Number; + public var y:Number; + public var z:Number; + + public static function multiply(q1:Quaternion, q2:Quaternion, result:Quaternion):void { + result.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z; + result.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y; + result.y = q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z; + result.z = q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x; + } + + public static function createFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngle(axis, angle); + return q; + } + + public static function createFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngleComponents(x, y, z, angle); + return q; + } + + private static var _q:Quaternion = new Quaternion(); + + public function Quaternion(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0) { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + } + + public function reset(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0):Quaternion { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + return this; + } + + public function normalize():Quaternion { + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * Умножает на указанный кватернион слева: this * q + * + * @param q множитель + */ + public function prepend(q:Quaternion):Quaternion { + var ww:Number = w*q.w - x*q.x - y*q.y - z*q.z; + var xx:Number = w*q.x + x*q.w + y*q.z - z*q.y; + var yy:Number = w*q.y + y*q.w + z*q.x - x*q.z; + var zz:Number = w*q.z + z*q.w + x*q.y - y*q.x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Умножает на указанный кватернион справа: q * this + * + * @param q множитель + */ + public function append(q:Quaternion):Quaternion { + var ww:Number = q.w*w - q.x*x - q.y*y - q.z*z; + var xx:Number = q.w*x + q.x*w + q.y*z - q.z*y; + var yy:Number = q.w*y + q.y*w + q.z*x - q.x*z; + var zz:Number = q.w*z + q.z*w + q.x*y - q.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * + * @param vector + */ + public function rotateByVector(v:Vector3):Quaternion { + var ww:Number = -v.x*x - v.y*y - v.z*z; + var xx:Number = v.x*w + v.y*z - v.z*y; + var yy:Number = v.y*w + v.z*x - v.x*z; + var zz:Number = v.z*w + v.x*y - v.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Добавляет вращение, приданное вектором угловой скорости за указанное время. + * + * @param v + * @param scale + */ + public function addScaledVector(v:Vector3, scale:Number):Quaternion { + var vx:Number = v.x*scale; + var vy:Number = v.y*scale; + var vz:Number = v.z*scale; + var ww:Number = -x*vx - y*vy - z*vz; + var xx:Number = vx*w + vy*z - vz*y; + var yy:Number = vy*w + vz*x - vx*z; + var zz:Number = vz*w + vx*y - vy*x; + w += 0.5*ww; + x += 0.5*xx; + y += 0.5*yy; + z += 0.5*zz; + // inlined normalize + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param m + * @return + */ + public function toMatrix3(m:Matrix3):Quaternion { + var xx2:Number = 2.0*x*x; + var yy2:Number = 2.0*y*y; + var zz2:Number = 2.0*z*z; + var xy2:Number = 2.0*x*y; + var yz2:Number = 2.0*y*z; + var zx2:Number = 2.0*z*x; + var wx2:Number = 2.0*w*x; + var wy2:Number = 2.0*w*y; + var wz2:Number = 2.0*w*z; + + m.a = 1.0 - yy2 - zz2; + m.b = xy2 - wz2; + m.c = zx2 + wy2; + + m.e = xy2 + wz2; + m.f = 1.0 - xx2 - zz2; + m.g = yz2 - wx2; + + m.i = zx2 - wy2; + m.j = yz2 + wx2; + m.k = 1.0 - xx2 - yy2; + return this; + } + + /** + * + * @return + */ + public function length():Number { + return Math.sqrt(w*w + x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function lengthSqr():Number { + return w*w + x*x + y*y + z*z; + } + + /** + * + * @param axis + * @param angle + * @return + */ + public function setFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z); + x = axis.x*k; + y = axis.y*k; + z = axis.z*k; + return this; + } + + /** + * + * @param x + * @param y + * @param z + * @param angle + */ + public function setFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(x*x + y*y + z*z); + this.x = x*k; + this.y = y*k; + this.z = z*k; + return this; + } + + /** + * + * @param vector + */ + public function toAxisVector(v:Vector3 = null):Vector3 { + if (w < -1 || w > 1) { + normalize(); + } + if (v == null) { + v = new Vector3(); + } + if (w > -1 && w < 1) { + if (w == 0) { + v.x = x; + v.y = y; + v.z = z; + } else { + var angle:Number = 2*Math.acos(w); + var coeff:Number = 1/Math.sqrt(1 - w*w); + v.x = x*coeff*angle; + v.y = y*coeff*angle; + v.z = z*coeff*angle; + } + } else { + v.x = 0; + v.y = 0; + v.z = 0; + } + return v; + } + + /** + * + * @param rotations + */ + public function getEulerAngles(angles:Vector3):Vector3 { + var qi2:Number = 2*x*x; + var qj2:Number = 2*y*y; + var qk2:Number = 2*z*z; + var qij:Number = 2*x*y; + var qjk:Number = 2*y*z; + var qki:Number = 2*z*x; + var qri:Number = 2*w*x; + var qrj:Number = 2*w*y; + var qrk:Number = 2*w*z; + + var aa:Number = 1 - qj2 - qk2; + var bb:Number = qij - qrk; + var ee:Number = qij + qrk; + var ff:Number = 1 - qi2 - qk2; + var ii:Number = qki - qrj; + var jj:Number = qjk + qri; + var kk:Number = 1 - qi2 - qj2; + + if (-1 < ii && ii < 1) { + if (angles == null) { + angles = new Vector3(Math.atan2(jj, kk), -Math.asin(ii), Math.atan2(ee, aa)); + } else { + angles.x = Math.atan2(jj, kk); + angles.y = -Math.asin(ii); + angles.z = Math.atan2(ee, aa); + } + } else { + if (angles == null) { + angles = new Vector3(0, 0.5*((ii <= -1) ? Math.PI : -Math.PI), Math.atan2(-bb, ff)); + } else { + angles.x = 0; + angles.y = (ii <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-bb, ff); + } + } + return angles; + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setFromEulerAnglesXYZ(x:Number, y:Number, z:Number):void { + setFromAxisAngleComponents(1, 0, 0, x); + + _q.setFromAxisAngleComponents(0, 1, 0, y); + append(_q); + normalize(); + + _q.setFromAxisAngleComponents(0, 0, 1, z); + append(_q); + normalize(); + } + + /** + * + */ + public function conjugate():void { + x = -x; + y = -y; + z = -z; + } + + /** + * Выполняет линейную интерполяцию. + * + * @param q1 начало отрезка + * @param q2 конец отрезка + * @param t время, обычно задаётся в интервале [0, 1] + */ + public function nlerp(q1:Quaternion, q2:Quaternion, t:Number):Quaternion { + var d:Number = 1 - t; + w = q1.w*d + q2.w*t; + x = q1.x*d + q2.x*t; + y = q1.y*d + q2.y*t; + z = q1.z*d + q2.z*t; + // inlined normalize + d = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param q + * @return + */ + public function subtract(q:Quaternion):Quaternion { + w -= q.w; + x -= q.x; + y -= q.y; + z -= q.z; + return this; + } + + /** + * + * @param q1 + * @param q2 + * @return + */ + public function diff(q1:Quaternion, q2:Quaternion):Quaternion { + w = q2.w - q1.w; + x = q2.x - q1.x; + y = q2.y - q1.y; + z = q2.z - q1.z; + return this; + } + + /** + * + * @param q + * @return + */ + public function copy(q:Quaternion):Quaternion { + w = q.w; + x = q.x; + y = q.y; + z = q.z; + return this; + } + + /** + * @param reslult + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + result.w = w; + return result; + } + + /** + * + * @return + */ + public function clone():Quaternion { + return new Quaternion(w, x, y, z); + } + + /** + * + * @return + */ + public function toString():String { + return "[" + w + ", " + x + ", " + y + ", " + z + "]"; + } + + /** + * Выполняет сферическую интерполяцию между двумя заданными кватернионами по наименьшему расстоянию. + * + * @param a первый кватерион + * @param b второй кватернион + * @param t параметр интерполяции, обычно принадлежит отрезку [0, 1] + * @return this + */ + public function slerp(a:Quaternion, b:Quaternion, t:Number):Quaternion { + var flip:Number = 1; + // Так как одна и та же ориентация представляется двумя значениями q и -q, нужно сменить знак одного из кватернионов + // если скалярное произведение отрицательно. Иначе будет получено интерполированное значение по наибольшему расстоянию. + var cosine:Number = a.w*b.w + a.x*b.x + a.y*b.y + a.z*b.z; + if (cosine < 0) { + cosine = -cosine; + flip = -1; + } + + if ((1 - cosine) < 0.001) { + // Вблизи нуля используется линейная интерполяция + var k1:Number = 1 - t; + var k2:Number = t*flip; + w = a.w*k1 + b.w*k2; + x = a.x*k1 + b.x*k2; + y = a.y*k1 + b.y*k2; + z = a.z*k1 + b.z*k2; + normalize(); + } else { + var theta:Number = Math.acos(cosine); + var sine:Number = Math.sin(theta); + var beta:Number = Math.sin((1 - t)*theta)/sine; + var alpha:Number = Math.sin(t*theta)/sine*flip; + w = a.w*beta + b.w*alpha; + x = a.x*beta + b.x*alpha; + y = a.y*beta + b.y*alpha; + z = a.z*beta + b.z*alpha; + } + + return this; + } + + } +} diff --git a/0.0.9.5/src/alternativa/math/.svn/text-base/Vector3.as.svn-base b/0.0.9.5/src/alternativa/math/.svn/text-base/Vector3.as.svn-base new file mode 100644 index 0000000..89010a8 --- /dev/null +++ b/0.0.9.5/src/alternativa/math/.svn/text-base/Vector3.as.svn-base @@ -0,0 +1,334 @@ +package alternativa.math { + import flash.geom.Vector3D; + + /** + * + */ + public class Vector3 extends Vector3D { + + public static const ZERO:Vector3 = new Vector3(0, 0, 0); + public static const X_AXIS:Vector3 = new Vector3(1, 0, 0); + public static const Y_AXIS:Vector3 = new Vector3(0, 1, 0); + public static const Z_AXIS:Vector3 = new Vector3(0, 0, 1); + + /** + * + * @param x + * @param y + * @param z + */ + public function Vector3(x:Number = 0, y:Number = 0, z:Number = 0) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * + * @return + */ + public function vLength():Number { + return Math.sqrt(x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function vLengthSqr():Number { + return x*x + y*y + z*z; + } + + /** + * + * @param length + * @return + */ + public function vSetLength(length:Number):Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = length; + } else { + var k:Number = length/Math.sqrt(x*x + y*y + z*z); + x *= k; + y *= k; + z *= k; + } + return this; + } + + /** + * + * @return + */ + public function vNormalize():Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = 1; + } else { + d = Math.sqrt(d); + x /= d; + y /= d; + z /= d; + } + return this; + } + + /** + * Сложение координат. + * + * @param v точка, координаты которой прибавляются к собственным + */ + public function vAdd(v:Vector3):Vector3 { + x += v.x; + y += v.y; + z += v.z; + return this; + } + + /** + * + * @param k + * @param v + * @return + */ + public function vAddScaled(k:Number, v:Vector3):Vector3 { + x += k*v.x; + y += k*v.y; + z += k*v.z; + return this; + } + + /** + * Вычитание координат. + * + * @param v точка, координаты которой вычитаются из собственных + */ + public function vSubtract(v:Vector3):Vector3 { + x -= v.x; + y -= v.y; + z -= v.z; + return this; + } + + /** + * Вычисляет сумму векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + * @return this + */ + public function vSum(a:Vector3, b:Vector3):Vector3 { + x = a.x + b.x; + y = a.y + b.y; + z = a.z + b.z; + return this; + } + + /** + * Вычисление разности векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + */ + public function vDiff(a:Vector3, b:Vector3):Vector3 { + x = a.x - b.x; + y = a.y - b.y; + z = a.z - b.z; + return this; + } + + /** + * Умножение на скаляр. + * + * @param k число, на которое умножаются координаты + */ + public function vScale(k:Number):Vector3 { + x *= k; + y *= k; + z *= k; + return this; + } + + /** + * Инвертирование вектора. + */ + public function vReverse():Vector3 { + x = -x; + y = -y; + z = -z; + return this; + } + + /** + * + * @param v + * @return + */ + public function vDot(v:Vector3):Number { + return x*v.x + y*v.y + z*v.z; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross(v:Vector3):Vector3 { + var xx:Number = y*v.z - z*v.y; + var yy:Number = z*v.x - x*v.z; + var zz:Number = x*v.y - y*v.x; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross2(a:Vector3, b:Vector3):Vector3 { + x = a.y*b.z - a.z*b.y; + y = a.z*b.x - a.x*b.z; + z = a.x*b.y - a.y*b.x; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy3(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * + * @param matrix + */ + public function vTransformBy3Tr(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz + m.d; + y = m.e*xx + m.f*yy + m.g*zz + m.h; + z = m.i*xx + m.j*yy + m.k*zz + m.l; + return this; + } + + /** + * + * @param m + * @return + */ + public function vTransformInverseBy4(m:Matrix4):Vector3 { + var xx:Number = x - m.d; + var yy:Number = y - m.h; + var zz:Number = z - m.l; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * + * @param m + */ + public function vDeltaTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * Установка координат. + */ + public function vReset(x:Number = 0, y:Number = 0, z:Number = 0):Vector3 { + this.x = x; + this.y = y; + this.z = z; + return this; + } + + /** + * Копирование координат точки. + * + * @param v точка, координаты которой копируются + */ + public function vCopy(v:Vector3):Vector3 { + x = v.x; + y = v.y; + z = v.z; + return this; + } + + /** + * Клонирование точки. + * + * @return клонированная точка + */ + public function vClone():Vector3 { + return new Vector3(x, y, z); + } + + /** + * Копирует координаты вектора в экземпляр класса Vector3D. + * + * @param result экземпляр, в который копируются координаты + * @return переданный параметр + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + return result; + } + + /** + * @param source + * @return + */ + public function copyFromVector3D(source:Vector3D):Vector3 { + x = source.x; + y = source.y; + z = source.z; + return this; + } + + override public function toString():String { + return "Vector3(" + x + "," + y + ", " + z + ")"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/math/Matrix3.as b/0.0.9.5/src/alternativa/math/Matrix3.as new file mode 100644 index 0000000..244cc98 --- /dev/null +++ b/0.0.9.5/src/alternativa/math/Matrix3.as @@ -0,0 +1,390 @@ +package alternativa.math { + import flash.geom.Vector3D; + + /** + * + */ + public class Matrix3 { + + public static const ZERO:Matrix3 = new Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0); + public static const IDENTITY:Matrix3 = new Matrix3(); + + public var a:Number; + public var b:Number; + public var c:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix3(a:Number = 1, b:Number = 0, c:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1) { + this.a = a; + this.b = b; + this.c = c; + + this.e = e; + this.f = f; + this.g = g; + + this.i = i; + this.j = j; + this.k = k; + } + + /** + * + * @return + */ + public function toIdentity():Matrix3 { + a = f = k = 1; + b = c = e = g = i = j = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + var det:Number = 1/(-cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk); + + a = (ff*kk - gg*jj)*det; + b = (cc*jj - bb*kk)*det; + c = (bb*gg - cc*ff)*det; + e = (gg*ii - ee*kk)*det; + f = (aa*kk - cc*ii)*det; + g = (cc*ee - aa*gg)*det; + i = (ee*jj - ff*ii)*det; + j = (bb*ii - aa*jj)*det; + k = (aa*ff - bb*ee)*det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prependTransposed(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.b + cc*m.c; + b = aa*m.e + bb*m.f + cc*m.g; + c = aa*m.i + bb*m.j + cc*m.k; + e = ee*m.a + ff*m.b + gg*m.c; + f = ee*m.e + ff*m.f + gg*m.g; + g = ee*m.i + ff*m.j + gg*m.k; + i = ii*m.a + jj*m.b + kk*m.c; + j = ii*m.e + jj*m.f + kk*m.g; + k = ii*m.i + jj*m.j + kk*m.k; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix3):Matrix3 { + a += m.a; + b += m.b; + c += m.c; + e += m.e; + f += m.f; + g += m.g; + i += m.i; + j += m.j; + k += m.k; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix3):Matrix3 { + a -= m.a; + b -= m.b; + c -= m.c; + e -= m.e; + f -= m.f; + g -= m.g; + i -= m.i; + j -= m.j; + k -= m.k; + + return this; + } + + /** + * + * @return + */ + public function transpose():Matrix3 { + var tmp:Number = b; + b = e; + e = tmp; + tmp = c; + c = i; + i = tmp; + tmp = g; + g = j; + j = tmp; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector3To3D(vin:Vector3, vout:Vector3D):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * + * @param matrix + */ + public function createSkewSymmetric(v:Vector3):Matrix3 { + a = f = k = 0; + b = -v.z; + c = v.y; + e = v.z; + g = -v.x; + i = -v.y; + j = v.x; + return this; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix3):Matrix3 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @return + */ + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):Matrix3 { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + + return this; + } + + /** + * Метод формирует матрицу поворота на заданный угол относительно заданной оси. + * + * @param axis нормализованный вектор, задающий ось, относительно которой выполняется поворот + * @param angle угол поворота в радианах + */ + public function fromAxisAngle(axis:Vector3, angle:Number):void { + var c1:Number = Math.cos(angle); + var s:Number = Math.sin(angle); + var t:Number = 1 - c1; + var x:Number = axis.x; + var y:Number = axis.y; + var z:Number = axis.z; + + a = t*x*x + c1; + b = t*x*y - z*s; + c = t*x*z + y*s; + + e = t*x*y + z*s; + f = t*y*y + c1; + g = t*y*z - x*s; + + i = t*x*z - y*s; + j = t*y*z + x*s; + k = t*z*z + c1; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix3 { + return new Matrix3(a, b, c, e, f, g, i, j, k); + } + + /** + * + * @return + */ + public function toString():String { + return "[Matrix3 (" + a + ", " + b + ", " + c + "), (" + e + ", " + f + ", " + g + "), (" + i + ", " + j + ", " + k + ")]"; + } + + /** + * @param angles + */ + public function getEulerAngles(angles:Vector3):void { + if (-1 < i && i < 1) { + angles.x = Math.atan2(j, k); + angles.y = -Math.asin(i); + angles.z = Math.atan2(e, a); + } else { + angles.x = 0; + angles.y = (i <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-b, f); + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/math/Matrix4.as b/0.0.9.5/src/alternativa/math/Matrix4.as new file mode 100644 index 0000000..66c259c --- /dev/null +++ b/0.0.9.5/src/alternativa/math/Matrix4.as @@ -0,0 +1,530 @@ +package alternativa.math { + + /** + * + */ + public class Matrix4 { + + public static const IDENTITY:Matrix4 = new Matrix4(); + + public var a:Number; + public var b:Number; + public var c:Number; + public var d:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + public var h:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + public var l:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix4(a:Number = 1, b:Number = 0, c:Number = 0, d:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, h:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1, l:Number = 0) { + this.a = a; + this.b = b; + this.c = c; + this.d = d; + + this.e = e; + this.f = f; + this.g = g; + this.h = h; + + this.i = i; + this.j = j; + this.k = k; + this.l = l; + } + + /** + * + * @return + */ + public function toIdentity():Matrix4 { + a = f = k = 1; + b = c = e = g = i = j = d = h = l = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + var det:Number = -cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk; + + a = (-gg*jj + ff*kk)/det; + b = (cc*jj - bb*kk)/det; + c = (-cc*ff + bb*gg)/det; + d = (dd*gg*jj - cc*hh*jj - dd*ff*kk + bb*hh*kk + cc*ff*ll - bb*gg*ll)/det; + e = (gg*ii - ee*kk)/det; + f = (-cc*ii + aa*kk)/det; + g = (cc*ee - aa*gg)/det; + h = (cc*hh*ii - dd*gg*ii + dd*ee*kk - aa*hh*kk - cc*ee*ll + aa*gg*ll)/det; + i = (-ff*ii + ee*jj)/det; + j = (bb*ii - aa*jj)/det; + k = (-bb*ee + aa*ff)/det; + l = (dd*ff*ii - bb*hh*ii - dd*ee*jj + aa*hh*jj + bb*ee*ll - aa*ff*ll)/det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + d = m.a*dd + m.b*hh + m.c*ll + m.d; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + h = m.e*dd + m.f*hh + m.g*ll + m.h; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + l = m.i*dd + m.j*hh + m.k*ll + m.l; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + d = aa*m.d + bb*m.h + cc*m.l + dd; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + h = ee*m.d + ff*m.h + gg*m.l + hh; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + l = ii*m.d + jj*m.h + kk*m.l + ll; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix4):Matrix4 { + a += m.a; + b += m.b; + c += m.c; + d += m.d; + e += m.e; + f += m.f; + g += m.g; + h += m.h; + i += m.i; + j += m.j; + k += m.k; + l += m.l; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix4):Matrix4 { + a -= m.a; + b -= m.b; + c -= m.c; + d -= m.d; + e -= m.e; + f -= m.f; + g -= m.g; + h -= m.h; + i -= m.i; + j -= m.j; + k -= m.k; + l -= m.l; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectors(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + * @param len + */ + public function transformVectorsN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverse(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverseN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(idx:int, axis:Vector3):void { + switch (idx) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + case 3: + axis.x = d; + axis.y = h; + axis.z = l; + return; + } + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix4):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = m.d; + e = m.e; + f = m.f; + g = m.g; + h = m.h; + i = m.i; + j = m.j; + k = m.k; + l = m.l; + + return this; + } + + public function setFromMatrix3(m:Matrix3, offset:Vector3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = offset.x; + e = m.e; + f = m.f; + g = m.g; + h = offset.y; + i = m.i; + j = m.j; + k = m.k; + l = offset.z; + + return this; + } + + public function setOrientationFromMatrix3(m:Matrix3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):Matrix4 { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + + return this; + } + + public function setMatrix(x:Number, y:Number, z:Number, rx:Number, ry:Number, rz:Number):Matrix4 { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + d = x; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + h = y; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + l = z; + + return this; + } + + /** + * @param angles + */ + public function getEulerAngles(angles:Vector3):void { + if (-1 < i && i < 1) { + angles.x = Math.atan2(j, k); + angles.y = -Math.asin(i); + angles.z = Math.atan2(e, a); + } else { + angles.x = 0; + angles.y = (i <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-b, f); + } + } + + /** + * Устанавливает координаты матрицы. + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + d = pos.x; + h = pos.y; + l = pos.z; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix4 { + return new Matrix4(a, b, c, d, e, f, g, h, i, j, k, l); + } + + /** + * + * @return + */ + public function toString():String { + return "[Matrix4 [" + a.toFixed(3) + " " + b.toFixed(3) + " " + c.toFixed(3) + " " + d.toFixed(3) + "] [" + e.toFixed(3) + " " + f.toFixed(3) + " " + g.toFixed(3) + " " + h.toFixed(3) + "] [" + i.toFixed(3) + " " + j.toFixed(3) + " " + k.toFixed(3) + " " + l.toFixed(3) + "]]"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/math/Quaternion.as b/0.0.9.5/src/alternativa/math/Quaternion.as new file mode 100644 index 0000000..ddc85f5 --- /dev/null +++ b/0.0.9.5/src/alternativa/math/Quaternion.as @@ -0,0 +1,458 @@ +package alternativa.math { + import flash.geom.Vector3D; + + public class Quaternion { + + public var w:Number; + public var x:Number; + public var y:Number; + public var z:Number; + + public static function multiply(q1:Quaternion, q2:Quaternion, result:Quaternion):void { + result.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z; + result.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y; + result.y = q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z; + result.z = q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x; + } + + public static function createFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngle(axis, angle); + return q; + } + + public static function createFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngleComponents(x, y, z, angle); + return q; + } + + private static var _q:Quaternion = new Quaternion(); + + public function Quaternion(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0) { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + } + + public function reset(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0):Quaternion { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + return this; + } + + public function normalize():Quaternion { + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * Умножает на указанный кватернион слева: this * q + * + * @param q множитель + */ + public function prepend(q:Quaternion):Quaternion { + var ww:Number = w*q.w - x*q.x - y*q.y - z*q.z; + var xx:Number = w*q.x + x*q.w + y*q.z - z*q.y; + var yy:Number = w*q.y + y*q.w + z*q.x - x*q.z; + var zz:Number = w*q.z + z*q.w + x*q.y - y*q.x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Умножает на указанный кватернион справа: q * this + * + * @param q множитель + */ + public function append(q:Quaternion):Quaternion { + var ww:Number = q.w*w - q.x*x - q.y*y - q.z*z; + var xx:Number = q.w*x + q.x*w + q.y*z - q.z*y; + var yy:Number = q.w*y + q.y*w + q.z*x - q.x*z; + var zz:Number = q.w*z + q.z*w + q.x*y - q.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * + * @param vector + */ + public function rotateByVector(v:Vector3):Quaternion { + var ww:Number = -v.x*x - v.y*y - v.z*z; + var xx:Number = v.x*w + v.y*z - v.z*y; + var yy:Number = v.y*w + v.z*x - v.x*z; + var zz:Number = v.z*w + v.x*y - v.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Добавляет вращение, приданное вектором угловой скорости за указанное время. + * + * @param v + * @param scale + */ + public function addScaledVector(v:Vector3, scale:Number):Quaternion { + var vx:Number = v.x*scale; + var vy:Number = v.y*scale; + var vz:Number = v.z*scale; + var ww:Number = -x*vx - y*vy - z*vz; + var xx:Number = vx*w + vy*z - vz*y; + var yy:Number = vy*w + vz*x - vx*z; + var zz:Number = vz*w + vx*y - vy*x; + w += 0.5*ww; + x += 0.5*xx; + y += 0.5*yy; + z += 0.5*zz; + // inlined normalize + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param m + * @return + */ + public function toMatrix3(m:Matrix3):Quaternion { + var xx2:Number = 2.0*x*x; + var yy2:Number = 2.0*y*y; + var zz2:Number = 2.0*z*z; + var xy2:Number = 2.0*x*y; + var yz2:Number = 2.0*y*z; + var zx2:Number = 2.0*z*x; + var wx2:Number = 2.0*w*x; + var wy2:Number = 2.0*w*y; + var wz2:Number = 2.0*w*z; + + m.a = 1.0 - yy2 - zz2; + m.b = xy2 - wz2; + m.c = zx2 + wy2; + + m.e = xy2 + wz2; + m.f = 1.0 - xx2 - zz2; + m.g = yz2 - wx2; + + m.i = zx2 - wy2; + m.j = yz2 + wx2; + m.k = 1.0 - xx2 - yy2; + return this; + } + + /** + * + * @return + */ + public function length():Number { + return Math.sqrt(w*w + x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function lengthSqr():Number { + return w*w + x*x + y*y + z*z; + } + + /** + * + * @param axis + * @param angle + * @return + */ + public function setFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z); + x = axis.x*k; + y = axis.y*k; + z = axis.z*k; + return this; + } + + /** + * + * @param x + * @param y + * @param z + * @param angle + */ + public function setFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(x*x + y*y + z*z); + this.x = x*k; + this.y = y*k; + this.z = z*k; + return this; + } + + /** + * + * @param vector + */ + public function toAxisVector(v:Vector3 = null):Vector3 { + if (w < -1 || w > 1) { + normalize(); + } + if (v == null) { + v = new Vector3(); + } + if (w > -1 && w < 1) { + if (w == 0) { + v.x = x; + v.y = y; + v.z = z; + } else { + var angle:Number = 2*Math.acos(w); + var coeff:Number = 1/Math.sqrt(1 - w*w); + v.x = x*coeff*angle; + v.y = y*coeff*angle; + v.z = z*coeff*angle; + } + } else { + v.x = 0; + v.y = 0; + v.z = 0; + } + return v; + } + + /** + * + * @param rotations + */ + public function getEulerAngles(angles:Vector3):Vector3 { + var qi2:Number = 2*x*x; + var qj2:Number = 2*y*y; + var qk2:Number = 2*z*z; + var qij:Number = 2*x*y; + var qjk:Number = 2*y*z; + var qki:Number = 2*z*x; + var qri:Number = 2*w*x; + var qrj:Number = 2*w*y; + var qrk:Number = 2*w*z; + + var aa:Number = 1 - qj2 - qk2; + var bb:Number = qij - qrk; + var ee:Number = qij + qrk; + var ff:Number = 1 - qi2 - qk2; + var ii:Number = qki - qrj; + var jj:Number = qjk + qri; + var kk:Number = 1 - qi2 - qj2; + + if (-1 < ii && ii < 1) { + if (angles == null) { + angles = new Vector3(Math.atan2(jj, kk), -Math.asin(ii), Math.atan2(ee, aa)); + } else { + angles.x = Math.atan2(jj, kk); + angles.y = -Math.asin(ii); + angles.z = Math.atan2(ee, aa); + } + } else { + if (angles == null) { + angles = new Vector3(0, 0.5*((ii <= -1) ? Math.PI : -Math.PI), Math.atan2(-bb, ff)); + } else { + angles.x = 0; + angles.y = (ii <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-bb, ff); + } + } + return angles; + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setFromEulerAnglesXYZ(x:Number, y:Number, z:Number):void { + setFromAxisAngleComponents(1, 0, 0, x); + + _q.setFromAxisAngleComponents(0, 1, 0, y); + append(_q); + normalize(); + + _q.setFromAxisAngleComponents(0, 0, 1, z); + append(_q); + normalize(); + } + + /** + * + */ + public function conjugate():void { + x = -x; + y = -y; + z = -z; + } + + /** + * Выполняет линейную интерполяцию. + * + * @param q1 начало отрезка + * @param q2 конец отрезка + * @param t время, обычно задаётся в интервале [0, 1] + */ + public function nlerp(q1:Quaternion, q2:Quaternion, t:Number):Quaternion { + var d:Number = 1 - t; + w = q1.w*d + q2.w*t; + x = q1.x*d + q2.x*t; + y = q1.y*d + q2.y*t; + z = q1.z*d + q2.z*t; + // inlined normalize + d = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param q + * @return + */ + public function subtract(q:Quaternion):Quaternion { + w -= q.w; + x -= q.x; + y -= q.y; + z -= q.z; + return this; + } + + /** + * + * @param q1 + * @param q2 + * @return + */ + public function diff(q1:Quaternion, q2:Quaternion):Quaternion { + w = q2.w - q1.w; + x = q2.x - q1.x; + y = q2.y - q1.y; + z = q2.z - q1.z; + return this; + } + + /** + * + * @param q + * @return + */ + public function copy(q:Quaternion):Quaternion { + w = q.w; + x = q.x; + y = q.y; + z = q.z; + return this; + } + + /** + * @param reslult + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + result.w = w; + return result; + } + + /** + * + * @return + */ + public function clone():Quaternion { + return new Quaternion(w, x, y, z); + } + + /** + * + * @return + */ + public function toString():String { + return "[" + w + ", " + x + ", " + y + ", " + z + "]"; + } + + /** + * Выполняет сферическую интерполяцию между двумя заданными кватернионами по наименьшему расстоянию. + * + * @param a первый кватерион + * @param b второй кватернион + * @param t параметр интерполяции, обычно принадлежит отрезку [0, 1] + * @return this + */ + public function slerp(a:Quaternion, b:Quaternion, t:Number):Quaternion { + var flip:Number = 1; + // Так как одна и та же ориентация представляется двумя значениями q и -q, нужно сменить знак одного из кватернионов + // если скалярное произведение отрицательно. Иначе будет получено интерполированное значение по наибольшему расстоянию. + var cosine:Number = a.w*b.w + a.x*b.x + a.y*b.y + a.z*b.z; + if (cosine < 0) { + cosine = -cosine; + flip = -1; + } + + if ((1 - cosine) < 0.001) { + // Вблизи нуля используется линейная интерполяция + var k1:Number = 1 - t; + var k2:Number = t*flip; + w = a.w*k1 + b.w*k2; + x = a.x*k1 + b.x*k2; + y = a.y*k1 + b.y*k2; + z = a.z*k1 + b.z*k2; + normalize(); + } else { + var theta:Number = Math.acos(cosine); + var sine:Number = Math.sin(theta); + var beta:Number = Math.sin((1 - t)*theta)/sine; + var alpha:Number = Math.sin(t*theta)/sine*flip; + w = a.w*beta + b.w*alpha; + x = a.x*beta + b.x*alpha; + y = a.y*beta + b.y*alpha; + z = a.z*beta + b.z*alpha; + } + + return this; + } + + } +} diff --git a/0.0.9.5/src/alternativa/math/Vector3.as b/0.0.9.5/src/alternativa/math/Vector3.as new file mode 100644 index 0000000..89010a8 --- /dev/null +++ b/0.0.9.5/src/alternativa/math/Vector3.as @@ -0,0 +1,334 @@ +package alternativa.math { + import flash.geom.Vector3D; + + /** + * + */ + public class Vector3 extends Vector3D { + + public static const ZERO:Vector3 = new Vector3(0, 0, 0); + public static const X_AXIS:Vector3 = new Vector3(1, 0, 0); + public static const Y_AXIS:Vector3 = new Vector3(0, 1, 0); + public static const Z_AXIS:Vector3 = new Vector3(0, 0, 1); + + /** + * + * @param x + * @param y + * @param z + */ + public function Vector3(x:Number = 0, y:Number = 0, z:Number = 0) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * + * @return + */ + public function vLength():Number { + return Math.sqrt(x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function vLengthSqr():Number { + return x*x + y*y + z*z; + } + + /** + * + * @param length + * @return + */ + public function vSetLength(length:Number):Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = length; + } else { + var k:Number = length/Math.sqrt(x*x + y*y + z*z); + x *= k; + y *= k; + z *= k; + } + return this; + } + + /** + * + * @return + */ + public function vNormalize():Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = 1; + } else { + d = Math.sqrt(d); + x /= d; + y /= d; + z /= d; + } + return this; + } + + /** + * Сложение координат. + * + * @param v точка, координаты которой прибавляются к собственным + */ + public function vAdd(v:Vector3):Vector3 { + x += v.x; + y += v.y; + z += v.z; + return this; + } + + /** + * + * @param k + * @param v + * @return + */ + public function vAddScaled(k:Number, v:Vector3):Vector3 { + x += k*v.x; + y += k*v.y; + z += k*v.z; + return this; + } + + /** + * Вычитание координат. + * + * @param v точка, координаты которой вычитаются из собственных + */ + public function vSubtract(v:Vector3):Vector3 { + x -= v.x; + y -= v.y; + z -= v.z; + return this; + } + + /** + * Вычисляет сумму векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + * @return this + */ + public function vSum(a:Vector3, b:Vector3):Vector3 { + x = a.x + b.x; + y = a.y + b.y; + z = a.z + b.z; + return this; + } + + /** + * Вычисление разности векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + */ + public function vDiff(a:Vector3, b:Vector3):Vector3 { + x = a.x - b.x; + y = a.y - b.y; + z = a.z - b.z; + return this; + } + + /** + * Умножение на скаляр. + * + * @param k число, на которое умножаются координаты + */ + public function vScale(k:Number):Vector3 { + x *= k; + y *= k; + z *= k; + return this; + } + + /** + * Инвертирование вектора. + */ + public function vReverse():Vector3 { + x = -x; + y = -y; + z = -z; + return this; + } + + /** + * + * @param v + * @return + */ + public function vDot(v:Vector3):Number { + return x*v.x + y*v.y + z*v.z; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross(v:Vector3):Vector3 { + var xx:Number = y*v.z - z*v.y; + var yy:Number = z*v.x - x*v.z; + var zz:Number = x*v.y - y*v.x; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross2(a:Vector3, b:Vector3):Vector3 { + x = a.y*b.z - a.z*b.y; + y = a.z*b.x - a.x*b.z; + z = a.x*b.y - a.y*b.x; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy3(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * + * @param matrix + */ + public function vTransformBy3Tr(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz + m.d; + y = m.e*xx + m.f*yy + m.g*zz + m.h; + z = m.i*xx + m.j*yy + m.k*zz + m.l; + return this; + } + + /** + * + * @param m + * @return + */ + public function vTransformInverseBy4(m:Matrix4):Vector3 { + var xx:Number = x - m.d; + var yy:Number = y - m.h; + var zz:Number = z - m.l; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * + * @param m + */ + public function vDeltaTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * Установка координат. + */ + public function vReset(x:Number = 0, y:Number = 0, z:Number = 0):Vector3 { + this.x = x; + this.y = y; + this.z = z; + return this; + } + + /** + * Копирование координат точки. + * + * @param v точка, координаты которой копируются + */ + public function vCopy(v:Vector3):Vector3 { + x = v.x; + y = v.y; + z = v.z; + return this; + } + + /** + * Клонирование точки. + * + * @return клонированная точка + */ + public function vClone():Vector3 { + return new Vector3(x, y, z); + } + + /** + * Копирует координаты вектора в экземпляр класса Vector3D. + * + * @param result экземпляр, в который копируются координаты + * @return переданный параметр + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + return result; + } + + /** + * @param source + * @return + */ + public function copyFromVector3D(source:Vector3D):Vector3 { + x = source.x; + y = source.y; + z = source.z; + return this; + } + + override public function toString():String { + return "Vector3(" + x + "," + y + ", " + z + ")"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/.svn/all-wcprops b/0.0.9.5/src/alternativa/physics/.svn/all-wcprops new file mode 100644 index 0000000..d75130b --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/.svn/all-wcprops @@ -0,0 +1,77 @@ +K 25 +svn:wc:ra_dav:version-url +V 103 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics +END +Body.as +K 25 +svn:wc:ra_dav:version-url +V 111 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/Body.as +END +BodyState.as +K 25 +svn:wc:ra_dav:version-url +V 116 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/BodyState.as +END +CollisionPrimitiveListItem.as +K 25 +svn:wc:ra_dav:version-url +V 133 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/CollisionPrimitiveListItem.as +END +CollisionPrimitiveList.as +K 25 +svn:wc:ra_dav:version-url +V 129 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/CollisionPrimitiveList.as +END +PhysicsScene.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/PhysicsScene.as +END +PhysicsUtils.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/PhysicsUtils.as +END +altphysics.as +K 25 +svn:wc:ra_dav:version-url +V 117 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/altphysics.as +END +ContactPoint.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/ContactPoint.as +END +BodyListItem.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/BodyListItem.as +END +BodyList.as +K 25 +svn:wc:ra_dav:version-url +V 115 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/BodyList.as +END +Contact.as +K 25 +svn:wc:ra_dav:version-url +V 114 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/Contact.as +END +BodyMaterial.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/BodyMaterial.as +END diff --git a/0.0.9.5/src/alternativa/physics/.svn/entries b/0.0.9.5/src/alternativa/physics/.svn/entries new file mode 100644 index 0000000..d6c6439 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/.svn/entries @@ -0,0 +1,181 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics +http://svndev.alternativaplatform.com + + + +2010-08-20T07:58:00.614935Z +39732 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +CollisionPrimitiveList.as +file + + + + +2010-10-28T04:32:45.000000Z +5752b1f4909c5146720b58fbfffed468 +2009-11-01T12:35:52.270571Z +22955 +mike + +altphysics.as +file + + + + +2010-10-28T04:32:45.000000Z +04fbe40a27502dbbd0dba02a76b2df50 +2009-04-20T20:09:14.715403Z +11579 +mike + +collision +dir + +primitives +dir + +BodyList.as +file + + + + +2010-10-28T04:32:45.000000Z +08a90332660bd0273728006d1402dd25 +2009-11-01T12:35:52.270571Z +22955 +mike + +Contact.as +file + + + + +2010-10-28T04:32:45.000000Z +eb89f4f37bcf2d21eadc733f244fcf64 +2010-02-12T12:34:25.192477Z +28424 +mike + +Body.as +file + + + + +2010-10-28T04:32:45.000000Z +355d0fb70dbb4946d4090a7c94c0d58a +2010-02-12T12:34:25.192477Z +28424 +mike + +BodyState.as +file + + + + +2010-10-28T04:32:45.000000Z +5be68820e6f6185d62b3928c39dde285 +2009-11-24T08:44:15.941480Z +23758 +mike + +CollisionPrimitiveListItem.as +file + + + + +2010-10-28T04:32:45.000000Z +cec5dbfefef812d87481478455f367b1 +2009-10-19T07:17:33.113306Z +22253 +mike + +PhysicsScene.as +file + + + + +2010-10-28T04:32:45.000000Z +96c74784bb71c06141cc469eec57ccb8 +2010-02-19T13:09:00.412754Z +28731 +mike + +PhysicsUtils.as +file + + + + +2010-10-28T04:32:45.000000Z +dce0a94f8a861291a2a8942ed0ba54a5 +2009-11-24T08:44:15.941480Z +23758 +mike + +constraints +dir + +ContactPoint.as +file + + + + +2010-10-28T04:32:45.000000Z +14f23db94810328dedd7da2992812ced +2009-11-24T08:44:15.941480Z +23758 +mike + +BodyListItem.as +file + + + + +2010-10-28T04:32:45.000000Z +2fc2b598310b24aef3f8b90e14a8ba4f +2009-10-19T07:17:33.113306Z +22253 +mike + +BodyMaterial.as +file + + + + +2010-10-28T04:32:45.000000Z +9b700692ebb8017882f3e8cec095e1a9 +2009-10-19T07:17:33.113306Z +22253 +mike + diff --git a/0.0.9.5/src/alternativa/physics/.svn/format b/0.0.9.5/src/alternativa/physics/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.5/src/alternativa/physics/.svn/text-base/Body.as.svn-base b/0.0.9.5/src/alternativa/physics/.svn/text-base/Body.as.svn-base new file mode 100644 index 0000000..cd22c5b --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/.svn/text-base/Body.as.svn-base @@ -0,0 +1,405 @@ +package alternativa.physics { + + import alternativa.math.Matrix3; + import alternativa.math.Matrix4; + import alternativa.math.Quaternion; + import alternativa.math.Vector3; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.IBodyCollisionPredicate; + import alternativa.physics.collision.types.BoundBox; + + use namespace altphysics; + + /** + * + */ + public class Body { + + public static var linDamping:Number = 0.997; + public static var rotDamping:Number = 0.997; + + // Идентификатор тела, уникальный в пределах мира + public var id:int; + // Имя тела + public var name:String; + // Мир, в котором находится тело + public var world:PhysicsScene; + // Флаг подвижности тела + public var movable:Boolean = true; + // Флаг указывает, может ли тело быть заморожено + public var canFreeze:Boolean = false; + + public var freezeCounter:int; + public var frozen:Boolean = false; + // Ограничивающий бокс тела + public var aabb:BoundBox = new BoundBox(); + // Предикат, через который тело получает сообщения о столкновениях + public var postCollisionPredicate:IBodyCollisionPredicate; + + // Текущее состояние тела + public var state:BodyState = new BodyState(); + // Предыдущее состояние тела + public var prevState:BodyState = new BodyState(); + // Линейное ускорение тела на текущем шаге симуляции + public var accel:Vector3 = new Vector3(); + // Угловое ускорение тела на текущем шаге симуляции + public var angleAccel:Vector3 = new Vector3(); + // Физический материал тела + public var material:BodyMaterial = new BodyMaterial(); + // Обратная масса тела + public var invMass:Number = 1; + // Обратная матрица тензора инерции в локальных координатах + public var invInertia:Matrix3 = new Matrix3(); + // Обратная матрица тензора инерции в мировых координатах + public var invInertiaWorld:Matrix3 = new Matrix3(); + // Базисная матрица тела в мировых координатах + public var baseMatrix:Matrix3 = new Matrix3(); + + public const MAX_CONTACTS:int = 20; + public var contacts:Vector. = new Vector.(MAX_CONTACTS); + public var contactsNum:int; + + public var collisionPrimitives:CollisionPrimitiveList; + + // Аккумулятор сил + public var forceAccum:Vector3 = new Vector3(); + // Аккумулятор моментов + public var torqueAccum:Vector3 = new Vector3(); + + // Внутренние переменные для избежания создания экземпляров + private static var _r:Vector3 = new Vector3(); + private static var _f:Vector3 = new Vector3(); + + /** + * + * @param invMass + * @param invInertia + */ + public function Body(invMass:Number, invInertia:Matrix3) { + this.invMass = invMass; + this.invInertia.copy(invInertia); + } + + /** + * @param primitive + * @param localTransform + */ + public function addCollisionPrimitive(primitive:CollisionPrimitive, localTransform:Matrix4 = null):void { + if (primitive == null) { + throw new ArgumentError("Primitive cannot be null"); + } + if (collisionPrimitives == null) { + collisionPrimitives = new CollisionPrimitiveList(); + } + collisionPrimitives.append(primitive); + primitive.setBody(this, localTransform); + } + + /** + * + * @param primitive + */ + public function removeCollisionPrimitive(primitive:CollisionPrimitive):void { + if (collisionPrimitives == null) return; + primitive.setBody(null); + collisionPrimitives.remove(primitive); + if (collisionPrimitives.size == 0) { + collisionPrimitives = null; + } + } + + /** + * @param t + * @param result + */ + public function interpolate(t:Number, pos:Vector3, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.w = prevState.orientation.w*t1 + state.orientation.w*t; + orientation.x = prevState.orientation.x*t1 + state.orientation.x*t; + orientation.y = prevState.orientation.y*t1 + state.orientation.y*t; + orientation.z = prevState.orientation.z*t1 + state.orientation.z*t; + } + + /** + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + state.pos.vCopy(pos); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setPositionXYZ(x:Number, y:Number, z:Number):void { + state.pos.vReset(x, y, z); + } + + /** + * + * @param vel + */ + public function setVelocity(vel:Vector3):void { + state.velocity.vCopy(vel); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setVelocityXYZ(x:Number, y:Number, z:Number):void { + state.velocity.vReset(x, y, z); + } + + /** + * + * @param rot + */ + public function setRotation(rot:Vector3):void { + state.rotation.vCopy(rot); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setRotationXYZ(x:Number, y:Number, z:Number):void { + state.rotation.vReset(x, y, z); + } + + /** + * + * @param q + */ + public function setOrientation(q:Quaternion):void { + state.orientation.copy(q); + } + + /** + * @param r + * @param dir + * @param magnitude + */ + public function applyRelPosWorldImpulse(r:Vector3, dir:Vector3, magnitude:Number):void { + var d:Number = magnitude*invMass; + // Линейная часть + state.velocity.x += d*dir.x; + state.velocity.y += d*dir.y; + state.velocity.z += d*dir.z; + + // Вращательная часть + var x:Number = (r.y*dir.z - r.z*dir.y)*magnitude; + var y:Number = (r.z*dir.x - r.x*dir.z)*magnitude; + var z:Number = (r.x*dir.y - r.y*dir.x)*magnitude; + + state.rotation.x += invInertiaWorld.a*x + invInertiaWorld.b*y + invInertiaWorld.c*z; + state.rotation.y += invInertiaWorld.e*x + invInertiaWorld.f*y + invInertiaWorld.g*z; + state.rotation.z += invInertiaWorld.i*x + invInertiaWorld.j*y + invInertiaWorld.k*z; + } + + /** + * @param f + */ + public function addForce(f:Vector3):void { + forceAccum.vAdd(f); + } + + /** + * + * @param fx + * @param fy + * @param fz + */ + public function addForceXYZ(fx:Number, fy:Number, fz:Number):void { + forceAccum.x += fx; + forceAccum.y += fy; + forceAccum.z += fz; + } + + /** + * @param pos + * @param f + */ + public function addWorldForceXYZ(px:Number, py:Number, pz:Number, fx:Number, fy:Number, fz:Number):void { + forceAccum.x += fx; + forceAccum.y += fy; + forceAccum.z += fz; + + var pos:Vector3 = state.pos; + var rx:Number = px - pos.x; + var ry:Number = py - pos.y; + var rz:Number = pz - pos.z; + +// var x:Number = ry*fz - rz*fy; +// var y:Number = rz*fx - rx*fz; +// var z:Number = rx*fy - ry*fx; + + torqueAccum.x += ry*fz - rz*fy; + torqueAccum.y += rz*fx - rx*fz; + torqueAccum.z += rx*fy - ry*fx; + } + + /** + * @param pos + * @param f + */ + public function addWorldForce(pos:Vector3, force:Vector3):void { + forceAccum.vAdd(force); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(force)); + } + + /** + * @param pos + * @param f + */ + public function addWorldForceScaled(pos:Vector3, force:Vector3, scale:Number):void { + _f.x = scale*force.x; + _f.y = scale*force.y; + _f.z = scale*force.z; + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(_f)); + } + + /** + * @param pos + * @param f + */ + public function addLocalForce(pos:Vector3, force:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(pos, _r); + // Трансформируем вектор силы в мировую систему + baseMatrix.transformVector(force, _f); + // Добавляем силу и момент + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vCross(_f)); + } + + /** + * + * @param localPos + * @param worldForce + */ + public function addWorldForceAtLocalPoint(localPos:Vector3, worldForce:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(localPos, _r); + // Добавляем силу и момент + forceAccum.vAdd(worldForce); + torqueAccum.vAdd(_r.vCross(worldForce)); + } + + /** + * @param dt + */ + public function beforePhysicsStep(dt:Number):void { + } + + /** + * @param t + */ + public function addTorque(t:Vector3):void { + torqueAccum.vAdd(t); + } + + /** + * + */ + altphysics function clearAccumulators():void { + forceAccum.x = forceAccum.y = forceAccum.z = 0; + torqueAccum.x = torqueAccum.y = torqueAccum.z = 0; + } + + /** + * + */ + altphysics function calcAccelerations():void { + accel.x = forceAccum.x*invMass; + accel.y = forceAccum.y*invMass; + accel.z = forceAccum.z*invMass; + angleAccel.x = invInertiaWorld.a*torqueAccum.x + invInertiaWorld.b*torqueAccum.y + invInertiaWorld.c*torqueAccum.z; + angleAccel.y = invInertiaWorld.e*torqueAccum.x + invInertiaWorld.f*torqueAccum.y + invInertiaWorld.g*torqueAccum.z; + angleAccel.z = invInertiaWorld.i*torqueAccum.x + invInertiaWorld.j*torqueAccum.y + invInertiaWorld.k*torqueAccum.z; + } + + /** + * Вычисляет производные данные. + */ + public function calcDerivedData():void { + // Вычисление базисной матрицы и обратного тензора инерции в мировых координатах + state.orientation.toMatrix3(baseMatrix); + invInertiaWorld.copy(invInertia).append(baseMatrix).prependTransposed(baseMatrix); + if (collisionPrimitives != null) { + aabb.infinity(); + var item:CollisionPrimitiveListItem = collisionPrimitives.head; + while (item != null) { + var primitive:CollisionPrimitive = item.primitive; + primitive.transform.setFromMatrix3(baseMatrix, state.pos); + if (primitive.localTransform != null) { + primitive.transform.prepend(primitive.localTransform); + } + primitive.calculateAABB(); + aabb.addBoundBox(primitive.aabb); + item = item.next; + } + } + } + + /** + * + */ + altphysics function saveState():void { + prevState.copy(state); + } + + /** + * + */ + altphysics function restoreState():void { + state.copy(prevState); + } + + /** + * @param dt + */ + altphysics function integrateVelocity(dt:Number):void { + // v = v + a*t + state.velocity.x += accel.x*dt; + state.velocity.y += accel.y*dt; + state.velocity.z += accel.z*dt; + // rot = rot + eps*t + state.rotation.x += angleAccel.x*dt; + state.rotation.y += angleAccel.y*dt; + state.rotation.z += angleAccel.z*dt; + + state.velocity.x *= linDamping; + state.velocity.y *= linDamping; + state.velocity.z *= linDamping; + + state.rotation.x *= rotDamping; + state.rotation.y *= rotDamping; + state.rotation.z *= rotDamping; + } + + /** + * + */ + altphysics function integratePosition(dt:Number):void { + // pos = pos + v*t + state.pos.x += state.velocity.x*dt; + state.pos.y += state.velocity.y*dt; + state.pos.z += state.velocity.z*dt; + // q = q + 0.5*rot*q + state.orientation.addScaledVector(state.rotation, dt); + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/.svn/text-base/BodyList.as.svn-base b/0.0.9.5/src/alternativa/physics/.svn/text-base/BodyList.as.svn-base new file mode 100644 index 0000000..aa8f79f --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/.svn/text-base/BodyList.as.svn-base @@ -0,0 +1,76 @@ +package alternativa.physics { + + /** + * + */ + public class BodyList { + + public var head:BodyListItem; + public var tail:BodyListItem; + public var size:int; + + /** + * + */ + public function BodyList() { + } + + /** + * + * @param body + */ + public function append(body:Body):void { + var item:BodyListItem = BodyListItem.create(body); + if (head == null) { + head = tail = item; + } else { + tail.next = item; + item.prev = tail; + tail = item; + } + size++; + } + + /** + * + * @param body + */ + public function remove(body:Body):Boolean { + var item:BodyListItem = findItem(body); + if (item == null) return false; + if (item == head) { + if (size == 1) { + head = tail = null; + } else { + head = item.next; + head.prev = null; + } + } else { + if (item == tail) { + tail = item.prev; + tail.next = null; + } else { + item.prev.next = item.next; + item.next.prev = item.prev; + } + } + item.dispose(); + size--; + return true; + } + + /** + * + * @param body + * @return + */ + public function findItem(body:Body):BodyListItem { + var item:BodyListItem = head; + while (item != null && item.body != body) { + item = item.next; + } + return item; + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/.svn/text-base/BodyListItem.as.svn-base b/0.0.9.5/src/alternativa/physics/.svn/text-base/BodyListItem.as.svn-base new file mode 100644 index 0000000..382e11c --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/.svn/text-base/BodyListItem.as.svn-base @@ -0,0 +1,74 @@ +package alternativa.physics { + + /** + * + */ + public class BodyListItem { + + // Верхний элемент хранилища + private static var poolTop:BodyListItem; + + /** + * Создаёт новый элемент списка. + * + * @param primitive примитив, содержащийся в элементе + * @return новый элемент списка + */ + public static function create(body:Body):BodyListItem { + var item:BodyListItem; + if (poolTop == null) { + item = new BodyListItem(body); + } else { + item = poolTop; + poolTop = item.next; + item.next = null; + item.body = body; + } + return item; + } + + /** + * Очищает хранилище. + */ + public static function clearPool():void { + var item:BodyListItem = poolTop; + while (item != null) { + poolTop = item.next; + item.next = null; + item = poolTop; + } + } + + /** + * + */ + public var body:Body; + /** + * + */ + public var next:BodyListItem; + /** + * + */ + public var prev:BodyListItem; + + /** + * + * @param body + */ + public function BodyListItem(body:Body) { + this.body = body; + } + + /** + * + */ + public function dispose():void { + body = null; + prev = null; + next = poolTop; + poolTop = this; + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/.svn/text-base/BodyMaterial.as.svn-base b/0.0.9.5/src/alternativa/physics/.svn/text-base/BodyMaterial.as.svn-base new file mode 100644 index 0000000..7a7c47d --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/.svn/text-base/BodyMaterial.as.svn-base @@ -0,0 +1,11 @@ +package alternativa.physics { + + public class BodyMaterial { + + public var restitution:Number = 0; + public var friction:Number = 0.3; +// public var dynamicFriction:Number = 0.2; +// public var dynamicFriction:Number = 0.2; + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/.svn/text-base/BodyState.as.svn-base b/0.0.9.5/src/alternativa/physics/.svn/text-base/BodyState.as.svn-base new file mode 100644 index 0000000..3987388 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/.svn/text-base/BodyState.as.svn-base @@ -0,0 +1,39 @@ +package alternativa.physics { + import alternativa.math.Quaternion; + import alternativa.math.Vector3; + + /** + * Класс описывает состояние твёрдого тела. + */ + public class BodyState { + /** + * Положение тела. + */ + public var pos:Vector3 = new Vector3(); + /** + * Ориентация тела. + */ + public var orientation:Quaternion = new Quaternion(); + /** + * Скорость тела. + */ + public var velocity:Vector3 = new Vector3(); + /** + * Угловая скорость тела. + */ + public var rotation:Vector3 = new Vector3(); + + /** + * Копирует значение указанного объекта. + * + * @param state + */ + public function copy(state:BodyState):void { + pos.vCopy(state.pos); + orientation.copy(state.orientation); + velocity.vCopy(state.velocity); + rotation.vCopy(state.rotation); + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/.svn/text-base/CollisionPrimitiveList.as.svn-base b/0.0.9.5/src/alternativa/physics/.svn/text-base/CollisionPrimitiveList.as.svn-base new file mode 100644 index 0000000..5d5304f --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/.svn/text-base/CollisionPrimitiveList.as.svn-base @@ -0,0 +1,89 @@ +package alternativa.physics { + import alternativa.physics.collision.CollisionPrimitive; + + /** + * + */ + public class CollisionPrimitiveList { + + public var head:CollisionPrimitiveListItem; + public var tail:CollisionPrimitiveListItem; + public var size:int; + + /** + * + */ + public function CollisionPrimitiveList() { + } + + /** + * + * @param primitive + */ + public function append(primitive:CollisionPrimitive):void { + var item:CollisionPrimitiveListItem = CollisionPrimitiveListItem.create(primitive); + if (head == null) { + head = tail = item; + } else { + tail.next = item; + item.prev = tail; + tail = item; + } + size++; + } + + /** + * + * @param primitve + */ + public function remove(primitve:CollisionPrimitive):void { + var item:CollisionPrimitiveListItem = findItem(primitve); + if (item == null) return; + if (item == head) { + if (size == 1) { + head = tail = null; + } else { + head = item.next; + head.prev = null; + } + } else { + if (item == tail) { + tail = tail.prev; + tail.next = null; + } else { + item.prev.next = item.next; + item.next.prev = item.prev; + } + } + item.dispose(); + size--; + } + + /** + * + * @param primitive + * @return + */ + public function findItem(primitive:CollisionPrimitive):CollisionPrimitiveListItem { + var item:CollisionPrimitiveListItem = head; + while (item != null && item.primitive != primitive) { + item = item.next; + } + return item; + } + + /** + * + */ + public function clear():void { + while (head != null) { + var item:CollisionPrimitiveListItem = head; + head = head.next; + item.dispose(); + } + tail = null; + size = 0; + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/.svn/text-base/CollisionPrimitiveListItem.as.svn-base b/0.0.9.5/src/alternativa/physics/.svn/text-base/CollisionPrimitiveListItem.as.svn-base new file mode 100644 index 0000000..d204532 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/.svn/text-base/CollisionPrimitiveListItem.as.svn-base @@ -0,0 +1,76 @@ +package alternativa.physics { + import alternativa.physics.collision.CollisionPrimitive; + + /** + * Элемент списка примитивов. + */ + public class CollisionPrimitiveListItem { + + // Верхний элемент хранилища + private static var poolTop:CollisionPrimitiveListItem; + + /** + * Создаёт новый элемент списка. + * + * @param primitive примитив, содержащийся в элементе + * @return новый элемент списка + */ + public static function create(primitive:CollisionPrimitive):CollisionPrimitiveListItem { + var item:CollisionPrimitiveListItem; + if (poolTop == null) { + item = new CollisionPrimitiveListItem(primitive); + } else { + item = poolTop; + item.primitive = primitive; + poolTop = item.next; + item.next = null; + } + return item; + } + + /** + * Очищает хранилище. + */ + public static function clearPool():void { + var curr:CollisionPrimitiveListItem = poolTop; + while (curr != null) { + poolTop = curr.next; + curr.next = null; + curr = poolTop; + } + } + + /** + * Примитив, хранящийся в элементе списка. + */ + public var primitive:CollisionPrimitive; + /** + * Ссылка не следующий элемент списка. + */ + public var next:CollisionPrimitiveListItem; + /** + * Ссылка не предыдущий элемент списка. + */ + public var prev:CollisionPrimitiveListItem; + + /** + * Создаёт новый экземпляр. + * + * @param primitive примитив, хранящийся в элементе списка + */ + public function CollisionPrimitiveListItem(primitive:CollisionPrimitive) { + this.primitive = primitive; + } + + /** + * Очищает внутренние ссылки и помещает элемент в хранилище для дальнейшего использования. + */ + public function dispose():void { + primitive = null; + prev = null; + next = poolTop; + poolTop = this; + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/.svn/text-base/Contact.as.svn-base b/0.0.9.5/src/alternativa/physics/.svn/text-base/Contact.as.svn-base new file mode 100644 index 0000000..3f010ec --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/.svn/text-base/Contact.as.svn-base @@ -0,0 +1,69 @@ +package alternativa.physics { + + + import alternativa.math.Vector3; + + /** + * Описывает контакт одного или двух тел. + */ + public class Contact { + /** + * Первое тело контакта, не может быть равно null. + */ + public var body1:Body; + /** + * Второе тело контакта, может быть равно null. + */ + public var body2:Body; + /** + * Предрассчитанный взаимный коэффициент отскока. + */ + public var restitution:Number; + /** + * Предрассчитанный взаимный коэффициент трения. + */ + public var friction:Number; + /** + * Нормаль контакта. Направлена к первому телу. + */ + public var normal:Vector3 = new Vector3(); + /** + * Список точек контакта. + */ + public var points:Vector. = new Vector.(MAX_POINTS, true); + /** + * Количество точек контакта. + */ + public var pcount:int; + /** + * Максимальная глубина пересечения. + */ + public var maxPenetration:Number = 0; + /** + * Флаг показывает, разрешён контакт или нет. + */ + public var satisfied:Boolean; + /** + * Следующий контакт в списке. + */ + public var next:Contact; + /** + * Индекс контакта. Первый контакт в списке имеет индекс 0. + */ + public var index:int; + + // Максимальное количество точек контакта + private const MAX_POINTS:int = 8; + + /** + * + */ + public function Contact(index:int) { + this.index = index; + for (var i:int = 0; i < MAX_POINTS; i++) { + points[i] = new ContactPoint(); + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/.svn/text-base/ContactPoint.as.svn-base b/0.0.9.5/src/alternativa/physics/.svn/text-base/ContactPoint.as.svn-base new file mode 100644 index 0000000..395d2dc --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/.svn/text-base/ContactPoint.as.svn-base @@ -0,0 +1,55 @@ +package alternativa.physics { + import alternativa.math.Vector3; + + /** + * + */ + public class ContactPoint { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + + // Величины, расчитываемые перед началом фазы решения контактов + + // Требуемая проекция конечной скорости на нормаль для упругого контакта + public var normalVel:Number; + // Минимальная скорость разделения неупругого контакта + public var minSepVel:Number; + // Изменение проекции скорости на единицу нормального импульса + public var velByUnitImpulseN:Number; + + public var angularInertia1:Number; + public var angularInertia2:Number; + + // Радиус-вектор точки контакта относительно центра первого тела + public var r1:Vector3 = new Vector3(); + // Радиус-вектор точки контакта относительно центра второго тела + public var r2:Vector3 = new Vector3(); + + // Величины, накапливаемые во время фазы решения контактов + + // Накопленный импульс, применяемый для получения требуемой относительной скорости в точке контакта + public var accumImpulseN:Number; + // Накопленный импульс, применяемый для разделения тел в точке контакта. Не создаёт момента. +// public var accumSepImpulse:Number; + + public var satisfied:Boolean; + + /** + * + * @param cp + */ + public function copyFrom(cp:ContactPoint):void { + pos.vCopy(cp.pos); + penetration = cp.penetration; + feature1 = cp.feature1; + feature2 = cp.feature2; + r1.vCopy(cp.r1); + r2.vCopy(cp.r2); + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/.svn/text-base/PhysicsScene.as.svn-base b/0.0.9.5/src/alternativa/physics/.svn/text-base/PhysicsScene.as.svn-base new file mode 100644 index 0000000..abdc762 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/.svn/text-base/PhysicsScene.as.svn-base @@ -0,0 +1,743 @@ +package alternativa.physics { + + + import alternativa.physics.collision.ICollisionDetector; + import alternativa.physics.collision.KdTreeCollisionDetector; + import alternativa.physics.constraints.Constraint; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * Класс реализует физическую симуляцию поведения твёрдых тел. + */ + public class PhysicsScene { + + private static var lastBodyId:int; + + // Максимальное количество контактов + public const MAX_CONTACTS:int = 1000; + + ////////////////////////////////////////////////////////////////// + // Настроечные параметры симуляции. Могут быть изменены в любой + // момент времени без нарушения корректной работы симуляции. + ////////////////////////////////////////////////////////////////// + // Количество шагов, за которое пересекающиеся тела должны разделиться + public var penResolutionSteps:int = 10; + // Величина допустимой глубины пересечения + public var allowedPenetration:Number = 0.1; + // Максимальная скорость, добавляемая с целью разделения тел + public var maxPenResolutionSpeed:Number = 0.5; + // Количество итераций для обработки упругих контактов + public var collisionIterations:int = 5; + // Количество итераций для обработки неупругих контактов + public var contactIterations:int = 5; + // Флаг использования предсказания состояний + public var usePrediction:Boolean = false; + + public var freezeSteps:int = 10; + public var linSpeedFreezeLimit:Number = 1; + public var angSpeedFreezeLimit:Number = 0.01; + + // Переменные для процедуры разделения тел путём непосредственного их перемещенеия. + // !!!!!!!!!!!!!!!!!!!!!!!! + // !!! Экспериментально !!! + // !!!!!!!!!!!!!!!!!!!!!!!! + public var staticSeparationIterations:int = 10; + public var staticSeparationSteps:int = 10; + public var maxAngleMove:Number = 10; + public var useStaticSeparation:Boolean = false; + + // Вектор гравитации + public var _gravity:Vector3 = new Vector3(0, 0, -9.8); + // Модуль вектора гравитации + public var _gravityMagnitude:Number = 9.8; + + // Использующийся детектор столкновений + public var collisionDetector:ICollisionDetector; + + // Список тел, участвующих в симуляции + public var bodies:BodyList = new BodyList(); + + // Список контактов на текущем шаге симуляции + altphysics var contacts:Contact; + // Количество контактов на текущем шаге симуляции +// altphysics var contactsNum:int; + // Список ограничений + altphysics var constraints:Vector. = new Vector.(); + // Количество ограничений + altphysics var constraintsNum:int; + // Временная метка. Число прошедших шагов с начала симуляции. + public var timeStamp:int; + // Время с начала симуляции, мс + public var time:int; + // Первый неиспользованный контакт на текущем шаге симуляции + private var borderContact:Contact; + + // Временные переменные для избежания создания экземпляров + private var _r:Vector3 = new Vector3(); + private var _t:Vector3 = new Vector3(); + private var _v:Vector3 = new Vector3(); + private var _v1:Vector3 = new Vector3(); + private var _v2:Vector3 = new Vector3(); + + /** + * + */ + public function PhysicsScene() { + contacts = new Contact(0); + var contact:Contact = contacts; + for (var i:int = 1; i < MAX_CONTACTS; i++) { + contact.next = new Contact(i); + contact = contact.next; + } + collisionDetector = new KdTreeCollisionDetector(); + } + + /** + * Вектор гравитации. + */ + public function get gravity():Vector3 { + return _gravity.vClone(); + } + + /** + * @private + */ + public function set gravity(value:Vector3):void { + _gravity.vCopy(value); + _gravityMagnitude = _gravity.vLength(); + } + + /** + * Добавляет тело в симуляцию. + * + * @param body + */ + public function addBody(body:Body):void { + body.id = lastBodyId++; + body.world = this; + bodies.append(body); + } + + /** + * Удаляет тело из симуляции. + * @param body + * @return + */ + public function removeBody(body:Body):void { + if (bodies.remove(body)) { + body.world = null; + } + } + + /** + * Добавляет ограничение. + * @param c + */ + public function addConstraint(c:Constraint):void { + constraints[constraintsNum++] = c; + c.world = this; + } + + /** + * Удаляет ограничение. + * @param c + */ + public function removeConstraint(c:Constraint):Boolean { + var idx:int = constraints.indexOf(c); + if (idx < 0) return false; + constraints.splice(idx, 1) + constraintsNum--; + c.world = null; + return true; + } + + /** + * Применяет к телам действующте на них силы. Аккумуляторы сил и моментов тел очищаются после завершения шага + * симуляции, поэтому на момент вызова метода могут уже содержать некоторые значения. + * + * @param dt промежуток времени, в течении которого действуют силы + */ + private function applyForces(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + body.beforePhysicsStep(dt); + body.calcAccelerations(); + // Ускорение свободного падения применяется только к подвижным телам во избежание некорректного изменения + // фиктивной скорости неподвижных тел. + if (body.movable && !body.frozen) { + body.accel.x += _gravity.x; + body.accel.y += _gravity.y; + body.accel.z += _gravity.z; + } + item = item.next; + } + } + + /** + * Определяет все столкновения на текущем шаге симуляции и заполняет список получившихся контактов. + * + * @param dt длительность шага симуляции + */ + private function detectCollisions(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + if (!body.frozen) { + body.contactsNum = 0; + body.saveState(); + // При включённом режиме предсказания состояние тел интегрируется на один шаг вперёд + if (usePrediction) { + body.integrateVelocity(dt); + body.integratePosition(dt); + } + body.calcDerivedData(); + } + item = item.next; + } + + borderContact = collisionDetector.getAllContacts(contacts); + + // Расчёт относительных векторов точки контакта вынесен сюда из-за необходимости учитывать + // положение тел в предсказанном состоянии + var contact:Contact = contacts; + while (contact != borderContact) { + var b1:Body = contact.body1; + var b2:Body = contact.body2; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + var bPos:Vector3 = b1.state.pos; + cp.r1.x = cp.pos.x - bPos.x; + cp.r1.y = cp.pos.y - bPos.y; + cp.r1.z = cp.pos.z - bPos.z; + if (b2 != null) { + bPos = b2.state.pos; + cp.r2.x = cp.pos.x - bPos.x; + cp.r2.y = cp.pos.y - bPos.y; + cp.r2.z = cp.pos.z - bPos.z; + } + } + contact = contact.next; + } + + // Восстановление состояния тел + if (usePrediction) { + item = bodies.head; + while (item != null) { + body = item.body; + if (!body.frozen) { + body.restoreState(); + body.calcDerivedData(); + } + item = item.next; + } + } + } + + /** + * Подготваливает полученные из детектора столкновений контакты, расчитывая значения, не меняющиеся + * в ходе шага симуляции. + */ + private function preProcessContacts(dt:Number):void { + var contact:Contact = contacts; + while (contact != borderContact) { + var b1:Body = contact.body1; + var b2:Body = contact.body2; + // Столкнувшиеся тела размораживаются + if (b1.frozen) { + b1.frozen = false; + b1.freezeCounter = 0; + } + if (b2 != null && b2.frozen) { + b2.frozen = false; + b2.freezeCounter = 0; + } + contact.restitution = b1.material.restitution; + if (b2 != null && b2.material.restitution < contact.restitution) contact.restitution = b2.material.restitution; + contact.friction = b1.material.friction; + if (b2 != null && b2.material.friction < contact.friction) contact.friction = b2.material.friction; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.accumImpulseN = 0; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + cp.velByUnitImpulseN = 0; + if (b1.movable) { + cp.angularInertia1 = _v.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1).vDot(contact.normal); + cp.velByUnitImpulseN += b1.invMass + cp.angularInertia1; + } + if (b2 != null && b2.movable) { + cp.angularInertia2 = _v.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2).vDot(contact.normal); + cp.velByUnitImpulseN += b2.invMass + cp.angularInertia2; + } + // Расчёт требуемой конечной скорости для упругого контакта + calcSepVelocity(b1, b2, cp, _v); + cp.normalVel = _v.vDot(contact.normal); + if (cp.normalVel < 0) cp.normalVel = - contact.restitution*cp.normalVel; + // Скорость разделения неупругого контакта + cp.minSepVel = cp.penetration > allowedPenetration ? (cp.penetration - allowedPenetration)/(penResolutionSteps*dt) : 0; + if (cp.minSepVel > maxPenResolutionSpeed) cp.minSepVel = maxPenResolutionSpeed; + } + contact = contact.next; + } + for (var i:int = 0; i < constraintsNum; i++) { + var constraint:Constraint = constraints[i]; + constraint.preProcess(dt); + } + } + + /** + * + * @param dt + * @param forceInelastic + */ + private function processContacts(dt:Number, forceInelastic:Boolean):void { + var iterNum:int = forceInelastic ? contactIterations : collisionIterations; + var i:int; + var forwardLoop:Boolean = false; + for (var iter:int = 0; iter < iterNum; iter++) { + forwardLoop = !forwardLoop; + var contact:Contact = contacts; + while (contact != borderContact) { + resolveContact(contact, forceInelastic, forwardLoop); + contact = contact.next; + } + // Ограничения + for (i = 0; i < constraintsNum; i++) { + var constraint:Constraint = constraints[i]; + constraint.apply(dt); + } + } + + // Разделение контактов путём непосредственного изменения координат и ориентации. Экспериментально. +// if (forceInelastic && useStaticSeparation) performStaticSeparation(); + } + + /** + * + */ + private function resolveContact(contactInfo:Contact, forceInelastic:Boolean, forwardLoop:Boolean):void { + var b1:Body = contactInfo.body1; + var b2:Body = contactInfo.body2; + var normal:Vector3 = contactInfo.normal; + var i:int; + if (forwardLoop) { + for (i = 0; i < contactInfo.pcount; i++) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } else { + for (i = contactInfo.pcount - 1; i >= 0; i--) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } + } + + /** + * + * @param idx + * @param b1 + * @param b2 + * @param colInfo + * @param normal + * @param forceInelastic + */ + private function resolveContactPoint(idx:int, b1:Body, b2:Body, contact:Contact, normal:Vector3, forceInelastic:Boolean):void { + var cp:ContactPoint = contact.points[idx]; + if (!forceInelastic) { + cp.satisfied = true; + } + + var newVel:Number = 0; + calcSepVelocity(b1, b2, cp, _v); + var cnormal:Vector3 = contact.normal; + var sepVel:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + if (forceInelastic) { + var minSpeVel:Number = useStaticSeparation ? 0 : cp.minSepVel; + if (sepVel < minSpeVel) { + cp.satisfied = false; + } else if (cp.satisfied) return; + newVel = minSpeVel; + } else { + newVel = cp.normalVel; + } + var deltaVel:Number = newVel - sepVel; + var impulse:Number = deltaVel/cp.velByUnitImpulseN; + var accumImpulse:Number = cp.accumImpulseN + impulse; + if (accumImpulse < 0) { + accumImpulse = 0; + } + var deltaImpulse:Number = accumImpulse - cp.accumImpulseN; + cp.accumImpulseN = accumImpulse; + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, normal, deltaImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, normal, -deltaImpulse); + } + + // Учёт силы трения + calcSepVelocity(b1, b2, cp, _v); + // Расчитываем изменение касательной скорости на единицу касательного импульса + var tanSpeedByUnitImpulse:Number = 0; + +// _v.vAddScaled(-_v.vDot(contact.normal), contact.normal); + var dot:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + _v.x -= dot*cnormal.x; + _v.y -= dot*cnormal.y; + _v.z -= dot*cnormal.z; + + var tanSpeed:Number = _v.vLength(); + if (tanSpeed < 0.001) return; + +// _t.vCopy(_v).vNormalize().vReverse(); + _t.x = -_v.x; + _t.y = -_v.y; + _t.z = -_v.z; + _t.vNormalize(); + + var r:Vector3; + var m:Matrix3; + var xx:Number; + var yy:Number; + var zz:Number; + // dV = b.invMass + ((invI * (r % t)) % r) * t + if (b1.movable) { +// _v.vCross2(cp.r1, _t).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1); + r = cp.r1; + m = b1.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b1.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + if (b2 != null && b2.movable) { +// _v.vCross2(cp.r2, _t).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2); + + r = cp.r2; + m = b2.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b2.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + + var tanImpulse:Number = tanSpeed/tanSpeedByUnitImpulse; + var max:Number = contact.friction*cp.accumImpulseN; + if (max < 0) { + if (tanImpulse < max) tanImpulse = max; + } else { + if (tanImpulse > max) tanImpulse = max; + } + + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, _t, tanImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, _t, -tanImpulse); + } + } + + /** + * + * @param cp + * @param normal + * @return + */ + private function calcSepVelocity(body1:Body, body2:Body, cp:ContactPoint, result:Vector3):void { + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 +// result.vCopy(body1.state.velocity).vAdd(_v1.vCross2(body1.state.rotation, cp.r1)); + var rot:Vector3 = body1.state.rotation; + var v:Vector3 = cp.r1; + var x:Number = rot.y*v.z - rot.z*v.y; + var y:Number = rot.z*v.x - rot.x*v.z; + var z:Number = rot.x*v.y - rot.y*v.x; + v = body1.state.velocity; + result.x = v.x + x; + result.y = v.y + y; + result.z = v.z + z; + // V2 = V2_c + w2%r2 + if (body2 != null) { +// result.vSubtract(body2.state.velocity).vSubtract(_v2.vCross2(body2.state.rotation, cp.r2)); + rot = body2.state.rotation; + v = cp.r2; + x = rot.y*v.z - rot.z*v.y; + y = rot.z*v.x - rot.x*v.z; + z = rot.x*v.y - rot.y*v.x; + v = body2.state.velocity; + result.x -= v.x + x; + result.y -= v.y + y; + result.z -= v.z + z; + } + } + + /** + * + * @param dt + */ + private function intergateVelocities(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + item.body.integrateVelocity(dt); + item = item.next; + } + } + + /** + * + * @param dt + */ + private function integratePositions(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + if (body.movable && !body.frozen) { + body.integratePosition(dt); + } + item = item.next; + } + } + + /** + * + */ + private function performStaticSeparation():void { + var iterNum:int = staticSeparationIterations; +// iterNum = 100; + + // 1. В начале каждой итерации для всех контактов сбрасывается флаг satisfied с одновременным поиском наихудшего контакта, т.е. имеющего наибольшую величину пересечения тел. + // 2. Если найденный контакт имеет величину пересечения меньше предельно допустимой процедура прерывается, т.к. разделение контактов не требуется. + // 3. Если процедура продолжается, то выполняется разделение найденного контакта и установка его флага satisfied в true, после чего запускается внутренний цикл, состоящий + // из contactsNum - 1 итераций. На каждой итерации ищется наихудший контакт среди оставшихся (satisfied == false). Если такой контакт найден, то выполняется его разделение + // и переход к следующей итерации внутреннего цикла, иначе внутренний цикл прерывается и выполняется переход к следующей итерации внешнего цикла (шаг 1.). +// for (var iter:int = 0; iter < iterNum; iter++) { +// // Ищем контакт с максимальной величиной пересечения тел, одновременно сбрасывая флаги satisfied в false +// var worstContact:Contact = contacts[0]; +// var i:int; +// for (i = 1; i < contactsNum; i++) { +// var contact:Contact = contacts[i]; +// contact.satisfied = false; +// if (contact.maxPenetration > worstContact.maxPenetration) worstContact = contact; +// } +// if (worstContact.maxPenetration <= allowedPenetration) return; +// +// resolveInterpenetration(worstContact); +// // Внутренний цикл по оставшимся контактам +// for (i = 1; i < contactsNum; i++) { +// worstContact = getWorstContact(); +// if (worstContact == null) break; +// resolveInterpenetration(worstContact); +// } +// } + } + + /** + * + */ +// private function getWorstContact():Contact { +// var maxPen:Number = 0; +// var worst:Contact = null; +// for (var i:int = 0; i < contactsNum; i++) { +// var c:Contact = contacts[i]; +// if (!c.satisfied && c.maxPenetration > maxPen) { +// worst = c; +// maxPen = c.maxPenetration; +// } +// } +// return maxPen > allowedPenetration ? worst : null; +// } + + /** + * Разделяет указанный контакт, выполняя staticSeparationSteps итераций по списку точек контакта. + * 1. В начале каждой итерации ищется точка с наибольшим пересечением, одновременно сбрасываются флаги satisfied у точек. + * 2. Для найденной точки выполняется процедура разделения. + * 3. После выполняется pcount - 1 итерация по списку точек, каждый раз ищется наихудшая среди имеющих satisfied == false, для которой выполняется процедура разделения. + * Если наихудшая точка не найдена, то выполнется переход к следующей итерации внешнего цикла (шаг 1). + * + * В конце процедуры разделения обновляются значения пересечений для остальных точек контакта, а также для всех прочих контактов, относящихся к телам текущего. + * + * @param contact контакт для разделения + */ + private function resolveInterpenetration(contact:Contact):void { + contact.satisfied = true; + + for (var step:int = 0; step < staticSeparationSteps; step++) { + var worstCp:ContactPoint = contact.points[0]; + var cp:ContactPoint; + var i:int; + for (i = 1; i < contact.pcount; i++) { + cp = contact.points[i]; + cp.satisfied = false; + if (cp.penetration > worstCp.penetration) worstCp = cp; + } + if (worstCp.penetration <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + // Разделяем оставшиеся точки + var maxPen:Number = 0; + for (i = 1; i < contact.pcount; i++) { + // Поиск наихудшей точки + for (var j:int = 0; j < contact.pcount; j++) { + cp = contact.points[j]; + if (cp.satisfied) continue; + if (cp.penetration > maxPen) { + maxPen = cp.penetration; + worstCp = cp; + } + } + if (maxPen <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + } + } + } + + /** + * + * @param cp + * @param contact + */ + private function separateContactPoint(cp:ContactPoint, contact:Contact):void { +// cp.satisfied = true; +// +// var b1:Body = contact.body1; +// var b2:Body = contact.body2; +// var totalMove:Number = cp.penetration - allowedPenetration; +// var moveCoeff:Number = totalMove/cp.velByUnitImpulseN; +// var linMove1:Number; +// var angleMove1:Number; +// if (b1.movable) { +// linMove1 = b1.invMass*moveCoeff; +// angleMove1 = cp.angularInertia1*moveCoeff; +// if (angleMove1 > maxAngleMove) { +// linMove1 += angleMove1 - maxAngleMove; +// angleMove1 -= maxAngleMove; +// } +// b1.state.pos.vAddScaled(linMove1, contact.normal); +// _v1.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vScale(angleMove1); +// b1.state.orientation.addScaledVector(_v1, 1); +// } +// var linMove2:Number; +// var angleMove2:Number; +// if (b2 != null && b2.movable) { +// linMove2 = b2.invMass*moveCoeff; +// angleMove2 = cp.angularInertia2*moveCoeff; +// if (angleMove2 > maxAngleMove) { +// linMove2 += angleMove2 - maxAngleMove; +// angleMove2 -= maxAngleMove; +// } +// b2.state.pos.vAddScaled(-linMove2, contact.normal); +// _v2.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vScale(angleMove2); +// _v2.vReverse(); +// b2.state.orientation.addScaledVector(_v2, 1); +// } +// cp.penetration = allowedPenetration; +// // Обновляем пересечения в других точках +// var i:int; +// for (i = 0; i < contact.pcount; i++) { +// var cp1:ContactPoint = contact.points[i]; +// if (cp1 == cp) continue; +// var angularMove:Number; +// if (b1.movable) { +// angularMove = _v.vCross2(_v1, cp1.r1).vDot(contact.normal); +// cp1.penetration -= linMove1 + angularMove; +// } +// if (b2 != null && b2.movable) { +// angularMove = _v.vCross2(_v2, cp1.r2).vDot(contact.normal); +// cp1.penetration -= linMove2 - angularMove; +// } +// // Обновление максимального значения для контакта +// if (cp1.penetration > contact.maxPenetration) contact.maxPenetration = cp1.penetration; +// } +// // Обновляем пересечения для других контактов +// var c:Contact; +// var j:int; +// if (b1.movable) { +// for (i = 0; i < b1.contactsNum; i++) { +// c = b1.contacts[i]; +// if (c == contact) continue; +// for (j = 0; j < c.pcount; j++) { +// cp1 = c.points[j]; +// if (b1 == c.body1) cp1.penetration -= linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r1).vDot(c.normal); +// else cp1.penetration += linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r2).vDot(c.normal); +// if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; +// } +// } +// } +// if (b2 != null && b2.movable) { +// for (i = 0; i < b2.contactsNum; i++) { +// c = b2.contacts[i]; +// if (c == contact) continue; +// for (j = 0; j < c.pcount; j++) { +// cp1 = c.points[j]; +// if (b2 == c.body1) cp1.penetration -= linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r1).vDot(c.normal); +// else cp1.penetration += linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r2).vDot(c.normal); +// if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; +// } +// } +// } + } + + /** + * + */ + private function postPhysics():void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + body.clearAccumulators(); + body.calcDerivedData(); + if (body.canFreeze) { + if (body.state.velocity.vLength() < linSpeedFreezeLimit && body.state.rotation.vLength() < angSpeedFreezeLimit) { + if (!body.frozen) { + body.freezeCounter++; + if (body.freezeCounter >= freezeSteps) body.frozen = true; + } + } else { + body.freezeCounter = 0; + body.frozen = false; + } + } + item = item.next; + } + } + + /** + * + * @param delta + */ + public function update(delta:int):void { + timeStamp++; + time += delta; + var dt:Number = 0.001*delta; + applyForces(dt); + detectCollisions(dt); + preProcessContacts(dt); + processContacts(dt, false); + intergateVelocities(dt); + processContacts(dt, true); + integratePositions(dt); + postPhysics(); + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/.svn/text-base/PhysicsUtils.as.svn-base b/0.0.9.5/src/alternativa/physics/.svn/text-base/PhysicsUtils.as.svn-base new file mode 100644 index 0000000..5a2e46f --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/.svn/text-base/PhysicsUtils.as.svn-base @@ -0,0 +1,49 @@ +package alternativa.physics { + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + public class PhysicsUtils { + + /** + * @param mass + * @param halfSize + * @param result + */ + public static function getBoxInvInertia(mass:Number, halfSize:Vector3, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + result.a = 3/(mass*(yy + zz)); + result.f = 3/(mass*(zz + xx)); + result.k = 3/(mass*(xx + yy)); + } + + /** + * @param mass + * @param r + * @param h + * @param result + */ + public static function getCylinderInvInertia(mass:Number, r:Number, h:Number, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + + result.a = result.f = 1/(mass*(h*h/12 + r*r/4)); + result.k = 2/(mass*r*r); + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base b/0.0.9.5/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base new file mode 100644 index 0000000..2715307 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base @@ -0,0 +1,3 @@ +package alternativa.physics { + public namespace altphysics = "http://alternativaplatform.com/en/altphysics"; +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/Body.as b/0.0.9.5/src/alternativa/physics/Body.as new file mode 100644 index 0000000..cd22c5b --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/Body.as @@ -0,0 +1,405 @@ +package alternativa.physics { + + import alternativa.math.Matrix3; + import alternativa.math.Matrix4; + import alternativa.math.Quaternion; + import alternativa.math.Vector3; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.IBodyCollisionPredicate; + import alternativa.physics.collision.types.BoundBox; + + use namespace altphysics; + + /** + * + */ + public class Body { + + public static var linDamping:Number = 0.997; + public static var rotDamping:Number = 0.997; + + // Идентификатор тела, уникальный в пределах мира + public var id:int; + // Имя тела + public var name:String; + // Мир, в котором находится тело + public var world:PhysicsScene; + // Флаг подвижности тела + public var movable:Boolean = true; + // Флаг указывает, может ли тело быть заморожено + public var canFreeze:Boolean = false; + + public var freezeCounter:int; + public var frozen:Boolean = false; + // Ограничивающий бокс тела + public var aabb:BoundBox = new BoundBox(); + // Предикат, через который тело получает сообщения о столкновениях + public var postCollisionPredicate:IBodyCollisionPredicate; + + // Текущее состояние тела + public var state:BodyState = new BodyState(); + // Предыдущее состояние тела + public var prevState:BodyState = new BodyState(); + // Линейное ускорение тела на текущем шаге симуляции + public var accel:Vector3 = new Vector3(); + // Угловое ускорение тела на текущем шаге симуляции + public var angleAccel:Vector3 = new Vector3(); + // Физический материал тела + public var material:BodyMaterial = new BodyMaterial(); + // Обратная масса тела + public var invMass:Number = 1; + // Обратная матрица тензора инерции в локальных координатах + public var invInertia:Matrix3 = new Matrix3(); + // Обратная матрица тензора инерции в мировых координатах + public var invInertiaWorld:Matrix3 = new Matrix3(); + // Базисная матрица тела в мировых координатах + public var baseMatrix:Matrix3 = new Matrix3(); + + public const MAX_CONTACTS:int = 20; + public var contacts:Vector. = new Vector.(MAX_CONTACTS); + public var contactsNum:int; + + public var collisionPrimitives:CollisionPrimitiveList; + + // Аккумулятор сил + public var forceAccum:Vector3 = new Vector3(); + // Аккумулятор моментов + public var torqueAccum:Vector3 = new Vector3(); + + // Внутренние переменные для избежания создания экземпляров + private static var _r:Vector3 = new Vector3(); + private static var _f:Vector3 = new Vector3(); + + /** + * + * @param invMass + * @param invInertia + */ + public function Body(invMass:Number, invInertia:Matrix3) { + this.invMass = invMass; + this.invInertia.copy(invInertia); + } + + /** + * @param primitive + * @param localTransform + */ + public function addCollisionPrimitive(primitive:CollisionPrimitive, localTransform:Matrix4 = null):void { + if (primitive == null) { + throw new ArgumentError("Primitive cannot be null"); + } + if (collisionPrimitives == null) { + collisionPrimitives = new CollisionPrimitiveList(); + } + collisionPrimitives.append(primitive); + primitive.setBody(this, localTransform); + } + + /** + * + * @param primitive + */ + public function removeCollisionPrimitive(primitive:CollisionPrimitive):void { + if (collisionPrimitives == null) return; + primitive.setBody(null); + collisionPrimitives.remove(primitive); + if (collisionPrimitives.size == 0) { + collisionPrimitives = null; + } + } + + /** + * @param t + * @param result + */ + public function interpolate(t:Number, pos:Vector3, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.w = prevState.orientation.w*t1 + state.orientation.w*t; + orientation.x = prevState.orientation.x*t1 + state.orientation.x*t; + orientation.y = prevState.orientation.y*t1 + state.orientation.y*t; + orientation.z = prevState.orientation.z*t1 + state.orientation.z*t; + } + + /** + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + state.pos.vCopy(pos); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setPositionXYZ(x:Number, y:Number, z:Number):void { + state.pos.vReset(x, y, z); + } + + /** + * + * @param vel + */ + public function setVelocity(vel:Vector3):void { + state.velocity.vCopy(vel); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setVelocityXYZ(x:Number, y:Number, z:Number):void { + state.velocity.vReset(x, y, z); + } + + /** + * + * @param rot + */ + public function setRotation(rot:Vector3):void { + state.rotation.vCopy(rot); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setRotationXYZ(x:Number, y:Number, z:Number):void { + state.rotation.vReset(x, y, z); + } + + /** + * + * @param q + */ + public function setOrientation(q:Quaternion):void { + state.orientation.copy(q); + } + + /** + * @param r + * @param dir + * @param magnitude + */ + public function applyRelPosWorldImpulse(r:Vector3, dir:Vector3, magnitude:Number):void { + var d:Number = magnitude*invMass; + // Линейная часть + state.velocity.x += d*dir.x; + state.velocity.y += d*dir.y; + state.velocity.z += d*dir.z; + + // Вращательная часть + var x:Number = (r.y*dir.z - r.z*dir.y)*magnitude; + var y:Number = (r.z*dir.x - r.x*dir.z)*magnitude; + var z:Number = (r.x*dir.y - r.y*dir.x)*magnitude; + + state.rotation.x += invInertiaWorld.a*x + invInertiaWorld.b*y + invInertiaWorld.c*z; + state.rotation.y += invInertiaWorld.e*x + invInertiaWorld.f*y + invInertiaWorld.g*z; + state.rotation.z += invInertiaWorld.i*x + invInertiaWorld.j*y + invInertiaWorld.k*z; + } + + /** + * @param f + */ + public function addForce(f:Vector3):void { + forceAccum.vAdd(f); + } + + /** + * + * @param fx + * @param fy + * @param fz + */ + public function addForceXYZ(fx:Number, fy:Number, fz:Number):void { + forceAccum.x += fx; + forceAccum.y += fy; + forceAccum.z += fz; + } + + /** + * @param pos + * @param f + */ + public function addWorldForceXYZ(px:Number, py:Number, pz:Number, fx:Number, fy:Number, fz:Number):void { + forceAccum.x += fx; + forceAccum.y += fy; + forceAccum.z += fz; + + var pos:Vector3 = state.pos; + var rx:Number = px - pos.x; + var ry:Number = py - pos.y; + var rz:Number = pz - pos.z; + +// var x:Number = ry*fz - rz*fy; +// var y:Number = rz*fx - rx*fz; +// var z:Number = rx*fy - ry*fx; + + torqueAccum.x += ry*fz - rz*fy; + torqueAccum.y += rz*fx - rx*fz; + torqueAccum.z += rx*fy - ry*fx; + } + + /** + * @param pos + * @param f + */ + public function addWorldForce(pos:Vector3, force:Vector3):void { + forceAccum.vAdd(force); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(force)); + } + + /** + * @param pos + * @param f + */ + public function addWorldForceScaled(pos:Vector3, force:Vector3, scale:Number):void { + _f.x = scale*force.x; + _f.y = scale*force.y; + _f.z = scale*force.z; + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(_f)); + } + + /** + * @param pos + * @param f + */ + public function addLocalForce(pos:Vector3, force:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(pos, _r); + // Трансформируем вектор силы в мировую систему + baseMatrix.transformVector(force, _f); + // Добавляем силу и момент + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vCross(_f)); + } + + /** + * + * @param localPos + * @param worldForce + */ + public function addWorldForceAtLocalPoint(localPos:Vector3, worldForce:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(localPos, _r); + // Добавляем силу и момент + forceAccum.vAdd(worldForce); + torqueAccum.vAdd(_r.vCross(worldForce)); + } + + /** + * @param dt + */ + public function beforePhysicsStep(dt:Number):void { + } + + /** + * @param t + */ + public function addTorque(t:Vector3):void { + torqueAccum.vAdd(t); + } + + /** + * + */ + altphysics function clearAccumulators():void { + forceAccum.x = forceAccum.y = forceAccum.z = 0; + torqueAccum.x = torqueAccum.y = torqueAccum.z = 0; + } + + /** + * + */ + altphysics function calcAccelerations():void { + accel.x = forceAccum.x*invMass; + accel.y = forceAccum.y*invMass; + accel.z = forceAccum.z*invMass; + angleAccel.x = invInertiaWorld.a*torqueAccum.x + invInertiaWorld.b*torqueAccum.y + invInertiaWorld.c*torqueAccum.z; + angleAccel.y = invInertiaWorld.e*torqueAccum.x + invInertiaWorld.f*torqueAccum.y + invInertiaWorld.g*torqueAccum.z; + angleAccel.z = invInertiaWorld.i*torqueAccum.x + invInertiaWorld.j*torqueAccum.y + invInertiaWorld.k*torqueAccum.z; + } + + /** + * Вычисляет производные данные. + */ + public function calcDerivedData():void { + // Вычисление базисной матрицы и обратного тензора инерции в мировых координатах + state.orientation.toMatrix3(baseMatrix); + invInertiaWorld.copy(invInertia).append(baseMatrix).prependTransposed(baseMatrix); + if (collisionPrimitives != null) { + aabb.infinity(); + var item:CollisionPrimitiveListItem = collisionPrimitives.head; + while (item != null) { + var primitive:CollisionPrimitive = item.primitive; + primitive.transform.setFromMatrix3(baseMatrix, state.pos); + if (primitive.localTransform != null) { + primitive.transform.prepend(primitive.localTransform); + } + primitive.calculateAABB(); + aabb.addBoundBox(primitive.aabb); + item = item.next; + } + } + } + + /** + * + */ + altphysics function saveState():void { + prevState.copy(state); + } + + /** + * + */ + altphysics function restoreState():void { + state.copy(prevState); + } + + /** + * @param dt + */ + altphysics function integrateVelocity(dt:Number):void { + // v = v + a*t + state.velocity.x += accel.x*dt; + state.velocity.y += accel.y*dt; + state.velocity.z += accel.z*dt; + // rot = rot + eps*t + state.rotation.x += angleAccel.x*dt; + state.rotation.y += angleAccel.y*dt; + state.rotation.z += angleAccel.z*dt; + + state.velocity.x *= linDamping; + state.velocity.y *= linDamping; + state.velocity.z *= linDamping; + + state.rotation.x *= rotDamping; + state.rotation.y *= rotDamping; + state.rotation.z *= rotDamping; + } + + /** + * + */ + altphysics function integratePosition(dt:Number):void { + // pos = pos + v*t + state.pos.x += state.velocity.x*dt; + state.pos.y += state.velocity.y*dt; + state.pos.z += state.velocity.z*dt; + // q = q + 0.5*rot*q + state.orientation.addScaledVector(state.rotation, dt); + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/BodyList.as b/0.0.9.5/src/alternativa/physics/BodyList.as new file mode 100644 index 0000000..aa8f79f --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/BodyList.as @@ -0,0 +1,76 @@ +package alternativa.physics { + + /** + * + */ + public class BodyList { + + public var head:BodyListItem; + public var tail:BodyListItem; + public var size:int; + + /** + * + */ + public function BodyList() { + } + + /** + * + * @param body + */ + public function append(body:Body):void { + var item:BodyListItem = BodyListItem.create(body); + if (head == null) { + head = tail = item; + } else { + tail.next = item; + item.prev = tail; + tail = item; + } + size++; + } + + /** + * + * @param body + */ + public function remove(body:Body):Boolean { + var item:BodyListItem = findItem(body); + if (item == null) return false; + if (item == head) { + if (size == 1) { + head = tail = null; + } else { + head = item.next; + head.prev = null; + } + } else { + if (item == tail) { + tail = item.prev; + tail.next = null; + } else { + item.prev.next = item.next; + item.next.prev = item.prev; + } + } + item.dispose(); + size--; + return true; + } + + /** + * + * @param body + * @return + */ + public function findItem(body:Body):BodyListItem { + var item:BodyListItem = head; + while (item != null && item.body != body) { + item = item.next; + } + return item; + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/BodyListItem.as b/0.0.9.5/src/alternativa/physics/BodyListItem.as new file mode 100644 index 0000000..382e11c --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/BodyListItem.as @@ -0,0 +1,74 @@ +package alternativa.physics { + + /** + * + */ + public class BodyListItem { + + // Верхний элемент хранилища + private static var poolTop:BodyListItem; + + /** + * Создаёт новый элемент списка. + * + * @param primitive примитив, содержащийся в элементе + * @return новый элемент списка + */ + public static function create(body:Body):BodyListItem { + var item:BodyListItem; + if (poolTop == null) { + item = new BodyListItem(body); + } else { + item = poolTop; + poolTop = item.next; + item.next = null; + item.body = body; + } + return item; + } + + /** + * Очищает хранилище. + */ + public static function clearPool():void { + var item:BodyListItem = poolTop; + while (item != null) { + poolTop = item.next; + item.next = null; + item = poolTop; + } + } + + /** + * + */ + public var body:Body; + /** + * + */ + public var next:BodyListItem; + /** + * + */ + public var prev:BodyListItem; + + /** + * + * @param body + */ + public function BodyListItem(body:Body) { + this.body = body; + } + + /** + * + */ + public function dispose():void { + body = null; + prev = null; + next = poolTop; + poolTop = this; + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/BodyMaterial.as b/0.0.9.5/src/alternativa/physics/BodyMaterial.as new file mode 100644 index 0000000..7a7c47d --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/BodyMaterial.as @@ -0,0 +1,11 @@ +package alternativa.physics { + + public class BodyMaterial { + + public var restitution:Number = 0; + public var friction:Number = 0.3; +// public var dynamicFriction:Number = 0.2; +// public var dynamicFriction:Number = 0.2; + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/BodyState.as b/0.0.9.5/src/alternativa/physics/BodyState.as new file mode 100644 index 0000000..3987388 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/BodyState.as @@ -0,0 +1,39 @@ +package alternativa.physics { + import alternativa.math.Quaternion; + import alternativa.math.Vector3; + + /** + * Класс описывает состояние твёрдого тела. + */ + public class BodyState { + /** + * Положение тела. + */ + public var pos:Vector3 = new Vector3(); + /** + * Ориентация тела. + */ + public var orientation:Quaternion = new Quaternion(); + /** + * Скорость тела. + */ + public var velocity:Vector3 = new Vector3(); + /** + * Угловая скорость тела. + */ + public var rotation:Vector3 = new Vector3(); + + /** + * Копирует значение указанного объекта. + * + * @param state + */ + public function copy(state:BodyState):void { + pos.vCopy(state.pos); + orientation.copy(state.orientation); + velocity.vCopy(state.velocity); + rotation.vCopy(state.rotation); + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/CollisionPrimitiveList.as b/0.0.9.5/src/alternativa/physics/CollisionPrimitiveList.as new file mode 100644 index 0000000..5d5304f --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/CollisionPrimitiveList.as @@ -0,0 +1,89 @@ +package alternativa.physics { + import alternativa.physics.collision.CollisionPrimitive; + + /** + * + */ + public class CollisionPrimitiveList { + + public var head:CollisionPrimitiveListItem; + public var tail:CollisionPrimitiveListItem; + public var size:int; + + /** + * + */ + public function CollisionPrimitiveList() { + } + + /** + * + * @param primitive + */ + public function append(primitive:CollisionPrimitive):void { + var item:CollisionPrimitiveListItem = CollisionPrimitiveListItem.create(primitive); + if (head == null) { + head = tail = item; + } else { + tail.next = item; + item.prev = tail; + tail = item; + } + size++; + } + + /** + * + * @param primitve + */ + public function remove(primitve:CollisionPrimitive):void { + var item:CollisionPrimitiveListItem = findItem(primitve); + if (item == null) return; + if (item == head) { + if (size == 1) { + head = tail = null; + } else { + head = item.next; + head.prev = null; + } + } else { + if (item == tail) { + tail = tail.prev; + tail.next = null; + } else { + item.prev.next = item.next; + item.next.prev = item.prev; + } + } + item.dispose(); + size--; + } + + /** + * + * @param primitive + * @return + */ + public function findItem(primitive:CollisionPrimitive):CollisionPrimitiveListItem { + var item:CollisionPrimitiveListItem = head; + while (item != null && item.primitive != primitive) { + item = item.next; + } + return item; + } + + /** + * + */ + public function clear():void { + while (head != null) { + var item:CollisionPrimitiveListItem = head; + head = head.next; + item.dispose(); + } + tail = null; + size = 0; + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/CollisionPrimitiveListItem.as b/0.0.9.5/src/alternativa/physics/CollisionPrimitiveListItem.as new file mode 100644 index 0000000..d204532 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/CollisionPrimitiveListItem.as @@ -0,0 +1,76 @@ +package alternativa.physics { + import alternativa.physics.collision.CollisionPrimitive; + + /** + * Элемент списка примитивов. + */ + public class CollisionPrimitiveListItem { + + // Верхний элемент хранилища + private static var poolTop:CollisionPrimitiveListItem; + + /** + * Создаёт новый элемент списка. + * + * @param primitive примитив, содержащийся в элементе + * @return новый элемент списка + */ + public static function create(primitive:CollisionPrimitive):CollisionPrimitiveListItem { + var item:CollisionPrimitiveListItem; + if (poolTop == null) { + item = new CollisionPrimitiveListItem(primitive); + } else { + item = poolTop; + item.primitive = primitive; + poolTop = item.next; + item.next = null; + } + return item; + } + + /** + * Очищает хранилище. + */ + public static function clearPool():void { + var curr:CollisionPrimitiveListItem = poolTop; + while (curr != null) { + poolTop = curr.next; + curr.next = null; + curr = poolTop; + } + } + + /** + * Примитив, хранящийся в элементе списка. + */ + public var primitive:CollisionPrimitive; + /** + * Ссылка не следующий элемент списка. + */ + public var next:CollisionPrimitiveListItem; + /** + * Ссылка не предыдущий элемент списка. + */ + public var prev:CollisionPrimitiveListItem; + + /** + * Создаёт новый экземпляр. + * + * @param primitive примитив, хранящийся в элементе списка + */ + public function CollisionPrimitiveListItem(primitive:CollisionPrimitive) { + this.primitive = primitive; + } + + /** + * Очищает внутренние ссылки и помещает элемент в хранилище для дальнейшего использования. + */ + public function dispose():void { + primitive = null; + prev = null; + next = poolTop; + poolTop = this; + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/Contact.as b/0.0.9.5/src/alternativa/physics/Contact.as new file mode 100644 index 0000000..3f010ec --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/Contact.as @@ -0,0 +1,69 @@ +package alternativa.physics { + + + import alternativa.math.Vector3; + + /** + * Описывает контакт одного или двух тел. + */ + public class Contact { + /** + * Первое тело контакта, не может быть равно null. + */ + public var body1:Body; + /** + * Второе тело контакта, может быть равно null. + */ + public var body2:Body; + /** + * Предрассчитанный взаимный коэффициент отскока. + */ + public var restitution:Number; + /** + * Предрассчитанный взаимный коэффициент трения. + */ + public var friction:Number; + /** + * Нормаль контакта. Направлена к первому телу. + */ + public var normal:Vector3 = new Vector3(); + /** + * Список точек контакта. + */ + public var points:Vector. = new Vector.(MAX_POINTS, true); + /** + * Количество точек контакта. + */ + public var pcount:int; + /** + * Максимальная глубина пересечения. + */ + public var maxPenetration:Number = 0; + /** + * Флаг показывает, разрешён контакт или нет. + */ + public var satisfied:Boolean; + /** + * Следующий контакт в списке. + */ + public var next:Contact; + /** + * Индекс контакта. Первый контакт в списке имеет индекс 0. + */ + public var index:int; + + // Максимальное количество точек контакта + private const MAX_POINTS:int = 8; + + /** + * + */ + public function Contact(index:int) { + this.index = index; + for (var i:int = 0; i < MAX_POINTS; i++) { + points[i] = new ContactPoint(); + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/ContactPoint.as b/0.0.9.5/src/alternativa/physics/ContactPoint.as new file mode 100644 index 0000000..395d2dc --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/ContactPoint.as @@ -0,0 +1,55 @@ +package alternativa.physics { + import alternativa.math.Vector3; + + /** + * + */ + public class ContactPoint { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + + // Величины, расчитываемые перед началом фазы решения контактов + + // Требуемая проекция конечной скорости на нормаль для упругого контакта + public var normalVel:Number; + // Минимальная скорость разделения неупругого контакта + public var minSepVel:Number; + // Изменение проекции скорости на единицу нормального импульса + public var velByUnitImpulseN:Number; + + public var angularInertia1:Number; + public var angularInertia2:Number; + + // Радиус-вектор точки контакта относительно центра первого тела + public var r1:Vector3 = new Vector3(); + // Радиус-вектор точки контакта относительно центра второго тела + public var r2:Vector3 = new Vector3(); + + // Величины, накапливаемые во время фазы решения контактов + + // Накопленный импульс, применяемый для получения требуемой относительной скорости в точке контакта + public var accumImpulseN:Number; + // Накопленный импульс, применяемый для разделения тел в точке контакта. Не создаёт момента. +// public var accumSepImpulse:Number; + + public var satisfied:Boolean; + + /** + * + * @param cp + */ + public function copyFrom(cp:ContactPoint):void { + pos.vCopy(cp.pos); + penetration = cp.penetration; + feature1 = cp.feature1; + feature2 = cp.feature2; + r1.vCopy(cp.r1); + r2.vCopy(cp.r2); + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/PhysicsScene.as b/0.0.9.5/src/alternativa/physics/PhysicsScene.as new file mode 100644 index 0000000..abdc762 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/PhysicsScene.as @@ -0,0 +1,743 @@ +package alternativa.physics { + + + import alternativa.physics.collision.ICollisionDetector; + import alternativa.physics.collision.KdTreeCollisionDetector; + import alternativa.physics.constraints.Constraint; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * Класс реализует физическую симуляцию поведения твёрдых тел. + */ + public class PhysicsScene { + + private static var lastBodyId:int; + + // Максимальное количество контактов + public const MAX_CONTACTS:int = 1000; + + ////////////////////////////////////////////////////////////////// + // Настроечные параметры симуляции. Могут быть изменены в любой + // момент времени без нарушения корректной работы симуляции. + ////////////////////////////////////////////////////////////////// + // Количество шагов, за которое пересекающиеся тела должны разделиться + public var penResolutionSteps:int = 10; + // Величина допустимой глубины пересечения + public var allowedPenetration:Number = 0.1; + // Максимальная скорость, добавляемая с целью разделения тел + public var maxPenResolutionSpeed:Number = 0.5; + // Количество итераций для обработки упругих контактов + public var collisionIterations:int = 5; + // Количество итераций для обработки неупругих контактов + public var contactIterations:int = 5; + // Флаг использования предсказания состояний + public var usePrediction:Boolean = false; + + public var freezeSteps:int = 10; + public var linSpeedFreezeLimit:Number = 1; + public var angSpeedFreezeLimit:Number = 0.01; + + // Переменные для процедуры разделения тел путём непосредственного их перемещенеия. + // !!!!!!!!!!!!!!!!!!!!!!!! + // !!! Экспериментально !!! + // !!!!!!!!!!!!!!!!!!!!!!!! + public var staticSeparationIterations:int = 10; + public var staticSeparationSteps:int = 10; + public var maxAngleMove:Number = 10; + public var useStaticSeparation:Boolean = false; + + // Вектор гравитации + public var _gravity:Vector3 = new Vector3(0, 0, -9.8); + // Модуль вектора гравитации + public var _gravityMagnitude:Number = 9.8; + + // Использующийся детектор столкновений + public var collisionDetector:ICollisionDetector; + + // Список тел, участвующих в симуляции + public var bodies:BodyList = new BodyList(); + + // Список контактов на текущем шаге симуляции + altphysics var contacts:Contact; + // Количество контактов на текущем шаге симуляции +// altphysics var contactsNum:int; + // Список ограничений + altphysics var constraints:Vector. = new Vector.(); + // Количество ограничений + altphysics var constraintsNum:int; + // Временная метка. Число прошедших шагов с начала симуляции. + public var timeStamp:int; + // Время с начала симуляции, мс + public var time:int; + // Первый неиспользованный контакт на текущем шаге симуляции + private var borderContact:Contact; + + // Временные переменные для избежания создания экземпляров + private var _r:Vector3 = new Vector3(); + private var _t:Vector3 = new Vector3(); + private var _v:Vector3 = new Vector3(); + private var _v1:Vector3 = new Vector3(); + private var _v2:Vector3 = new Vector3(); + + /** + * + */ + public function PhysicsScene() { + contacts = new Contact(0); + var contact:Contact = contacts; + for (var i:int = 1; i < MAX_CONTACTS; i++) { + contact.next = new Contact(i); + contact = contact.next; + } + collisionDetector = new KdTreeCollisionDetector(); + } + + /** + * Вектор гравитации. + */ + public function get gravity():Vector3 { + return _gravity.vClone(); + } + + /** + * @private + */ + public function set gravity(value:Vector3):void { + _gravity.vCopy(value); + _gravityMagnitude = _gravity.vLength(); + } + + /** + * Добавляет тело в симуляцию. + * + * @param body + */ + public function addBody(body:Body):void { + body.id = lastBodyId++; + body.world = this; + bodies.append(body); + } + + /** + * Удаляет тело из симуляции. + * @param body + * @return + */ + public function removeBody(body:Body):void { + if (bodies.remove(body)) { + body.world = null; + } + } + + /** + * Добавляет ограничение. + * @param c + */ + public function addConstraint(c:Constraint):void { + constraints[constraintsNum++] = c; + c.world = this; + } + + /** + * Удаляет ограничение. + * @param c + */ + public function removeConstraint(c:Constraint):Boolean { + var idx:int = constraints.indexOf(c); + if (idx < 0) return false; + constraints.splice(idx, 1) + constraintsNum--; + c.world = null; + return true; + } + + /** + * Применяет к телам действующте на них силы. Аккумуляторы сил и моментов тел очищаются после завершения шага + * симуляции, поэтому на момент вызова метода могут уже содержать некоторые значения. + * + * @param dt промежуток времени, в течении которого действуют силы + */ + private function applyForces(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + body.beforePhysicsStep(dt); + body.calcAccelerations(); + // Ускорение свободного падения применяется только к подвижным телам во избежание некорректного изменения + // фиктивной скорости неподвижных тел. + if (body.movable && !body.frozen) { + body.accel.x += _gravity.x; + body.accel.y += _gravity.y; + body.accel.z += _gravity.z; + } + item = item.next; + } + } + + /** + * Определяет все столкновения на текущем шаге симуляции и заполняет список получившихся контактов. + * + * @param dt длительность шага симуляции + */ + private function detectCollisions(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + if (!body.frozen) { + body.contactsNum = 0; + body.saveState(); + // При включённом режиме предсказания состояние тел интегрируется на один шаг вперёд + if (usePrediction) { + body.integrateVelocity(dt); + body.integratePosition(dt); + } + body.calcDerivedData(); + } + item = item.next; + } + + borderContact = collisionDetector.getAllContacts(contacts); + + // Расчёт относительных векторов точки контакта вынесен сюда из-за необходимости учитывать + // положение тел в предсказанном состоянии + var contact:Contact = contacts; + while (contact != borderContact) { + var b1:Body = contact.body1; + var b2:Body = contact.body2; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + var bPos:Vector3 = b1.state.pos; + cp.r1.x = cp.pos.x - bPos.x; + cp.r1.y = cp.pos.y - bPos.y; + cp.r1.z = cp.pos.z - bPos.z; + if (b2 != null) { + bPos = b2.state.pos; + cp.r2.x = cp.pos.x - bPos.x; + cp.r2.y = cp.pos.y - bPos.y; + cp.r2.z = cp.pos.z - bPos.z; + } + } + contact = contact.next; + } + + // Восстановление состояния тел + if (usePrediction) { + item = bodies.head; + while (item != null) { + body = item.body; + if (!body.frozen) { + body.restoreState(); + body.calcDerivedData(); + } + item = item.next; + } + } + } + + /** + * Подготваливает полученные из детектора столкновений контакты, расчитывая значения, не меняющиеся + * в ходе шага симуляции. + */ + private function preProcessContacts(dt:Number):void { + var contact:Contact = contacts; + while (contact != borderContact) { + var b1:Body = contact.body1; + var b2:Body = contact.body2; + // Столкнувшиеся тела размораживаются + if (b1.frozen) { + b1.frozen = false; + b1.freezeCounter = 0; + } + if (b2 != null && b2.frozen) { + b2.frozen = false; + b2.freezeCounter = 0; + } + contact.restitution = b1.material.restitution; + if (b2 != null && b2.material.restitution < contact.restitution) contact.restitution = b2.material.restitution; + contact.friction = b1.material.friction; + if (b2 != null && b2.material.friction < contact.friction) contact.friction = b2.material.friction; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.accumImpulseN = 0; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + cp.velByUnitImpulseN = 0; + if (b1.movable) { + cp.angularInertia1 = _v.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1).vDot(contact.normal); + cp.velByUnitImpulseN += b1.invMass + cp.angularInertia1; + } + if (b2 != null && b2.movable) { + cp.angularInertia2 = _v.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2).vDot(contact.normal); + cp.velByUnitImpulseN += b2.invMass + cp.angularInertia2; + } + // Расчёт требуемой конечной скорости для упругого контакта + calcSepVelocity(b1, b2, cp, _v); + cp.normalVel = _v.vDot(contact.normal); + if (cp.normalVel < 0) cp.normalVel = - contact.restitution*cp.normalVel; + // Скорость разделения неупругого контакта + cp.minSepVel = cp.penetration > allowedPenetration ? (cp.penetration - allowedPenetration)/(penResolutionSteps*dt) : 0; + if (cp.minSepVel > maxPenResolutionSpeed) cp.minSepVel = maxPenResolutionSpeed; + } + contact = contact.next; + } + for (var i:int = 0; i < constraintsNum; i++) { + var constraint:Constraint = constraints[i]; + constraint.preProcess(dt); + } + } + + /** + * + * @param dt + * @param forceInelastic + */ + private function processContacts(dt:Number, forceInelastic:Boolean):void { + var iterNum:int = forceInelastic ? contactIterations : collisionIterations; + var i:int; + var forwardLoop:Boolean = false; + for (var iter:int = 0; iter < iterNum; iter++) { + forwardLoop = !forwardLoop; + var contact:Contact = contacts; + while (contact != borderContact) { + resolveContact(contact, forceInelastic, forwardLoop); + contact = contact.next; + } + // Ограничения + for (i = 0; i < constraintsNum; i++) { + var constraint:Constraint = constraints[i]; + constraint.apply(dt); + } + } + + // Разделение контактов путём непосредственного изменения координат и ориентации. Экспериментально. +// if (forceInelastic && useStaticSeparation) performStaticSeparation(); + } + + /** + * + */ + private function resolveContact(contactInfo:Contact, forceInelastic:Boolean, forwardLoop:Boolean):void { + var b1:Body = contactInfo.body1; + var b2:Body = contactInfo.body2; + var normal:Vector3 = contactInfo.normal; + var i:int; + if (forwardLoop) { + for (i = 0; i < contactInfo.pcount; i++) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } else { + for (i = contactInfo.pcount - 1; i >= 0; i--) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } + } + + /** + * + * @param idx + * @param b1 + * @param b2 + * @param colInfo + * @param normal + * @param forceInelastic + */ + private function resolveContactPoint(idx:int, b1:Body, b2:Body, contact:Contact, normal:Vector3, forceInelastic:Boolean):void { + var cp:ContactPoint = contact.points[idx]; + if (!forceInelastic) { + cp.satisfied = true; + } + + var newVel:Number = 0; + calcSepVelocity(b1, b2, cp, _v); + var cnormal:Vector3 = contact.normal; + var sepVel:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + if (forceInelastic) { + var minSpeVel:Number = useStaticSeparation ? 0 : cp.minSepVel; + if (sepVel < minSpeVel) { + cp.satisfied = false; + } else if (cp.satisfied) return; + newVel = minSpeVel; + } else { + newVel = cp.normalVel; + } + var deltaVel:Number = newVel - sepVel; + var impulse:Number = deltaVel/cp.velByUnitImpulseN; + var accumImpulse:Number = cp.accumImpulseN + impulse; + if (accumImpulse < 0) { + accumImpulse = 0; + } + var deltaImpulse:Number = accumImpulse - cp.accumImpulseN; + cp.accumImpulseN = accumImpulse; + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, normal, deltaImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, normal, -deltaImpulse); + } + + // Учёт силы трения + calcSepVelocity(b1, b2, cp, _v); + // Расчитываем изменение касательной скорости на единицу касательного импульса + var tanSpeedByUnitImpulse:Number = 0; + +// _v.vAddScaled(-_v.vDot(contact.normal), contact.normal); + var dot:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + _v.x -= dot*cnormal.x; + _v.y -= dot*cnormal.y; + _v.z -= dot*cnormal.z; + + var tanSpeed:Number = _v.vLength(); + if (tanSpeed < 0.001) return; + +// _t.vCopy(_v).vNormalize().vReverse(); + _t.x = -_v.x; + _t.y = -_v.y; + _t.z = -_v.z; + _t.vNormalize(); + + var r:Vector3; + var m:Matrix3; + var xx:Number; + var yy:Number; + var zz:Number; + // dV = b.invMass + ((invI * (r % t)) % r) * t + if (b1.movable) { +// _v.vCross2(cp.r1, _t).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1); + r = cp.r1; + m = b1.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b1.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + if (b2 != null && b2.movable) { +// _v.vCross2(cp.r2, _t).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2); + + r = cp.r2; + m = b2.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b2.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + + var tanImpulse:Number = tanSpeed/tanSpeedByUnitImpulse; + var max:Number = contact.friction*cp.accumImpulseN; + if (max < 0) { + if (tanImpulse < max) tanImpulse = max; + } else { + if (tanImpulse > max) tanImpulse = max; + } + + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, _t, tanImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, _t, -tanImpulse); + } + } + + /** + * + * @param cp + * @param normal + * @return + */ + private function calcSepVelocity(body1:Body, body2:Body, cp:ContactPoint, result:Vector3):void { + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 +// result.vCopy(body1.state.velocity).vAdd(_v1.vCross2(body1.state.rotation, cp.r1)); + var rot:Vector3 = body1.state.rotation; + var v:Vector3 = cp.r1; + var x:Number = rot.y*v.z - rot.z*v.y; + var y:Number = rot.z*v.x - rot.x*v.z; + var z:Number = rot.x*v.y - rot.y*v.x; + v = body1.state.velocity; + result.x = v.x + x; + result.y = v.y + y; + result.z = v.z + z; + // V2 = V2_c + w2%r2 + if (body2 != null) { +// result.vSubtract(body2.state.velocity).vSubtract(_v2.vCross2(body2.state.rotation, cp.r2)); + rot = body2.state.rotation; + v = cp.r2; + x = rot.y*v.z - rot.z*v.y; + y = rot.z*v.x - rot.x*v.z; + z = rot.x*v.y - rot.y*v.x; + v = body2.state.velocity; + result.x -= v.x + x; + result.y -= v.y + y; + result.z -= v.z + z; + } + } + + /** + * + * @param dt + */ + private function intergateVelocities(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + item.body.integrateVelocity(dt); + item = item.next; + } + } + + /** + * + * @param dt + */ + private function integratePositions(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + if (body.movable && !body.frozen) { + body.integratePosition(dt); + } + item = item.next; + } + } + + /** + * + */ + private function performStaticSeparation():void { + var iterNum:int = staticSeparationIterations; +// iterNum = 100; + + // 1. В начале каждой итерации для всех контактов сбрасывается флаг satisfied с одновременным поиском наихудшего контакта, т.е. имеющего наибольшую величину пересечения тел. + // 2. Если найденный контакт имеет величину пересечения меньше предельно допустимой процедура прерывается, т.к. разделение контактов не требуется. + // 3. Если процедура продолжается, то выполняется разделение найденного контакта и установка его флага satisfied в true, после чего запускается внутренний цикл, состоящий + // из contactsNum - 1 итераций. На каждой итерации ищется наихудший контакт среди оставшихся (satisfied == false). Если такой контакт найден, то выполняется его разделение + // и переход к следующей итерации внутреннего цикла, иначе внутренний цикл прерывается и выполняется переход к следующей итерации внешнего цикла (шаг 1.). +// for (var iter:int = 0; iter < iterNum; iter++) { +// // Ищем контакт с максимальной величиной пересечения тел, одновременно сбрасывая флаги satisfied в false +// var worstContact:Contact = contacts[0]; +// var i:int; +// for (i = 1; i < contactsNum; i++) { +// var contact:Contact = contacts[i]; +// contact.satisfied = false; +// if (contact.maxPenetration > worstContact.maxPenetration) worstContact = contact; +// } +// if (worstContact.maxPenetration <= allowedPenetration) return; +// +// resolveInterpenetration(worstContact); +// // Внутренний цикл по оставшимся контактам +// for (i = 1; i < contactsNum; i++) { +// worstContact = getWorstContact(); +// if (worstContact == null) break; +// resolveInterpenetration(worstContact); +// } +// } + } + + /** + * + */ +// private function getWorstContact():Contact { +// var maxPen:Number = 0; +// var worst:Contact = null; +// for (var i:int = 0; i < contactsNum; i++) { +// var c:Contact = contacts[i]; +// if (!c.satisfied && c.maxPenetration > maxPen) { +// worst = c; +// maxPen = c.maxPenetration; +// } +// } +// return maxPen > allowedPenetration ? worst : null; +// } + + /** + * Разделяет указанный контакт, выполняя staticSeparationSteps итераций по списку точек контакта. + * 1. В начале каждой итерации ищется точка с наибольшим пересечением, одновременно сбрасываются флаги satisfied у точек. + * 2. Для найденной точки выполняется процедура разделения. + * 3. После выполняется pcount - 1 итерация по списку точек, каждый раз ищется наихудшая среди имеющих satisfied == false, для которой выполняется процедура разделения. + * Если наихудшая точка не найдена, то выполнется переход к следующей итерации внешнего цикла (шаг 1). + * + * В конце процедуры разделения обновляются значения пересечений для остальных точек контакта, а также для всех прочих контактов, относящихся к телам текущего. + * + * @param contact контакт для разделения + */ + private function resolveInterpenetration(contact:Contact):void { + contact.satisfied = true; + + for (var step:int = 0; step < staticSeparationSteps; step++) { + var worstCp:ContactPoint = contact.points[0]; + var cp:ContactPoint; + var i:int; + for (i = 1; i < contact.pcount; i++) { + cp = contact.points[i]; + cp.satisfied = false; + if (cp.penetration > worstCp.penetration) worstCp = cp; + } + if (worstCp.penetration <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + // Разделяем оставшиеся точки + var maxPen:Number = 0; + for (i = 1; i < contact.pcount; i++) { + // Поиск наихудшей точки + for (var j:int = 0; j < contact.pcount; j++) { + cp = contact.points[j]; + if (cp.satisfied) continue; + if (cp.penetration > maxPen) { + maxPen = cp.penetration; + worstCp = cp; + } + } + if (maxPen <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + } + } + } + + /** + * + * @param cp + * @param contact + */ + private function separateContactPoint(cp:ContactPoint, contact:Contact):void { +// cp.satisfied = true; +// +// var b1:Body = contact.body1; +// var b2:Body = contact.body2; +// var totalMove:Number = cp.penetration - allowedPenetration; +// var moveCoeff:Number = totalMove/cp.velByUnitImpulseN; +// var linMove1:Number; +// var angleMove1:Number; +// if (b1.movable) { +// linMove1 = b1.invMass*moveCoeff; +// angleMove1 = cp.angularInertia1*moveCoeff; +// if (angleMove1 > maxAngleMove) { +// linMove1 += angleMove1 - maxAngleMove; +// angleMove1 -= maxAngleMove; +// } +// b1.state.pos.vAddScaled(linMove1, contact.normal); +// _v1.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vScale(angleMove1); +// b1.state.orientation.addScaledVector(_v1, 1); +// } +// var linMove2:Number; +// var angleMove2:Number; +// if (b2 != null && b2.movable) { +// linMove2 = b2.invMass*moveCoeff; +// angleMove2 = cp.angularInertia2*moveCoeff; +// if (angleMove2 > maxAngleMove) { +// linMove2 += angleMove2 - maxAngleMove; +// angleMove2 -= maxAngleMove; +// } +// b2.state.pos.vAddScaled(-linMove2, contact.normal); +// _v2.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vScale(angleMove2); +// _v2.vReverse(); +// b2.state.orientation.addScaledVector(_v2, 1); +// } +// cp.penetration = allowedPenetration; +// // Обновляем пересечения в других точках +// var i:int; +// for (i = 0; i < contact.pcount; i++) { +// var cp1:ContactPoint = contact.points[i]; +// if (cp1 == cp) continue; +// var angularMove:Number; +// if (b1.movable) { +// angularMove = _v.vCross2(_v1, cp1.r1).vDot(contact.normal); +// cp1.penetration -= linMove1 + angularMove; +// } +// if (b2 != null && b2.movable) { +// angularMove = _v.vCross2(_v2, cp1.r2).vDot(contact.normal); +// cp1.penetration -= linMove2 - angularMove; +// } +// // Обновление максимального значения для контакта +// if (cp1.penetration > contact.maxPenetration) contact.maxPenetration = cp1.penetration; +// } +// // Обновляем пересечения для других контактов +// var c:Contact; +// var j:int; +// if (b1.movable) { +// for (i = 0; i < b1.contactsNum; i++) { +// c = b1.contacts[i]; +// if (c == contact) continue; +// for (j = 0; j < c.pcount; j++) { +// cp1 = c.points[j]; +// if (b1 == c.body1) cp1.penetration -= linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r1).vDot(c.normal); +// else cp1.penetration += linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r2).vDot(c.normal); +// if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; +// } +// } +// } +// if (b2 != null && b2.movable) { +// for (i = 0; i < b2.contactsNum; i++) { +// c = b2.contacts[i]; +// if (c == contact) continue; +// for (j = 0; j < c.pcount; j++) { +// cp1 = c.points[j]; +// if (b2 == c.body1) cp1.penetration -= linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r1).vDot(c.normal); +// else cp1.penetration += linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r2).vDot(c.normal); +// if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; +// } +// } +// } + } + + /** + * + */ + private function postPhysics():void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + body.clearAccumulators(); + body.calcDerivedData(); + if (body.canFreeze) { + if (body.state.velocity.vLength() < linSpeedFreezeLimit && body.state.rotation.vLength() < angSpeedFreezeLimit) { + if (!body.frozen) { + body.freezeCounter++; + if (body.freezeCounter >= freezeSteps) body.frozen = true; + } + } else { + body.freezeCounter = 0; + body.frozen = false; + } + } + item = item.next; + } + } + + /** + * + * @param delta + */ + public function update(delta:int):void { + timeStamp++; + time += delta; + var dt:Number = 0.001*delta; + applyForces(dt); + detectCollisions(dt); + preProcessContacts(dt); + processContacts(dt, false); + intergateVelocities(dt); + processContacts(dt, true); + integratePositions(dt); + postPhysics(); + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/PhysicsUtils.as b/0.0.9.5/src/alternativa/physics/PhysicsUtils.as new file mode 100644 index 0000000..5a2e46f --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/PhysicsUtils.as @@ -0,0 +1,49 @@ +package alternativa.physics { + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + public class PhysicsUtils { + + /** + * @param mass + * @param halfSize + * @param result + */ + public static function getBoxInvInertia(mass:Number, halfSize:Vector3, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + result.a = 3/(mass*(yy + zz)); + result.f = 3/(mass*(zz + xx)); + result.k = 3/(mass*(xx + yy)); + } + + /** + * @param mass + * @param r + * @param h + * @param result + */ + public static function getCylinderInvInertia(mass:Number, r:Number, h:Number, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + + result.a = result.f = 1/(mass*(h*h/12 + r*r/4)); + result.k = 2/(mass*r*r); + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/altphysics.as b/0.0.9.5/src/alternativa/physics/altphysics.as new file mode 100644 index 0000000..2715307 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/altphysics.as @@ -0,0 +1,3 @@ +package alternativa.physics { + public namespace altphysics = "http://alternativaplatform.com/en/altphysics"; +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/collision/.svn/all-wcprops b/0.0.9.5/src/alternativa/physics/collision/.svn/all-wcprops new file mode 100644 index 0000000..8a44c00 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/.svn/all-wcprops @@ -0,0 +1,65 @@ +K 25 +svn:wc:ra_dav:version-url +V 113 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/collision +END +ICollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 135 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/collision/ICollisionDetector.as +END +CollisionKdNode.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/collision/CollisionKdNode.as +END +CollisionKdTree2D.as +K 25 +svn:wc:ra_dav:version-url +V 134 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/collision/CollisionKdTree2D.as +END +IRayCollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 139 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/collision/IRayCollisionPredicate.as +END +IBodyCollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/collision/IBodyCollisionPredicate.as +END +CollisionPrimitive.as +K 25 +svn:wc:ra_dav:version-url +V 135 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/collision/CollisionPrimitive.as +END +KdTreeCollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/collision/KdTreeCollisionDetector.as +END +ICollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 136 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/collision/ICollisionPredicate.as +END +ICollider.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/collision/ICollider.as +END +CollisionKdTree.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/collision/CollisionKdTree.as +END diff --git a/0.0.9.5/src/alternativa/physics/collision/.svn/entries b/0.0.9.5/src/alternativa/physics/collision/.svn/entries new file mode 100644 index 0000000..16a0b06 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/.svn/entries @@ -0,0 +1,157 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/collision +http://svndev.alternativaplatform.com + + + +2010-08-20T07:58:00.614935Z +39732 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +colliders +dir + +ICollisionDetector.as +file + + + + +2010-10-28T04:32:45.000000Z +9f76503bdaf92b4170b05b6e66c45de1 +2010-02-12T12:34:25.192477Z +28424 +mike + +CollisionKdNode.as +file + + + + +2010-10-28T04:32:45.000000Z +a6fab491cd9bd45a5472bf4ffc321e08 +2010-02-12T12:34:25.192477Z +28424 +mike + +CollisionKdTree2D.as +file + + + + +2010-10-28T04:32:45.000000Z +82bb636d8b49cdfbc22694dd8b01e71d +2010-02-12T12:34:25.192477Z +28424 +mike + +IRayCollisionPredicate.as +file + + + + +2010-10-28T04:32:45.000000Z +32f9b1eaeacfcb40e7ae1927f52b8ab8 +2009-10-19T07:17:33.113306Z +22253 +mike + +types +dir + +IBodyCollisionPredicate.as +file + + + + +2010-10-28T04:32:45.000000Z +618f907a588979a8d06d4f4bcf6618a0 +2009-10-19T07:17:33.113306Z +22253 +mike + +CollisionPrimitive.as +file + + + + +2010-10-28T04:32:45.000000Z +8501ce34a3effa2380c472cc9b823afa +2009-11-24T08:44:15.941480Z +23758 +mike + +KdTreeCollisionDetector.as +file + + + + +2010-10-28T04:32:45.000000Z +ea670ad5d0f72f6a6df5163d65f204f9 +2010-02-12T12:34:25.192477Z +28424 +mike + +ICollisionPredicate.as +file + + + + +2010-10-28T04:32:45.000000Z +65f3250d36556fffc457c740fb7d38b3 +2009-10-19T07:17:33.113306Z +22253 +mike + +ICollider.as +file + + + + +2010-10-28T04:32:45.000000Z +128a7d7479e13caeccecb45161b27b22 +2009-10-19T07:17:33.113306Z +22253 +mike + +primitives +dir + +CollisionKdTree.as +file + + + + +2010-10-28T04:32:45.000000Z +8e815c1f1e527b0884b3f0124257a603 +2010-02-12T12:34:25.192477Z +28424 +mike + diff --git a/0.0.9.5/src/alternativa/physics/collision/.svn/format b/0.0.9.5/src/alternativa/physics/collision/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.5/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base b/0.0.9.5/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base new file mode 100644 index 0000000..f450f1e --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base @@ -0,0 +1,18 @@ +package alternativa.physics.collision { + + + import alternativa.physics.collision.types.BoundBox; + + public class CollisionKdNode { + public var indices:Vector.; + public var splitIndices:Vector.; + public var boundBox:BoundBox; + public var parent:CollisionKdNode; + public var splitTree:CollisionKdTree2D; + + public var axis:int = -1; // 0 - x, 1 - y, 2 - z + public var coord:Number; + public var positiveNode:CollisionKdNode; + public var negativeNode:CollisionKdNode; + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base b/0.0.9.5/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base new file mode 100644 index 0000000..d3dc307 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base @@ -0,0 +1,261 @@ +package alternativa.physics.collision { + + + + import alternativa.physics.collision.types.BoundBox; + + /** + * @author mike + */ + public class CollisionKdTree { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + public var rootNode:CollisionKdNode; + public var staticChildren:Vector.; + public var numStaticChildren:int; + public var staticBoundBoxes:Vector. = new Vector.(); + + private var splitAxis:int; + private var splitCoord:Number; + private var splitCost:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + + /** + * @param boundBox + */ + public function createTree(collisionPrimitives:Vector., boundBox:BoundBox = null):void { + staticChildren = collisionPrimitives.concat(); + numStaticChildren = staticChildren.length; + // Создаём корневую ноду + rootNode = new CollisionKdNode(); + rootNode.indices = new Vector.(); + // Расчитываем баунды объектов и рутовой ноды + var rootNodeBoundBox:BoundBox = rootNode.boundBox = (boundBox != null) ? boundBox : new BoundBox(); + for (var i:int = 0; i < numStaticChildren; ++i) { + var child:CollisionPrimitive = staticChildren[i]; + var childBoundBox:BoundBox = staticBoundBoxes[i] = child.calculateAABB(); + rootNodeBoundBox.addBoundBox(childBoundBox); + rootNode.indices[i] = i; + } + staticBoundBoxes.length = numStaticChildren; + // Разделяем рутовую ноду + splitNode(rootNode); + + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + var indices:Vector. = node.indices; + var numPrimitives:int = indices.length; + if (numPrimitives <= minPrimitivesPerNode) return; + + // Подготовка баунда с погрешностями + var nodeBoundBox:BoundBox = node.boundBox; + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + // Собираем опорные координаты + var i:int; + var j:int; + var numSplitCoordsX:int = 0 + var numSplitCoordsY:int = 0; + var numSplitCoordsZ:int = 0; + for (i = 0; i < numPrimitives; ++i) { + var boundBox:BoundBox = staticBoundBoxes[indices[i]]; + + if (boundBox.maxX - boundBox.minX <= doubleThreshold) { + if (boundBox.minX <= nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.minX; + else if (boundBox.maxX >= nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.maxX; + else splitCoordsX[numSplitCoordsX++] = (boundBox.minX + boundBox.maxX)*0.5; + } else { + if (boundBox.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = boundBox.minX; + if (boundBox.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = boundBox.maxX; + } + + if (boundBox.maxY - boundBox.minY <= doubleThreshold) { + if (boundBox.minY <= nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.minY; + else if (boundBox.maxY >= nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.maxY; + else splitCoordsY[numSplitCoordsY++] = (boundBox.minY + boundBox.maxY)*0.5; + } else { + if (boundBox.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = boundBox.minY; + if (boundBox.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = boundBox.maxY; + } + + if (boundBox.maxZ - boundBox.minZ <= doubleThreshold) { + if (boundBox.minZ <= nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.minZ; + else if (boundBox.maxZ >= nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.maxZ; + else splitCoordsZ[numSplitCoordsZ++] = (boundBox.minZ + boundBox.maxZ)*0.5; + } else { + if (boundBox.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.minZ; + if (boundBox.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numPrimitives; ++i) { + boundBox = staticBoundBoxes[indices[i]]; + var min:Number = axisX ? boundBox.minX : (axisY ? boundBox.minY : boundBox.minZ); + var max:Number = axisX ? boundBox.maxX : (axisY ? boundBox.maxY : boundBox.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(indices[i]); + indices[i] = -1; + } else { + if (node.splitIndices == null) node.splitIndices = new Vector.(); + node.splitIndices.push(indices[i]); + indices[i] = -1; + } + } else { + if (min >= coordMin) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(indices[i]); + indices[i] = -1; + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numPrimitives; ++i) { + if (indices[i] >= 0) indices[j++] = indices[i]; + } + if (j > 0) indices.length = j; + else node.indices = null; + + if (node.splitIndices != null) { + node.splitTree = new CollisionKdTree2D(this, node); + node.splitTree.createTree(); + } + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + /** + * + */ + public function traceTree():void { + traceNode("", rootNode); + } + + /** + * @param str + * @param node + */ + private function traceNode(str:String, node:CollisionKdNode):void { + if (node == null) return; + trace(str, node.axis == -1 ? "end" : ((node.axis == 0) ? "X" : ((node.axis == 1) ? "Y" : "Z")), "splitCoord=" + splitCoord, "bound", node.boundBox, "objs:", node.indices); + traceNode(str + "-", node.negativeNode); + traceNode(str + "+", node.positiveNode); + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base b/0.0.9.5/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base new file mode 100644 index 0000000..2621517 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base @@ -0,0 +1,226 @@ +package alternativa.physics.collision { + + + import alternativa.physics.collision.types.BoundBox; + + /** + * + */ + public class CollisionKdTree2D { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + + public var parentTree:CollisionKdTree; + public var parentNode:CollisionKdNode; + public var rootNode:CollisionKdNode; + + private var splitAxis:int; + private var splitCost:Number; + private var splitCoord:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + /** + * + * @param parentTree + * @param parentNode + */ + public function CollisionKdTree2D(parentTree:CollisionKdTree, parentNode:CollisionKdNode) { + this.parentTree = parentTree; + this.parentNode = parentNode; + } + + /** + * + */ + public function createTree():void { + rootNode = new CollisionKdNode(); + rootNode.boundBox = parentNode.boundBox.clone(); + rootNode.indices = new Vector.(); + var numObjects:int = parentNode.splitIndices.length; + for (var i:int = 0; i < numObjects; ++i) rootNode.indices[i] = parentNode.splitIndices[i]; + + splitNode(rootNode); + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + if (node.indices.length <= minPrimitivesPerNode) return; + + var objects:Vector. = node.indices; + var i:int; + var j:int; + + var nodeBoundBox:BoundBox = node.boundBox; + + // Подготовка баунда с погрешностями + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + // Собираем опорные координаты + var numSplitCoordsX:int; + var numSplitCoordsY:int; + var numSplitCoordsZ:int; + var numObjects:int = objects.length; + for (i = 0; i < numObjects; ++i) { + var bb:BoundBox = staticBoundBoxes[objects[i]]; + + if (parentNode.axis != 0) { + if (bb.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = bb.minX; + if (bb.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = bb.maxX; + } + + if (parentNode.axis != 1) { + if (bb.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = bb.minY; + if (bb.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = bb.maxY; + } + + if (parentNode.axis != 2) { + if (bb.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = bb.minZ; + if (bb.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = bb.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + if (parentNode.axis != 0) checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + if (parentNode.axis != 1) checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + if (parentNode.axis != 2) checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numObjects; ++i) { + bb = staticBoundBoxes[objects[i]]; + var min:Number = axisX ? bb.minX : (axisY ? bb.minY : bb.minZ); + var max:Number = axisX ? bb.maxX : (axisY ? bb.maxY : bb.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(objects[i]); + objects[i] = -1; + } + } else { + if (min >= coordMin) { + if (max > coordMax) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(objects[i]); + objects[i] = -1; + } + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numObjects; ++i) { + if (objects[i] >= 0) objects[j++] = objects[i]; + } + if (j > 0) objects.length = j; + else node.indices = null; + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/collision/.svn/text-base/CollisionPrimitive.as.svn-base b/0.0.9.5/src/alternativa/physics/collision/.svn/text-base/CollisionPrimitive.as.svn-base new file mode 100644 index 0000000..e03cfee --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/.svn/text-base/CollisionPrimitive.as.svn-base @@ -0,0 +1,140 @@ +package alternativa.physics.collision { + + import alternativa.physics.Body; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * Базовый класс для примитивов, использующихся детектором столкновений. + */ + public class CollisionPrimitive { + // Константы типов примитива + public static const BOX:int = 1; + public static const PLANE:int = 2; + public static const SPHERE:int = 3; + public static const RECT:int = 4; + public static const TRIANGLE:int = 5; + + // Тип примитива + public var type:int; + // Группы примитива. Каждая группа определяется установленным битом. Столкновения проверяются только для примитивов, + // имеющих хотя бы одну общую группу. + public var collisionGroup:int; + // Предикат примитива, вызывающийся детектором при нахождении столкновения. В зависимости от возвращённого + // предикатом результата столкновение либо регистрируется, либо игнорируется. + // Nullable + public var postCollisionPredicate:ICollisionPredicate; + // Тело, владеющее примитивом. Поле сделано открытым для быстрого доступа на чтение. Установка значения должна выполняться вызовом метода setBody(). + // Nullable + public var body:Body; + // Трансформация примитива в системе координат тела, если оно указано. Не допускается масштабирование матрицы. + public var localTransform:Matrix4; + // Полная трансформация примитива. Не допускается масштабирование матрицы. + public var transform:Matrix4 = new Matrix4(); + // AABB в мировой системе координат. Расчитывается системой вызовом функции calculateBoundBox(). + public var aabb:BoundBox = new BoundBox(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param type тип примитива + * @param collisionGroup группа примитива + */ + public function CollisionPrimitive(type:int, collisionGroup:int) { + this.type = type; + this.collisionGroup = collisionGroup; + } + + /** + * Устанавливает тело, владеющее примитивом. + * + * @param body тело, которое владеет примитивом + * @param localTransform трансформация примитива в системе координат тела. Указание значения null равносильно + * заданию единичной матрицы, однако в первом случае не будет дополнительного умножения матриц при вычислении полной трансформации примитива. + */ + public function setBody(body:Body, localTransform:Matrix4 = null):void { + if (this.body == body) return; + this.body = body; + if (body != null) { + if (localTransform != null) { + if (this.localTransform == null) { + this.localTransform = new Matrix4(); + } + this.localTransform.copy(localTransform); + } else { + this.localTransform = null; + } + } + } + + /** + * Рассчитывает AABB примитива. Наследники должны переопределять этот метод, реализуя в нём корректный рассчёт. + * + * @return ссылка на свой AABB + */ + public function calculateAABB():BoundBox { + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + return -1; + } + + /** + * Клонирует примитив. Переопределять не рекомендуется. Вместо этого переопределяются методы createPrimitive() и copyFrom(). + * + * @return клон примитива + */ + public function clone():CollisionPrimitive { + var p:CollisionPrimitive = createPrimitive(); + return p.copyFrom(this); + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + if (source == null) { + throw new ArgumentError("Parameter source cannot be null"); + } + type = source.type; + transform.copy(source.transform); + collisionGroup = source.collisionGroup; + setBody(source.body, source.localTransform); + aabb.copyFrom(source.aabb); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + public function toString():String { + return "[CollisionPrimitive type=" + type + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + protected function createPrimitive():CollisionPrimitive { + return new CollisionPrimitive(type, collisionGroup); + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base b/0.0.9.5/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base new file mode 100644 index 0000000..4ace681 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.Body; + + public interface IBodyCollisionPredicate { + function considerBodies(body1:Body, body2:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base b/0.0.9.5/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base new file mode 100644 index 0000000..e6aac2a --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base @@ -0,0 +1,29 @@ +package alternativa.physics.collision { + + import alternativa.physics.Contact; + + /** + * Интерфейс определителя столкновений между двумя примитивами. + */ + public interface ICollider { + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base b/0.0.9.5/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base new file mode 100644 index 0000000..916f67b --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base @@ -0,0 +1,64 @@ +package alternativa.physics.collision { + import alternativa.physics.Contact; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.math.Vector3; + + /** + * Интерфейс детектора столкновений. + */ + public interface ICollisionDetector { + + /** + * Получает все столкновения в текущей конфигурации физической геометрии. + * + * @param contacts список контактов, в кторые будет записана информация о столкновении + * @return количество найденных столкновений + */ + function getAllContacts(contacts:Contact):Contact; + + /** + * Тестирует луч на пересечение с физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param direction направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRay(origin:Vector3, direction:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * + * @param origin + * @param direction + * @param collisionGroup + * @param maxTime + * @param predicate + * @param result + * @return + */ + function intersectRayWithStatic(origin:Vector3, direction:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @return + */ + function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base b/0.0.9.5/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base new file mode 100644 index 0000000..33d30ca --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base @@ -0,0 +1,8 @@ +package alternativa.physics.collision { + + public interface ICollisionPredicate { + + function considerCollision(primitive:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base b/0.0.9.5/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base new file mode 100644 index 0000000..b319f62 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.Body; + + public interface IRayCollisionPredicate { + function considerBody(body:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base b/0.0.9.5/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base new file mode 100644 index 0000000..9ca4c2a --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base @@ -0,0 +1,486 @@ +package alternativa.physics.collision { + + + 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.math.Vector3; + + use namespace altphysics; + + /** + * Детектор, хранящий статическую геометрию в kD-дереве и использующий дерево для ускорения тестов на пересечения. + */ + public class KdTreeCollisionDetector implements ICollisionDetector { + + altphysics var tree:CollisionKdTree; + altphysics var dynamicPrimitives:Vector.; + 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.(); + + 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.):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. = tree.staticChildren; +// var indices:Vector. = 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., indices:Vector., 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; + } \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/collision/CollisionKdNode.as b/0.0.9.5/src/alternativa/physics/collision/CollisionKdNode.as new file mode 100644 index 0000000..f450f1e --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/CollisionKdNode.as @@ -0,0 +1,18 @@ +package alternativa.physics.collision { + + + import alternativa.physics.collision.types.BoundBox; + + public class CollisionKdNode { + public var indices:Vector.; + public var splitIndices:Vector.; + public var boundBox:BoundBox; + public var parent:CollisionKdNode; + public var splitTree:CollisionKdTree2D; + + public var axis:int = -1; // 0 - x, 1 - y, 2 - z + public var coord:Number; + public var positiveNode:CollisionKdNode; + public var negativeNode:CollisionKdNode; + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/collision/CollisionKdTree.as b/0.0.9.5/src/alternativa/physics/collision/CollisionKdTree.as new file mode 100644 index 0000000..d3dc307 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/CollisionKdTree.as @@ -0,0 +1,261 @@ +package alternativa.physics.collision { + + + + import alternativa.physics.collision.types.BoundBox; + + /** + * @author mike + */ + public class CollisionKdTree { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + public var rootNode:CollisionKdNode; + public var staticChildren:Vector.; + public var numStaticChildren:int; + public var staticBoundBoxes:Vector. = new Vector.(); + + private var splitAxis:int; + private var splitCoord:Number; + private var splitCost:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + + /** + * @param boundBox + */ + public function createTree(collisionPrimitives:Vector., boundBox:BoundBox = null):void { + staticChildren = collisionPrimitives.concat(); + numStaticChildren = staticChildren.length; + // Создаём корневую ноду + rootNode = new CollisionKdNode(); + rootNode.indices = new Vector.(); + // Расчитываем баунды объектов и рутовой ноды + var rootNodeBoundBox:BoundBox = rootNode.boundBox = (boundBox != null) ? boundBox : new BoundBox(); + for (var i:int = 0; i < numStaticChildren; ++i) { + var child:CollisionPrimitive = staticChildren[i]; + var childBoundBox:BoundBox = staticBoundBoxes[i] = child.calculateAABB(); + rootNodeBoundBox.addBoundBox(childBoundBox); + rootNode.indices[i] = i; + } + staticBoundBoxes.length = numStaticChildren; + // Разделяем рутовую ноду + splitNode(rootNode); + + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + var indices:Vector. = node.indices; + var numPrimitives:int = indices.length; + if (numPrimitives <= minPrimitivesPerNode) return; + + // Подготовка баунда с погрешностями + var nodeBoundBox:BoundBox = node.boundBox; + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + // Собираем опорные координаты + var i:int; + var j:int; + var numSplitCoordsX:int = 0 + var numSplitCoordsY:int = 0; + var numSplitCoordsZ:int = 0; + for (i = 0; i < numPrimitives; ++i) { + var boundBox:BoundBox = staticBoundBoxes[indices[i]]; + + if (boundBox.maxX - boundBox.minX <= doubleThreshold) { + if (boundBox.minX <= nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.minX; + else if (boundBox.maxX >= nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.maxX; + else splitCoordsX[numSplitCoordsX++] = (boundBox.minX + boundBox.maxX)*0.5; + } else { + if (boundBox.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = boundBox.minX; + if (boundBox.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = boundBox.maxX; + } + + if (boundBox.maxY - boundBox.minY <= doubleThreshold) { + if (boundBox.minY <= nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.minY; + else if (boundBox.maxY >= nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.maxY; + else splitCoordsY[numSplitCoordsY++] = (boundBox.minY + boundBox.maxY)*0.5; + } else { + if (boundBox.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = boundBox.minY; + if (boundBox.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = boundBox.maxY; + } + + if (boundBox.maxZ - boundBox.minZ <= doubleThreshold) { + if (boundBox.minZ <= nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.minZ; + else if (boundBox.maxZ >= nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.maxZ; + else splitCoordsZ[numSplitCoordsZ++] = (boundBox.minZ + boundBox.maxZ)*0.5; + } else { + if (boundBox.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.minZ; + if (boundBox.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numPrimitives; ++i) { + boundBox = staticBoundBoxes[indices[i]]; + var min:Number = axisX ? boundBox.minX : (axisY ? boundBox.minY : boundBox.minZ); + var max:Number = axisX ? boundBox.maxX : (axisY ? boundBox.maxY : boundBox.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(indices[i]); + indices[i] = -1; + } else { + if (node.splitIndices == null) node.splitIndices = new Vector.(); + node.splitIndices.push(indices[i]); + indices[i] = -1; + } + } else { + if (min >= coordMin) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(indices[i]); + indices[i] = -1; + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numPrimitives; ++i) { + if (indices[i] >= 0) indices[j++] = indices[i]; + } + if (j > 0) indices.length = j; + else node.indices = null; + + if (node.splitIndices != null) { + node.splitTree = new CollisionKdTree2D(this, node); + node.splitTree.createTree(); + } + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + /** + * + */ + public function traceTree():void { + traceNode("", rootNode); + } + + /** + * @param str + * @param node + */ + private function traceNode(str:String, node:CollisionKdNode):void { + if (node == null) return; + trace(str, node.axis == -1 ? "end" : ((node.axis == 0) ? "X" : ((node.axis == 1) ? "Y" : "Z")), "splitCoord=" + splitCoord, "bound", node.boundBox, "objs:", node.indices); + traceNode(str + "-", node.negativeNode); + traceNode(str + "+", node.positiveNode); + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/collision/CollisionKdTree2D.as b/0.0.9.5/src/alternativa/physics/collision/CollisionKdTree2D.as new file mode 100644 index 0000000..2621517 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/CollisionKdTree2D.as @@ -0,0 +1,226 @@ +package alternativa.physics.collision { + + + import alternativa.physics.collision.types.BoundBox; + + /** + * + */ + public class CollisionKdTree2D { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + + public var parentTree:CollisionKdTree; + public var parentNode:CollisionKdNode; + public var rootNode:CollisionKdNode; + + private var splitAxis:int; + private var splitCost:Number; + private var splitCoord:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + /** + * + * @param parentTree + * @param parentNode + */ + public function CollisionKdTree2D(parentTree:CollisionKdTree, parentNode:CollisionKdNode) { + this.parentTree = parentTree; + this.parentNode = parentNode; + } + + /** + * + */ + public function createTree():void { + rootNode = new CollisionKdNode(); + rootNode.boundBox = parentNode.boundBox.clone(); + rootNode.indices = new Vector.(); + var numObjects:int = parentNode.splitIndices.length; + for (var i:int = 0; i < numObjects; ++i) rootNode.indices[i] = parentNode.splitIndices[i]; + + splitNode(rootNode); + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + if (node.indices.length <= minPrimitivesPerNode) return; + + var objects:Vector. = node.indices; + var i:int; + var j:int; + + var nodeBoundBox:BoundBox = node.boundBox; + + // Подготовка баунда с погрешностями + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + // Собираем опорные координаты + var numSplitCoordsX:int; + var numSplitCoordsY:int; + var numSplitCoordsZ:int; + var numObjects:int = objects.length; + for (i = 0; i < numObjects; ++i) { + var bb:BoundBox = staticBoundBoxes[objects[i]]; + + if (parentNode.axis != 0) { + if (bb.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = bb.minX; + if (bb.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = bb.maxX; + } + + if (parentNode.axis != 1) { + if (bb.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = bb.minY; + if (bb.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = bb.maxY; + } + + if (parentNode.axis != 2) { + if (bb.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = bb.minZ; + if (bb.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = bb.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + if (parentNode.axis != 0) checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + if (parentNode.axis != 1) checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + if (parentNode.axis != 2) checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numObjects; ++i) { + bb = staticBoundBoxes[objects[i]]; + var min:Number = axisX ? bb.minX : (axisY ? bb.minY : bb.minZ); + var max:Number = axisX ? bb.maxX : (axisY ? bb.maxY : bb.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(objects[i]); + objects[i] = -1; + } + } else { + if (min >= coordMin) { + if (max > coordMax) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(objects[i]); + objects[i] = -1; + } + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numObjects; ++i) { + if (objects[i] >= 0) objects[j++] = objects[i]; + } + if (j > 0) objects.length = j; + else node.indices = null; + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/collision/CollisionPrimitive.as b/0.0.9.5/src/alternativa/physics/collision/CollisionPrimitive.as new file mode 100644 index 0000000..e03cfee --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/CollisionPrimitive.as @@ -0,0 +1,140 @@ +package alternativa.physics.collision { + + import alternativa.physics.Body; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * Базовый класс для примитивов, использующихся детектором столкновений. + */ + public class CollisionPrimitive { + // Константы типов примитива + public static const BOX:int = 1; + public static const PLANE:int = 2; + public static const SPHERE:int = 3; + public static const RECT:int = 4; + public static const TRIANGLE:int = 5; + + // Тип примитива + public var type:int; + // Группы примитива. Каждая группа определяется установленным битом. Столкновения проверяются только для примитивов, + // имеющих хотя бы одну общую группу. + public var collisionGroup:int; + // Предикат примитива, вызывающийся детектором при нахождении столкновения. В зависимости от возвращённого + // предикатом результата столкновение либо регистрируется, либо игнорируется. + // Nullable + public var postCollisionPredicate:ICollisionPredicate; + // Тело, владеющее примитивом. Поле сделано открытым для быстрого доступа на чтение. Установка значения должна выполняться вызовом метода setBody(). + // Nullable + public var body:Body; + // Трансформация примитива в системе координат тела, если оно указано. Не допускается масштабирование матрицы. + public var localTransform:Matrix4; + // Полная трансформация примитива. Не допускается масштабирование матрицы. + public var transform:Matrix4 = new Matrix4(); + // AABB в мировой системе координат. Расчитывается системой вызовом функции calculateBoundBox(). + public var aabb:BoundBox = new BoundBox(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param type тип примитива + * @param collisionGroup группа примитива + */ + public function CollisionPrimitive(type:int, collisionGroup:int) { + this.type = type; + this.collisionGroup = collisionGroup; + } + + /** + * Устанавливает тело, владеющее примитивом. + * + * @param body тело, которое владеет примитивом + * @param localTransform трансформация примитива в системе координат тела. Указание значения null равносильно + * заданию единичной матрицы, однако в первом случае не будет дополнительного умножения матриц при вычислении полной трансформации примитива. + */ + public function setBody(body:Body, localTransform:Matrix4 = null):void { + if (this.body == body) return; + this.body = body; + if (body != null) { + if (localTransform != null) { + if (this.localTransform == null) { + this.localTransform = new Matrix4(); + } + this.localTransform.copy(localTransform); + } else { + this.localTransform = null; + } + } + } + + /** + * Рассчитывает AABB примитива. Наследники должны переопределять этот метод, реализуя в нём корректный рассчёт. + * + * @return ссылка на свой AABB + */ + public function calculateAABB():BoundBox { + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + return -1; + } + + /** + * Клонирует примитив. Переопределять не рекомендуется. Вместо этого переопределяются методы createPrimitive() и copyFrom(). + * + * @return клон примитива + */ + public function clone():CollisionPrimitive { + var p:CollisionPrimitive = createPrimitive(); + return p.copyFrom(this); + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + if (source == null) { + throw new ArgumentError("Parameter source cannot be null"); + } + type = source.type; + transform.copy(source.transform); + collisionGroup = source.collisionGroup; + setBody(source.body, source.localTransform); + aabb.copyFrom(source.aabb); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + public function toString():String { + return "[CollisionPrimitive type=" + type + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + protected function createPrimitive():CollisionPrimitive { + return new CollisionPrimitive(type, collisionGroup); + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/collision/IBodyCollisionPredicate.as b/0.0.9.5/src/alternativa/physics/collision/IBodyCollisionPredicate.as new file mode 100644 index 0000000..4ace681 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/IBodyCollisionPredicate.as @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.Body; + + public interface IBodyCollisionPredicate { + function considerBodies(body1:Body, body2:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/collision/ICollider.as b/0.0.9.5/src/alternativa/physics/collision/ICollider.as new file mode 100644 index 0000000..e6aac2a --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/ICollider.as @@ -0,0 +1,29 @@ +package alternativa.physics.collision { + + import alternativa.physics.Contact; + + /** + * Интерфейс определителя столкновений между двумя примитивами. + */ + public interface ICollider { + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/collision/ICollisionDetector.as b/0.0.9.5/src/alternativa/physics/collision/ICollisionDetector.as new file mode 100644 index 0000000..916f67b --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/ICollisionDetector.as @@ -0,0 +1,64 @@ +package alternativa.physics.collision { + import alternativa.physics.Contact; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.math.Vector3; + + /** + * Интерфейс детектора столкновений. + */ + public interface ICollisionDetector { + + /** + * Получает все столкновения в текущей конфигурации физической геометрии. + * + * @param contacts список контактов, в кторые будет записана информация о столкновении + * @return количество найденных столкновений + */ + function getAllContacts(contacts:Contact):Contact; + + /** + * Тестирует луч на пересечение с физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param direction направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRay(origin:Vector3, direction:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * + * @param origin + * @param direction + * @param collisionGroup + * @param maxTime + * @param predicate + * @param result + * @return + */ + function intersectRayWithStatic(origin:Vector3, direction:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @return + */ + function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/collision/ICollisionPredicate.as b/0.0.9.5/src/alternativa/physics/collision/ICollisionPredicate.as new file mode 100644 index 0000000..33d30ca --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/ICollisionPredicate.as @@ -0,0 +1,8 @@ +package alternativa.physics.collision { + + public interface ICollisionPredicate { + + function considerCollision(primitive:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/collision/IRayCollisionPredicate.as b/0.0.9.5/src/alternativa/physics/collision/IRayCollisionPredicate.as new file mode 100644 index 0000000..b319f62 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/IRayCollisionPredicate.as @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.Body; + + public interface IRayCollisionPredicate { + function considerBody(body:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/collision/KdTreeCollisionDetector.as b/0.0.9.5/src/alternativa/physics/collision/KdTreeCollisionDetector.as new file mode 100644 index 0000000..9ca4c2a --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/KdTreeCollisionDetector.as @@ -0,0 +1,486 @@ +package alternativa.physics.collision { + + + 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.math.Vector3; + + use namespace altphysics; + + /** + * Детектор, хранящий статическую геометрию в kD-дереве и использующий дерево для ускорения тестов на пересечения. + */ + public class KdTreeCollisionDetector implements ICollisionDetector { + + altphysics var tree:CollisionKdTree; + altphysics var dynamicPrimitives:Vector.; + 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.(); + + 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.):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. = tree.staticChildren; +// var indices:Vector. = 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., indices:Vector., 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; + } \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/collision/colliders/.svn/all-wcprops b/0.0.9.5/src/alternativa/physics/collision/colliders/.svn/all-wcprops new file mode 100644 index 0000000..c8121cb --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/colliders/.svn/all-wcprops @@ -0,0 +1,53 @@ +K 25 +svn:wc:ra_dav:version-url +V 123 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/collision/colliders +END +BoxSphereCollider.as +K 25 +svn:wc:ra_dav:version-url +V 144 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/collision/colliders/BoxSphereCollider.as +END +BoxBoxCollider.as +K 25 +svn:wc:ra_dav:version-url +V 141 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/collision/colliders/BoxBoxCollider.as +END +BoxRectCollider.as +K 25 +svn:wc:ra_dav:version-url +V 142 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/collision/colliders/BoxRectCollider.as +END +SpherePlaneCollider.as +K 25 +svn:wc:ra_dav:version-url +V 146 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/collision/colliders/SpherePlaneCollider.as +END +BoxCollider.as +K 25 +svn:wc:ra_dav:version-url +V 138 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/collision/colliders/BoxCollider.as +END +BoxPlaneCollider.as +K 25 +svn:wc:ra_dav:version-url +V 143 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/collision/colliders/BoxPlaneCollider.as +END +SphereSphereCollider.as +K 25 +svn:wc:ra_dav:version-url +V 147 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/collision/colliders/SphereSphereCollider.as +END +BoxTriangleCollider.as +K 25 +svn:wc:ra_dav:version-url +V 146 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/collision/colliders/BoxTriangleCollider.as +END diff --git a/0.0.9.5/src/alternativa/physics/collision/colliders/.svn/entries b/0.0.9.5/src/alternativa/physics/collision/colliders/.svn/entries new file mode 100644 index 0000000..54ce4d9 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/colliders/.svn/entries @@ -0,0 +1,124 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/collision/colliders +http://svndev.alternativaplatform.com + + + +2010-03-06T16:51:19.781174Z +29404 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +BoxSphereCollider.as +file + + + + +2010-10-28T04:32:45.000000Z +25170bcb9a1eec2a0941f4b50a06a554 +2009-12-08T15:53:11.094448Z +24506 +mike + +BoxBoxCollider.as +file + + + + +2010-10-28T04:32:45.000000Z +15da13b1383e9c1892edc205c8c055b0 +2010-02-12T12:34:25.192477Z +28424 +mike + +BoxRectCollider.as +file + + + + +2010-10-28T04:32:45.000000Z +4a35f94d3ef1af0a5b42e1a100580f6d +2010-03-06T16:51:19.781174Z +29404 +mike + +SpherePlaneCollider.as +file + + + + +2010-10-28T04:32:45.000000Z +67621ed47d7e90b72498acf2802c4e2d +2009-11-24T08:44:15.941480Z +23758 +mike + +BoxCollider.as +file + + + + +2010-10-28T04:32:45.000000Z +52e72ed30a24a3a43ce16a19930546b5 +2010-02-12T12:34:25.192477Z +28424 +mike + +BoxPlaneCollider.as +file + + + + +2010-10-28T04:32:45.000000Z +204c7b02a9a1913269c558658bad4262 +2010-02-12T12:34:25.192477Z +28424 +mike + +SphereSphereCollider.as +file + + + + +2010-10-28T04:32:45.000000Z +301c7102f91a7f3ede83e35ca202db0b +2009-11-24T08:44:15.941480Z +23758 +mike + +BoxTriangleCollider.as +file + + + + +2010-10-28T04:32:45.000000Z +448502271cac4f09491ca128b4bb485a +2010-03-06T16:51:19.781174Z +29404 +mike + diff --git a/0.0.9.5/src/alternativa/physics/collision/colliders/.svn/format b/0.0.9.5/src/alternativa/physics/collision/colliders/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/colliders/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.5/src/alternativa/physics/collision/colliders/.svn/text-base/BoxBoxCollider.as.svn-base b/0.0.9.5/src/alternativa/physics/collision/colliders/.svn/text-base/BoxBoxCollider.as.svn-base new file mode 100644 index 0000000..2d3f04c --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/colliders/.svn/text-base/BoxBoxCollider.as.svn-base @@ -0,0 +1,589 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * Расчитывает точки контакта двух боксов. Нормаль контакта направляется в сторону бокса с меньшим ID. + */ + public class BoxBoxCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + private var vectorToBox1:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var bestAxisIndex:int; + private var minOverlap:Number; + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + private var tmpPoints:Vector. = new Vector.(8, true); + private var pcount:int; + + /** + * + */ + public function BoxBoxCollider() { + for (var i:int = 0; i < 8; i++) { + tmpPoints[i] = new ContactPoint(); + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + + if (bestAxisIndex < 6) { + // Контакт грань-(грань|ребро|вершина) + if (!findFaceContactPoints(box1, box2, vectorToBox1, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 6; + findEdgesIntersection(box1, box2, vectorToBox1, int(bestAxisIndex/3), bestAxisIndex%3, contact); + } + contact.body1 = box1.body; + contact.body2 = box2.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + // Вектор из центра второго бокса в центр первого + vectorToBox1.x = transform1.d - transform2.d; + vectorToBox1.y = transform1.h - transform2.h; + vectorToBox1.z = transform1.l - transform2.l; + + // Проверка пересечения по основным осям + axis10.x = transform1.a; + axis10.y = transform1.e; + axis10.z = transform1.i; + if (!testMainAxis(box1, box2, axis10, 0, vectorToBox1)) return false; + axis11.x = transform1.b; + axis11.y = transform1.f; + axis11.z = transform1.j; + if (!testMainAxis(box1, box2, axis11, 1, vectorToBox1)) return false; + axis12.x = transform1.c; + axis12.y = transform1.g; + axis12.z = transform1.k; + if (!testMainAxis(box1, box2, axis12, 2, vectorToBox1)) return false; + + axis20.x = transform2.a; + axis20.y = transform2.e; + axis20.z = transform2.i; + if (!testMainAxis(box1, box2, axis20, 3, vectorToBox1)) return false; + axis21.x = transform2.b; + axis21.y = transform2.f; + axis21.z = transform2.j; + if (!testMainAxis(box1, box2, axis21, 4, vectorToBox1)) return false; + axis22.x = transform2.c; + axis22.y = transform2.g; + axis22.z = transform2.k; + if (!testMainAxis(box1, box2, axis22, 5, vectorToBox1)) return false; + + // Проверка производных осей + if (!testDerivedAxis(box1, box2, axis10, axis20, 6, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis21, 7, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis22, 8, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis11, axis20, 9, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis21, 10, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis22, 11, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis12, axis20, 12, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis21, 13, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis22, 14, vectorToBox1)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта грани одного бокса с гранью/ребром/вершиной другого. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param faceAxisIdx индекс оси первого бокса, перпендикулярной грани, с которой произошло столкновение + * @param contactInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var swapNormal:Boolean = false; + if (faceAxisIdx > 2) { + // Столкновение с гранью второго бокса. Для дальнейших расчётов боксы меняются местами, + // но нормаль контакта всё равно должна быть направлена в сторону первоначального box1 + var tmpBox:CollisionBox = box1; + box1 = box2; + box2 = tmpBox; + vectorToBox1.x = -vectorToBox1.x; + vectorToBox1.y = -vectorToBox1.y; + vectorToBox1.z = -vectorToBox1.z; + faceAxisIdx -= 3; + swapNormal = true; + } + + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + var colAxis:Vector3 = contact.normal; + + transform1.getAxis(faceAxisIdx, colAxis); + var negativeFace:Boolean = colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z > 0; + + var code:int = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box1.excludedFaces) != 0) return false; + + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + // Ищем ось второго бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + transform2.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisDot = dot; + incidentAxisIdx = axisIdx; + } + } + // Получаем список вершин грани второго бокса, переводим их в систему координат первого бокса и выполняем обрезку + // по грани первого бокса. Таким образом получается список потенциальных точек контакта. + transform2.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box2.hs, incidentAxisIdx, incidentAxisDot < 0, points1); + + // TODO: Вычислить результирующую матрицу, затем преобразовать векторы за один заход + transform2.transformVectorsN(points1, points2, 4); + transform1.transformVectorsInverseN(points2, points1, 4); + + var pnum:int = clip(box1.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points1[i]; + if ((pen = getPointBoxPenetration(box1.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + var cp:ContactPoint = tmpPoints[pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = transform1.a*v.x + transform1.b*v.y + transform1.c*v.z + transform1.d; + cpPos.y = transform1.e*v.x + transform1.f*v.y + transform1.g*v.z + transform1.h; + cpPos.z = transform1.i*v.x + transform1.j*v.y + transform1.k*v.z + transform1.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + + cp.penetration = pen; + } + } + if (swapNormal) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (pcount > 4) { + reducePoints(); + } + for (i = 0; i < pcount; i++) { + cp = contact.points[i]; + cp.copyFrom(tmpPoints[i]); + } + contact.pcount = pcount; + return true; + } + + /** + * + * @param contactInfo + */ + private function reducePoints():void { + var i:int; + var minIdx:int; + var cp1:ContactPoint; + var cp2:ContactPoint; + while (pcount > 4) { + var minLen:Number = 1e10; + var p1:Vector3 = (tmpPoints[int(pcount - 1)] as ContactPoint).pos; + var p2:Vector3; + for (i = 0; i < pcount; i++) { + p2 = (tmpPoints[i] as ContactPoint).pos; + var dx:Number = p2.x - p1.x; + var dy:Number = p2.y - p1.y; + var dz:Number = p2.z - p1.z; + var len:Number = dx*dx + dy*dy + dz*dz; + if (len < minLen) { + minLen = len; + minIdx = i; + } + p1 = p2; + } + var ii:int = minIdx == 0 ? (pcount - 1) : (minIdx - 1); + cp1 = tmpPoints[ii]; + cp2 = tmpPoints[minIdx]; + p1 = cp1.pos; + p2 = cp2.pos; + p2.x = 0.5*(p1.x + p2.x); + p2.y = 0.5*(p1.y + p2.y); + p2.z = 0.5*(p1.z + p2.z); + cp2.penetration = 0.5*(cp1.penetration + cp2.penetration); + if (minIdx > 0) { + for (i = minIdx; i < pcount; i++) { + tmpPoints[int(i - 1)] = tmpPoints[i]; + } + tmpPoints[int(pcount - 1)] = cp1; + } + pcount--; + } + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clip(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param contactInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):void { + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + transform1.getAxis(axisIdx1, axis10); + transform2.getAxis(axisIdx2, axis20); + + var colAxis:Vector3 = contact.normal; + + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону первого бокса + if (colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + + var tx:Number = box1.hs.x; + var ty:Number = box1.hs.y; + var tz:Number = box1.hs.z; + + var x2:Number = box2.hs.x; + var y2:Number = box2.hs.y; + var z2:Number = box2.hs.z; + // x + if (axisIdx1 == 0) { + tx = 0; + halfLen1 = box1.hs.x; + } else { + if (colAxis.x*transform1.a + colAxis.y*transform1.e + colAxis.z*transform1.i > 0) { + tx = -tx; + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = box2.hs.x; + } else { + if (colAxis.x*transform2.a + colAxis.y*transform2.e + colAxis.z*transform2.i < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + ty = 0; + halfLen1 = box1.hs.y; + } else { + if (colAxis.x*transform1.b + colAxis.y*transform1.f + colAxis.z*transform1.j > 0) { + ty = -ty; + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = box2.hs.y; + } else { + if (colAxis.x*transform2.b + colAxis.y*transform2.f + colAxis.z*transform2.j < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + tz = 0; + halfLen1 = box1.hs.z; + } else { + if (colAxis.x*transform1.c + colAxis.y*transform1.g + colAxis.z*transform1.k > 0) { + tz = -tz; + } + } + if (axisIdx2 == 2) { + z2 = 0; + halfLen2 = box2.hs.z; + } else { + if (colAxis.x*transform2.c + colAxis.y*transform2.g + colAxis.z*transform2.k < 0) { + z2 = -z2; + } + } + // Получаем глобальные координаты средних точек рёбер + var x1:Number = transform1.a*tx + transform1.b*ty + transform1.c*tz + transform1.d; + var y1:Number = transform1.e*tx + transform1.f*ty + transform1.g*tz + transform1.h; + var z1:Number = transform1.i*tx + transform1.j*ty + transform1.k*tz + transform1.l; + tx = x2; + ty = y2; + tz = z2; + x2 = transform2.a*tx + transform2.b*ty + transform2.c*tz + transform2.d; + y2 = transform2.e*tx + transform2.f*ty + transform2.g*tz + transform2.h; + z2 = transform2.i*tx + transform2.j*ty + transform2.k*tz + transform2.l; + + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + tx = x2 - x1; + ty = y2 - y1; + tz = z2 - z1; + var c1:Number = axis10.x*tx + axis10.y*ty + axis10.z*tz; + var c2:Number = axis20.x*tx + axis20.y*ty + axis20.z*tz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cp.pos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cp.pos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение боксов вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box1 + * @param box2 + * @param axis + * @param axisIndex + * @param toBox1 + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box1 + * @param box2 + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox1 + * @return + */ + private function testDerivedAxis(box1:CollisionBox, box2:CollisionBox, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия двух боксов вдоль заданной оси. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param axis ось + * @param vectorToBox1 вектор, соединяющий центр второго бокса с центром первого + * @return величина перекрытия боксов вдоль оси + */ + public function overlapOnAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, vectorToBox1:Vector3):Number { + var m:Matrix4 = box1.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box1.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box1.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box1.hs.z; + if (d < 0) d = -d; + projection += d; + + m = box2.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box2.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box2.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box2.hs.z; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox1.x*axis.x + vectorToBox1.y*axis.y + vectorToBox1.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} + import alternativa.math.Vector3; + + class CollisionPointTmp { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + } diff --git a/0.0.9.5/src/alternativa/physics/collision/colliders/.svn/text-base/BoxCollider.as.svn-base b/0.0.9.5/src/alternativa/physics/collision/colliders/.svn/text-base/BoxCollider.as.svn-base new file mode 100644 index 0000000..2429bb1 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/colliders/.svn/text-base/BoxCollider.as.svn-base @@ -0,0 +1,427 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + /** + * + */ + public class BoxCollider implements ICollider { + + /** + * + */ + public function BoxCollider() { + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + return false; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + protected function getFaceVertsByAxis(hs:Vector3, axisIdx:int, negativeFace:Boolean, result:Vector.):void { + var v:Vector3; + switch (axisIdx) { + case 0: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + } + break; + case 1: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + } + break; + case 2: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + } else { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } + break; + } + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/collision/colliders/.svn/text-base/BoxPlaneCollider.as.svn-base b/0.0.9.5/src/alternativa/physics/collision/colliders/.svn/text-base/BoxPlaneCollider.as.svn-base new file mode 100644 index 0000000..3bebdc3 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/colliders/.svn/text-base/BoxPlaneCollider.as.svn-base @@ -0,0 +1,88 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + public class BoxPlaneCollider implements ICollider { + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var normal:Vector3 = new Vector3(); + + public function BoxPlaneCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var box:RigidBox = body1 as RigidBox; +// var plane:RigidPlane; +// if (box == null) { +// box = body2 as RigidBox; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные координаты вершин бокса +// var sx:Number = box.halfSize.x; +// var sy:Number = box.halfSize.y; +// var sz:Number = box.halfSize.z; +// (verts1[0] as Vector3).reset(-sx, -sy, -sz); +// (verts1[1] as Vector3).reset(sx, -sy, -sz); +// (verts1[2] as Vector3).reset(sx, sy, -sz); +// (verts1[3] as Vector3).reset(-sx, sy, -sz); +// (verts1[4] as Vector3).reset(-sx, -sy, sz); +// (verts1[5] as Vector3).reset(sx, -sy, sz); +// (verts1[6] as Vector3).reset(sx, sy, sz); +// (verts1[7] as Vector3).reset(-sx, sy, sz); +// +// box.transform.transformVectors(verts1, verts2); +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// // Проверяем наличие столкновений с каждой вершиной +// collisionInfo.pcount = 0; +// for (var i:int = 0; i < 8; i++) { +// // Вершина добавляется в список точек столкновения, если лежит под плоскостью +// var dist:Number = (verts2[i] as Vector3).dot(normal); +// if (dist < offset) { +// var cp:ContactPoint; +// if (collisionInfo.pcount == collisionInfo.points.length) { +// cp = new ContactPoint(); +// collisionInfo.points[collisionInfo.pcount] = cp; +// } else { +// cp = collisionInfo.points[collisionInfo.pcount]; +// } +// cp.pos.copy(verts2[i]); +// cp.r1.diff(cp.pos, box.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); +// cp.penetration = offset - dist; +// collisionInfo.pcount++; +// } +// } +// if (collisionInfo.pcount > 0) { +// collisionInfo.body1 = box; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// return true; +// } + return false; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/collision/colliders/.svn/text-base/BoxRectCollider.as.svn-base b/0.0.9.5/src/alternativa/physics/collision/colliders/.svn/text-base/BoxRectCollider.as.svn-base new file mode 100644 index 0000000..d6ba302 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/colliders/.svn/text-base/BoxRectCollider.as.svn-base @@ -0,0 +1,575 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * + */ + public class BoxRectCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + + private var vectorToBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxRectCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, rect, vectorToBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + if (!findEdgesIntersection(box, rect, vectorToBox, int(bestAxisIndex/2), bestAxisIndex%2, contact)) { + return false; + } + } + contact.body1 = box.body; + contact.body2 = rect.body; + + // Хак для танков, чтобы исключить утыкания танков в стыки статических примитивов + if (rect.transform.k > 0.99999) { + contact.normal.x = 0; + contact.normal.y = 0; + contact.normal.z = 1; + } + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + // Вектор из центра прямоугольника в центр бокса + vectorToBox.x = boxTransform.d - rectTransform.d; + vectorToBox.y = boxTransform.h - rectTransform.h; + vectorToBox.z = boxTransform.l - rectTransform.l; + + // Проверка пересечения по нормали прямоугольника + rectTransform.getAxis(2, axis22); + if (!testMainAxis(box, rect, axis22, 0, vectorToBox)) return false; + + // Проверка пересечения по основным осям бокса + boxTransform.getAxis(0, axis10); + if (!testMainAxis(box, rect, axis10, 1, vectorToBox)) return false; + boxTransform.getAxis(1, axis11); + if (!testMainAxis(box, rect, axis11, 2, vectorToBox)) return false; + boxTransform.getAxis(2, axis12); + if (!testMainAxis(box, rect, axis12, 3, vectorToBox)) return false; + + // Получаем направляющие рёбер прямоугольника + rectTransform.getAxis(0, axis20); + rectTransform.getAxis(1, axis21); + + // Проверка производных осей + if (!testDerivedAxis(box, rect, axis10, axis20, 4, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis10, axis21, 5, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis11, axis20, 6, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis11, axis21, 7, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis12, axis20, 8, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis12, axis21, 9, vectorToBox)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта бокса с прямоугольником. + * + * @param box бокс + * @param rect прямоугольник + * @param vectorToBox вектор, направленный из центра прямоугольника в центр бокса + * @param faceAxisIdx индекс оси, идентифицирующей полскость столкновения (грань бокса или полскость прямоугольника) + * @param colInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var pnum:int; + var i:int; + var v:Vector3; + var cp:ContactPoint; + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + var colAxis:Vector3 = contact.normal; + + var negativeFace:Boolean; + var code:int; + + if (faceAxisIdx == 0) { + // Столкновение с плоскостью прямоугольника + + // Проверим положение бокса относительно плоскости прямоугольника + colAxis.x = rectTransform.c; + colAxis.y = rectTransform.g; + colAxis.z = rectTransform.k; + +// var offset:Number = colAxis.x*rectTransform.d + colAxis.y*rectTransform.h + colAxis.z*rectTransform.l; +// if (bbPos.vDot(colAxis) < offset) return false; + + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisIdx = axisIdx; + incidentAxisDot = dot; + } + } + negativeFace = incidentAxisDot > 0; + + code = 1 << (incidentAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + + // Получаем список вершин грани бокса, переводим их в систему координат прямоугольника и выполняем обрезку + // по прямоугольнику. Таким образом получается список потенциальных точек контакта. + boxTransform.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box.hs, incidentAxisIdx, negativeFace, points1); + boxTransform.transformVectorsN(points1, points2, 4); + rectTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByRect(rect.hs); + // Проверяем каждую потенциальную точку на принадлежность нижней полуплоскости прямоугольника и добавляем такие точки в список контактов + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if (v.z < epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = -v.z; + var cpPos:Vector3 = cp.pos; + cpPos.x = rectTransform.a*v.x + rectTransform.b*v.y + rectTransform.c*v.z + rectTransform.d; + cpPos.y = rectTransform.e*v.x + rectTransform.f*v.y + rectTransform.g*v.z + rectTransform.h; + cpPos.z = rectTransform.i*v.x + rectTransform.j*v.y + rectTransform.k*v.z + rectTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } else { + // Столкновение с гранью бокса + faceAxisIdx--; + boxTransform.getAxis(faceAxisIdx, colAxis); + negativeFace = colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z > 0; + + code = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) { + return false; + } + + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (rectTransform.c*colAxis.x + rectTransform.g*colAxis.y + rectTransform.k*colAxis.z < 0) return false; + + getFaceVertsByAxis(rect.hs, 2, false, points1); + rectTransform.transformVectorsN(points1, points2, 4); + boxTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if ((pen = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = pen; + cpPos = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } + return true; + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * + * @param hs + * @return + */ + private function clipByRect(hs:Vector3):int { + var pnum:int = 4; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box первый бокс + * @param rect второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param colInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + boxTransform.getAxis(axisIdx1, axis10); + rectTransform.getAxis(axisIdx2, axis20); + var colAxis:Vector3 = contact.normal; + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону бокса + if (colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + var vx:Number = box.hs.x; + var vy:Number = box.hs.y; + var vz:Number = box.hs.z; + var x2:Number = rect.hs.x; + var y2:Number = rect.hs.y; + var z2:Number = rect.hs.z; + // x + if (axisIdx1 == 0) { + vx = 0; + halfLen1 = box.hs.x; + } else { + if (boxTransform.a*colAxis.x + boxTransform.e*colAxis.y + boxTransform.i*colAxis.z > 0) { + vx = -vx; + if ((box.excludedFaces & 2) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 1) != 0) { + return false; + } + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = rect.hs.x; + } else { + if (rectTransform.a*colAxis.x + rectTransform.e*colAxis.y + rectTransform.i*colAxis.z < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + vy = 0; + halfLen1 = box.hs.y; + } else { + if (boxTransform.b*colAxis.x + boxTransform.f*colAxis.y + boxTransform.j*colAxis.z > 0) { + vy = -vy; + if ((box.excludedFaces & 8) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 4) != 0) { + return false; + } + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = rect.hs.y; + } else { + if (rectTransform.b*colAxis.x + rectTransform.f*colAxis.y + rectTransform.j*colAxis.z < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + vz = 0; + halfLen1 = box.hs.z; + } else { + if (boxTransform.c*colAxis.x + boxTransform.g*colAxis.y + boxTransform.k*colAxis.z > 0) { + vz = -vz; + if ((box.excludedFaces & 32) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 16) != 0) { + return false; + } + } + } + // Получаем глобальные координаты средних точек рёбер + + var x1:Number = boxTransform.a*vx + boxTransform.b*vy + boxTransform.c*vz + boxTransform.d; + var y1:Number = boxTransform.e*vx + boxTransform.f*vy + boxTransform.g*vz + boxTransform.h; + var z1:Number = boxTransform.i*vx + boxTransform.j*vy + boxTransform.k*vz + boxTransform.l; + vx = x2; + vy = y2; + vz = z2; + x2 = rectTransform.a*vx + rectTransform.b*vy + rectTransform.c*vz + rectTransform.d; + y2 = rectTransform.e*vx + rectTransform.f*vy + rectTransform.g*vz + rectTransform.h; + z2 = rectTransform.i*vx + rectTransform.j*vy + rectTransform.k*vz + rectTransform.l; + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + vx = x2 - x1; + vy = y2 - y1; + vz = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cpPos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cpPos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cpPos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var v:Vector3 = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + return true; + } + + /** + * Проверяет пересечение вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box + * @param rect + * @param axis + * @param axisIndex + * @param vectorToBox + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param rect + * @param axis1 + * @param axis2 + * @param axisIndex + * @param vectorToBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, rect:CollisionRect, axis1:Vector3, axis2:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия вдоль заданной оси. + * + * @param box бокс + * @param rect прямоугольник + * @param axis ось + * @param vectorToBox вектор, соединяющий центр прямоугольника с центром бокса + * @return величина перекрытия вдоль оси + */ + public function overlapOnAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, vectorToBox:Vector3):Number { + var m:Matrix4 = box.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box.hs.z; + if (d < 0) d = -d; + projection += d; + + m = rect.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*rect.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*rect.hs.y; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox.x*axis.x + vectorToBox.y*axis.y + vectorToBox.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} diff --git a/0.0.9.5/src/alternativa/physics/collision/colliders/.svn/text-base/BoxSphereCollider.as.svn-base b/0.0.9.5/src/alternativa/physics/collision/colliders/.svn/text-base/BoxSphereCollider.as.svn-base new file mode 100644 index 0000000..da5e36d --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/colliders/.svn/text-base/BoxSphereCollider.as.svn-base @@ -0,0 +1,168 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.math.Vector3; + use namespace altphysics; + /** + * + */ + public class BoxSphereCollider implements ICollider { + + private var center:Vector3 = new Vector3(); + private var closestPt:Vector3 = new Vector3(); + + private var bPos:Vector3 = new Vector3(); + private var sPos:Vector3 = new Vector3(); + + /** + * + */ + public function BoxSphereCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + // Зафиксированно столкновение + contact.body1 = sphere.body; + contact.body2 = box.body; + contact.normal.vCopy(closestPt).vTransformBy4(box.transform).vSubtract(sPos).vNormalize().vReverse(); + contact.pcount = 1; + + var cp:ContactPoint = contact.points[0]; + cp.penetration = sphere.r - Math.sqrt(distSqr); + cp.pos.vCopy(contact.normal).vScale(-sphere.r).vAdd(sPos); + cp.r1.vDiff(cp.pos, sPos); + cp.r2.vDiff(cp.pos, bPos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + + return true; + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/collision/colliders/.svn/text-base/BoxTriangleCollider.as.svn-base b/0.0.9.5/src/alternativa/physics/collision/colliders/.svn/text-base/BoxTriangleCollider.as.svn-base new file mode 100644 index 0000000..935525d --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/colliders/.svn/text-base/BoxTriangleCollider.as.svn-base @@ -0,0 +1,725 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionTriangle; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * + */ + public class BoxTriangleCollider extends BoxCollider { + + public var epsilon:Number = 0.001; + + private var bestAxisIndex:int; + private var minOverlap:Number; + private var toBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var colNormal:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxTriangleCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, tri, toBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + if (!findEdgesIntersection(box, tri, toBox, bestAxisIndex%3, int(bestAxisIndex/3), contact)) return false; + } + + contact.body1 = box.body; + contact.body2 = tri.body; + + // Хак для танков, чтобы исключить утыкания танков в стыки статических примитивов + if (tri.transform.k > 0.99999) { + contact.normal.x = 0; + contact.normal.y = 0; + contact.normal.z = 1; + } + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + toBox.x = boxTransform.d - triTransform.d; + toBox.y = boxTransform.h - triTransform.h; + toBox.z = boxTransform.l - triTransform.l; + + minOverlap = 1e308; + + // Сначала проверяется нормаль треугольника + axis.x = triTransform.c; + axis.y = triTransform.g; + axis.z = triTransform.k; + if (!testMainAxis(box, tri, axis, 0, toBox)) return false; + + // Проверка основных осей бокса + axis10.x = boxTransform.a; + axis10.y = boxTransform.e; + axis10.z = boxTransform.i; + if (!testMainAxis(box, tri, axis10, 1, toBox)) return false; + axis11.x = boxTransform.b; + axis11.y = boxTransform.f; + axis11.z = boxTransform.j; + if (!testMainAxis(box, tri, axis11, 2, toBox)) return false; + axis12.x = boxTransform.c; + axis12.y = boxTransform.g; + axis12.z = boxTransform.k; + if (!testMainAxis(box, tri, axis12, 3, toBox)) return false; + + // Проверка производных осей + // TODO: заменить вычисления векторных произведений инлайнами + var v:Vector3 = tri.e0; + axis20.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis20.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis20.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis20, 4, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis20, 5, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis20, 6, toBox)) return false; + + v = tri.e1; + axis21.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis21.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis21.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis21, 7, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis21, 8, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis21, 9, toBox)) return false; + + v = tri.e2; + axis22.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis22.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis22.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis22, 10, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis22, 11, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis22, 12, toBox)) return false; + + return true; + } + + /** + * Тестирует пересечение примитивов вдоль заданной оси. + * + * @param box бокс + * @param tri треугольник + * @param axis ось + * @param axisIndex индекс оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return true, если примитивы имеют перекрытие проекций вдоль указанной оси, иначе false + */ + private function testMainAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, axisIndex:int, toBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param tri + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, tri:CollisionTriangle, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Рассчитывает величину перекрытия проекций бокса и треугольника на заданную ось. + * + * @param box бокс + * @param tri треугольник + * @param axis единичный направляющий вектор оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return величина перекрытия. Положительное значение указывает на наличие перекрытия, нулевое или отрицательное значение указывает на отсутствие перекрытия. + */ + private function overlapOnAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, toBox:Vector3):Number { + var t:Matrix4 = box.transform; + var projection:Number = (t.a*axis.x + t.e*axis.y + t.i*axis.z)*box.hs.x; + if (projection < 0) projection = -projection; + var d:Number = (t.b*axis.x + t.f*axis.y + t.j*axis.z)*box.hs.y; + projection += d < 0 ? -d : d; + d = (t.c*axis.x + t.g*axis.y + t.k*axis.z)*box.hs.z; + projection += d < 0 ? -d : d; + + var vectorProjection:Number = toBox.x*axis.x + toBox.y*axis.y + toBox.z*axis.z; + // Для оптимизации ось преобразуется в систему треугольника, а не его вершины в мировую систему + t = tri.transform; + var ax:Number = t.a*axis.x + t.e*axis.y + t.i*axis.z; + var ay:Number = t.b*axis.x + t.f*axis.y + t.j*axis.z; + var az:Number = t.c*axis.x + t.g*axis.y + t.k*axis.z; + var max:Number = 0; + if (vectorProjection < 0) { + vectorProjection = -vectorProjection; + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d < max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d < max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d < max) max = d; + max = -max; + } else { + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d > max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d > max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d > max) max = d; + } + + return projection + max - vectorProjection; + } + + /** + * Определяет точки контакта бокса и треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function findFaceContactPoints(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIndex:int, contact:Contact):Boolean { + if (faceAxisIndex == 0) { + // Столкновение с плоскостью треугольника + return getBoxToTriContact(box, tri, toBox, contact); + } else { + // Столкновение с гранью бокса + return getTriToBoxContact(box, tri, toBox, faceAxisIndex, contact); + } + } + + /** + * Определяет точки контакта бокса с плоскостью треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getBoxToTriContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + colNormal.x = triTransform.c; + colNormal.y = triTransform.g; + colNormal.z = triTransform.k; + + var over:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + if (!over) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var incAxisDot:Number = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colNormal.x + axis.y*colNormal.y + axis.z*colNormal.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incAxisDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Обрезка грани + var negativeFace:Boolean = incAxisDot > 0; + + var code:int = 1 << (incFaceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + getFaceVertsByAxis(box.hs, incFaceAxisIdx, negativeFace, points1); + boxTransform.transformVectorsN(points1, points2, 4); + triTransform.transformVectorsInverseN(points2, points1, 4); + var pnum:int = clipByTriangle(tri); + // Среди конечного списка точек определяются лежащие под плоскостью треугольника + var cp:ContactPoint; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points2[i]; + if ((over && v.z < 0) || (!over && v.z > 0)) { + cp = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z + triTransform.d; + cpPos.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z + triTransform.h; + cpPos.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z + triTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = over ? -v.z : v.z; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Оперделяет точки контакта треугольника с гранью бокса. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getTriToBoxContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + faceAxisIdx--; + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + boxTransform.getAxis(faceAxisIdx, colNormal); + var negativeFace:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + + var code:int = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + if (!negativeFace) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + var v:Vector3 = points1[0]; + v.x = tri.v0.x; + v.y = tri.v0.y; + v.z = tri.v0.z; + + v = points1[1]; + v.x = tri.v1.x; + v.y = tri.v1.y; + v.z = tri.v1.z; + + v = points1[2]; + v.x = tri.v2.x; + v.y = tri.v2.y; + v.z = tri.v2.z; + + triTransform.transformVectorsN(points1, points2, 3); + boxTransform.transformVectorsInverseN(points2, points1, 3); + + var pnum:int = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность боксу и добавляем такие точки в список контактов + var penetration:Number; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + v = points1[i]; + penetration = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace); + if (penetration > -epsilon) { + var cp:ContactPoint = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = penetration; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Вычисляет величину проникновения точки в бокс. + * + * @param hs вектор половинных размеров бокса + * @param p точка в системе координат бокса + * @param axisIndex индекс оси + * @param negativeFace если true, проверяется нижняя грань + * @return величина проникновения точки в бокс + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, negativeFace:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (negativeFace) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (negativeFace) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (negativeFace) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, вершины которой заданы в списке points1. Результат сохраняется в этом же списке. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 3; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Обрезает грань прямоугольника сторонами треугольника. Обрезка выполняется в системе координат и в проекции на + * плоскость треугольника. Входные вершины грани в количестве четырёх штук должны находиться в списке points1. + * Конечные вершины сохраняются в списке points2. + * + * @param tri треугольник + * @return количество врешин в конечном списке + */ + private function clipByTriangle(tri:CollisionTriangle):int { + var vnum:int = 4; + vnum = clipByLine(tri.v0, tri.e0, points1, vnum, points2); + if (vnum == 0) return 0; + vnum = clipByLine(tri.v1, tri.e1, points2, vnum, points1); + if (vnum == 0) return 0; + return clipByLine(tri.v2, tri.e2, points1, vnum, points2);; + } + + /** + * Обрезает полигон указанной прямой. + * + * @param linePoint точка на прямой + * @param lineDir единичный направляющий вектор прямой + * @param verticesIn список вершин исходного полигона + * @param vnum количество вершин исходного полигона + * @param verticesOut список, куда будут записаны вершины конечного полигона + * @return количество вершин конечного полигона + */ + private function clipByLine(linePoint:Vector3, lineDir:Vector3, verticesIn:Vector., vnum:int, verticesOut:Vector.):int { + var nx:Number = -lineDir.y; + var ny:Number = lineDir.x; + var offset:Number = linePoint.x*nx + linePoint.y*ny; + var v1:Vector3 = verticesIn[int(vnum - 1)]; + var offset1:Number = v1.x*nx + v1.y*ny; + var t:Number; + var v:Vector3; + var num:int = 0; + for (var i:int = 0; i < vnum; i++) { + var v2:Vector3 = verticesIn[i]; + var offset2:Number = v2.x*nx + v2.y*ny; + if (offset1 < offset) { + // Первая точка ребра во внешней полуплоскости + if (offset2 > offset) { + // Вторая точка ребра во внутренней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } else { + // Первая точка ребра во внутренней полуплоскости. Добавляем её в конечный список. + v = verticesOut[num]; + v.x = v1.x; + v.y = v1.y; + v.z = v1.z; + num++; + if (offset2 < offset) { + // Вторая точка ребра во внешней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } + v1 = v2; + offset1 = offset2; + } + return num; + } + + /** + * + * @param box + * @param tri + * @param toBox + * @param boxAxisIdx + * @param triAxisIdx + * @param contact + */ + private function findEdgesIntersection(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, boxAxisIdx:int, triAxisIdx:int, contact:Contact):Boolean { + // Определение точки и направляющего вектора ребра треугольника + var tmpx1:Number; + var tmpy1:Number; + var tmpz1:Number; + var tmpx2:Number; + var tmpy2:Number; + var tmpz2:Number; + switch (triAxisIdx) { + case 0: + tmpx1 = tri.e0.x; + tmpy1 = tri.e0.y; + tmpz1 = tri.e0.z; + tmpx2 = tri.v0.x; + tmpy2 = tri.v0.y; + tmpz2 = tri.v0.z; + break; + case 1: + tmpx1 = tri.e1.x; + tmpy1 = tri.e1.y; + tmpz1 = tri.e1.z; + tmpx2 = tri.v1.x; + tmpy2 = tri.v1.y; + tmpz2 = tri.v1.z; + break; + case 2: + tmpx1 = tri.e2.x; + tmpy1 = tri.e2.y; + tmpz1 = tri.e2.z; + tmpx2 = tri.v2.x; + tmpy2 = tri.v2.y; + tmpz2 = tri.v2.z; + break; + } + var triTransform:Matrix4 = tri.transform; + axis20.x = triTransform.a*tmpx1 + triTransform.b*tmpy1 + triTransform.c*tmpz1; + axis20.y = triTransform.e*tmpx1 + triTransform.f*tmpy1 + triTransform.g*tmpz1; + axis20.z = triTransform.i*tmpx1 + triTransform.j*tmpy1 + triTransform.k*tmpz1; + var x2:Number = triTransform.a*tmpx2 + triTransform.b*tmpy2 + triTransform.c*tmpz2 + triTransform.d; + var y2:Number = triTransform.e*tmpx2 + triTransform.f*tmpy2 + triTransform.g*tmpz2 + triTransform.h; + var z2:Number = triTransform.i*tmpx2 + triTransform.j*tmpy2 + triTransform.k*tmpz2 + triTransform.l; + + // Определение нормали контакта, точки и направляющего вектора ребра бокса + var boxTransform:Matrix4 = box.transform; + boxTransform.getAxis(boxAxisIdx, axis10); + + // Нормаль контакта + var v:Vector3 = contact.normal; + v.x = axis10.y*axis20.z - axis10.z*axis20.y; + v.y = axis10.z*axis20.x - axis10.x*axis20.z; + v.z = axis10.x*axis20.y - axis10.y*axis20.x; + k = 1/Math.sqrt(v.x*v.x + v.y*v.y + v.z*v.z); + v.x *= k; + v.y *= k; + v.z *= k; + // Разворот нормали в сторону бокса + if (v.x*toBox.x + v.y*toBox.y + v.z*toBox.z < 0) { + v.x = -v.x; + v.y = -v.y; + v.z = -v.z; + } + + var boxHalfLen:Number; + tmpx1 = box.hs.x; + tmpy1 = box.hs.y; + tmpz1 = box.hs.z; + // X + if (boxAxisIdx == 0) { + tmpx1 = 0; + boxHalfLen = box.hs.x; + } else { + if (boxTransform.a*v.x + boxTransform.e*v.y + boxTransform.i*v.z > 0) { + tmpx1 = -tmpx1; + if ((box.excludedFaces & 2) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 1) != 0) { + return false; + } + } + } + // Y + if (boxAxisIdx == 1) { + tmpy1 = 0; + boxHalfLen = box.hs.y; + } else { + if (boxTransform.b*v.x + boxTransform.f*v.y + boxTransform.j*v.z > 0) { + tmpy1 = -tmpy1; + if ((box.excludedFaces & 8) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 4) != 0) { + return false; + } + } + } + // Z + if (boxAxisIdx == 2) { + tmpz1 = 0; + boxHalfLen = box.hs.z; + } else { + if (boxTransform.c*v.x + boxTransform.g*v.y + boxTransform.k*v.z > 0) { + tmpz1 = -tmpz1; + if ((box.excludedFaces & 32) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 16) != 0) { + return false; + } + } + } + var x1:Number = boxTransform.a*tmpx1 + boxTransform.b*tmpy1 + boxTransform.c*tmpz1 + boxTransform.d; + var y1:Number = boxTransform.e*tmpx1 + boxTransform.f*tmpy1 + boxTransform.g*tmpz1 + boxTransform.h; + var z1:Number = boxTransform.i*tmpx1 + boxTransform.j*tmpy1 + boxTransform.k*tmpz1 + boxTransform.l; + + // Находим точку пересечения рёбер, решая систему уравнений + // axis10 - направляющий вектор ребра бокса + // x1, y1, z1 - средняя точка ребра бокса + // axis20 - направляющий вектор ребра треугольника + // x2, y2, z2 - начальная точка ребра треугольника + var k:Number = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + var vx:Number = x2 - x1; + var vy:Number = y2 - y1; + var vz:Number = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + + // Запись данных о контакте + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + v = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + v.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + v.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + v.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = v.x - boxTransform.d; + r.y = v.y - boxTransform.h; + r.z = v.z - boxTransform.l; + r = cp.r2; + r.x = v.x - triTransform.d; + r.y = v.y - triTransform.h; + r.z = v.z - triTransform.l; + return true; + } + + } +} diff --git a/0.0.9.5/src/alternativa/physics/collision/colliders/.svn/text-base/SpherePlaneCollider.as.svn-base b/0.0.9.5/src/alternativa/physics/collision/colliders/.svn/text-base/SpherePlaneCollider.as.svn-base new file mode 100644 index 0000000..30cd55f --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/colliders/.svn/text-base/SpherePlaneCollider.as.svn-base @@ -0,0 +1,68 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + /** + * + */ + public class SpherePlaneCollider implements ICollider { + + private var normal:Vector3 = new Vector3(); + + /** + * + */ + public function SpherePlaneCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var sphere:RigidSphere = body1 as RigidSphere; +// var plane:RigidPlane; +// if (sphere == null) { +// sphere = body2 as RigidSphere; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// +// var dist:Number = sphere.state.pos.dot(normal) - offset; +// if (dist > sphere.r) return false; +// +// collisionInfo.body1 = sphere; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// collisionInfo.pcount = 1; +// +// var cp:ContactPoint = collisionInfo.points[0]; +// cp.penetration = sphere.r - dist; +// cp.pos.copy(normal).reverse().scale(sphere.r).add(sphere.state.pos); +// cp.r1.diff(cp.pos, sphere.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/collision/colliders/.svn/text-base/SphereSphereCollider.as.svn-base b/0.0.9.5/src/alternativa/physics/collision/colliders/.svn/text-base/SphereSphereCollider.as.svn-base new file mode 100644 index 0000000..3332f3c --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/colliders/.svn/text-base/SphereSphereCollider.as.svn-base @@ -0,0 +1,70 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.math.Vector3; + use namespace altphysics; + + public class SphereSphereCollider implements ICollider { + + private var p1:Vector3 = new Vector3(); + private var p2:Vector3 = new Vector3(); + + public function SphereSphereCollider() { + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var s1:CollisionSphere; + var s2:CollisionSphere; + if (prim1.body != null) { + s1 = prim1 as CollisionSphere; + s2 = prim2 as CollisionSphere; + } else { + s1 = prim2 as CollisionSphere; + s2 = prim1 as CollisionSphere; + } + + s1.transform.getAxis(3, p1); + s2.transform.getAxis(3, p2); + var dx:Number = p1.x - p2.x; + var dy:Number = p1.y - p2.y; + var dz:Number = p1.z - p2.z; + var len:Number = dx*dx + dy*dy + dz*dz; + var sum:Number = s1.r + s2.r; + if (len > sum*sum) return false; + len = Math.sqrt(len); + dx /= len; + dy /= len; + dz /= len; + + contact.body1 = s1.body; + contact.body2 = s2.body; + contact.normal.x = dx; + contact.normal.y = dy; + contact.normal.z = dz; + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = sum - len; + cp.pos.x = p1.x - dx*s1.r; + cp.pos.y = p1.y - dy*s1.r; + cp.pos.z = p1.z - dz*s1.r; + cp.r1.vDiff(cp.pos, p1); + cp.r2.vDiff(cp.pos, p2); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/collision/colliders/BoxBoxCollider.as b/0.0.9.5/src/alternativa/physics/collision/colliders/BoxBoxCollider.as new file mode 100644 index 0000000..2d3f04c --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/colliders/BoxBoxCollider.as @@ -0,0 +1,589 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * Расчитывает точки контакта двух боксов. Нормаль контакта направляется в сторону бокса с меньшим ID. + */ + public class BoxBoxCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + private var vectorToBox1:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var bestAxisIndex:int; + private var minOverlap:Number; + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + private var tmpPoints:Vector. = new Vector.(8, true); + private var pcount:int; + + /** + * + */ + public function BoxBoxCollider() { + for (var i:int = 0; i < 8; i++) { + tmpPoints[i] = new ContactPoint(); + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + + if (bestAxisIndex < 6) { + // Контакт грань-(грань|ребро|вершина) + if (!findFaceContactPoints(box1, box2, vectorToBox1, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 6; + findEdgesIntersection(box1, box2, vectorToBox1, int(bestAxisIndex/3), bestAxisIndex%3, contact); + } + contact.body1 = box1.body; + contact.body2 = box2.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + // Вектор из центра второго бокса в центр первого + vectorToBox1.x = transform1.d - transform2.d; + vectorToBox1.y = transform1.h - transform2.h; + vectorToBox1.z = transform1.l - transform2.l; + + // Проверка пересечения по основным осям + axis10.x = transform1.a; + axis10.y = transform1.e; + axis10.z = transform1.i; + if (!testMainAxis(box1, box2, axis10, 0, vectorToBox1)) return false; + axis11.x = transform1.b; + axis11.y = transform1.f; + axis11.z = transform1.j; + if (!testMainAxis(box1, box2, axis11, 1, vectorToBox1)) return false; + axis12.x = transform1.c; + axis12.y = transform1.g; + axis12.z = transform1.k; + if (!testMainAxis(box1, box2, axis12, 2, vectorToBox1)) return false; + + axis20.x = transform2.a; + axis20.y = transform2.e; + axis20.z = transform2.i; + if (!testMainAxis(box1, box2, axis20, 3, vectorToBox1)) return false; + axis21.x = transform2.b; + axis21.y = transform2.f; + axis21.z = transform2.j; + if (!testMainAxis(box1, box2, axis21, 4, vectorToBox1)) return false; + axis22.x = transform2.c; + axis22.y = transform2.g; + axis22.z = transform2.k; + if (!testMainAxis(box1, box2, axis22, 5, vectorToBox1)) return false; + + // Проверка производных осей + if (!testDerivedAxis(box1, box2, axis10, axis20, 6, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis21, 7, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis22, 8, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis11, axis20, 9, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis21, 10, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis22, 11, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis12, axis20, 12, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis21, 13, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis22, 14, vectorToBox1)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта грани одного бокса с гранью/ребром/вершиной другого. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param faceAxisIdx индекс оси первого бокса, перпендикулярной грани, с которой произошло столкновение + * @param contactInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var swapNormal:Boolean = false; + if (faceAxisIdx > 2) { + // Столкновение с гранью второго бокса. Для дальнейших расчётов боксы меняются местами, + // но нормаль контакта всё равно должна быть направлена в сторону первоначального box1 + var tmpBox:CollisionBox = box1; + box1 = box2; + box2 = tmpBox; + vectorToBox1.x = -vectorToBox1.x; + vectorToBox1.y = -vectorToBox1.y; + vectorToBox1.z = -vectorToBox1.z; + faceAxisIdx -= 3; + swapNormal = true; + } + + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + var colAxis:Vector3 = contact.normal; + + transform1.getAxis(faceAxisIdx, colAxis); + var negativeFace:Boolean = colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z > 0; + + var code:int = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box1.excludedFaces) != 0) return false; + + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + // Ищем ось второго бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + transform2.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisDot = dot; + incidentAxisIdx = axisIdx; + } + } + // Получаем список вершин грани второго бокса, переводим их в систему координат первого бокса и выполняем обрезку + // по грани первого бокса. Таким образом получается список потенциальных точек контакта. + transform2.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box2.hs, incidentAxisIdx, incidentAxisDot < 0, points1); + + // TODO: Вычислить результирующую матрицу, затем преобразовать векторы за один заход + transform2.transformVectorsN(points1, points2, 4); + transform1.transformVectorsInverseN(points2, points1, 4); + + var pnum:int = clip(box1.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points1[i]; + if ((pen = getPointBoxPenetration(box1.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + var cp:ContactPoint = tmpPoints[pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = transform1.a*v.x + transform1.b*v.y + transform1.c*v.z + transform1.d; + cpPos.y = transform1.e*v.x + transform1.f*v.y + transform1.g*v.z + transform1.h; + cpPos.z = transform1.i*v.x + transform1.j*v.y + transform1.k*v.z + transform1.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + + cp.penetration = pen; + } + } + if (swapNormal) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (pcount > 4) { + reducePoints(); + } + for (i = 0; i < pcount; i++) { + cp = contact.points[i]; + cp.copyFrom(tmpPoints[i]); + } + contact.pcount = pcount; + return true; + } + + /** + * + * @param contactInfo + */ + private function reducePoints():void { + var i:int; + var minIdx:int; + var cp1:ContactPoint; + var cp2:ContactPoint; + while (pcount > 4) { + var minLen:Number = 1e10; + var p1:Vector3 = (tmpPoints[int(pcount - 1)] as ContactPoint).pos; + var p2:Vector3; + for (i = 0; i < pcount; i++) { + p2 = (tmpPoints[i] as ContactPoint).pos; + var dx:Number = p2.x - p1.x; + var dy:Number = p2.y - p1.y; + var dz:Number = p2.z - p1.z; + var len:Number = dx*dx + dy*dy + dz*dz; + if (len < minLen) { + minLen = len; + minIdx = i; + } + p1 = p2; + } + var ii:int = minIdx == 0 ? (pcount - 1) : (minIdx - 1); + cp1 = tmpPoints[ii]; + cp2 = tmpPoints[minIdx]; + p1 = cp1.pos; + p2 = cp2.pos; + p2.x = 0.5*(p1.x + p2.x); + p2.y = 0.5*(p1.y + p2.y); + p2.z = 0.5*(p1.z + p2.z); + cp2.penetration = 0.5*(cp1.penetration + cp2.penetration); + if (minIdx > 0) { + for (i = minIdx; i < pcount; i++) { + tmpPoints[int(i - 1)] = tmpPoints[i]; + } + tmpPoints[int(pcount - 1)] = cp1; + } + pcount--; + } + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clip(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param contactInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):void { + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + transform1.getAxis(axisIdx1, axis10); + transform2.getAxis(axisIdx2, axis20); + + var colAxis:Vector3 = contact.normal; + + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону первого бокса + if (colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + + var tx:Number = box1.hs.x; + var ty:Number = box1.hs.y; + var tz:Number = box1.hs.z; + + var x2:Number = box2.hs.x; + var y2:Number = box2.hs.y; + var z2:Number = box2.hs.z; + // x + if (axisIdx1 == 0) { + tx = 0; + halfLen1 = box1.hs.x; + } else { + if (colAxis.x*transform1.a + colAxis.y*transform1.e + colAxis.z*transform1.i > 0) { + tx = -tx; + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = box2.hs.x; + } else { + if (colAxis.x*transform2.a + colAxis.y*transform2.e + colAxis.z*transform2.i < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + ty = 0; + halfLen1 = box1.hs.y; + } else { + if (colAxis.x*transform1.b + colAxis.y*transform1.f + colAxis.z*transform1.j > 0) { + ty = -ty; + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = box2.hs.y; + } else { + if (colAxis.x*transform2.b + colAxis.y*transform2.f + colAxis.z*transform2.j < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + tz = 0; + halfLen1 = box1.hs.z; + } else { + if (colAxis.x*transform1.c + colAxis.y*transform1.g + colAxis.z*transform1.k > 0) { + tz = -tz; + } + } + if (axisIdx2 == 2) { + z2 = 0; + halfLen2 = box2.hs.z; + } else { + if (colAxis.x*transform2.c + colAxis.y*transform2.g + colAxis.z*transform2.k < 0) { + z2 = -z2; + } + } + // Получаем глобальные координаты средних точек рёбер + var x1:Number = transform1.a*tx + transform1.b*ty + transform1.c*tz + transform1.d; + var y1:Number = transform1.e*tx + transform1.f*ty + transform1.g*tz + transform1.h; + var z1:Number = transform1.i*tx + transform1.j*ty + transform1.k*tz + transform1.l; + tx = x2; + ty = y2; + tz = z2; + x2 = transform2.a*tx + transform2.b*ty + transform2.c*tz + transform2.d; + y2 = transform2.e*tx + transform2.f*ty + transform2.g*tz + transform2.h; + z2 = transform2.i*tx + transform2.j*ty + transform2.k*tz + transform2.l; + + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + tx = x2 - x1; + ty = y2 - y1; + tz = z2 - z1; + var c1:Number = axis10.x*tx + axis10.y*ty + axis10.z*tz; + var c2:Number = axis20.x*tx + axis20.y*ty + axis20.z*tz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cp.pos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cp.pos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение боксов вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box1 + * @param box2 + * @param axis + * @param axisIndex + * @param toBox1 + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box1 + * @param box2 + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox1 + * @return + */ + private function testDerivedAxis(box1:CollisionBox, box2:CollisionBox, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия двух боксов вдоль заданной оси. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param axis ось + * @param vectorToBox1 вектор, соединяющий центр второго бокса с центром первого + * @return величина перекрытия боксов вдоль оси + */ + public function overlapOnAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, vectorToBox1:Vector3):Number { + var m:Matrix4 = box1.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box1.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box1.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box1.hs.z; + if (d < 0) d = -d; + projection += d; + + m = box2.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box2.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box2.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box2.hs.z; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox1.x*axis.x + vectorToBox1.y*axis.y + vectorToBox1.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} + import alternativa.math.Vector3; + + class CollisionPointTmp { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + } diff --git a/0.0.9.5/src/alternativa/physics/collision/colliders/BoxCollider.as b/0.0.9.5/src/alternativa/physics/collision/colliders/BoxCollider.as new file mode 100644 index 0000000..2429bb1 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/colliders/BoxCollider.as @@ -0,0 +1,427 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + /** + * + */ + public class BoxCollider implements ICollider { + + /** + * + */ + public function BoxCollider() { + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + return false; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + protected function getFaceVertsByAxis(hs:Vector3, axisIdx:int, negativeFace:Boolean, result:Vector.):void { + var v:Vector3; + switch (axisIdx) { + case 0: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + } + break; + case 1: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + } + break; + case 2: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + } else { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } + break; + } + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/collision/colliders/BoxPlaneCollider.as b/0.0.9.5/src/alternativa/physics/collision/colliders/BoxPlaneCollider.as new file mode 100644 index 0000000..3bebdc3 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/colliders/BoxPlaneCollider.as @@ -0,0 +1,88 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + public class BoxPlaneCollider implements ICollider { + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var normal:Vector3 = new Vector3(); + + public function BoxPlaneCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var box:RigidBox = body1 as RigidBox; +// var plane:RigidPlane; +// if (box == null) { +// box = body2 as RigidBox; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные координаты вершин бокса +// var sx:Number = box.halfSize.x; +// var sy:Number = box.halfSize.y; +// var sz:Number = box.halfSize.z; +// (verts1[0] as Vector3).reset(-sx, -sy, -sz); +// (verts1[1] as Vector3).reset(sx, -sy, -sz); +// (verts1[2] as Vector3).reset(sx, sy, -sz); +// (verts1[3] as Vector3).reset(-sx, sy, -sz); +// (verts1[4] as Vector3).reset(-sx, -sy, sz); +// (verts1[5] as Vector3).reset(sx, -sy, sz); +// (verts1[6] as Vector3).reset(sx, sy, sz); +// (verts1[7] as Vector3).reset(-sx, sy, sz); +// +// box.transform.transformVectors(verts1, verts2); +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// // Проверяем наличие столкновений с каждой вершиной +// collisionInfo.pcount = 0; +// for (var i:int = 0; i < 8; i++) { +// // Вершина добавляется в список точек столкновения, если лежит под плоскостью +// var dist:Number = (verts2[i] as Vector3).dot(normal); +// if (dist < offset) { +// var cp:ContactPoint; +// if (collisionInfo.pcount == collisionInfo.points.length) { +// cp = new ContactPoint(); +// collisionInfo.points[collisionInfo.pcount] = cp; +// } else { +// cp = collisionInfo.points[collisionInfo.pcount]; +// } +// cp.pos.copy(verts2[i]); +// cp.r1.diff(cp.pos, box.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); +// cp.penetration = offset - dist; +// collisionInfo.pcount++; +// } +// } +// if (collisionInfo.pcount > 0) { +// collisionInfo.body1 = box; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// return true; +// } + return false; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/collision/colliders/BoxRectCollider.as b/0.0.9.5/src/alternativa/physics/collision/colliders/BoxRectCollider.as new file mode 100644 index 0000000..d6ba302 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/colliders/BoxRectCollider.as @@ -0,0 +1,575 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * + */ + public class BoxRectCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + + private var vectorToBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxRectCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, rect, vectorToBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + if (!findEdgesIntersection(box, rect, vectorToBox, int(bestAxisIndex/2), bestAxisIndex%2, contact)) { + return false; + } + } + contact.body1 = box.body; + contact.body2 = rect.body; + + // Хак для танков, чтобы исключить утыкания танков в стыки статических примитивов + if (rect.transform.k > 0.99999) { + contact.normal.x = 0; + contact.normal.y = 0; + contact.normal.z = 1; + } + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + // Вектор из центра прямоугольника в центр бокса + vectorToBox.x = boxTransform.d - rectTransform.d; + vectorToBox.y = boxTransform.h - rectTransform.h; + vectorToBox.z = boxTransform.l - rectTransform.l; + + // Проверка пересечения по нормали прямоугольника + rectTransform.getAxis(2, axis22); + if (!testMainAxis(box, rect, axis22, 0, vectorToBox)) return false; + + // Проверка пересечения по основным осям бокса + boxTransform.getAxis(0, axis10); + if (!testMainAxis(box, rect, axis10, 1, vectorToBox)) return false; + boxTransform.getAxis(1, axis11); + if (!testMainAxis(box, rect, axis11, 2, vectorToBox)) return false; + boxTransform.getAxis(2, axis12); + if (!testMainAxis(box, rect, axis12, 3, vectorToBox)) return false; + + // Получаем направляющие рёбер прямоугольника + rectTransform.getAxis(0, axis20); + rectTransform.getAxis(1, axis21); + + // Проверка производных осей + if (!testDerivedAxis(box, rect, axis10, axis20, 4, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis10, axis21, 5, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis11, axis20, 6, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis11, axis21, 7, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis12, axis20, 8, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis12, axis21, 9, vectorToBox)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта бокса с прямоугольником. + * + * @param box бокс + * @param rect прямоугольник + * @param vectorToBox вектор, направленный из центра прямоугольника в центр бокса + * @param faceAxisIdx индекс оси, идентифицирующей полскость столкновения (грань бокса или полскость прямоугольника) + * @param colInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var pnum:int; + var i:int; + var v:Vector3; + var cp:ContactPoint; + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + var colAxis:Vector3 = contact.normal; + + var negativeFace:Boolean; + var code:int; + + if (faceAxisIdx == 0) { + // Столкновение с плоскостью прямоугольника + + // Проверим положение бокса относительно плоскости прямоугольника + colAxis.x = rectTransform.c; + colAxis.y = rectTransform.g; + colAxis.z = rectTransform.k; + +// var offset:Number = colAxis.x*rectTransform.d + colAxis.y*rectTransform.h + colAxis.z*rectTransform.l; +// if (bbPos.vDot(colAxis) < offset) return false; + + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisIdx = axisIdx; + incidentAxisDot = dot; + } + } + negativeFace = incidentAxisDot > 0; + + code = 1 << (incidentAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + + // Получаем список вершин грани бокса, переводим их в систему координат прямоугольника и выполняем обрезку + // по прямоугольнику. Таким образом получается список потенциальных точек контакта. + boxTransform.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box.hs, incidentAxisIdx, negativeFace, points1); + boxTransform.transformVectorsN(points1, points2, 4); + rectTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByRect(rect.hs); + // Проверяем каждую потенциальную точку на принадлежность нижней полуплоскости прямоугольника и добавляем такие точки в список контактов + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if (v.z < epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = -v.z; + var cpPos:Vector3 = cp.pos; + cpPos.x = rectTransform.a*v.x + rectTransform.b*v.y + rectTransform.c*v.z + rectTransform.d; + cpPos.y = rectTransform.e*v.x + rectTransform.f*v.y + rectTransform.g*v.z + rectTransform.h; + cpPos.z = rectTransform.i*v.x + rectTransform.j*v.y + rectTransform.k*v.z + rectTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } else { + // Столкновение с гранью бокса + faceAxisIdx--; + boxTransform.getAxis(faceAxisIdx, colAxis); + negativeFace = colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z > 0; + + code = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) { + return false; + } + + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (rectTransform.c*colAxis.x + rectTransform.g*colAxis.y + rectTransform.k*colAxis.z < 0) return false; + + getFaceVertsByAxis(rect.hs, 2, false, points1); + rectTransform.transformVectorsN(points1, points2, 4); + boxTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if ((pen = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = pen; + cpPos = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } + return true; + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * + * @param hs + * @return + */ + private function clipByRect(hs:Vector3):int { + var pnum:int = 4; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box первый бокс + * @param rect второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param colInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + boxTransform.getAxis(axisIdx1, axis10); + rectTransform.getAxis(axisIdx2, axis20); + var colAxis:Vector3 = contact.normal; + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону бокса + if (colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + var vx:Number = box.hs.x; + var vy:Number = box.hs.y; + var vz:Number = box.hs.z; + var x2:Number = rect.hs.x; + var y2:Number = rect.hs.y; + var z2:Number = rect.hs.z; + // x + if (axisIdx1 == 0) { + vx = 0; + halfLen1 = box.hs.x; + } else { + if (boxTransform.a*colAxis.x + boxTransform.e*colAxis.y + boxTransform.i*colAxis.z > 0) { + vx = -vx; + if ((box.excludedFaces & 2) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 1) != 0) { + return false; + } + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = rect.hs.x; + } else { + if (rectTransform.a*colAxis.x + rectTransform.e*colAxis.y + rectTransform.i*colAxis.z < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + vy = 0; + halfLen1 = box.hs.y; + } else { + if (boxTransform.b*colAxis.x + boxTransform.f*colAxis.y + boxTransform.j*colAxis.z > 0) { + vy = -vy; + if ((box.excludedFaces & 8) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 4) != 0) { + return false; + } + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = rect.hs.y; + } else { + if (rectTransform.b*colAxis.x + rectTransform.f*colAxis.y + rectTransform.j*colAxis.z < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + vz = 0; + halfLen1 = box.hs.z; + } else { + if (boxTransform.c*colAxis.x + boxTransform.g*colAxis.y + boxTransform.k*colAxis.z > 0) { + vz = -vz; + if ((box.excludedFaces & 32) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 16) != 0) { + return false; + } + } + } + // Получаем глобальные координаты средних точек рёбер + + var x1:Number = boxTransform.a*vx + boxTransform.b*vy + boxTransform.c*vz + boxTransform.d; + var y1:Number = boxTransform.e*vx + boxTransform.f*vy + boxTransform.g*vz + boxTransform.h; + var z1:Number = boxTransform.i*vx + boxTransform.j*vy + boxTransform.k*vz + boxTransform.l; + vx = x2; + vy = y2; + vz = z2; + x2 = rectTransform.a*vx + rectTransform.b*vy + rectTransform.c*vz + rectTransform.d; + y2 = rectTransform.e*vx + rectTransform.f*vy + rectTransform.g*vz + rectTransform.h; + z2 = rectTransform.i*vx + rectTransform.j*vy + rectTransform.k*vz + rectTransform.l; + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + vx = x2 - x1; + vy = y2 - y1; + vz = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cpPos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cpPos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cpPos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var v:Vector3 = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + return true; + } + + /** + * Проверяет пересечение вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box + * @param rect + * @param axis + * @param axisIndex + * @param vectorToBox + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param rect + * @param axis1 + * @param axis2 + * @param axisIndex + * @param vectorToBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, rect:CollisionRect, axis1:Vector3, axis2:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия вдоль заданной оси. + * + * @param box бокс + * @param rect прямоугольник + * @param axis ось + * @param vectorToBox вектор, соединяющий центр прямоугольника с центром бокса + * @return величина перекрытия вдоль оси + */ + public function overlapOnAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, vectorToBox:Vector3):Number { + var m:Matrix4 = box.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box.hs.z; + if (d < 0) d = -d; + projection += d; + + m = rect.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*rect.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*rect.hs.y; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox.x*axis.x + vectorToBox.y*axis.y + vectorToBox.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} diff --git a/0.0.9.5/src/alternativa/physics/collision/colliders/BoxSphereCollider.as b/0.0.9.5/src/alternativa/physics/collision/colliders/BoxSphereCollider.as new file mode 100644 index 0000000..da5e36d --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/colliders/BoxSphereCollider.as @@ -0,0 +1,168 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.math.Vector3; + use namespace altphysics; + /** + * + */ + public class BoxSphereCollider implements ICollider { + + private var center:Vector3 = new Vector3(); + private var closestPt:Vector3 = new Vector3(); + + private var bPos:Vector3 = new Vector3(); + private var sPos:Vector3 = new Vector3(); + + /** + * + */ + public function BoxSphereCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + // Зафиксированно столкновение + contact.body1 = sphere.body; + contact.body2 = box.body; + contact.normal.vCopy(closestPt).vTransformBy4(box.transform).vSubtract(sPos).vNormalize().vReverse(); + contact.pcount = 1; + + var cp:ContactPoint = contact.points[0]; + cp.penetration = sphere.r - Math.sqrt(distSqr); + cp.pos.vCopy(contact.normal).vScale(-sphere.r).vAdd(sPos); + cp.r1.vDiff(cp.pos, sPos); + cp.r2.vDiff(cp.pos, bPos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + + return true; + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/collision/colliders/BoxTriangleCollider.as b/0.0.9.5/src/alternativa/physics/collision/colliders/BoxTriangleCollider.as new file mode 100644 index 0000000..935525d --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/colliders/BoxTriangleCollider.as @@ -0,0 +1,725 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionTriangle; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * + */ + public class BoxTriangleCollider extends BoxCollider { + + public var epsilon:Number = 0.001; + + private var bestAxisIndex:int; + private var minOverlap:Number; + private var toBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var colNormal:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxTriangleCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, tri, toBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + if (!findEdgesIntersection(box, tri, toBox, bestAxisIndex%3, int(bestAxisIndex/3), contact)) return false; + } + + contact.body1 = box.body; + contact.body2 = tri.body; + + // Хак для танков, чтобы исключить утыкания танков в стыки статических примитивов + if (tri.transform.k > 0.99999) { + contact.normal.x = 0; + contact.normal.y = 0; + contact.normal.z = 1; + } + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + toBox.x = boxTransform.d - triTransform.d; + toBox.y = boxTransform.h - triTransform.h; + toBox.z = boxTransform.l - triTransform.l; + + minOverlap = 1e308; + + // Сначала проверяется нормаль треугольника + axis.x = triTransform.c; + axis.y = triTransform.g; + axis.z = triTransform.k; + if (!testMainAxis(box, tri, axis, 0, toBox)) return false; + + // Проверка основных осей бокса + axis10.x = boxTransform.a; + axis10.y = boxTransform.e; + axis10.z = boxTransform.i; + if (!testMainAxis(box, tri, axis10, 1, toBox)) return false; + axis11.x = boxTransform.b; + axis11.y = boxTransform.f; + axis11.z = boxTransform.j; + if (!testMainAxis(box, tri, axis11, 2, toBox)) return false; + axis12.x = boxTransform.c; + axis12.y = boxTransform.g; + axis12.z = boxTransform.k; + if (!testMainAxis(box, tri, axis12, 3, toBox)) return false; + + // Проверка производных осей + // TODO: заменить вычисления векторных произведений инлайнами + var v:Vector3 = tri.e0; + axis20.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis20.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis20.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis20, 4, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis20, 5, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis20, 6, toBox)) return false; + + v = tri.e1; + axis21.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis21.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis21.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis21, 7, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis21, 8, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis21, 9, toBox)) return false; + + v = tri.e2; + axis22.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis22.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis22.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis22, 10, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis22, 11, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis22, 12, toBox)) return false; + + return true; + } + + /** + * Тестирует пересечение примитивов вдоль заданной оси. + * + * @param box бокс + * @param tri треугольник + * @param axis ось + * @param axisIndex индекс оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return true, если примитивы имеют перекрытие проекций вдоль указанной оси, иначе false + */ + private function testMainAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, axisIndex:int, toBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param tri + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, tri:CollisionTriangle, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Рассчитывает величину перекрытия проекций бокса и треугольника на заданную ось. + * + * @param box бокс + * @param tri треугольник + * @param axis единичный направляющий вектор оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return величина перекрытия. Положительное значение указывает на наличие перекрытия, нулевое или отрицательное значение указывает на отсутствие перекрытия. + */ + private function overlapOnAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, toBox:Vector3):Number { + var t:Matrix4 = box.transform; + var projection:Number = (t.a*axis.x + t.e*axis.y + t.i*axis.z)*box.hs.x; + if (projection < 0) projection = -projection; + var d:Number = (t.b*axis.x + t.f*axis.y + t.j*axis.z)*box.hs.y; + projection += d < 0 ? -d : d; + d = (t.c*axis.x + t.g*axis.y + t.k*axis.z)*box.hs.z; + projection += d < 0 ? -d : d; + + var vectorProjection:Number = toBox.x*axis.x + toBox.y*axis.y + toBox.z*axis.z; + // Для оптимизации ось преобразуется в систему треугольника, а не его вершины в мировую систему + t = tri.transform; + var ax:Number = t.a*axis.x + t.e*axis.y + t.i*axis.z; + var ay:Number = t.b*axis.x + t.f*axis.y + t.j*axis.z; + var az:Number = t.c*axis.x + t.g*axis.y + t.k*axis.z; + var max:Number = 0; + if (vectorProjection < 0) { + vectorProjection = -vectorProjection; + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d < max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d < max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d < max) max = d; + max = -max; + } else { + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d > max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d > max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d > max) max = d; + } + + return projection + max - vectorProjection; + } + + /** + * Определяет точки контакта бокса и треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function findFaceContactPoints(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIndex:int, contact:Contact):Boolean { + if (faceAxisIndex == 0) { + // Столкновение с плоскостью треугольника + return getBoxToTriContact(box, tri, toBox, contact); + } else { + // Столкновение с гранью бокса + return getTriToBoxContact(box, tri, toBox, faceAxisIndex, contact); + } + } + + /** + * Определяет точки контакта бокса с плоскостью треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getBoxToTriContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + colNormal.x = triTransform.c; + colNormal.y = triTransform.g; + colNormal.z = triTransform.k; + + var over:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + if (!over) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var incAxisDot:Number = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colNormal.x + axis.y*colNormal.y + axis.z*colNormal.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incAxisDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Обрезка грани + var negativeFace:Boolean = incAxisDot > 0; + + var code:int = 1 << (incFaceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + getFaceVertsByAxis(box.hs, incFaceAxisIdx, negativeFace, points1); + boxTransform.transformVectorsN(points1, points2, 4); + triTransform.transformVectorsInverseN(points2, points1, 4); + var pnum:int = clipByTriangle(tri); + // Среди конечного списка точек определяются лежащие под плоскостью треугольника + var cp:ContactPoint; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points2[i]; + if ((over && v.z < 0) || (!over && v.z > 0)) { + cp = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z + triTransform.d; + cpPos.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z + triTransform.h; + cpPos.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z + triTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = over ? -v.z : v.z; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Оперделяет точки контакта треугольника с гранью бокса. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getTriToBoxContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + faceAxisIdx--; + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + boxTransform.getAxis(faceAxisIdx, colNormal); + var negativeFace:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + + var code:int = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + if (!negativeFace) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + var v:Vector3 = points1[0]; + v.x = tri.v0.x; + v.y = tri.v0.y; + v.z = tri.v0.z; + + v = points1[1]; + v.x = tri.v1.x; + v.y = tri.v1.y; + v.z = tri.v1.z; + + v = points1[2]; + v.x = tri.v2.x; + v.y = tri.v2.y; + v.z = tri.v2.z; + + triTransform.transformVectorsN(points1, points2, 3); + boxTransform.transformVectorsInverseN(points2, points1, 3); + + var pnum:int = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность боксу и добавляем такие точки в список контактов + var penetration:Number; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + v = points1[i]; + penetration = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace); + if (penetration > -epsilon) { + var cp:ContactPoint = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = penetration; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Вычисляет величину проникновения точки в бокс. + * + * @param hs вектор половинных размеров бокса + * @param p точка в системе координат бокса + * @param axisIndex индекс оси + * @param negativeFace если true, проверяется нижняя грань + * @return величина проникновения точки в бокс + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, negativeFace:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (negativeFace) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (negativeFace) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (negativeFace) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, вершины которой заданы в списке points1. Результат сохраняется в этом же списке. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 3; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Обрезает грань прямоугольника сторонами треугольника. Обрезка выполняется в системе координат и в проекции на + * плоскость треугольника. Входные вершины грани в количестве четырёх штук должны находиться в списке points1. + * Конечные вершины сохраняются в списке points2. + * + * @param tri треугольник + * @return количество врешин в конечном списке + */ + private function clipByTriangle(tri:CollisionTriangle):int { + var vnum:int = 4; + vnum = clipByLine(tri.v0, tri.e0, points1, vnum, points2); + if (vnum == 0) return 0; + vnum = clipByLine(tri.v1, tri.e1, points2, vnum, points1); + if (vnum == 0) return 0; + return clipByLine(tri.v2, tri.e2, points1, vnum, points2);; + } + + /** + * Обрезает полигон указанной прямой. + * + * @param linePoint точка на прямой + * @param lineDir единичный направляющий вектор прямой + * @param verticesIn список вершин исходного полигона + * @param vnum количество вершин исходного полигона + * @param verticesOut список, куда будут записаны вершины конечного полигона + * @return количество вершин конечного полигона + */ + private function clipByLine(linePoint:Vector3, lineDir:Vector3, verticesIn:Vector., vnum:int, verticesOut:Vector.):int { + var nx:Number = -lineDir.y; + var ny:Number = lineDir.x; + var offset:Number = linePoint.x*nx + linePoint.y*ny; + var v1:Vector3 = verticesIn[int(vnum - 1)]; + var offset1:Number = v1.x*nx + v1.y*ny; + var t:Number; + var v:Vector3; + var num:int = 0; + for (var i:int = 0; i < vnum; i++) { + var v2:Vector3 = verticesIn[i]; + var offset2:Number = v2.x*nx + v2.y*ny; + if (offset1 < offset) { + // Первая точка ребра во внешней полуплоскости + if (offset2 > offset) { + // Вторая точка ребра во внутренней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } else { + // Первая точка ребра во внутренней полуплоскости. Добавляем её в конечный список. + v = verticesOut[num]; + v.x = v1.x; + v.y = v1.y; + v.z = v1.z; + num++; + if (offset2 < offset) { + // Вторая точка ребра во внешней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } + v1 = v2; + offset1 = offset2; + } + return num; + } + + /** + * + * @param box + * @param tri + * @param toBox + * @param boxAxisIdx + * @param triAxisIdx + * @param contact + */ + private function findEdgesIntersection(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, boxAxisIdx:int, triAxisIdx:int, contact:Contact):Boolean { + // Определение точки и направляющего вектора ребра треугольника + var tmpx1:Number; + var tmpy1:Number; + var tmpz1:Number; + var tmpx2:Number; + var tmpy2:Number; + var tmpz2:Number; + switch (triAxisIdx) { + case 0: + tmpx1 = tri.e0.x; + tmpy1 = tri.e0.y; + tmpz1 = tri.e0.z; + tmpx2 = tri.v0.x; + tmpy2 = tri.v0.y; + tmpz2 = tri.v0.z; + break; + case 1: + tmpx1 = tri.e1.x; + tmpy1 = tri.e1.y; + tmpz1 = tri.e1.z; + tmpx2 = tri.v1.x; + tmpy2 = tri.v1.y; + tmpz2 = tri.v1.z; + break; + case 2: + tmpx1 = tri.e2.x; + tmpy1 = tri.e2.y; + tmpz1 = tri.e2.z; + tmpx2 = tri.v2.x; + tmpy2 = tri.v2.y; + tmpz2 = tri.v2.z; + break; + } + var triTransform:Matrix4 = tri.transform; + axis20.x = triTransform.a*tmpx1 + triTransform.b*tmpy1 + triTransform.c*tmpz1; + axis20.y = triTransform.e*tmpx1 + triTransform.f*tmpy1 + triTransform.g*tmpz1; + axis20.z = triTransform.i*tmpx1 + triTransform.j*tmpy1 + triTransform.k*tmpz1; + var x2:Number = triTransform.a*tmpx2 + triTransform.b*tmpy2 + triTransform.c*tmpz2 + triTransform.d; + var y2:Number = triTransform.e*tmpx2 + triTransform.f*tmpy2 + triTransform.g*tmpz2 + triTransform.h; + var z2:Number = triTransform.i*tmpx2 + triTransform.j*tmpy2 + triTransform.k*tmpz2 + triTransform.l; + + // Определение нормали контакта, точки и направляющего вектора ребра бокса + var boxTransform:Matrix4 = box.transform; + boxTransform.getAxis(boxAxisIdx, axis10); + + // Нормаль контакта + var v:Vector3 = contact.normal; + v.x = axis10.y*axis20.z - axis10.z*axis20.y; + v.y = axis10.z*axis20.x - axis10.x*axis20.z; + v.z = axis10.x*axis20.y - axis10.y*axis20.x; + k = 1/Math.sqrt(v.x*v.x + v.y*v.y + v.z*v.z); + v.x *= k; + v.y *= k; + v.z *= k; + // Разворот нормали в сторону бокса + if (v.x*toBox.x + v.y*toBox.y + v.z*toBox.z < 0) { + v.x = -v.x; + v.y = -v.y; + v.z = -v.z; + } + + var boxHalfLen:Number; + tmpx1 = box.hs.x; + tmpy1 = box.hs.y; + tmpz1 = box.hs.z; + // X + if (boxAxisIdx == 0) { + tmpx1 = 0; + boxHalfLen = box.hs.x; + } else { + if (boxTransform.a*v.x + boxTransform.e*v.y + boxTransform.i*v.z > 0) { + tmpx1 = -tmpx1; + if ((box.excludedFaces & 2) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 1) != 0) { + return false; + } + } + } + // Y + if (boxAxisIdx == 1) { + tmpy1 = 0; + boxHalfLen = box.hs.y; + } else { + if (boxTransform.b*v.x + boxTransform.f*v.y + boxTransform.j*v.z > 0) { + tmpy1 = -tmpy1; + if ((box.excludedFaces & 8) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 4) != 0) { + return false; + } + } + } + // Z + if (boxAxisIdx == 2) { + tmpz1 = 0; + boxHalfLen = box.hs.z; + } else { + if (boxTransform.c*v.x + boxTransform.g*v.y + boxTransform.k*v.z > 0) { + tmpz1 = -tmpz1; + if ((box.excludedFaces & 32) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 16) != 0) { + return false; + } + } + } + var x1:Number = boxTransform.a*tmpx1 + boxTransform.b*tmpy1 + boxTransform.c*tmpz1 + boxTransform.d; + var y1:Number = boxTransform.e*tmpx1 + boxTransform.f*tmpy1 + boxTransform.g*tmpz1 + boxTransform.h; + var z1:Number = boxTransform.i*tmpx1 + boxTransform.j*tmpy1 + boxTransform.k*tmpz1 + boxTransform.l; + + // Находим точку пересечения рёбер, решая систему уравнений + // axis10 - направляющий вектор ребра бокса + // x1, y1, z1 - средняя точка ребра бокса + // axis20 - направляющий вектор ребра треугольника + // x2, y2, z2 - начальная точка ребра треугольника + var k:Number = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + var vx:Number = x2 - x1; + var vy:Number = y2 - y1; + var vz:Number = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + + // Запись данных о контакте + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + v = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + v.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + v.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + v.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = v.x - boxTransform.d; + r.y = v.y - boxTransform.h; + r.z = v.z - boxTransform.l; + r = cp.r2; + r.x = v.x - triTransform.d; + r.y = v.y - triTransform.h; + r.z = v.z - triTransform.l; + return true; + } + + } +} diff --git a/0.0.9.5/src/alternativa/physics/collision/colliders/SpherePlaneCollider.as b/0.0.9.5/src/alternativa/physics/collision/colliders/SpherePlaneCollider.as new file mode 100644 index 0000000..30cd55f --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/colliders/SpherePlaneCollider.as @@ -0,0 +1,68 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + /** + * + */ + public class SpherePlaneCollider implements ICollider { + + private var normal:Vector3 = new Vector3(); + + /** + * + */ + public function SpherePlaneCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var sphere:RigidSphere = body1 as RigidSphere; +// var plane:RigidPlane; +// if (sphere == null) { +// sphere = body2 as RigidSphere; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// +// var dist:Number = sphere.state.pos.dot(normal) - offset; +// if (dist > sphere.r) return false; +// +// collisionInfo.body1 = sphere; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// collisionInfo.pcount = 1; +// +// var cp:ContactPoint = collisionInfo.points[0]; +// cp.penetration = sphere.r - dist; +// cp.pos.copy(normal).reverse().scale(sphere.r).add(sphere.state.pos); +// cp.r1.diff(cp.pos, sphere.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/collision/colliders/SphereSphereCollider.as b/0.0.9.5/src/alternativa/physics/collision/colliders/SphereSphereCollider.as new file mode 100644 index 0000000..3332f3c --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/colliders/SphereSphereCollider.as @@ -0,0 +1,70 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.math.Vector3; + use namespace altphysics; + + public class SphereSphereCollider implements ICollider { + + private var p1:Vector3 = new Vector3(); + private var p2:Vector3 = new Vector3(); + + public function SphereSphereCollider() { + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var s1:CollisionSphere; + var s2:CollisionSphere; + if (prim1.body != null) { + s1 = prim1 as CollisionSphere; + s2 = prim2 as CollisionSphere; + } else { + s1 = prim2 as CollisionSphere; + s2 = prim1 as CollisionSphere; + } + + s1.transform.getAxis(3, p1); + s2.transform.getAxis(3, p2); + var dx:Number = p1.x - p2.x; + var dy:Number = p1.y - p2.y; + var dz:Number = p1.z - p2.z; + var len:Number = dx*dx + dy*dy + dz*dz; + var sum:Number = s1.r + s2.r; + if (len > sum*sum) return false; + len = Math.sqrt(len); + dx /= len; + dy /= len; + dz /= len; + + contact.body1 = s1.body; + contact.body2 = s2.body; + contact.normal.x = dx; + contact.normal.y = dy; + contact.normal.z = dz; + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = sum - len; + cp.pos.x = p1.x - dx*s1.r; + cp.pos.y = p1.y - dy*s1.r; + cp.pos.z = p1.z - dz*s1.r; + cp.r1.vDiff(cp.pos, p1); + cp.r2.vDiff(cp.pos, p2); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/collision/primitives/.svn/all-wcprops b/0.0.9.5/src/alternativa/physics/collision/primitives/.svn/all-wcprops new file mode 100644 index 0000000..f10c09c --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/primitives/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 124 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/collision/primitives +END +CollisionTriangle.as +K 25 +svn:wc:ra_dav:version-url +V 145 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/collision/primitives/CollisionTriangle.as +END +CollisionSphere.as +K 25 +svn:wc:ra_dav:version-url +V 143 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/collision/primitives/CollisionSphere.as +END +CollisionBox.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/collision/primitives/CollisionBox.as +END +CollisionRect.as +K 25 +svn:wc:ra_dav:version-url +V 141 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/collision/primitives/CollisionRect.as +END diff --git a/0.0.9.5/src/alternativa/physics/collision/primitives/.svn/entries b/0.0.9.5/src/alternativa/physics/collision/primitives/.svn/entries new file mode 100644 index 0000000..99440c9 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/primitives/.svn/entries @@ -0,0 +1,76 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/collision/primitives +http://svndev.alternativaplatform.com + + + +2010-08-20T07:58:00.614935Z +39732 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +CollisionTriangle.as +file + + + + +2010-10-28T04:32:45.000000Z +23ebb96cda309378ce15ead4cf865446 +2009-11-24T08:44:15.941480Z +23758 +mike + +CollisionSphere.as +file + + + + +2010-10-28T04:32:45.000000Z +335b6b72039123d6d54fee4d6b17f74e +2009-11-24T08:44:15.941480Z +23758 +mike + +CollisionBox.as +file + + + + +2010-10-28T04:32:45.000000Z +f20e92414d5140bd3eb8759052f89bdc +2010-08-20T07:58:00.614935Z +39732 +mike + +CollisionRect.as +file + + + + +2010-10-28T04:32:45.000000Z +91b913679dca5ae4baabafb09c5aa051 +2009-11-24T08:44:15.941480Z +23758 +mike + diff --git a/0.0.9.5/src/alternativa/physics/collision/primitives/.svn/format b/0.0.9.5/src/alternativa/physics/collision/primitives/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/primitives/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.5/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base b/0.0.9.5/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base new file mode 100644 index 0000000..764bd1b --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base @@ -0,0 +1,215 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * Ориентированный бокс. + */ + public class CollisionBox extends CollisionPrimitive { + + // Половинные размеры вдоль каждой из осей + public var hs:Vector3 = new Vector3(); + + public var excludedFaces:int; + + /** + * @param hs + * @param collisionGroup + */ + public function CollisionBox(hs:Vector3, collisionGroup:int) { + super(BOX, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * @return - + */ + override public function calculateAABB():BoundBox { + var t:Matrix4 = transform; + + var xx:Number; + var yy:Number; + var zz:Number; + + xx = t.a < 0 ? -t.a : t.a; + yy = t.b < 0 ? -t.b : t.b; + zz = t.c < 0 ? -t.c : t.c; + aabb.maxX = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minX = -aabb.maxX; + + xx = t.e < 0 ? -t.e : t.e; + yy = t.f < 0 ? -t.f : t.f; + zz = t.g < 0 ? -t.g : t.g; + aabb.maxY = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minY = -aabb.maxY; + + xx = t.i < 0 ? -t.i : t.i; + yy = t.j < 0 ? -t.j : t.j; + zz = t.k < 0 ? -t.k : t.k; + aabb.maxZ = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return - + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var box:CollisionBox = source as CollisionBox; + if (box == null) return this; + super.copyFrom(box); + hs.vCopy(box.hs); + return this; + } + + /** + * @return - + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionBox(hs, collisionGroup); + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + var tMin:Number = -1; + var tMax:Number = 1e308; + var t1:Number; + var t2:Number; + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + + // Проверка, не находится ли начало луча внутри бокса. В этом случае точкой пересечения будет начало луча, + // а нормалью будет нормированный вектор, противоположный вектору луча. +// if (ox >= -hs.x && ox <= hs.x && oy >= -hs.y && oy <= hs.y && oz >= -hs.z && oz <= hs.z) { +// normal.x = -vector.x; +// normal.y = -vector.y; +// normal.z = -vector.z; +// normal.vNormalize(); +// return 0; +// } + + // X + if (vx < epsilon && vx > -epsilon) { + if (ox < -hs.x || ox > hs.x) return -1; + } else { + t1 = (-hs.x - ox)/vx; + t2 = (hs.x - ox)/vx; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.x = -1; + normal.y = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.x = 1; + normal.y = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Y + if (vy < epsilon && vy > -epsilon) { + if (oy < -hs.y || oy > hs.y) return -1; + } else { + t1 = (-hs.y - oy)/vy; + t2 = (hs.y - oy)/vy; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.y = -1; + normal.x = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.y = 1; + normal.x = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Z + if (vz < epsilon && vz > -epsilon) { + if (oz < -hs.z || oz > hs.z) return -1; + } else { + t1 = (-hs.z - oz)/vz; + t2 = (hs.z - oz)/vz; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.z = -1; + normal.x = normal.y = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.z = 1; + normal.x = normal.y = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Перевод нормали в мировую систему координат + // Inlined normal.vDeltaTransformBy4(transform); + vx = normal.x; + vy = normal.y; + vz = normal.z; + normal.x = transform.a*vx + transform.b*vy + transform.c*vz; + normal.y = transform.e*vx + transform.f*vy + transform.g*vz; + normal.z = transform.i*vx + transform.j*vy + transform.k*vz; + + return tMin; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionBox hs=" + hs + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base b/0.0.9.5/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base new file mode 100644 index 0000000..7a33b79 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base @@ -0,0 +1,143 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * Ориентированный прямоугольник. Задаётся половинами размеров вдоль осей X (ширина) и Y (длина) локальной системы + * координат примитива. Таким образом, прямоугольник лежит в плоскости XY, его стороны параллельны осям этой + * плоскости, а нормаль направлена вдоль локальной оси Z. + * + * Прямоугольник может быть одно- или двусторонним. В случае одностороннего прямоугольника, + * столкновения с внутренней стороны не регистрируются. + */ + public class CollisionRect extends CollisionPrimitive { + // Половинные размеры прямоугольника вдоль осей X и Y. Нормаль направлена вдоль оси Z. + public var hs:Vector3 = new Vector3(); + // Флаг указывает, является примитив одно- или двусторонним + public var twoSided:Boolean = true; + + // Малое значение. Используется для указания фиктивной высоты примитива и в функции определения пересечения с сегментом. + private static const EPSILON:Number = 0.005; + + /** + * Создаёт новый экземпляр примитива. + * + * @param hs половинные размерв прямоугольника вдоль осей X и Y. Значение z игнорируется. + * @param collisionGroup группа примитива + */ + public function CollisionRect(hs:Vector3, collisionGroup:int) { + super(RECT, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * Расчитывает ограничивающий бокс прямоугольника. Для избежания проблем высота примитива принимается равной + * не нулю, а малому значению. + * + * @return + */ + override public function calculateAABB():BoundBox { + // Баунд бокс прямоугольника имеет минимальную высоту, отличную от нуля во избежание проблем с построением kd-дерева + var t:Matrix4 = transform; + var xx:Number = t.a < 0 ? -t.a : t.a; + var yy:Number = t.b < 0 ? -t.b : t.b; + var zz:Number = t.c < 0 ? -t.c : t.c; + aabb.maxX = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minX = -aabb.maxX; + + xx = t.e < 0 ? -t.e : t.e; + yy = t.f < 0 ? -t.f : t.f; + zz = t.g < 0 ? -t.g : t.g; + aabb.maxY = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minY = -aabb.maxY; + + xx = t.i < 0 ? -t.i : t.i; + yy = t.j < 0 ? -t.j : t.j; + zz = t.k < 0 ? -t.k : t.k; + aabb.maxZ = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var rect:CollisionRect = source as CollisionRect; + if (rect == null) return this; + super.copyFrom(rect); + hs.vCopy(rect.hs); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionRect hs=" + hs + "]"; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionRect(hs, collisionGroup); + } + + /** + * @param origin + * @param vector + * @param collisionGroup + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + + // Проверка параллельности сегмента и плоскости примитива + if (vz > -threshold && vz < threshold) return -1; + var t:Number = -oz/vz; + if (t < 0) return -1; + // Проверка вхождения точки пересечения в прямоугольник + ox += vx*t; + oy += vy*t; + oz = 0; + if (ox < (-hs.x - threshold) || ox > (hs.x + threshold) || oy < (-hs.y - threshold) || oy > (hs.y + threshold)) return -1; + + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + return t; + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base b/0.0.9.5/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base new file mode 100644 index 0000000..b912a5b --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base @@ -0,0 +1,81 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Vector3; + use namespace altphysics; + + + /** + * Сфера. + */ + public class CollisionSphere extends CollisionPrimitive { + + // Радиус сферы + public var r:Number = 0; + + /** + * + * @param r + * @param collisionGroup + */ + public function CollisionSphere(r:Number, collisionGroup:int) { + super(SPHERE, collisionGroup); + this.r = r; + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + aabb.maxX = transform.d + r; + aabb.minX = transform.d - r; + + aabb.maxY = transform.h + r; + aabb.minY = transform.h - r; + + aabb.maxZ = transform.l + r; + aabb.minZ = transform.l - r; + + return aabb; + } + + /** + * @param origin + * @param vector + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var px:Number = origin.x - transform.d; + var py:Number = origin.y - transform.h; + var pz:Number = origin.z - transform.l; + var k:Number = vector.x*px + vector.y*py + vector.z*pz; + if (k > 0) return -1; + var a:Number = vector.x*vector.x + vector.y*vector.y + vector.z*vector.z; + var D:Number = k*k - a*(px*px + py*py + pz*pz - r*r); + if (D < 0) return -1; + return -(k + Math.sqrt(D))/a; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var sphere:CollisionSphere = source as CollisionSphere; + if (sphere == null) return this; + super.copyFrom(sphere); + r = sphere.r; + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionSphere(r, collisionGroup); + } + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base b/0.0.9.5/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base new file mode 100644 index 0000000..bdc3166 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base @@ -0,0 +1,244 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * Примитив треугольник. + */ + public class CollisionTriangle extends CollisionPrimitive { + + public var v0:Vector3 = new Vector3(); + public var v1:Vector3 = new Vector3(); + public var v2:Vector3 = new Vector3(); + + public var e0:Vector3 = new Vector3(); + public var e1:Vector3 = new Vector3(); + public var e2:Vector3 = new Vector3(); + +// public var len0:Number; +// public var len1:Number; +// public var len2:Number; +// + /** + * + * @param v0 + * @param v1 + * @param v2 + * @param collisionGroup + */ + public function CollisionTriangle(v0:Vector3, v1:Vector3, v2:Vector3, collisionGroup:int) { + super(TRIANGLE, collisionGroup); + initVertices(v0, v1, v2); + } + + /** + * Рассчитывает AABB примитива. + * + * @return ссылка на свой AABB + */ + override public function calculateAABB():BoundBox { + var epsilon:Number = 0.005; + var a:Number; + var b:Number; + var eps_c:Number = epsilon*transform.c; + var eps_g:Number = epsilon*transform.g; + var eps_k:Number = epsilon*transform.k; + + // Вершина 0 + // Ось X + a = v0.x*transform.a + v0.y*transform.b; + aabb.minX = aabb.maxX = a + eps_c; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v0.x*transform.e + v0.y*transform.f; + aabb.minY = aabb.maxY = a + eps_g; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v0.x*transform.i + v0.y*transform.j; + aabb.minZ = aabb.maxZ = a + eps_k; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 1 + // Ось X + a = v1.x*transform.a + v1.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v1.x*transform.e + v1.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v1.x*transform.i + v1.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 2 + // Ось X + a = v2.x*transform.a + v2.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v2.x*transform.e + v2.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v2.x*transform.i + v2.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + aabb.minX += transform.d; + aabb.maxX += transform.d; + + aabb.minY += transform.h; + aabb.maxY += transform.h; + + aabb.minZ += transform.l; + aabb.maxZ += transform.l; + + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + // Луч трансформируется в систему координат примитива, затем проверяется пересечение + + var vz:Number = vector.x*transform.c + vector.y*transform.g + vector.z*transform.k; + // Если луч параллелен плоскости птимитива, то пересечения нет + if (vz < epsilon && vz > -epsilon) return -1; + + var tx:Number = origin.x - transform.d; + var ty:Number = origin.y - transform.h; + var tz:Number = origin.z - transform.l; + + var oz:Number = tx*transform.c + ty*transform.g + tz*transform.k; + var t:Number = -oz/vz; + if (t < 0) return -1; + + var ox:Number = tx*transform.a + ty*transform.e + tz*transform.i; + var oy:Number = tx*transform.b + ty*transform.f + tz*transform.j; + + tx = ox + t*(vector.x*transform.a + vector.y*transform.e + vector.z*transform.i); + ty = oy + t*(vector.x*transform.b + vector.y*transform.f + vector.z*transform.j); + tz = oz + t*vz; + + // Проверка вхождения точки в треугольник + if ((e0.x*(ty - v0.y) - e0.y*(tx - v0.x) < 0) || (e1.x*(ty - v1.y) - e1.y*(tx - v1.x) < 0) || (e2.x*(ty - v2.y) - e2.y*(tx - v2.x) < 0)) return -1; + + // Запись нормали + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + + return t; + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + super.copyFrom(source); + var tri:CollisionTriangle = source as CollisionTriangle; + if (tri != null) { + v0.vCopy(tri.v0); + v1.vCopy(tri.v1); + v2.vCopy(tri.v2); + + e0.vCopy(tri.e0); + e1.vCopy(tri.e1); + e2.vCopy(tri.e2); + +// len0 = tri.len0; +// len1 = tri.len1; +// len2 = tri.len2; + } + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionTriangle v0=" + v0 + ", v1=" + v1 + ", v2=" + v2 + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionTriangle(v0, v1, v2, collisionGroup); + } + + /** + * + * @param v0 + * @param v1 + * @param v2 + */ + private function initVertices(v0:Vector3, v1:Vector3, v2:Vector3):void { + this.v0.vCopy(v0); + this.v1.vCopy(v1); + this.v2.vCopy(v2); + + e0.vDiff(v1, v0); +// len0 = e0.vLength(); + e0.vNormalize(); + + e1.vDiff(v2, v1); +// len1 = e1.vLength(); + e1.vNormalize(); + + e2.vDiff(v0, v2); +// len2 = e2.vLength(); + e2.vNormalize(); + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/collision/primitives/CollisionBox.as b/0.0.9.5/src/alternativa/physics/collision/primitives/CollisionBox.as new file mode 100644 index 0000000..764bd1b --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/primitives/CollisionBox.as @@ -0,0 +1,215 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * Ориентированный бокс. + */ + public class CollisionBox extends CollisionPrimitive { + + // Половинные размеры вдоль каждой из осей + public var hs:Vector3 = new Vector3(); + + public var excludedFaces:int; + + /** + * @param hs + * @param collisionGroup + */ + public function CollisionBox(hs:Vector3, collisionGroup:int) { + super(BOX, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * @return - + */ + override public function calculateAABB():BoundBox { + var t:Matrix4 = transform; + + var xx:Number; + var yy:Number; + var zz:Number; + + xx = t.a < 0 ? -t.a : t.a; + yy = t.b < 0 ? -t.b : t.b; + zz = t.c < 0 ? -t.c : t.c; + aabb.maxX = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minX = -aabb.maxX; + + xx = t.e < 0 ? -t.e : t.e; + yy = t.f < 0 ? -t.f : t.f; + zz = t.g < 0 ? -t.g : t.g; + aabb.maxY = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minY = -aabb.maxY; + + xx = t.i < 0 ? -t.i : t.i; + yy = t.j < 0 ? -t.j : t.j; + zz = t.k < 0 ? -t.k : t.k; + aabb.maxZ = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return - + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var box:CollisionBox = source as CollisionBox; + if (box == null) return this; + super.copyFrom(box); + hs.vCopy(box.hs); + return this; + } + + /** + * @return - + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionBox(hs, collisionGroup); + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + var tMin:Number = -1; + var tMax:Number = 1e308; + var t1:Number; + var t2:Number; + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + + // Проверка, не находится ли начало луча внутри бокса. В этом случае точкой пересечения будет начало луча, + // а нормалью будет нормированный вектор, противоположный вектору луча. +// if (ox >= -hs.x && ox <= hs.x && oy >= -hs.y && oy <= hs.y && oz >= -hs.z && oz <= hs.z) { +// normal.x = -vector.x; +// normal.y = -vector.y; +// normal.z = -vector.z; +// normal.vNormalize(); +// return 0; +// } + + // X + if (vx < epsilon && vx > -epsilon) { + if (ox < -hs.x || ox > hs.x) return -1; + } else { + t1 = (-hs.x - ox)/vx; + t2 = (hs.x - ox)/vx; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.x = -1; + normal.y = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.x = 1; + normal.y = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Y + if (vy < epsilon && vy > -epsilon) { + if (oy < -hs.y || oy > hs.y) return -1; + } else { + t1 = (-hs.y - oy)/vy; + t2 = (hs.y - oy)/vy; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.y = -1; + normal.x = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.y = 1; + normal.x = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Z + if (vz < epsilon && vz > -epsilon) { + if (oz < -hs.z || oz > hs.z) return -1; + } else { + t1 = (-hs.z - oz)/vz; + t2 = (hs.z - oz)/vz; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.z = -1; + normal.x = normal.y = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.z = 1; + normal.x = normal.y = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Перевод нормали в мировую систему координат + // Inlined normal.vDeltaTransformBy4(transform); + vx = normal.x; + vy = normal.y; + vz = normal.z; + normal.x = transform.a*vx + transform.b*vy + transform.c*vz; + normal.y = transform.e*vx + transform.f*vy + transform.g*vz; + normal.z = transform.i*vx + transform.j*vy + transform.k*vz; + + return tMin; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionBox hs=" + hs + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/collision/primitives/CollisionRect.as b/0.0.9.5/src/alternativa/physics/collision/primitives/CollisionRect.as new file mode 100644 index 0000000..7a33b79 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/primitives/CollisionRect.as @@ -0,0 +1,143 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * Ориентированный прямоугольник. Задаётся половинами размеров вдоль осей X (ширина) и Y (длина) локальной системы + * координат примитива. Таким образом, прямоугольник лежит в плоскости XY, его стороны параллельны осям этой + * плоскости, а нормаль направлена вдоль локальной оси Z. + * + * Прямоугольник может быть одно- или двусторонним. В случае одностороннего прямоугольника, + * столкновения с внутренней стороны не регистрируются. + */ + public class CollisionRect extends CollisionPrimitive { + // Половинные размеры прямоугольника вдоль осей X и Y. Нормаль направлена вдоль оси Z. + public var hs:Vector3 = new Vector3(); + // Флаг указывает, является примитив одно- или двусторонним + public var twoSided:Boolean = true; + + // Малое значение. Используется для указания фиктивной высоты примитива и в функции определения пересечения с сегментом. + private static const EPSILON:Number = 0.005; + + /** + * Создаёт новый экземпляр примитива. + * + * @param hs половинные размерв прямоугольника вдоль осей X и Y. Значение z игнорируется. + * @param collisionGroup группа примитива + */ + public function CollisionRect(hs:Vector3, collisionGroup:int) { + super(RECT, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * Расчитывает ограничивающий бокс прямоугольника. Для избежания проблем высота примитива принимается равной + * не нулю, а малому значению. + * + * @return + */ + override public function calculateAABB():BoundBox { + // Баунд бокс прямоугольника имеет минимальную высоту, отличную от нуля во избежание проблем с построением kd-дерева + var t:Matrix4 = transform; + var xx:Number = t.a < 0 ? -t.a : t.a; + var yy:Number = t.b < 0 ? -t.b : t.b; + var zz:Number = t.c < 0 ? -t.c : t.c; + aabb.maxX = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minX = -aabb.maxX; + + xx = t.e < 0 ? -t.e : t.e; + yy = t.f < 0 ? -t.f : t.f; + zz = t.g < 0 ? -t.g : t.g; + aabb.maxY = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minY = -aabb.maxY; + + xx = t.i < 0 ? -t.i : t.i; + yy = t.j < 0 ? -t.j : t.j; + zz = t.k < 0 ? -t.k : t.k; + aabb.maxZ = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var rect:CollisionRect = source as CollisionRect; + if (rect == null) return this; + super.copyFrom(rect); + hs.vCopy(rect.hs); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionRect hs=" + hs + "]"; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionRect(hs, collisionGroup); + } + + /** + * @param origin + * @param vector + * @param collisionGroup + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + + // Проверка параллельности сегмента и плоскости примитива + if (vz > -threshold && vz < threshold) return -1; + var t:Number = -oz/vz; + if (t < 0) return -1; + // Проверка вхождения точки пересечения в прямоугольник + ox += vx*t; + oy += vy*t; + oz = 0; + if (ox < (-hs.x - threshold) || ox > (hs.x + threshold) || oy < (-hs.y - threshold) || oy > (hs.y + threshold)) return -1; + + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + return t; + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/collision/primitives/CollisionSphere.as b/0.0.9.5/src/alternativa/physics/collision/primitives/CollisionSphere.as new file mode 100644 index 0000000..b912a5b --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/primitives/CollisionSphere.as @@ -0,0 +1,81 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Vector3; + use namespace altphysics; + + + /** + * Сфера. + */ + public class CollisionSphere extends CollisionPrimitive { + + // Радиус сферы + public var r:Number = 0; + + /** + * + * @param r + * @param collisionGroup + */ + public function CollisionSphere(r:Number, collisionGroup:int) { + super(SPHERE, collisionGroup); + this.r = r; + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + aabb.maxX = transform.d + r; + aabb.minX = transform.d - r; + + aabb.maxY = transform.h + r; + aabb.minY = transform.h - r; + + aabb.maxZ = transform.l + r; + aabb.minZ = transform.l - r; + + return aabb; + } + + /** + * @param origin + * @param vector + * @param threshold + * @param normal + * @return + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var px:Number = origin.x - transform.d; + var py:Number = origin.y - transform.h; + var pz:Number = origin.z - transform.l; + var k:Number = vector.x*px + vector.y*py + vector.z*pz; + if (k > 0) return -1; + var a:Number = vector.x*vector.x + vector.y*vector.y + vector.z*vector.z; + var D:Number = k*k - a*(px*px + py*py + pz*pz - r*r); + if (D < 0) return -1; + return -(k + Math.sqrt(D))/a; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var sphere:CollisionSphere = source as CollisionSphere; + if (sphere == null) return this; + super.copyFrom(sphere); + r = sphere.r; + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionSphere(r, collisionGroup); + } + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/collision/primitives/CollisionTriangle.as b/0.0.9.5/src/alternativa/physics/collision/primitives/CollisionTriangle.as new file mode 100644 index 0000000..bdc3166 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/primitives/CollisionTriangle.as @@ -0,0 +1,244 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Vector3; + use namespace altphysics; + + /** + * Примитив треугольник. + */ + public class CollisionTriangle extends CollisionPrimitive { + + public var v0:Vector3 = new Vector3(); + public var v1:Vector3 = new Vector3(); + public var v2:Vector3 = new Vector3(); + + public var e0:Vector3 = new Vector3(); + public var e1:Vector3 = new Vector3(); + public var e2:Vector3 = new Vector3(); + +// public var len0:Number; +// public var len1:Number; +// public var len2:Number; +// + /** + * + * @param v0 + * @param v1 + * @param v2 + * @param collisionGroup + */ + public function CollisionTriangle(v0:Vector3, v1:Vector3, v2:Vector3, collisionGroup:int) { + super(TRIANGLE, collisionGroup); + initVertices(v0, v1, v2); + } + + /** + * Рассчитывает AABB примитива. + * + * @return ссылка на свой AABB + */ + override public function calculateAABB():BoundBox { + var epsilon:Number = 0.005; + var a:Number; + var b:Number; + var eps_c:Number = epsilon*transform.c; + var eps_g:Number = epsilon*transform.g; + var eps_k:Number = epsilon*transform.k; + + // Вершина 0 + // Ось X + a = v0.x*transform.a + v0.y*transform.b; + aabb.minX = aabb.maxX = a + eps_c; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v0.x*transform.e + v0.y*transform.f; + aabb.minY = aabb.maxY = a + eps_g; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v0.x*transform.i + v0.y*transform.j; + aabb.minZ = aabb.maxZ = a + eps_k; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 1 + // Ось X + a = v1.x*transform.a + v1.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v1.x*transform.e + v1.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v1.x*transform.i + v1.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 2 + // Ось X + a = v2.x*transform.a + v2.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v2.x*transform.e + v2.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v2.x*transform.i + v2.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + aabb.minX += transform.d; + aabb.maxX += transform.d; + + aabb.minY += transform.h; + aabb.maxY += transform.h; + + aabb.minZ += transform.l; + aabb.maxZ += transform.l; + + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function getRayIntersection(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + // Луч трансформируется в систему координат примитива, затем проверяется пересечение + + var vz:Number = vector.x*transform.c + vector.y*transform.g + vector.z*transform.k; + // Если луч параллелен плоскости птимитива, то пересечения нет + if (vz < epsilon && vz > -epsilon) return -1; + + var tx:Number = origin.x - transform.d; + var ty:Number = origin.y - transform.h; + var tz:Number = origin.z - transform.l; + + var oz:Number = tx*transform.c + ty*transform.g + tz*transform.k; + var t:Number = -oz/vz; + if (t < 0) return -1; + + var ox:Number = tx*transform.a + ty*transform.e + tz*transform.i; + var oy:Number = tx*transform.b + ty*transform.f + tz*transform.j; + + tx = ox + t*(vector.x*transform.a + vector.y*transform.e + vector.z*transform.i); + ty = oy + t*(vector.x*transform.b + vector.y*transform.f + vector.z*transform.j); + tz = oz + t*vz; + + // Проверка вхождения точки в треугольник + if ((e0.x*(ty - v0.y) - e0.y*(tx - v0.x) < 0) || (e1.x*(ty - v1.y) - e1.y*(tx - v1.x) < 0) || (e2.x*(ty - v2.y) - e2.y*(tx - v2.x) < 0)) return -1; + + // Запись нормали + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + + return t; + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + super.copyFrom(source); + var tri:CollisionTriangle = source as CollisionTriangle; + if (tri != null) { + v0.vCopy(tri.v0); + v1.vCopy(tri.v1); + v2.vCopy(tri.v2); + + e0.vCopy(tri.e0); + e1.vCopy(tri.e1); + e2.vCopy(tri.e2); + +// len0 = tri.len0; +// len1 = tri.len1; +// len2 = tri.len2; + } + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionTriangle v0=" + v0 + ", v1=" + v1 + ", v2=" + v2 + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionTriangle(v0, v1, v2, collisionGroup); + } + + /** + * + * @param v0 + * @param v1 + * @param v2 + */ + private function initVertices(v0:Vector3, v1:Vector3, v2:Vector3):void { + this.v0.vCopy(v0); + this.v1.vCopy(v1); + this.v2.vCopy(v2); + + e0.vDiff(v1, v0); +// len0 = e0.vLength(); + e0.vNormalize(); + + e1.vDiff(v2, v1); +// len1 = e1.vLength(); + e1.vNormalize(); + + e2.vDiff(v0, v2); +// len2 = e2.vLength(); + e2.vNormalize(); + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/collision/types/.svn/all-wcprops b/0.0.9.5/src/alternativa/physics/collision/types/.svn/all-wcprops new file mode 100644 index 0000000..c312e7f --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/types/.svn/all-wcprops @@ -0,0 +1,23 @@ +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/collision/types +END +BoundBox.as +K 25 +svn:wc:ra_dav:version-url +V 131 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/collision/types/BoundBox.as +END +RayIntersection.as +K 25 +svn:wc:ra_dav:version-url +V 138 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/collision/types/RayIntersection.as +END +Ray.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/collision/types/Ray.as +END diff --git a/0.0.9.5/src/alternativa/physics/collision/types/.svn/entries b/0.0.9.5/src/alternativa/physics/collision/types/.svn/entries new file mode 100644 index 0000000..59b2fc5 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/types/.svn/entries @@ -0,0 +1,64 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/collision/types +http://svndev.alternativaplatform.com + + + +2009-11-24T08:44:15.941480Z +23758 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +BoundBox.as +file + + + + +2010-10-28T04:32:45.000000Z +d92d4ab3dde698feb674fb9488f59678 +2009-09-09T14:02:48.134744Z +19633 +mike + +RayIntersection.as +file + + + + +2010-10-28T04:32:45.000000Z +88c0836ead77b8eb0841f11159dfb836 +2009-11-24T08:44:15.941480Z +23758 +mike + +Ray.as +file + + + + +2010-10-28T04:32:45.000000Z +c65cc8bfae377f8f0520b5c79c8b3ab9 +2009-11-24T08:44:15.941480Z +23758 +mike + diff --git a/0.0.9.5/src/alternativa/physics/collision/types/.svn/format b/0.0.9.5/src/alternativa/physics/collision/types/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/types/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.5/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base b/0.0.9.5/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base new file mode 100644 index 0000000..b4d5278 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base @@ -0,0 +1,75 @@ +package alternativa.physics.collision.types { + + /** + * Ограничивающий параллелепипед. + */ + public class BoundBox { + + public var minX:Number = 1e308; + public var minY:Number = 1e308; + public var minZ:Number = 1e308; + public var maxX:Number = -1e308; + public var maxY:Number = -1e308; + public var maxZ:Number = -1e308; + + public function setSize(minX:Number, minY:Number, minZ:Number, maxX:Number, maxY:Number, maxZ:Number):void { + this.minX = minX; + this.minY = minY; + this.minZ = minZ; + this.maxX = maxX; + this.maxY = maxY; + this.maxZ = maxZ; + } + + public function addBoundBox(boundBox:BoundBox):void { + minX = (boundBox.minX < minX) ? boundBox.minX : minX; + minY = (boundBox.minY < minY) ? boundBox.minY : minY; + minZ = (boundBox.minZ < minZ) ? boundBox.minZ : minZ; + maxX = (boundBox.maxX > maxX) ? boundBox.maxX : maxX; + maxY = (boundBox.maxY > maxY) ? boundBox.maxY : maxY; + maxZ = (boundBox.maxZ > maxZ) ? boundBox.maxZ : maxZ; + } + + public function addPoint(x:Number, y:Number, z:Number):void { + if (x < minX) minX = x; + if (x > maxX) maxX = x; + if (y < minY) minY = y; + if (y > maxY) maxY = y; + if (z < minZ) minZ = z; + if (z > maxZ) maxZ = z; + } + + public function infinity():void { + minX = 1e308; + minY = 1e308; + minZ = 1e308; + maxX = -1e308; + maxY = -1e308; + maxZ = -1e308; + } + + public function intersects(bb:BoundBox, epsilon:Number):Boolean { + return !(minX > bb.maxX + epsilon || maxX < bb.minX - epsilon || minY > bb.maxY + epsilon || maxY < bb.minY - epsilon || minZ > bb.maxZ + epsilon || maxZ < bb.minZ - epsilon); + } + + public function copyFrom(boundBox:BoundBox):void { + minX = boundBox.minX; + minY = boundBox.minY; + minZ = boundBox.minZ; + maxX = boundBox.maxX; + maxY = boundBox.maxY; + maxZ = boundBox.maxZ; + } + + public function clone():BoundBox { + var clone:BoundBox = new BoundBox(); + clone.copyFrom(this); + return clone; + } + + public function toString():String { + return "BoundBox [" + minX + ", " + minY + ", " + minZ + " : " + maxX + ", " + maxY + ", " + maxZ + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base b/0.0.9.5/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base new file mode 100644 index 0000000..fb2ffbc --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base @@ -0,0 +1,13 @@ +package alternativa.physics.collision.types { + import alternativa.math.Vector3; + + /** + * Структура, описывающая луч и сегмент прямой в пространстве. + */ + public class Ray { + // Начало луча. Начальная точка сегмента на прямой луча. + public var origin:Vector3 = new Vector3(); + // Направляющий вектор луча. Длина вектора задаёт длину сегмента на луче. Конечная точка сегмента вычисляется как origin + dir. + public var dir:Vector3 = new Vector3(); + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base b/0.0.9.5/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base new file mode 100644 index 0000000..21e9290 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base @@ -0,0 +1,25 @@ +package alternativa.physics.collision.types { + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.math.Vector3; + + /** + * Структура описывает точку пересечения луча с физической геометрией. + */ + public class RayIntersection { + // Физический примитив, с которым пересекается луч + public var primitive:CollisionPrimitive; + // Положенеи точки пересечения + public var pos:Vector3 = new Vector3(); + // Нормаль поверхности физического примитива в точке пересечения + public var normal:Vector3 = new Vector3(); + // Время пересечения (|pos - ray.origin|/|ray.dir|) + public var t:Number = 0; + + public function copy(source:RayIntersection):void { + primitive = source.primitive; + pos.vCopy(source.pos); + normal.vCopy(source.normal); + t = source.t; + } + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/collision/types/BoundBox.as b/0.0.9.5/src/alternativa/physics/collision/types/BoundBox.as new file mode 100644 index 0000000..b4d5278 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/types/BoundBox.as @@ -0,0 +1,75 @@ +package alternativa.physics.collision.types { + + /** + * Ограничивающий параллелепипед. + */ + public class BoundBox { + + public var minX:Number = 1e308; + public var minY:Number = 1e308; + public var minZ:Number = 1e308; + public var maxX:Number = -1e308; + public var maxY:Number = -1e308; + public var maxZ:Number = -1e308; + + public function setSize(minX:Number, minY:Number, minZ:Number, maxX:Number, maxY:Number, maxZ:Number):void { + this.minX = minX; + this.minY = minY; + this.minZ = minZ; + this.maxX = maxX; + this.maxY = maxY; + this.maxZ = maxZ; + } + + public function addBoundBox(boundBox:BoundBox):void { + minX = (boundBox.minX < minX) ? boundBox.minX : minX; + minY = (boundBox.minY < minY) ? boundBox.minY : minY; + minZ = (boundBox.minZ < minZ) ? boundBox.minZ : minZ; + maxX = (boundBox.maxX > maxX) ? boundBox.maxX : maxX; + maxY = (boundBox.maxY > maxY) ? boundBox.maxY : maxY; + maxZ = (boundBox.maxZ > maxZ) ? boundBox.maxZ : maxZ; + } + + public function addPoint(x:Number, y:Number, z:Number):void { + if (x < minX) minX = x; + if (x > maxX) maxX = x; + if (y < minY) minY = y; + if (y > maxY) maxY = y; + if (z < minZ) minZ = z; + if (z > maxZ) maxZ = z; + } + + public function infinity():void { + minX = 1e308; + minY = 1e308; + minZ = 1e308; + maxX = -1e308; + maxY = -1e308; + maxZ = -1e308; + } + + public function intersects(bb:BoundBox, epsilon:Number):Boolean { + return !(minX > bb.maxX + epsilon || maxX < bb.minX - epsilon || minY > bb.maxY + epsilon || maxY < bb.minY - epsilon || minZ > bb.maxZ + epsilon || maxZ < bb.minZ - epsilon); + } + + public function copyFrom(boundBox:BoundBox):void { + minX = boundBox.minX; + minY = boundBox.minY; + minZ = boundBox.minZ; + maxX = boundBox.maxX; + maxY = boundBox.maxY; + maxZ = boundBox.maxZ; + } + + public function clone():BoundBox { + var clone:BoundBox = new BoundBox(); + clone.copyFrom(this); + return clone; + } + + public function toString():String { + return "BoundBox [" + minX + ", " + minY + ", " + minZ + " : " + maxX + ", " + maxY + ", " + maxZ + "]"; + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/collision/types/Ray.as b/0.0.9.5/src/alternativa/physics/collision/types/Ray.as new file mode 100644 index 0000000..fb2ffbc --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/types/Ray.as @@ -0,0 +1,13 @@ +package alternativa.physics.collision.types { + import alternativa.math.Vector3; + + /** + * Структура, описывающая луч и сегмент прямой в пространстве. + */ + public class Ray { + // Начало луча. Начальная точка сегмента на прямой луча. + public var origin:Vector3 = new Vector3(); + // Направляющий вектор луча. Длина вектора задаёт длину сегмента на луче. Конечная точка сегмента вычисляется как origin + dir. + public var dir:Vector3 = new Vector3(); + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/collision/types/RayIntersection.as b/0.0.9.5/src/alternativa/physics/collision/types/RayIntersection.as new file mode 100644 index 0000000..21e9290 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/collision/types/RayIntersection.as @@ -0,0 +1,25 @@ +package alternativa.physics.collision.types { + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.math.Vector3; + + /** + * Структура описывает точку пересечения луча с физической геометрией. + */ + public class RayIntersection { + // Физический примитив, с которым пересекается луч + public var primitive:CollisionPrimitive; + // Положенеи точки пересечения + public var pos:Vector3 = new Vector3(); + // Нормаль поверхности физического примитива в точке пересечения + public var normal:Vector3 = new Vector3(); + // Время пересечения (|pos - ray.origin|/|ray.dir|) + public var t:Number = 0; + + public function copy(source:RayIntersection):void { + primitive = source.primitive; + pos.vCopy(source.pos); + normal.vCopy(source.normal); + t = source.t; + } + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/constraints/.svn/all-wcprops b/0.0.9.5/src/alternativa/physics/constraints/.svn/all-wcprops new file mode 100644 index 0000000..b876459 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/constraints/.svn/all-wcprops @@ -0,0 +1,17 @@ +K 25 +svn:wc:ra_dav:version-url +V 115 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/constraints +END +Constraint.as +K 25 +svn:wc:ra_dav:version-url +V 129 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/constraints/Constraint.as +END +MaxDistanceConstraint.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/constraints/MaxDistanceConstraint.as +END diff --git a/0.0.9.5/src/alternativa/physics/constraints/.svn/entries b/0.0.9.5/src/alternativa/physics/constraints/.svn/entries new file mode 100644 index 0000000..d1c81a1 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/constraints/.svn/entries @@ -0,0 +1,52 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/constraints +http://svndev.alternativaplatform.com + + + +2010-01-20T17:53:18.555251Z +26849 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Constraint.as +file + + + + +2010-10-28T04:32:45.000000Z +02bdbd40eda515549956d6393551a63b +2010-01-20T17:53:18.555251Z +26849 +mike + +MaxDistanceConstraint.as +file + + + + +2010-10-28T04:32:45.000000Z +dbf792986b9dd7e2a1c720cf4a578f42 +2009-11-24T08:44:15.941480Z +23758 +mike + diff --git a/0.0.9.5/src/alternativa/physics/constraints/.svn/format b/0.0.9.5/src/alternativa/physics/constraints/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/constraints/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.5/src/alternativa/physics/constraints/.svn/text-base/Constraint.as.svn-base b/0.0.9.5/src/alternativa/physics/constraints/.svn/text-base/Constraint.as.svn-base new file mode 100644 index 0000000..8607442 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/constraints/.svn/text-base/Constraint.as.svn-base @@ -0,0 +1,35 @@ +package alternativa.physics.constraints { + + import alternativa.physics.PhysicsScene; + import alternativa.physics.altphysics; + + use namespace altphysics; + + /** + * + */ + public class Constraint { + + altphysics var satisfied:Boolean; + altphysics var world:PhysicsScene; + + /** + * + */ + public function Constraint() { + } + + /** + * @param dt + */ + public function preProcess(dt:Number):void { + } + + /** + * @param dt + */ + public function apply(dt:Number):void { + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base b/0.0.9.5/src/alternativa/physics/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base new file mode 100644 index 0000000..d29f518 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base @@ -0,0 +1,183 @@ +package alternativa.physics.constraints { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * + */ + public class MaxDistanceConstraint extends Constraint { + // Первое тело ограничения + altphysics var body1:Body; + // Второе тело ограничения. Может быть null. + altphysics var body2:Body; + // Первая точка крепления в системе координат первого тела + altphysics var r1:Vector3; + // Вторая точка привязки в системе координат второго тела. В случае отсутствия второго тела, координаты задаются в мировой системе. + altphysics var r2:Vector3; + // Максимально допустимое расстояние между точками крепления + altphysics var maxDistance:Number; + // Радиус-вектор первой точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr1:Vector3 = new Vector3(); + // Радиус-вектор второй точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr2:Vector3 = new Vector3(); + // Минимальная скорость сближения для удовлетворения ограничения + private var minClosingVel:Number; + // Изменение относительной скорости на единицу импульса + private var velByUnitImpulseN:Number; + // Направление корректирующего импульса + private var impulseDirection:Vector3 = new Vector3(); + + // Вспомогательные переменные +// private static var _v:Vector3 = new Vector3(); +// private static var _v1:Vector3 = new Vector3(); +// private static var _v2:Vector3 = new Vector3(); + + /** + * @param body1 + * @param body2 + * @param r1 + * @param r2 + * @param maxDistance + */ + public function MaxDistanceConstraint(body1:Body, body2:Body, r1:Vector3, r2:Vector3, maxDistance:Number) { + super(); + this.body1 = body1; + this.body2 = body2; + this.r1 = r1.vClone(); + this.r2 = r2.vClone(); + this.maxDistance = maxDistance; + } + + /** + * @param dt + */ + override public function preProcess(dt:Number):void { + // Вычислим расстояние между точками + var m:Matrix3 = body1.baseMatrix; + wr1.x = m.a*r1.x + m.b*r1.y + m.c*r1.z; + wr1.y = m.e*r1.x + m.f*r1.y + m.g*r1.z; + wr1.z = m.i*r1.x + m.j*r1.y + m.k*r1.z; + if (body2 != null) { + m = body2.baseMatrix; + wr2.x = m.a*r2.x + m.b*r2.y + m.c*r2.z; + wr2.y = m.e*r2.x + m.f*r2.y + m.g*r2.z; + wr2.z = m.i*r2.x + m.j*r2.y + m.k*r2.z; + } else { + wr2.x = r2.x; + wr2.y = r2.y; + wr2.z = r2.z; + } + var p1:Vector3 = body1.state.pos; + impulseDirection.x = wr2.x - wr1.x - p1.x; + impulseDirection.y = wr2.y - wr1.y - p1.y; + impulseDirection.z = wr2.z - wr1.z - p1.z; + if (body2 != null) { + var p2:Vector3 = body2.state.pos; + impulseDirection.x += p2.x; + impulseDirection.y += p2.y; + impulseDirection.z += p2.z; + } + + var len:Number = Math.sqrt(impulseDirection.x*impulseDirection.x + impulseDirection.y*impulseDirection.y + impulseDirection.z*impulseDirection.z); + var delta:Number = len - maxDistance; + if (delta > 0) { + satisfied = false; + if (len < 0.001) { + impulseDirection.x = 1; + } else { + len = 1/len; + impulseDirection.x *= len; + impulseDirection.y *= len; + impulseDirection.z *= len; + } + minClosingVel = delta/(world.penResolutionSteps*dt); + if (minClosingVel > world.maxPenResolutionSpeed) { + minClosingVel = world.maxPenResolutionSpeed; + } + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + var x:Number; + var y:Number; + var z:Number; + var vx:Number; + var vy:Number; + var vz:Number; + velByUnitImpulseN = 0; + if (body1.movable) { +// velByUnitImpulseN += body1.invMass + _v.vCross2(wr1, impulseDirection).vTransformBy3(body1.invInertiaWorld).vCross(wr1).vDot(impulseDirection); + vx = wr1.y*impulseDirection.z - wr1.z*impulseDirection.y; + vy = wr1.z*impulseDirection.x - wr1.x*impulseDirection.z; + vz = wr1.x*impulseDirection.y - wr1.y*impulseDirection.x; + + m = body1.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr1.z - z*wr1.y; + vy = z*wr1.x - x*wr1.z; + vz = x*wr1.y - y*wr1.x; + velByUnitImpulseN += body1.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + if (body2 != null && body2.movable) { +// velByUnitImpulseN += body2.invMass + _v.vCross2(wr2, impulseDirection).vTransformBy3(body2.invInertiaWorld).vCross(wr2).vDot(impulseDirection); + vx = wr2.y*impulseDirection.z - wr2.z*impulseDirection.y; + vy = wr2.z*impulseDirection.x - wr2.x*impulseDirection.z; + vz = wr2.x*impulseDirection.y - wr2.y*impulseDirection.x; + + m = body2.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr2.z - z*wr2.y; + vy = z*wr2.x - x*wr2.z; + vz = x*wr2.y - y*wr2.x; + velByUnitImpulseN += body2.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + } else { + satisfied = true; + } + } + + /** + * @param dt + */ + override public function apply(dt:Number):void { + if (satisfied) return; + // Расчитываем проекцию скорости относительной скорости + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 + var vel:Vector3 = body1.state.velocity; + var rot:Vector3 = body1.state.rotation; +// v1Cross2(state.rotation, wr1); + var vx:Number = vel.x + rot.y*wr1.z - rot.z*wr1.y; + var vy:Number = vel.y + rot.z*wr1.x - rot.x*wr1.z; + var vz:Number = vel.z + rot.x*wr1.y - rot.y*wr1.x; + // V2 = V2_c + w2%r2 + if (body2 != null) { + vel = body2.state.velocity; + rot = body2.state.rotation; +// _v2.vCross2(state.rotation, wr2); + vx -= vel.x + rot.y*wr2.z - rot.z*wr2.y; + vy -= vel.y + rot.z*wr2.x - rot.x*wr2.z; + vz -= vel.z + rot.x*wr2.y - rot.y*wr2.x; + } + var closingVel:Number = vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + if (closingVel > minClosingVel) return; + var impulse:Number = (minClosingVel - closingVel)/velByUnitImpulseN; + if (body1.movable) { + body1.applyRelPosWorldImpulse(wr1, impulseDirection, impulse); + } + if (body2 != null && body2.movable) { + body2.applyRelPosWorldImpulse(wr2, impulseDirection, -impulse); + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/constraints/Constraint.as b/0.0.9.5/src/alternativa/physics/constraints/Constraint.as new file mode 100644 index 0000000..8607442 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/constraints/Constraint.as @@ -0,0 +1,35 @@ +package alternativa.physics.constraints { + + import alternativa.physics.PhysicsScene; + import alternativa.physics.altphysics; + + use namespace altphysics; + + /** + * + */ + public class Constraint { + + altphysics var satisfied:Boolean; + altphysics var world:PhysicsScene; + + /** + * + */ + public function Constraint() { + } + + /** + * @param dt + */ + public function preProcess(dt:Number):void { + } + + /** + * @param dt + */ + public function apply(dt:Number):void { + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/constraints/MaxDistanceConstraint.as b/0.0.9.5/src/alternativa/physics/constraints/MaxDistanceConstraint.as new file mode 100644 index 0000000..d29f518 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/constraints/MaxDistanceConstraint.as @@ -0,0 +1,183 @@ +package alternativa.physics.constraints { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * + */ + public class MaxDistanceConstraint extends Constraint { + // Первое тело ограничения + altphysics var body1:Body; + // Второе тело ограничения. Может быть null. + altphysics var body2:Body; + // Первая точка крепления в системе координат первого тела + altphysics var r1:Vector3; + // Вторая точка привязки в системе координат второго тела. В случае отсутствия второго тела, координаты задаются в мировой системе. + altphysics var r2:Vector3; + // Максимально допустимое расстояние между точками крепления + altphysics var maxDistance:Number; + // Радиус-вектор первой точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr1:Vector3 = new Vector3(); + // Радиус-вектор второй точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr2:Vector3 = new Vector3(); + // Минимальная скорость сближения для удовлетворения ограничения + private var minClosingVel:Number; + // Изменение относительной скорости на единицу импульса + private var velByUnitImpulseN:Number; + // Направление корректирующего импульса + private var impulseDirection:Vector3 = new Vector3(); + + // Вспомогательные переменные +// private static var _v:Vector3 = new Vector3(); +// private static var _v1:Vector3 = new Vector3(); +// private static var _v2:Vector3 = new Vector3(); + + /** + * @param body1 + * @param body2 + * @param r1 + * @param r2 + * @param maxDistance + */ + public function MaxDistanceConstraint(body1:Body, body2:Body, r1:Vector3, r2:Vector3, maxDistance:Number) { + super(); + this.body1 = body1; + this.body2 = body2; + this.r1 = r1.vClone(); + this.r2 = r2.vClone(); + this.maxDistance = maxDistance; + } + + /** + * @param dt + */ + override public function preProcess(dt:Number):void { + // Вычислим расстояние между точками + var m:Matrix3 = body1.baseMatrix; + wr1.x = m.a*r1.x + m.b*r1.y + m.c*r1.z; + wr1.y = m.e*r1.x + m.f*r1.y + m.g*r1.z; + wr1.z = m.i*r1.x + m.j*r1.y + m.k*r1.z; + if (body2 != null) { + m = body2.baseMatrix; + wr2.x = m.a*r2.x + m.b*r2.y + m.c*r2.z; + wr2.y = m.e*r2.x + m.f*r2.y + m.g*r2.z; + wr2.z = m.i*r2.x + m.j*r2.y + m.k*r2.z; + } else { + wr2.x = r2.x; + wr2.y = r2.y; + wr2.z = r2.z; + } + var p1:Vector3 = body1.state.pos; + impulseDirection.x = wr2.x - wr1.x - p1.x; + impulseDirection.y = wr2.y - wr1.y - p1.y; + impulseDirection.z = wr2.z - wr1.z - p1.z; + if (body2 != null) { + var p2:Vector3 = body2.state.pos; + impulseDirection.x += p2.x; + impulseDirection.y += p2.y; + impulseDirection.z += p2.z; + } + + var len:Number = Math.sqrt(impulseDirection.x*impulseDirection.x + impulseDirection.y*impulseDirection.y + impulseDirection.z*impulseDirection.z); + var delta:Number = len - maxDistance; + if (delta > 0) { + satisfied = false; + if (len < 0.001) { + impulseDirection.x = 1; + } else { + len = 1/len; + impulseDirection.x *= len; + impulseDirection.y *= len; + impulseDirection.z *= len; + } + minClosingVel = delta/(world.penResolutionSteps*dt); + if (minClosingVel > world.maxPenResolutionSpeed) { + minClosingVel = world.maxPenResolutionSpeed; + } + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + var x:Number; + var y:Number; + var z:Number; + var vx:Number; + var vy:Number; + var vz:Number; + velByUnitImpulseN = 0; + if (body1.movable) { +// velByUnitImpulseN += body1.invMass + _v.vCross2(wr1, impulseDirection).vTransformBy3(body1.invInertiaWorld).vCross(wr1).vDot(impulseDirection); + vx = wr1.y*impulseDirection.z - wr1.z*impulseDirection.y; + vy = wr1.z*impulseDirection.x - wr1.x*impulseDirection.z; + vz = wr1.x*impulseDirection.y - wr1.y*impulseDirection.x; + + m = body1.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr1.z - z*wr1.y; + vy = z*wr1.x - x*wr1.z; + vz = x*wr1.y - y*wr1.x; + velByUnitImpulseN += body1.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + if (body2 != null && body2.movable) { +// velByUnitImpulseN += body2.invMass + _v.vCross2(wr2, impulseDirection).vTransformBy3(body2.invInertiaWorld).vCross(wr2).vDot(impulseDirection); + vx = wr2.y*impulseDirection.z - wr2.z*impulseDirection.y; + vy = wr2.z*impulseDirection.x - wr2.x*impulseDirection.z; + vz = wr2.x*impulseDirection.y - wr2.y*impulseDirection.x; + + m = body2.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr2.z - z*wr2.y; + vy = z*wr2.x - x*wr2.z; + vz = x*wr2.y - y*wr2.x; + velByUnitImpulseN += body2.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + } else { + satisfied = true; + } + } + + /** + * @param dt + */ + override public function apply(dt:Number):void { + if (satisfied) return; + // Расчитываем проекцию скорости относительной скорости + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 + var vel:Vector3 = body1.state.velocity; + var rot:Vector3 = body1.state.rotation; +// v1Cross2(state.rotation, wr1); + var vx:Number = vel.x + rot.y*wr1.z - rot.z*wr1.y; + var vy:Number = vel.y + rot.z*wr1.x - rot.x*wr1.z; + var vz:Number = vel.z + rot.x*wr1.y - rot.y*wr1.x; + // V2 = V2_c + w2%r2 + if (body2 != null) { + vel = body2.state.velocity; + rot = body2.state.rotation; +// _v2.vCross2(state.rotation, wr2); + vx -= vel.x + rot.y*wr2.z - rot.z*wr2.y; + vy -= vel.y + rot.z*wr2.x - rot.x*wr2.z; + vz -= vel.z + rot.x*wr2.y - rot.y*wr2.x; + } + var closingVel:Number = vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + if (closingVel > minClosingVel) return; + var impulse:Number = (minClosingVel - closingVel)/velByUnitImpulseN; + if (body1.movable) { + body1.applyRelPosWorldImpulse(wr1, impulseDirection, impulse); + } + if (body2 != null && body2.movable) { + body2.applyRelPosWorldImpulse(wr2, impulseDirection, -impulse); + } + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/primitives/.svn/all-wcprops b/0.0.9.5/src/alternativa/physics/primitives/.svn/all-wcprops new file mode 100644 index 0000000..6c56633 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/primitives/.svn/all-wcprops @@ -0,0 +1,35 @@ +K 25 +svn:wc:ra_dav:version-url +V 114 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/primitives +END +RigidPlane.as +K 25 +svn:wc:ra_dav:version-url +V 128 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/primitives/RigidPlane.as +END +RigidSphere.as +K 25 +svn:wc:ra_dav:version-url +V 129 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/primitives/RigidSphere.as +END +RigidBox.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/primitives/RigidBox.as +END +RigidCylinder.as +K 25 +svn:wc:ra_dav:version-url +V 131 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/primitives/RigidCylinder.as +END +RigidRect.as +K 25 +svn:wc:ra_dav:version-url +V 127 +/!svn/ver/39732/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/primitives/RigidRect.as +END diff --git a/0.0.9.5/src/alternativa/physics/primitives/.svn/entries b/0.0.9.5/src/alternativa/physics/primitives/.svn/entries new file mode 100644 index 0000000..65e7695 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/primitives/.svn/entries @@ -0,0 +1,88 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/0.0.9.5/src/alternativa/physics/primitives +http://svndev.alternativaplatform.com + + + +2009-11-24T08:44:15.941480Z +23758 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +RigidPlane.as +file + + + + +2010-10-28T04:32:45.000000Z +8faf607cf544d7388852668f5cd5672f +2009-11-24T08:44:15.941480Z +23758 +mike + +RigidSphere.as +file + + + + +2010-10-28T04:32:45.000000Z +774b0c23b084bfffb3381eb40e616deb +2009-11-24T08:44:15.941480Z +23758 +mike + +RigidBox.as +file + + + + +2010-10-28T04:32:45.000000Z +7b93f349a2818f22b76d9590cb05f84a +2009-11-24T08:44:15.941480Z +23758 +mike + +RigidCylinder.as +file + + + + +2010-10-28T04:32:45.000000Z +e9ce0eeb457428727cc10d2e53a0f164 +2009-11-24T08:44:15.941480Z +23758 +mike + +RigidRect.as +file + + + + +2010-10-28T04:32:45.000000Z +2eb6d0ebde8c2b46c4b0f810b1d64e1e +2009-11-24T08:44:15.941480Z +23758 +mike + diff --git a/0.0.9.5/src/alternativa/physics/primitives/.svn/format b/0.0.9.5/src/alternativa/physics/primitives/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/primitives/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.0.9.5/src/alternativa/physics/primitives/.svn/text-base/RigidBox.as.svn-base b/0.0.9.5/src/alternativa/physics/primitives/.svn/text-base/RigidBox.as.svn-base new file mode 100644 index 0000000..24e9b9f --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/primitives/.svn/text-base/RigidBox.as.svn-base @@ -0,0 +1,56 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * + */ + public class RigidBox extends Body { + + /** + * + * @param halfSize + * @param mass + */ + public function RigidBox(halfSize:Vector3, mass:Number) { + super(0, Matrix3.ZERO); + addCollisionPrimitive(new CollisionBox(halfSize, 1)); + setParams(halfSize, mass); + } + + /** + * @param halfSize + * @param mass + */ + public function setParams(halfSize:Vector3, mass:Number):void { + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + invInertia.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + invInertia.a = 3*invMass/(yy + zz); + invInertia.f = 3*invMass/(zz + xx); + invInertia.k = 3*invMass/(xx + yy); + } + var prim:CollisionBox = CollisionBox(collisionPrimitives.head.primitive); + prim.hs.vCopy(halfSize); + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/primitives/.svn/text-base/RigidCylinder.as.svn-base b/0.0.9.5/src/alternativa/physics/primitives/.svn/text-base/RigidCylinder.as.svn-base new file mode 100644 index 0000000..4ff65dd --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/primitives/.svn/text-base/RigidCylinder.as.svn-base @@ -0,0 +1,32 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.math.Matrix3; + + use namespace altphysics; + + public class RigidCylinder extends Body { + + altphysics var r:Number; + altphysics var h:Number; + + public function RigidCylinder(mass:Number, radius:Number, height:Number) { + super(0, Matrix3.ZERO); + setParams(mass, radius, height); + } + + public function setParams(mass:Number, radius:Number, height:Number):void { + r = radius; + h = height; + invInertiaWorld.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + invInertia.a = invInertia.f = mass*(h*h/12 + r*r/4); + invInertia.k = 0.5*mass*r*r; + invInertia.invert(); + } + } + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/primitives/.svn/text-base/RigidPlane.as.svn-base b/0.0.9.5/src/alternativa/physics/primitives/.svn/text-base/RigidPlane.as.svn-base new file mode 100644 index 0000000..d9ebf9b --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/primitives/.svn/text-base/RigidPlane.as.svn-base @@ -0,0 +1,23 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + public class RigidPlane extends Body { + + public var normal:Vector3 = new Vector3(); + public var offset:Number; + + public function RigidPlane() { + super(0, Matrix3.ZERO); + this.normal.vCopy(Vector3.Z_AXIS); + this.offset = 0; + movable = false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/primitives/.svn/text-base/RigidRect.as.svn-base b/0.0.9.5/src/alternativa/physics/primitives/.svn/text-base/RigidRect.as.svn-base new file mode 100644 index 0000000..4591931 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/primitives/.svn/text-base/RigidRect.as.svn-base @@ -0,0 +1,20 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + public class RigidRect extends Body { + + public function RigidRect(halfWidth:Number, halfLength:Number) { + super(0, Matrix3.ZERO); + movable = false; + addCollisionPrimitive(new CollisionRect(new Vector3(halfWidth, halfLength, 0), 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/primitives/.svn/text-base/RigidSphere.as.svn-base b/0.0.9.5/src/alternativa/physics/primitives/.svn/text-base/RigidSphere.as.svn-base new file mode 100644 index 0000000..8ad180c --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/primitives/.svn/text-base/RigidSphere.as.svn-base @@ -0,0 +1,29 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.math.Matrix3; + + use namespace altphysics; + + public class RigidSphere extends Body { + + public function RigidSphere(radius:Number, mass:Number) { + var m:Matrix3; + if (mass == Infinity) { + mass = 0; + m = Matrix3.ZERO; + } else { + mass = 1/mass; + var r2:Number = radius*radius; + m = new Matrix3(); + m.a = m.f = m.k = 2.5*mass/r2; + } + super(mass, m); + if (mass == 0) movable = false; + addCollisionPrimitive(new CollisionSphere(radius, 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/primitives/RigidBox.as b/0.0.9.5/src/alternativa/physics/primitives/RigidBox.as new file mode 100644 index 0000000..24e9b9f --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/primitives/RigidBox.as @@ -0,0 +1,56 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + /** + * + */ + public class RigidBox extends Body { + + /** + * + * @param halfSize + * @param mass + */ + public function RigidBox(halfSize:Vector3, mass:Number) { + super(0, Matrix3.ZERO); + addCollisionPrimitive(new CollisionBox(halfSize, 1)); + setParams(halfSize, mass); + } + + /** + * @param halfSize + * @param mass + */ + public function setParams(halfSize:Vector3, mass:Number):void { + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + invInertia.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + invInertia.a = 3*invMass/(yy + zz); + invInertia.f = 3*invMass/(zz + xx); + invInertia.k = 3*invMass/(xx + yy); + } + var prim:CollisionBox = CollisionBox(collisionPrimitives.head.primitive); + prim.hs.vCopy(halfSize); + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/primitives/RigidCylinder.as b/0.0.9.5/src/alternativa/physics/primitives/RigidCylinder.as new file mode 100644 index 0000000..4ff65dd --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/primitives/RigidCylinder.as @@ -0,0 +1,32 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.math.Matrix3; + + use namespace altphysics; + + public class RigidCylinder extends Body { + + altphysics var r:Number; + altphysics var h:Number; + + public function RigidCylinder(mass:Number, radius:Number, height:Number) { + super(0, Matrix3.ZERO); + setParams(mass, radius, height); + } + + public function setParams(mass:Number, radius:Number, height:Number):void { + r = radius; + h = height; + invInertiaWorld.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + invInertia.a = invInertia.f = mass*(h*h/12 + r*r/4); + invInertia.k = 0.5*mass*r*r; + invInertia.invert(); + } + } + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/primitives/RigidPlane.as b/0.0.9.5/src/alternativa/physics/primitives/RigidPlane.as new file mode 100644 index 0000000..d9ebf9b --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/primitives/RigidPlane.as @@ -0,0 +1,23 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + public class RigidPlane extends Body { + + public var normal:Vector3 = new Vector3(); + public var offset:Number; + + public function RigidPlane() { + super(0, Matrix3.ZERO); + this.normal.vCopy(Vector3.Z_AXIS); + this.offset = 0; + movable = false; + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/primitives/RigidRect.as b/0.0.9.5/src/alternativa/physics/primitives/RigidRect.as new file mode 100644 index 0000000..4591931 --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/primitives/RigidRect.as @@ -0,0 +1,20 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + use namespace altphysics; + + public class RigidRect extends Body { + + public function RigidRect(halfWidth:Number, halfLength:Number) { + super(0, Matrix3.ZERO); + movable = false; + addCollisionPrimitive(new CollisionRect(new Vector3(halfWidth, halfLength, 0), 1)); + } + + } +} \ No newline at end of file diff --git a/0.0.9.5/src/alternativa/physics/primitives/RigidSphere.as b/0.0.9.5/src/alternativa/physics/primitives/RigidSphere.as new file mode 100644 index 0000000..8ad180c --- /dev/null +++ b/0.0.9.5/src/alternativa/physics/primitives/RigidSphere.as @@ -0,0 +1,29 @@ +package alternativa.physics.primitives { + + import alternativa.physics.Body; + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.math.Matrix3; + + use namespace altphysics; + + public class RigidSphere extends Body { + + public function RigidSphere(radius:Number, mass:Number) { + var m:Matrix3; + if (mass == Infinity) { + mass = 0; + m = Matrix3.ZERO; + } else { + mass = 1/mass; + var r2:Number = radius*radius; + m = new Matrix3(); + m.a = m.f = m.k = 2.5*mass/r2; + } + super(mass, m); + if (mass == 0) movable = false; + addCollisionPrimitive(new CollisionSphere(radius, 1)); + } + + } +} \ No newline at end of file diff --git a/0.1.New/.actionScriptProperties b/0.1.New/.actionScriptProperties new file mode 100644 index 0000000..2ab49bb --- /dev/null +++ b/0.1.New/.actionScriptProperties @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/0.1.New/.flexLibProperties b/0.1.New/.flexLibProperties new file mode 100644 index 0000000..8e22de4 --- /dev/null +++ b/0.1.New/.flexLibProperties @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/0.1.New/.project b/0.1.New/.project new file mode 100644 index 0000000..9c0c3d6 --- /dev/null +++ b/0.1.New/.project @@ -0,0 +1,18 @@ + + + AlternativaPhysics + + + + + + com.adobe.flexbuilder.project.flexbuilder + + + + + + com.adobe.flexbuilder.project.flexlibnature + com.adobe.flexbuilder.project.actionscriptnature + + diff --git a/0.1.New/.settings/.svn/all-wcprops b/0.1.New/.settings/.svn/all-wcprops new file mode 100644 index 0000000..8da075d --- /dev/null +++ b/0.1.New/.settings/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 91 +/!svn/ver/8834/platform/clients/fp9/libraries/AlternativaPhysics/branches/0.1.New/.settings +END +org.eclipse.core.resources.prefs +K 25 +svn:wc:ra_dav:version-url +V 124 +/!svn/ver/8834/platform/clients/fp9/libraries/AlternativaPhysics/branches/0.1.New/.settings/org.eclipse.core.resources.prefs +END diff --git a/0.1.New/.settings/.svn/entries b/0.1.New/.settings/.svn/entries new file mode 100644 index 0000000..973510d --- /dev/null +++ b/0.1.New/.settings/.svn/entries @@ -0,0 +1,40 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp9/libraries/AlternativaPhysics/branches/0.1.New/.settings +http://svndev.alternativaplatform.com + + + +2008-11-24T06:53:44.272574Z +4196 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +org.eclipse.core.resources.prefs +file + + + + +2010-10-28T04:34:31.000000Z +c19efa7d2791d7241eed3ed7c100c9a6 +2008-11-24T06:53:44.272574Z +4196 +mike + diff --git a/0.1.New/.settings/.svn/format b/0.1.New/.settings/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.1.New/.settings/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.1.New/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base b/0.1.New/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base new file mode 100644 index 0000000..19ec774 --- /dev/null +++ b/0.1.New/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base @@ -0,0 +1,3 @@ +#Tue Oct 28 01:15:11 YEKT 2008 +eclipse.preferences.version=1 +encoding/=UTF-8 diff --git a/0.1.New/.settings/org.eclipse.core.resources.prefs b/0.1.New/.settings/org.eclipse.core.resources.prefs new file mode 100644 index 0000000..19ec774 --- /dev/null +++ b/0.1.New/.settings/org.eclipse.core.resources.prefs @@ -0,0 +1,3 @@ +#Tue Oct 28 01:15:11 YEKT 2008 +eclipse.preferences.version=1 +encoding/=UTF-8 diff --git a/0.1.New/.svn/all-wcprops b/0.1.New/.svn/all-wcprops new file mode 100644 index 0000000..55a8e29 --- /dev/null +++ b/0.1.New/.svn/all-wcprops @@ -0,0 +1,23 @@ +K 25 +svn:wc:ra_dav:version-url +V 81 +/!svn/ver/9697/platform/clients/fp9/libraries/AlternativaPhysics/branches/0.1.New +END +.flexLibProperties +K 25 +svn:wc:ra_dav:version-url +V 100 +/!svn/ver/9697/platform/clients/fp9/libraries/AlternativaPhysics/branches/0.1.New/.flexLibProperties +END +.project +K 25 +svn:wc:ra_dav:version-url +V 90 +/!svn/ver/9196/platform/clients/fp9/libraries/AlternativaPhysics/branches/0.1.New/.project +END +.actionScriptProperties +K 25 +svn:wc:ra_dav:version-url +V 105 +/!svn/ver/9196/platform/clients/fp9/libraries/AlternativaPhysics/branches/0.1.New/.actionScriptProperties +END diff --git a/0.1.New/.svn/entries b/0.1.New/.svn/entries new file mode 100644 index 0000000..3a84421 --- /dev/null +++ b/0.1.New/.svn/entries @@ -0,0 +1,70 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp9/libraries/AlternativaPhysics/branches/0.1.New +http://svndev.alternativaplatform.com + + + +2009-03-23T09:25:39.308058Z +9697 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +.flexLibProperties +file + + + + +2010-10-28T04:34:31.000000Z +ecde013ed947f7b6d11924a3fe4176bb +2009-03-23T09:25:39.308058Z +9697 +mike + +.project +file + + + + +2010-10-28T04:34:31.000000Z +c456c99a28bbc231cb9b01e28168d072 +2009-03-13T13:41:20.469156Z +9196 +mike + +src +dir + +.actionScriptProperties +file + + + + +2010-10-28T04:34:31.000000Z +658c6d5287aa2917250570a9f5854c21 +2009-03-13T13:41:20.469156Z +9196 +mike + +.settings +dir + diff --git a/0.1.New/.svn/format b/0.1.New/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.1.New/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.1.New/.svn/text-base/.actionScriptProperties.svn-base b/0.1.New/.svn/text-base/.actionScriptProperties.svn-base new file mode 100644 index 0000000..2ab49bb --- /dev/null +++ b/0.1.New/.svn/text-base/.actionScriptProperties.svn-base @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/0.1.New/.svn/text-base/.flexLibProperties.svn-base b/0.1.New/.svn/text-base/.flexLibProperties.svn-base new file mode 100644 index 0000000..8e22de4 --- /dev/null +++ b/0.1.New/.svn/text-base/.flexLibProperties.svn-base @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/0.1.New/.svn/text-base/.project.svn-base b/0.1.New/.svn/text-base/.project.svn-base new file mode 100644 index 0000000..9c0c3d6 --- /dev/null +++ b/0.1.New/.svn/text-base/.project.svn-base @@ -0,0 +1,18 @@ + + + AlternativaPhysics + + + + + + com.adobe.flexbuilder.project.flexbuilder + + + + + + com.adobe.flexbuilder.project.flexlibnature + com.adobe.flexbuilder.project.actionscriptnature + + diff --git a/0.1.New/src/.svn/all-wcprops b/0.1.New/src/.svn/all-wcprops new file mode 100644 index 0000000..65dda3e --- /dev/null +++ b/0.1.New/src/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 85 +/!svn/ver/9697/platform/clients/fp9/libraries/AlternativaPhysics/branches/0.1.New/src +END diff --git a/0.1.New/src/.svn/entries b/0.1.New/src/.svn/entries new file mode 100644 index 0000000..81bdef5 --- /dev/null +++ b/0.1.New/src/.svn/entries @@ -0,0 +1,31 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp9/libraries/AlternativaPhysics/branches/0.1.New/src +http://svndev.alternativaplatform.com + + + +2009-03-23T09:25:39.308058Z +9697 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +alternativa +dir + diff --git a/0.1.New/src/.svn/format b/0.1.New/src/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.1.New/src/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.1.New/src/alternativa/.svn/all-wcprops b/0.1.New/src/alternativa/.svn/all-wcprops new file mode 100644 index 0000000..c9ce9a8 --- /dev/null +++ b/0.1.New/src/alternativa/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 97 +/!svn/ver/9697/platform/clients/fp9/libraries/AlternativaPhysics/branches/0.1.New/src/alternativa +END diff --git a/0.1.New/src/alternativa/.svn/entries b/0.1.New/src/alternativa/.svn/entries new file mode 100644 index 0000000..53b7104 --- /dev/null +++ b/0.1.New/src/alternativa/.svn/entries @@ -0,0 +1,31 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp9/libraries/AlternativaPhysics/branches/0.1.New/src/alternativa +http://svndev.alternativaplatform.com + + + +2009-03-23T09:25:39.308058Z +9697 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +physics +dir + diff --git a/0.1.New/src/alternativa/.svn/format b/0.1.New/src/alternativa/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.1.New/src/alternativa/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.1.New/src/alternativa/physics/.svn/all-wcprops b/0.1.New/src/alternativa/physics/.svn/all-wcprops new file mode 100644 index 0000000..6d40d9d --- /dev/null +++ b/0.1.New/src/alternativa/physics/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 105 +/!svn/ver/9697/platform/clients/fp9/libraries/AlternativaPhysics/branches/0.1.New/src/alternativa/physics +END diff --git a/0.1.New/src/alternativa/physics/.svn/entries b/0.1.New/src/alternativa/physics/.svn/entries new file mode 100644 index 0000000..96ce244 --- /dev/null +++ b/0.1.New/src/alternativa/physics/.svn/entries @@ -0,0 +1,37 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp9/libraries/AlternativaPhysics/branches/0.1.New/src/alternativa/physics +http://svndev.alternativaplatform.com + + + +2009-03-23T09:25:39.308058Z +9697 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +registry +dir + +force +dir + +rigid +dir + diff --git a/0.1.New/src/alternativa/physics/.svn/format b/0.1.New/src/alternativa/physics/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.1.New/src/alternativa/physics/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.1.New/src/alternativa/physics/force/.svn/all-wcprops b/0.1.New/src/alternativa/physics/force/.svn/all-wcprops new file mode 100644 index 0000000..c5cb73a --- /dev/null +++ b/0.1.New/src/alternativa/physics/force/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 111 +/!svn/ver/9196/platform/clients/fp9/libraries/AlternativaPhysics/branches/0.1.New/src/alternativa/physics/force +END +IBodyForceGenerator.as +K 25 +svn:wc:ra_dav:version-url +V 134 +/!svn/ver/9196/platform/clients/fp9/libraries/AlternativaPhysics/branches/0.1.New/src/alternativa/physics/force/IBodyForceGenerator.as +END diff --git a/0.1.New/src/alternativa/physics/force/.svn/entries b/0.1.New/src/alternativa/physics/force/.svn/entries new file mode 100644 index 0000000..bba62cd --- /dev/null +++ b/0.1.New/src/alternativa/physics/force/.svn/entries @@ -0,0 +1,40 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp9/libraries/AlternativaPhysics/branches/0.1.New/src/alternativa/physics/force +http://svndev.alternativaplatform.com + + + +2009-03-13T13:41:20.469156Z +9196 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +IBodyForceGenerator.as +file + + + + +2010-10-28T04:34:31.000000Z +59fc8b59679b1d82cf7a333f5ec54bf3 +2009-03-13T13:41:20.469156Z +9196 +mike + diff --git a/0.1.New/src/alternativa/physics/force/.svn/format b/0.1.New/src/alternativa/physics/force/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.1.New/src/alternativa/physics/force/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.1.New/src/alternativa/physics/force/.svn/text-base/IBodyForceGenerator.as.svn-base b/0.1.New/src/alternativa/physics/force/.svn/text-base/IBodyForceGenerator.as.svn-base new file mode 100644 index 0000000..cb04ea4 --- /dev/null +++ b/0.1.New/src/alternativa/physics/force/.svn/text-base/IBodyForceGenerator.as.svn-base @@ -0,0 +1,10 @@ +package alternativa.physics.force { + import alternativa.physics.rigid.Body; + + /** + * + */ + public interface IBodyForceGenerator { + function updateForce(body:Body, time:Number):void; + } +} \ No newline at end of file diff --git a/0.1.New/src/alternativa/physics/force/IBodyForceGenerator.as b/0.1.New/src/alternativa/physics/force/IBodyForceGenerator.as new file mode 100644 index 0000000..cb04ea4 --- /dev/null +++ b/0.1.New/src/alternativa/physics/force/IBodyForceGenerator.as @@ -0,0 +1,10 @@ +package alternativa.physics.force { + import alternativa.physics.rigid.Body; + + /** + * + */ + public interface IBodyForceGenerator { + function updateForce(body:Body, time:Number):void; + } +} \ No newline at end of file diff --git a/0.1.New/src/alternativa/physics/registry/.svn/all-wcprops b/0.1.New/src/alternativa/physics/registry/.svn/all-wcprops new file mode 100644 index 0000000..c273382 --- /dev/null +++ b/0.1.New/src/alternativa/physics/registry/.svn/all-wcprops @@ -0,0 +1,17 @@ +K 25 +svn:wc:ra_dav:version-url +V 114 +/!svn/ver/9196/platform/clients/fp9/libraries/AlternativaPhysics/branches/0.1.New/src/alternativa/physics/registry +END +BodyForceRegistry.as +K 25 +svn:wc:ra_dav:version-url +V 135 +/!svn/ver/9196/platform/clients/fp9/libraries/AlternativaPhysics/branches/0.1.New/src/alternativa/physics/registry/BodyForceRegistry.as +END +BodyForceRegistration.as +K 25 +svn:wc:ra_dav:version-url +V 139 +/!svn/ver/9196/platform/clients/fp9/libraries/AlternativaPhysics/branches/0.1.New/src/alternativa/physics/registry/BodyForceRegistration.as +END diff --git a/0.1.New/src/alternativa/physics/registry/.svn/entries b/0.1.New/src/alternativa/physics/registry/.svn/entries new file mode 100644 index 0000000..b2253dc --- /dev/null +++ b/0.1.New/src/alternativa/physics/registry/.svn/entries @@ -0,0 +1,52 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp9/libraries/AlternativaPhysics/branches/0.1.New/src/alternativa/physics/registry +http://svndev.alternativaplatform.com + + + +2009-03-13T13:41:20.469156Z +9196 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +BodyForceRegistry.as +file + + + + +2010-10-28T04:34:31.000000Z +65dba6360bf97c61d318b43c3de6eab9 +2009-03-13T13:41:20.469156Z +9196 +mike + +BodyForceRegistration.as +file + + + + +2010-10-28T04:34:31.000000Z +b39edb762a4c9a8dca3e160df376bb8f +2009-03-13T13:41:20.469156Z +9196 +mike + diff --git a/0.1.New/src/alternativa/physics/registry/.svn/format b/0.1.New/src/alternativa/physics/registry/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.1.New/src/alternativa/physics/registry/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.1.New/src/alternativa/physics/registry/.svn/text-base/BodyForceRegistration.as.svn-base b/0.1.New/src/alternativa/physics/registry/.svn/text-base/BodyForceRegistration.as.svn-base new file mode 100644 index 0000000..1858628 --- /dev/null +++ b/0.1.New/src/alternativa/physics/registry/.svn/text-base/BodyForceRegistration.as.svn-base @@ -0,0 +1,17 @@ +package alternativa.physics.registry { + import alternativa.physics.force.IBodyForceGenerator; + import alternativa.physics.rigid.Body; + + public class BodyForceRegistration { + + public var body:Body; + public var forceGenerator:IBodyForceGenerator; + + public var next:BodyForceRegistration; + + public function BodyForceRegistration(body:Body, forceGenerator:IBodyForceGenerator) { + this.body = body; + this.forceGenerator = forceGenerator; + } + } +} \ No newline at end of file diff --git a/0.1.New/src/alternativa/physics/registry/.svn/text-base/BodyForceRegistry.as.svn-base b/0.1.New/src/alternativa/physics/registry/.svn/text-base/BodyForceRegistry.as.svn-base new file mode 100644 index 0000000..fb73da3 --- /dev/null +++ b/0.1.New/src/alternativa/physics/registry/.svn/text-base/BodyForceRegistry.as.svn-base @@ -0,0 +1,73 @@ +package alternativa.physics.registry { + import alternativa.physics.force.IBodyForceGenerator; + import alternativa.physics.rigid.Body; + + /** + * Реестр генераторов сил. + */ + public class BodyForceRegistry { + + protected var registrations:BodyForceRegistration; + + /** + * + */ + public function BodyForceRegistry() { + } + + /** + * Добавляет генератор для указанного тела. + * + * @param body тело, для которого добавляется генератор + * @param fg добавляемый генератор сил + */ + public function add(body:Body, fg:IBodyForceGenerator):void { + // Запись добавляется в начало списка + var registration:BodyForceRegistration = new BodyForceRegistration(body, fg); + registration.next = registrations; + registrations = registration; + } + + /** + * Удаляет генератор сил для тела. + * + * @param body + * @param fg + */ + public function remove(body:Body, fg:IBodyForceGenerator):void { + var curr:BodyForceRegistration = registrations; + var prev:BodyForceRegistration = null; + while (curr != null && (curr.body != body || curr.forceGenerator != fg)) { + prev = curr; + curr = curr.next; + } + if (curr != null) { + if (curr == registrations) { + registrations = curr.next; + } else { + prev.next = curr.next; + } + } + } + + /** + * Очищает реестр. + */ + public function clear():void { + registrations = null; + } + + /** + * Запускает все зарегистрированные генераторы. + * + * @param time время, в течении которого действуют генераторы + */ + public function updateForces(time:Number):void { + var curr:BodyForceRegistration = registrations; + while (curr != null) { + curr.forceGenerator.updateForce(curr.body, time); + curr = curr.next; + } + } + } +} \ No newline at end of file diff --git a/0.1.New/src/alternativa/physics/registry/BodyForceRegistration.as b/0.1.New/src/alternativa/physics/registry/BodyForceRegistration.as new file mode 100644 index 0000000..1858628 --- /dev/null +++ b/0.1.New/src/alternativa/physics/registry/BodyForceRegistration.as @@ -0,0 +1,17 @@ +package alternativa.physics.registry { + import alternativa.physics.force.IBodyForceGenerator; + import alternativa.physics.rigid.Body; + + public class BodyForceRegistration { + + public var body:Body; + public var forceGenerator:IBodyForceGenerator; + + public var next:BodyForceRegistration; + + public function BodyForceRegistration(body:Body, forceGenerator:IBodyForceGenerator) { + this.body = body; + this.forceGenerator = forceGenerator; + } + } +} \ No newline at end of file diff --git a/0.1.New/src/alternativa/physics/registry/BodyForceRegistry.as b/0.1.New/src/alternativa/physics/registry/BodyForceRegistry.as new file mode 100644 index 0000000..fb73da3 --- /dev/null +++ b/0.1.New/src/alternativa/physics/registry/BodyForceRegistry.as @@ -0,0 +1,73 @@ +package alternativa.physics.registry { + import alternativa.physics.force.IBodyForceGenerator; + import alternativa.physics.rigid.Body; + + /** + * Реестр генераторов сил. + */ + public class BodyForceRegistry { + + protected var registrations:BodyForceRegistration; + + /** + * + */ + public function BodyForceRegistry() { + } + + /** + * Добавляет генератор для указанного тела. + * + * @param body тело, для которого добавляется генератор + * @param fg добавляемый генератор сил + */ + public function add(body:Body, fg:IBodyForceGenerator):void { + // Запись добавляется в начало списка + var registration:BodyForceRegistration = new BodyForceRegistration(body, fg); + registration.next = registrations; + registrations = registration; + } + + /** + * Удаляет генератор сил для тела. + * + * @param body + * @param fg + */ + public function remove(body:Body, fg:IBodyForceGenerator):void { + var curr:BodyForceRegistration = registrations; + var prev:BodyForceRegistration = null; + while (curr != null && (curr.body != body || curr.forceGenerator != fg)) { + prev = curr; + curr = curr.next; + } + if (curr != null) { + if (curr == registrations) { + registrations = curr.next; + } else { + prev.next = curr.next; + } + } + } + + /** + * Очищает реестр. + */ + public function clear():void { + registrations = null; + } + + /** + * Запускает все зарегистрированные генераторы. + * + * @param time время, в течении которого действуют генераторы + */ + public function updateForces(time:Number):void { + var curr:BodyForceRegistration = registrations; + while (curr != null) { + curr.forceGenerator.updateForce(curr.body, time); + curr = curr.next; + } + } + } +} \ No newline at end of file diff --git a/0.1.New/src/alternativa/physics/rigid/.svn/all-wcprops b/0.1.New/src/alternativa/physics/rigid/.svn/all-wcprops new file mode 100644 index 0000000..cc18a69 --- /dev/null +++ b/0.1.New/src/alternativa/physics/rigid/.svn/all-wcprops @@ -0,0 +1,59 @@ +K 25 +svn:wc:ra_dav:version-url +V 111 +/!svn/ver/9697/platform/clients/fp9/libraries/AlternativaPhysics/branches/0.1.New/src/alternativa/physics/rigid +END +Body.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/9697/platform/clients/fp9/libraries/AlternativaPhysics/branches/0.1.New/src/alternativa/physics/rigid/Body.as +END +ContactResolver.as +K 25 +svn:wc:ra_dav:version-url +V 130 +/!svn/ver/9562/platform/clients/fp9/libraries/AlternativaPhysics/branches/0.1.New/src/alternativa/physics/rigid/ContactResolver.as +END +BodyState.as +K 25 +svn:wc:ra_dav:version-url +V 124 +/!svn/ver/9196/platform/clients/fp9/libraries/AlternativaPhysics/branches/0.1.New/src/alternativa/physics/rigid/BodyState.as +END +ContactGenerator.as +K 25 +svn:wc:ra_dav:version-url +V 131 +/!svn/ver/9562/platform/clients/fp9/libraries/AlternativaPhysics/branches/0.1.New/src/alternativa/physics/rigid/ContactGenerator.as +END +BodyForceRegistry.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/9196/platform/clients/fp9/libraries/AlternativaPhysics/branches/0.1.New/src/alternativa/physics/rigid/BodyForceRegistry.as +END +RigidWorld.as +K 25 +svn:wc:ra_dav:version-url +V 125 +/!svn/ver/9697/platform/clients/fp9/libraries/AlternativaPhysics/branches/0.1.New/src/alternativa/physics/rigid/RigidWorld.as +END +BodyForceRegistration.as +K 25 +svn:wc:ra_dav:version-url +V 136 +/!svn/ver/9196/platform/clients/fp9/libraries/AlternativaPhysics/branches/0.1.New/src/alternativa/physics/rigid/BodyForceRegistration.as +END +Contact.as +K 25 +svn:wc:ra_dav:version-url +V 122 +/!svn/ver/9697/platform/clients/fp9/libraries/AlternativaPhysics/branches/0.1.New/src/alternativa/physics/rigid/Contact.as +END +BodyMaterial.as +K 25 +svn:wc:ra_dav:version-url +V 127 +/!svn/ver/9196/platform/clients/fp9/libraries/AlternativaPhysics/branches/0.1.New/src/alternativa/physics/rigid/BodyMaterial.as +END diff --git a/0.1.New/src/alternativa/physics/rigid/.svn/entries b/0.1.New/src/alternativa/physics/rigid/.svn/entries new file mode 100644 index 0000000..72e25ed --- /dev/null +++ b/0.1.New/src/alternativa/physics/rigid/.svn/entries @@ -0,0 +1,142 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp9/libraries/AlternativaPhysics/branches/0.1.New/src/alternativa/physics/rigid +http://svndev.alternativaplatform.com + + + +2009-03-23T09:25:39.308058Z +9697 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Body.as +file + + + + +2010-10-28T04:34:31.000000Z +00f0c40b508ca33339c56b3b67ab9f65 +2009-03-23T09:25:39.308058Z +9697 +mike + +ContactResolver.as +file + + + + +2010-10-28T04:34:31.000000Z +163cede45a1389d68599184d77c3b0e6 +2009-03-18T12:14:56.932351Z +9562 +mike + +BodyState.as +file + + + + +2010-10-28T04:34:31.000000Z +ae6b756a5f954d1245d03aa6145d85f9 +2009-03-13T13:41:20.469156Z +9196 +mike + +ContactGenerator.as +file + + + + +2010-10-28T04:34:31.000000Z +ecf6bb51592ac88508db14f36b380c85 +2009-03-18T12:14:56.932351Z +9562 +mike + +BodyForceRegistry.as +file + + + + +2010-10-28T04:34:31.000000Z +8252b086ebb1200e3168086519abdd45 +2009-03-13T13:41:20.469156Z +9196 +mike + +RigidWorld.as +file + + + + +2010-10-28T04:34:31.000000Z +7ae9db8bc33a0247af95024a3f90b71e +2009-03-23T09:25:39.308058Z +9697 +mike + +generators +dir + +BodyForceRegistration.as +file + + + + +2010-10-28T04:34:31.000000Z +b581f22ae3bd694c0a294a93f2618927 +2009-03-13T13:41:20.469156Z +9196 +mike + +primitives +dir + +Contact.as +file + + + + +2010-10-28T04:34:31.000000Z +c7394fec4f744a212e2bce280a622313 +2009-03-23T09:25:39.308058Z +9697 +mike + +BodyMaterial.as +file + + + + +2010-10-28T04:34:31.000000Z +7611867e75964f564ca1838e6e887d20 +2009-03-13T13:41:20.469156Z +9196 +mike + diff --git a/0.1.New/src/alternativa/physics/rigid/.svn/format b/0.1.New/src/alternativa/physics/rigid/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.1.New/src/alternativa/physics/rigid/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.1.New/src/alternativa/physics/rigid/.svn/text-base/Body.as.svn-base b/0.1.New/src/alternativa/physics/rigid/.svn/text-base/Body.as.svn-base new file mode 100644 index 0000000..3ec9a69 --- /dev/null +++ b/0.1.New/src/alternativa/physics/rigid/.svn/text-base/Body.as.svn-base @@ -0,0 +1,169 @@ +package alternativa.physics.rigid { + + import alternativa.types.Matrix3D; + import alternativa.types.Point3D; + import alternativa.types.Quaternion; + + /** + * Класс представляет твёрдое тело. + */ + public class Body { + public var name:String; + // Текущее и предыдущее состояние тела. Промежуточное состояние вычисляется линейной интерполяцией. + public var state:BodyState = new BodyState(); + public var prevState:BodyState = new BodyState(); + // Линейное и угловое ускорение тела на текущем шаге симуляции + public var accel:Point3D = new Point3D(); + public var angleAccel:Point3D = new Point3D(); + // Материал тела + public var material:BodyMaterial = new BodyMaterial(); + // Постоянное ускорение, действующее на тело + public var acceleration:Point3D = new Point3D(); + + public var invMass:Number = 1; + public var invInertia:Matrix3D = new Matrix3D(); + public var invInertiaWorld:Matrix3D = new Matrix3D(); + public var baseMatrix:Matrix3D = new Matrix3D(); + + public var forceAccum:Point3D = new Point3D(); + public var torqueAccum:Point3D = new Point3D(); + + public var velAux:Point3D = new Point3D(); + public var rotAux:Point3D = new Point3D(); + + public var next:Body; + + /** + * + */ + public function Body() { + setParams(1, new Matrix3D()); + } + + /** + * + * @param mass + * @param inertia + */ + public function setParams(mass:Number, inertia:Matrix3D):void { + invMass = 1/mass; + invInertia.copy(inertia); + invInertia.invert(); + } + + /** + * + * @param invMass + * @param invInertia + */ + public function setInvParams(invMass:Number, invInertia:Matrix3D):void { + this.invMass = invMass; + this.invInertia.copy(invInertia); + } + + /** + * + */ + public function calcAccelerations():void { + calcDerivedData(); + + accel.x = forceAccum.x*invMass; + accel.y = forceAccum.y*invMass; + accel.z = forceAccum.z*invMass; + accel.add(acceleration); + + invInertiaWorld.deltaTransformVector(torqueAccum, angleAccel); + } + + /** + * + * @param dt + */ + public function integrateFull(dt:Number):void { + // v = v + a*t + var v:Point3D = state.velocity; + v.x += accel.x*dt; + v.y += accel.y*dt; + v.z += accel.z*dt; + // pos = pos + v*t + 0.5*a*t*t + var k:Number = 0.5*dt*dt; + state.pos.x += v.x*dt;// + accel.x*k; + state.pos.y += v.y*dt;// + accel.y*k; + state.pos.z += v.z*dt;// + accel.z*k; + // rot = rot + eps*t + state.rotation.x += angleAccel.x*dt; + state.rotation.y += angleAccel.y*dt; + state.rotation.z += angleAccel.z*dt; + // q = q + 0.5*rot*q + state.orientation.addScaledVector(state.rotation, dt); + state.orientation.normalize(); + + state.velocity.multiply(0.995); + state.rotation.multiply(0.995); + } + + /** + * + * @param dt + */ + public function integrateVelocity(dt:Number):void { + // v = v + a*t + state.velocity.x += accel.x*dt; + state.velocity.y += accel.y*dt; + state.velocity.z += accel.z*dt; + // rot = rot + eps*t + state.rotation.x += angleAccel.x*dt; + state.rotation.y += angleAccel.y*dt; + state.rotation.z += angleAccel.z*dt; + + state.velocity.multiply(0.995); + state.rotation.multiply(0.995); + } + + /** + * + */ + public function integratePosition(dt:Number):void { + // pos = pos + v*t + state.pos.x += state.velocity.x*dt; + state.pos.y += state.velocity.y*dt; + state.pos.z += state.velocity.z*dt; + // q = q + 0.5*rot*q + state.orientation.addScaledVector(state.rotation, dt); + state.orientation.normalize(); + } + + /** + * Вычисление вспомогательной информации. + */ + public function calcDerivedData():void { + // Вычисление базисной матрицы и обратного тензора инерции в мировых координатах + state.orientation.toMatrix3D(baseMatrix); + invInertiaWorld.copy(invInertia); + invInertiaWorld.combine(baseMatrix); + baseMatrix.transpose(); + invInertiaWorld.inverseCombine(baseMatrix); + baseMatrix.transpose(); + } + + /** + * + * @param t + * @param result + */ + public function interpolate(t:Number, pos:Point3D, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.w = prevState.orientation.w*t1 + state.orientation.w*t; + orientation.x = prevState.orientation.x*t1 + state.orientation.x*t; + orientation.y = prevState.orientation.y*t1 + state.orientation.y*t; + orientation.z = prevState.orientation.z*t1 + state.orientation.z*t; + } + + public function toString():String { + return "[Body name=" + name + "]"; + } + } +} \ No newline at end of file diff --git a/0.1.New/src/alternativa/physics/rigid/.svn/text-base/BodyForceRegistration.as.svn-base b/0.1.New/src/alternativa/physics/rigid/.svn/text-base/BodyForceRegistration.as.svn-base new file mode 100644 index 0000000..93019e6 --- /dev/null +++ b/0.1.New/src/alternativa/physics/rigid/.svn/text-base/BodyForceRegistration.as.svn-base @@ -0,0 +1,16 @@ +package alternativa.physics.rigid { + import alternativa.physics.force.IBodyForceGenerator; + import alternativa.physics.rigid.Body; + + public class BodyForceRegistration { + public var body:Body; + public var forceGenerator:IBodyForceGenerator; + + public var next:BodyForceRegistration; + + public function BodyForceRegistration(body:Body, forceGenerator:IBodyForceGenerator) { + this.body = body; + this.forceGenerator = forceGenerator; + } + } +} \ No newline at end of file diff --git a/0.1.New/src/alternativa/physics/rigid/.svn/text-base/BodyForceRegistry.as.svn-base b/0.1.New/src/alternativa/physics/rigid/.svn/text-base/BodyForceRegistry.as.svn-base new file mode 100644 index 0000000..dcc7759 --- /dev/null +++ b/0.1.New/src/alternativa/physics/rigid/.svn/text-base/BodyForceRegistry.as.svn-base @@ -0,0 +1,47 @@ +package alternativa.physics.rigid { + import alternativa.physics.force.IBodyForceGenerator; + import alternativa.physics.rigid.Body; + + public class BodyForceRegistry { + + protected var registrations:BodyForceRegistration; + + public function BodyForceRegistry() { + } + + public function add(body:Body, fg:IBodyForceGenerator):void { + var registration:BodyForceRegistration = new BodyForceRegistration(body, fg); + registration.next = registrations; + registrations = registration; + } + + public function remove(body:Body, fg:IBodyForceGenerator):void { + var current:BodyForceRegistration = registrations; + var prev:BodyForceRegistration = null; + while (current != null) { + if (current.body == body && current.forceGenerator == fg) { + if (current == registrations) { + registrations = current.next; + } else { + prev.next = current.next; + } + break; + } + prev = current; + current = current.next; + } + } + + public function clear():void { + registrations = null; + } + + public function updateForces(time:Number):void { + var current:BodyForceRegistration = registrations; + while (current != null) { + current.forceGenerator.updateForce(current.body, time); + current = current.next; + } + } + } +} \ No newline at end of file diff --git a/0.1.New/src/alternativa/physics/rigid/.svn/text-base/BodyMaterial.as.svn-base b/0.1.New/src/alternativa/physics/rigid/.svn/text-base/BodyMaterial.as.svn-base new file mode 100644 index 0000000..00eb88d --- /dev/null +++ b/0.1.New/src/alternativa/physics/rigid/.svn/text-base/BodyMaterial.as.svn-base @@ -0,0 +1,27 @@ +package alternativa.physics.rigid { + + /** + * Материал твёрдого тела. + */ + public class BodyMaterial { + /** + * Коэффициент отскока. + */ + public var restitution:Number; + /** + * Коэффициент трения. + */ + public var friction:Number; + + /** + * + * @param restitution коэффициент отскока + * @param friction коэффициент трения + */ + public function BodyMaterial(restitution:Number = 0, friction:Number = 0) { + this.restitution = restitution; + this.friction = friction; + } + + } +} \ No newline at end of file diff --git a/0.1.New/src/alternativa/physics/rigid/.svn/text-base/BodyState.as.svn-base b/0.1.New/src/alternativa/physics/rigid/.svn/text-base/BodyState.as.svn-base new file mode 100644 index 0000000..1ade4df --- /dev/null +++ b/0.1.New/src/alternativa/physics/rigid/.svn/text-base/BodyState.as.svn-base @@ -0,0 +1,39 @@ +package alternativa.physics.rigid { + import alternativa.types.Point3D; + import alternativa.types.Quaternion; + + /** + * Класс описывает состояние твёрдого тела. + */ + public class BodyState { + /** + * Положение тела. + */ + public var pos:Point3D = new Point3D(); + /** + * Ориентация тела. + */ + public var orientation:Quaternion = new Quaternion(); + /** + * Скорость тела. + */ + public var velocity:Point3D = new Point3D(); + /** + * Угловая скорость тела. + */ + public var rotation:Point3D = new Point3D(); + + /** + * Копирует значение указанного объекта. + * + * @param state + */ + public function copy(state:BodyState):void { + pos.copy(state.pos); + orientation.copy(state.orientation); + velocity.copy(state.velocity); + rotation.copy(state.rotation); + } + + } +} \ No newline at end of file diff --git a/0.1.New/src/alternativa/physics/rigid/.svn/text-base/Contact.as.svn-base b/0.1.New/src/alternativa/physics/rigid/.svn/text-base/Contact.as.svn-base new file mode 100644 index 0000000..5436282 --- /dev/null +++ b/0.1.New/src/alternativa/physics/rigid/.svn/text-base/Contact.as.svn-base @@ -0,0 +1,146 @@ +package alternativa.physics.rigid { + + import alternativa.types.Point3D; + + /** + * Класс описывает контакт между телами. Контакты могут составлять однонаправленный список. + */ + public class Contact { + + // Индекс контакта в списке. Используется для внутренних целей. + public var index:int; + // Следующий контакт в списке + public var next:Contact; + // Первое тело, к которому относится контакт + public var body1:Body; + // Второе тело, к которому относится контакт (nullable) + public var body2:Body; + // Координаты точки контакта + public var pos:Point3D = new Point3D(); + // Нормаль контакта. Нормаль направлена от второго тела к первому. В локальной системе + // координат контакта нормаль является базисным вектором, задающим направление оси X. + public var normal:Point3D = new Point3D(); + // Глубина проникновения в точке контакта + public var penetration:Number; + // Служебная переменная для отслеживания времени жизни контакта + public var timeStamp:int; + // Минимальная скорость разделения контакта на текущем шаге + public var minSepVelocity:Number; + + /** + * Создаёт новый экземпляр. + * + * @param index индекс контакта в списке + */ + public function Contact(index:int) { + this.index = index; + } + + + private var _v1:Point3D = new Point3D(); + private var _v2:Point3D = new Point3D(); + private var _r:Point3D = new Point3D(); + /** + * + * @return + */ + public function getSepVelocity():Number { + // sepVel = (V1 - V2)*normal + + // V1 = V1_c + w1%r1 + _r.difference(pos, body1.state.pos); + _v1.copy(body1.state.rotation); + _v1.cross(_r); + _v1.add(body1.state.velocity); + + if (body2 != null) { + // V2 = V2_c + w2%r2 + _r.difference(pos, body2.state.pos); + _v2.copy(body2.state.rotation); + _v2.cross(_r); + _v2.add(body2.state.velocity); + + _v1.subtract(_v2); + } + return _v1.dot(normal); + } + + private var _v:Point3D = new Point3D(); + /** + * + * @param forceInelastic + */ + public function resolve(forceInelastic:Boolean, dt:Number):void { + var restitution:Number; + if (forceInelastic) { + restitution = 0; + } else { + restitution = body1.material.restitution; + if ((body2 != null) && (body2.material.restitution < restitution)) { + restitution = body2.material.restitution; + } + } + // TODO: optimize getSepVelocity() + var sepVel:Number = getSepVelocity(); + if (sepVel > minSepVelocity) { +// trace("ret", sepVel); + return; + } + var velAfterBounce:Number = -restitution*sepVel; + if (velAfterBounce < minSepVelocity) { + velAfterBounce = minSepVelocity; + } + var deltaVel:Number = velAfterBounce - sepVel; + + // Находим изменение скорости сближения под действием единичного импульса вдоль нормали + + var deltaVelByUnitImpulse:Number = body1.invMass; + // {[invInertiaWorld * (r % n)] % r} * n + _r.difference(pos, body1.state.pos); + _v.cross2(_r, normal); + _v.deltaTransform(body1.invInertiaWorld); + _v.cross(_r); + deltaVelByUnitImpulse += _v.dot(normal); + + if (body2 != null) { + deltaVelByUnitImpulse += body2.invMass; + + _r.difference(pos, body2.state.pos); + _v.cross2(_r, normal); + _v.deltaTransform(body2.invInertiaWorld); + _v.cross(_r); + deltaVelByUnitImpulse += _v.dot(normal); + } + + // Вычисляем требуемый импульс вдоль нормали + var normalImpulse:Number = deltaVel/deltaVelByUnitImpulse; + + // Применяем импульс к телам + + var d:Number = normalImpulse*body1.invMass; + body1.state.velocity.x += normal.x*d; + body1.state.velocity.y += normal.y*d; + body1.state.velocity.z += normal.z*d; + + _r.difference(pos, body1.state.pos); + _v.cross2(_r, normal); + _v.multiply(normalImpulse); + _v.deltaTransform(body1.invInertiaWorld); + body1.state.rotation.add(_v); + + if (body2 != null) { + d = normalImpulse*body2.invMass; + body2.state.velocity.x -= normal.x*d; + body2.state.velocity.y -= normal.y*d; + body2.state.velocity.z -= normal.z*d; + + _r.difference(pos, body2.state.pos); + _v.cross2(_r, normal); + _v.multiply(-normalImpulse); + _v.deltaTransform(body2.invInertiaWorld); + body2.state.rotation.add(_v); + } + } + + } +} diff --git a/0.1.New/src/alternativa/physics/rigid/.svn/text-base/ContactGenerator.as.svn-base b/0.1.New/src/alternativa/physics/rigid/.svn/text-base/ContactGenerator.as.svn-base new file mode 100644 index 0000000..74e8344 --- /dev/null +++ b/0.1.New/src/alternativa/physics/rigid/.svn/text-base/ContactGenerator.as.svn-base @@ -0,0 +1,22 @@ +package alternativa.physics.rigid { + + /** + * Генератор контактов для твёрдых тел. + */ + public class ContactGenerator { + /** + * Следующий генератор в списке. + */ + public var next:ContactGenerator; + + /** + * Метод создаёт контакты. + * + * @param contact первый свободный для записи контакт в списке контактов мира + * @return следующий свободный для записи контакт после окончания работы метода. Значение null означает, что доступные контакты исчерпаны. + */ + public function addContacts(contact:Contact):Contact { + return null; + } + } +} \ No newline at end of file diff --git a/0.1.New/src/alternativa/physics/rigid/.svn/text-base/ContactResolver.as.svn-base b/0.1.New/src/alternativa/physics/rigid/.svn/text-base/ContactResolver.as.svn-base new file mode 100644 index 0000000..eebed68 --- /dev/null +++ b/0.1.New/src/alternativa/physics/rigid/.svn/text-base/ContactResolver.as.svn-base @@ -0,0 +1,17 @@ +package alternativa.physics.rigid { + + import alternativa.types.Point3D; + + /** + * + */ + public class ContactResolver { + + /** + * + */ + public function ContactResolver() { + } + + } +} \ No newline at end of file diff --git a/0.1.New/src/alternativa/physics/rigid/.svn/text-base/RigidWorld.as.svn-base b/0.1.New/src/alternativa/physics/rigid/.svn/text-base/RigidWorld.as.svn-base new file mode 100644 index 0000000..b872a2c --- /dev/null +++ b/0.1.New/src/alternativa/physics/rigid/.svn/text-base/RigidWorld.as.svn-base @@ -0,0 +1,276 @@ +package alternativa.physics.rigid { + import flash.utils.getTimer; + + + /** + * Физический симулятор. + */ + public class RigidWorld { + /** + * При установленном значении true, перед процедурой определения столкновений все скорости и положения тел будут временно проинтегрированы + * на один шаг, а после состояния тел будут возвращены к значениям на начало шага симуляции. + */ + public var predictCollisions:Boolean = false; + /** + * Максимально допустимая глубина пересечения тел в точке контакта. + */ + public var maxPenetration:Number = 0.01; + /** + * Максимальное значение дополнительной скорости разделения в точке контакта. + */ + public var maxSepVelocity:Number = 0.5; + /** + * Количество шагов симуляции, за которое должно быть устранено пересечение тел в отсутствие внешних воздействий. + * Исходя из значения данного параметра вычисляется дополнительный импульс разделения в точке контакта. + */ + public var penResolutionSteps:int = 5; + + public var conllisionIterations:int = 3; + + // Список тел в симуляторе. + public var bodies:Body; + // Ссылка на последнее тело в списке + private var lastBody:Body; + // Реестр генераторов сил. + public var forceRegistry:BodyForceRegistry = new BodyForceRegistry(); + // Список контактов + public var contacts:Contact; + + private var contactGenerators:ContactGenerator; + private var maxContacts:int; + private var numContacts:int; + + public var timeStamp:uint; + + /** + * + * @param maxContacts максимально допустимое количество контактов + */ + public function RigidWorld(maxContacts:int = 100) { + this.maxContacts = maxContacts; + createContactsList(maxContacts); + } + + /** + * Создаёт список контактов заданной длины. + */ + private function createContactsList(num:int):void { + contacts = new Contact(0); + var last:Contact = contacts; + for (var i:int = 1; i < num; i++) { + last = last.next = new Contact(i); + } + } + + /** + * Добавляет телов симулятор. + * + * @param body добавляемое тело + */ + public function addBody(body:Body):void { + if (bodies == null) { + bodies = lastBody = body; + } else { + lastBody = lastBody.next = body; + } + } + + /** + * Удаляет тело из симулятора. + * + * @param body удаляемое тело + */ + public function removeBody(body:Body):Boolean { + if (body == null) { + return false; + } + + if (body == bodies) { + if (lastBody == bodies) { + lastBody = null; + } + bodies = bodies.next; + return true; + } + + var b:Body = bodies; + var prev:Body; + while (b != null && b != body) { + prev = b; + b = b.next; + } + if (b == null) { + return false; + } else { + if (lastBody == b) { + lastBody = prev; + } + prev.next = b.next; + return true; + } + } + + /** + * + * @param gen + * + */ + public function addContactGenerator(gen:ContactGenerator):void { + if (contactGenerators == null) { + contactGenerators = gen; + } else { + gen.next = contactGenerators; + contactGenerators = gen; + } + } + + /** + * Выполняет шаг симуляции. + * + * @param dt длительность шага симуляции + */ + public function runPhysics(dt:Number):void { + // Применяем все силы, действующие на тела + forceRegistry.updateForces(dt); + // Сохраняем состояние тела и считаем ускорения + var body:Body = bodies; + while (body != null) { + body.prevState.copy(body.state); + body.calcAccelerations(); + body = body.next; + } + // Создаём контакты. Контакты получаются из детектора столкновений и ограничений. + generateContacts(dt); + // Разрешение контактов. Изменяются скорости тел. + processContacts(dt, false); + // Интегрирование скоростей тел с откорректированными начальными условиями + integrateVelocity(dt); + // Разрешение оставшихся актуальных контактов, используя абсолютно неупругое взаимодействие + processContacts(dt, true); + // Shock propagation + shockPropagation(); + // Интегрирование положения тел, используя ранее полученные скорости + integratePosition(dt); + + timeStamp++; + } + + /** + * + * @param dt + */ + private function integrateVelocity(dt:Number):void { + var curr:Body = bodies; + while (curr != null) { + curr.integrateVelocity(dt); + curr = curr.next; + } + } + + /** + * + * @param dt + */ + private function integratePosition(dt:Number):void { + var curr:Body = bodies; + while (curr != null) { + curr.integratePosition(dt); + curr = curr.next; + } + } + + /** + * + */ + private function generateContacts(dt:Number):void { + var body:Body; + if (predictCollisions) { + // Для всех тел сохраняем текущее состояние и интегрируем его + body = bodies; + while (body != null) { + body.integrateFull(dt); + body.calcDerivedData(); + body = body.next; + } + } + + var gen:ContactGenerator = contactGenerators; + var contact:Contact = contacts; + while (gen != null && contact != null) { + contact = gen.addContacts(contact); + gen = gen.next; + } + numContacts = contact == null ? maxContacts : contact.index; + + if (predictCollisions) { + // Восстанавливаем сохранённое состояние + body = bodies; + while (body != null) { + body.state.copy(body.prevState); + body = body.next; + } + } + } + + /** + * + * @param inelastic + */ + private function processContacts(dt:Number, forceInelastic:Boolean):void { + if (numContacts == 0) { + return; + } + + // Вычисление дополнительных скоростей разделения в точках контактов + var contact:Contact = contacts; + while (contact != null && contact.index < numContacts) { + var time:Number = penResolutionSteps*dt; + if (contact.penetration > maxPenetration) { + var minSepVel:Number = (contact.penetration - maxPenetration)/time; + contact.minSepVelocity = minSepVel < maxSepVelocity ? minSepVel : maxSepVelocity; + } else { + contact.minSepVelocity = 0; + } + contact = contact.next; + } + + // Итерационное применение импульсов к контактам + var iterations:int = conllisionIterations*numContacts; + trace("timeStamp", timeStamp); + for (var iter:int = 0; iter < iterations; iter++) { + contact = contacts; + var max:Number = 0; + var bestContact:Contact = null; + while (contact != null && contact.index < numContacts) { + if (contact.getSepVelocity() <= contact.minSepVelocity) { + if (contact.getSepVelocity() < max) { + max = contact.getSepVelocity(); + bestContact = contact; + } + } + contact = contact.next; + } + if (bestContact == null) { + return; + } +// trace(timeStamp, "bestContact", bestContact.index); + trace(bestContact.index, "before", bestContact.getSepVelocity(), max); + bestContact.resolve(forceInelastic, dt); + trace(bestContact.index, "after", bestContact.getSepVelocity(), bestContact.minSepVelocity); + } + contact = contacts; + while (contact != null && contact.index < numContacts) { + trace(contact.getSepVelocity()); + contact = contact.next; + } + } + + /** + * + */ + private function shockPropagation():void { + + } + + } +} \ No newline at end of file diff --git a/0.1.New/src/alternativa/physics/rigid/Body.as b/0.1.New/src/alternativa/physics/rigid/Body.as new file mode 100644 index 0000000..3ec9a69 --- /dev/null +++ b/0.1.New/src/alternativa/physics/rigid/Body.as @@ -0,0 +1,169 @@ +package alternativa.physics.rigid { + + import alternativa.types.Matrix3D; + import alternativa.types.Point3D; + import alternativa.types.Quaternion; + + /** + * Класс представляет твёрдое тело. + */ + public class Body { + public var name:String; + // Текущее и предыдущее состояние тела. Промежуточное состояние вычисляется линейной интерполяцией. + public var state:BodyState = new BodyState(); + public var prevState:BodyState = new BodyState(); + // Линейное и угловое ускорение тела на текущем шаге симуляции + public var accel:Point3D = new Point3D(); + public var angleAccel:Point3D = new Point3D(); + // Материал тела + public var material:BodyMaterial = new BodyMaterial(); + // Постоянное ускорение, действующее на тело + public var acceleration:Point3D = new Point3D(); + + public var invMass:Number = 1; + public var invInertia:Matrix3D = new Matrix3D(); + public var invInertiaWorld:Matrix3D = new Matrix3D(); + public var baseMatrix:Matrix3D = new Matrix3D(); + + public var forceAccum:Point3D = new Point3D(); + public var torqueAccum:Point3D = new Point3D(); + + public var velAux:Point3D = new Point3D(); + public var rotAux:Point3D = new Point3D(); + + public var next:Body; + + /** + * + */ + public function Body() { + setParams(1, new Matrix3D()); + } + + /** + * + * @param mass + * @param inertia + */ + public function setParams(mass:Number, inertia:Matrix3D):void { + invMass = 1/mass; + invInertia.copy(inertia); + invInertia.invert(); + } + + /** + * + * @param invMass + * @param invInertia + */ + public function setInvParams(invMass:Number, invInertia:Matrix3D):void { + this.invMass = invMass; + this.invInertia.copy(invInertia); + } + + /** + * + */ + public function calcAccelerations():void { + calcDerivedData(); + + accel.x = forceAccum.x*invMass; + accel.y = forceAccum.y*invMass; + accel.z = forceAccum.z*invMass; + accel.add(acceleration); + + invInertiaWorld.deltaTransformVector(torqueAccum, angleAccel); + } + + /** + * + * @param dt + */ + public function integrateFull(dt:Number):void { + // v = v + a*t + var v:Point3D = state.velocity; + v.x += accel.x*dt; + v.y += accel.y*dt; + v.z += accel.z*dt; + // pos = pos + v*t + 0.5*a*t*t + var k:Number = 0.5*dt*dt; + state.pos.x += v.x*dt;// + accel.x*k; + state.pos.y += v.y*dt;// + accel.y*k; + state.pos.z += v.z*dt;// + accel.z*k; + // rot = rot + eps*t + state.rotation.x += angleAccel.x*dt; + state.rotation.y += angleAccel.y*dt; + state.rotation.z += angleAccel.z*dt; + // q = q + 0.5*rot*q + state.orientation.addScaledVector(state.rotation, dt); + state.orientation.normalize(); + + state.velocity.multiply(0.995); + state.rotation.multiply(0.995); + } + + /** + * + * @param dt + */ + public function integrateVelocity(dt:Number):void { + // v = v + a*t + state.velocity.x += accel.x*dt; + state.velocity.y += accel.y*dt; + state.velocity.z += accel.z*dt; + // rot = rot + eps*t + state.rotation.x += angleAccel.x*dt; + state.rotation.y += angleAccel.y*dt; + state.rotation.z += angleAccel.z*dt; + + state.velocity.multiply(0.995); + state.rotation.multiply(0.995); + } + + /** + * + */ + public function integratePosition(dt:Number):void { + // pos = pos + v*t + state.pos.x += state.velocity.x*dt; + state.pos.y += state.velocity.y*dt; + state.pos.z += state.velocity.z*dt; + // q = q + 0.5*rot*q + state.orientation.addScaledVector(state.rotation, dt); + state.orientation.normalize(); + } + + /** + * Вычисление вспомогательной информации. + */ + public function calcDerivedData():void { + // Вычисление базисной матрицы и обратного тензора инерции в мировых координатах + state.orientation.toMatrix3D(baseMatrix); + invInertiaWorld.copy(invInertia); + invInertiaWorld.combine(baseMatrix); + baseMatrix.transpose(); + invInertiaWorld.inverseCombine(baseMatrix); + baseMatrix.transpose(); + } + + /** + * + * @param t + * @param result + */ + public function interpolate(t:Number, pos:Point3D, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.w = prevState.orientation.w*t1 + state.orientation.w*t; + orientation.x = prevState.orientation.x*t1 + state.orientation.x*t; + orientation.y = prevState.orientation.y*t1 + state.orientation.y*t; + orientation.z = prevState.orientation.z*t1 + state.orientation.z*t; + } + + public function toString():String { + return "[Body name=" + name + "]"; + } + } +} \ No newline at end of file diff --git a/0.1.New/src/alternativa/physics/rigid/BodyForceRegistration.as b/0.1.New/src/alternativa/physics/rigid/BodyForceRegistration.as new file mode 100644 index 0000000..93019e6 --- /dev/null +++ b/0.1.New/src/alternativa/physics/rigid/BodyForceRegistration.as @@ -0,0 +1,16 @@ +package alternativa.physics.rigid { + import alternativa.physics.force.IBodyForceGenerator; + import alternativa.physics.rigid.Body; + + public class BodyForceRegistration { + public var body:Body; + public var forceGenerator:IBodyForceGenerator; + + public var next:BodyForceRegistration; + + public function BodyForceRegistration(body:Body, forceGenerator:IBodyForceGenerator) { + this.body = body; + this.forceGenerator = forceGenerator; + } + } +} \ No newline at end of file diff --git a/0.1.New/src/alternativa/physics/rigid/BodyForceRegistry.as b/0.1.New/src/alternativa/physics/rigid/BodyForceRegistry.as new file mode 100644 index 0000000..dcc7759 --- /dev/null +++ b/0.1.New/src/alternativa/physics/rigid/BodyForceRegistry.as @@ -0,0 +1,47 @@ +package alternativa.physics.rigid { + import alternativa.physics.force.IBodyForceGenerator; + import alternativa.physics.rigid.Body; + + public class BodyForceRegistry { + + protected var registrations:BodyForceRegistration; + + public function BodyForceRegistry() { + } + + public function add(body:Body, fg:IBodyForceGenerator):void { + var registration:BodyForceRegistration = new BodyForceRegistration(body, fg); + registration.next = registrations; + registrations = registration; + } + + public function remove(body:Body, fg:IBodyForceGenerator):void { + var current:BodyForceRegistration = registrations; + var prev:BodyForceRegistration = null; + while (current != null) { + if (current.body == body && current.forceGenerator == fg) { + if (current == registrations) { + registrations = current.next; + } else { + prev.next = current.next; + } + break; + } + prev = current; + current = current.next; + } + } + + public function clear():void { + registrations = null; + } + + public function updateForces(time:Number):void { + var current:BodyForceRegistration = registrations; + while (current != null) { + current.forceGenerator.updateForce(current.body, time); + current = current.next; + } + } + } +} \ No newline at end of file diff --git a/0.1.New/src/alternativa/physics/rigid/BodyMaterial.as b/0.1.New/src/alternativa/physics/rigid/BodyMaterial.as new file mode 100644 index 0000000..00eb88d --- /dev/null +++ b/0.1.New/src/alternativa/physics/rigid/BodyMaterial.as @@ -0,0 +1,27 @@ +package alternativa.physics.rigid { + + /** + * Материал твёрдого тела. + */ + public class BodyMaterial { + /** + * Коэффициент отскока. + */ + public var restitution:Number; + /** + * Коэффициент трения. + */ + public var friction:Number; + + /** + * + * @param restitution коэффициент отскока + * @param friction коэффициент трения + */ + public function BodyMaterial(restitution:Number = 0, friction:Number = 0) { + this.restitution = restitution; + this.friction = friction; + } + + } +} \ No newline at end of file diff --git a/0.1.New/src/alternativa/physics/rigid/BodyState.as b/0.1.New/src/alternativa/physics/rigid/BodyState.as new file mode 100644 index 0000000..1ade4df --- /dev/null +++ b/0.1.New/src/alternativa/physics/rigid/BodyState.as @@ -0,0 +1,39 @@ +package alternativa.physics.rigid { + import alternativa.types.Point3D; + import alternativa.types.Quaternion; + + /** + * Класс описывает состояние твёрдого тела. + */ + public class BodyState { + /** + * Положение тела. + */ + public var pos:Point3D = new Point3D(); + /** + * Ориентация тела. + */ + public var orientation:Quaternion = new Quaternion(); + /** + * Скорость тела. + */ + public var velocity:Point3D = new Point3D(); + /** + * Угловая скорость тела. + */ + public var rotation:Point3D = new Point3D(); + + /** + * Копирует значение указанного объекта. + * + * @param state + */ + public function copy(state:BodyState):void { + pos.copy(state.pos); + orientation.copy(state.orientation); + velocity.copy(state.velocity); + rotation.copy(state.rotation); + } + + } +} \ No newline at end of file diff --git a/0.1.New/src/alternativa/physics/rigid/Contact.as b/0.1.New/src/alternativa/physics/rigid/Contact.as new file mode 100644 index 0000000..5436282 --- /dev/null +++ b/0.1.New/src/alternativa/physics/rigid/Contact.as @@ -0,0 +1,146 @@ +package alternativa.physics.rigid { + + import alternativa.types.Point3D; + + /** + * Класс описывает контакт между телами. Контакты могут составлять однонаправленный список. + */ + public class Contact { + + // Индекс контакта в списке. Используется для внутренних целей. + public var index:int; + // Следующий контакт в списке + public var next:Contact; + // Первое тело, к которому относится контакт + public var body1:Body; + // Второе тело, к которому относится контакт (nullable) + public var body2:Body; + // Координаты точки контакта + public var pos:Point3D = new Point3D(); + // Нормаль контакта. Нормаль направлена от второго тела к первому. В локальной системе + // координат контакта нормаль является базисным вектором, задающим направление оси X. + public var normal:Point3D = new Point3D(); + // Глубина проникновения в точке контакта + public var penetration:Number; + // Служебная переменная для отслеживания времени жизни контакта + public var timeStamp:int; + // Минимальная скорость разделения контакта на текущем шаге + public var minSepVelocity:Number; + + /** + * Создаёт новый экземпляр. + * + * @param index индекс контакта в списке + */ + public function Contact(index:int) { + this.index = index; + } + + + private var _v1:Point3D = new Point3D(); + private var _v2:Point3D = new Point3D(); + private var _r:Point3D = new Point3D(); + /** + * + * @return + */ + public function getSepVelocity():Number { + // sepVel = (V1 - V2)*normal + + // V1 = V1_c + w1%r1 + _r.difference(pos, body1.state.pos); + _v1.copy(body1.state.rotation); + _v1.cross(_r); + _v1.add(body1.state.velocity); + + if (body2 != null) { + // V2 = V2_c + w2%r2 + _r.difference(pos, body2.state.pos); + _v2.copy(body2.state.rotation); + _v2.cross(_r); + _v2.add(body2.state.velocity); + + _v1.subtract(_v2); + } + return _v1.dot(normal); + } + + private var _v:Point3D = new Point3D(); + /** + * + * @param forceInelastic + */ + public function resolve(forceInelastic:Boolean, dt:Number):void { + var restitution:Number; + if (forceInelastic) { + restitution = 0; + } else { + restitution = body1.material.restitution; + if ((body2 != null) && (body2.material.restitution < restitution)) { + restitution = body2.material.restitution; + } + } + // TODO: optimize getSepVelocity() + var sepVel:Number = getSepVelocity(); + if (sepVel > minSepVelocity) { +// trace("ret", sepVel); + return; + } + var velAfterBounce:Number = -restitution*sepVel; + if (velAfterBounce < minSepVelocity) { + velAfterBounce = minSepVelocity; + } + var deltaVel:Number = velAfterBounce - sepVel; + + // Находим изменение скорости сближения под действием единичного импульса вдоль нормали + + var deltaVelByUnitImpulse:Number = body1.invMass; + // {[invInertiaWorld * (r % n)] % r} * n + _r.difference(pos, body1.state.pos); + _v.cross2(_r, normal); + _v.deltaTransform(body1.invInertiaWorld); + _v.cross(_r); + deltaVelByUnitImpulse += _v.dot(normal); + + if (body2 != null) { + deltaVelByUnitImpulse += body2.invMass; + + _r.difference(pos, body2.state.pos); + _v.cross2(_r, normal); + _v.deltaTransform(body2.invInertiaWorld); + _v.cross(_r); + deltaVelByUnitImpulse += _v.dot(normal); + } + + // Вычисляем требуемый импульс вдоль нормали + var normalImpulse:Number = deltaVel/deltaVelByUnitImpulse; + + // Применяем импульс к телам + + var d:Number = normalImpulse*body1.invMass; + body1.state.velocity.x += normal.x*d; + body1.state.velocity.y += normal.y*d; + body1.state.velocity.z += normal.z*d; + + _r.difference(pos, body1.state.pos); + _v.cross2(_r, normal); + _v.multiply(normalImpulse); + _v.deltaTransform(body1.invInertiaWorld); + body1.state.rotation.add(_v); + + if (body2 != null) { + d = normalImpulse*body2.invMass; + body2.state.velocity.x -= normal.x*d; + body2.state.velocity.y -= normal.y*d; + body2.state.velocity.z -= normal.z*d; + + _r.difference(pos, body2.state.pos); + _v.cross2(_r, normal); + _v.multiply(-normalImpulse); + _v.deltaTransform(body2.invInertiaWorld); + body2.state.rotation.add(_v); + } + } + + } +} diff --git a/0.1.New/src/alternativa/physics/rigid/ContactGenerator.as b/0.1.New/src/alternativa/physics/rigid/ContactGenerator.as new file mode 100644 index 0000000..74e8344 --- /dev/null +++ b/0.1.New/src/alternativa/physics/rigid/ContactGenerator.as @@ -0,0 +1,22 @@ +package alternativa.physics.rigid { + + /** + * Генератор контактов для твёрдых тел. + */ + public class ContactGenerator { + /** + * Следующий генератор в списке. + */ + public var next:ContactGenerator; + + /** + * Метод создаёт контакты. + * + * @param contact первый свободный для записи контакт в списке контактов мира + * @return следующий свободный для записи контакт после окончания работы метода. Значение null означает, что доступные контакты исчерпаны. + */ + public function addContacts(contact:Contact):Contact { + return null; + } + } +} \ No newline at end of file diff --git a/0.1.New/src/alternativa/physics/rigid/ContactResolver.as b/0.1.New/src/alternativa/physics/rigid/ContactResolver.as new file mode 100644 index 0000000..eebed68 --- /dev/null +++ b/0.1.New/src/alternativa/physics/rigid/ContactResolver.as @@ -0,0 +1,17 @@ +package alternativa.physics.rigid { + + import alternativa.types.Point3D; + + /** + * + */ + public class ContactResolver { + + /** + * + */ + public function ContactResolver() { + } + + } +} \ No newline at end of file diff --git a/0.1.New/src/alternativa/physics/rigid/RigidWorld.as b/0.1.New/src/alternativa/physics/rigid/RigidWorld.as new file mode 100644 index 0000000..b872a2c --- /dev/null +++ b/0.1.New/src/alternativa/physics/rigid/RigidWorld.as @@ -0,0 +1,276 @@ +package alternativa.physics.rigid { + import flash.utils.getTimer; + + + /** + * Физический симулятор. + */ + public class RigidWorld { + /** + * При установленном значении true, перед процедурой определения столкновений все скорости и положения тел будут временно проинтегрированы + * на один шаг, а после состояния тел будут возвращены к значениям на начало шага симуляции. + */ + public var predictCollisions:Boolean = false; + /** + * Максимально допустимая глубина пересечения тел в точке контакта. + */ + public var maxPenetration:Number = 0.01; + /** + * Максимальное значение дополнительной скорости разделения в точке контакта. + */ + public var maxSepVelocity:Number = 0.5; + /** + * Количество шагов симуляции, за которое должно быть устранено пересечение тел в отсутствие внешних воздействий. + * Исходя из значения данного параметра вычисляется дополнительный импульс разделения в точке контакта. + */ + public var penResolutionSteps:int = 5; + + public var conllisionIterations:int = 3; + + // Список тел в симуляторе. + public var bodies:Body; + // Ссылка на последнее тело в списке + private var lastBody:Body; + // Реестр генераторов сил. + public var forceRegistry:BodyForceRegistry = new BodyForceRegistry(); + // Список контактов + public var contacts:Contact; + + private var contactGenerators:ContactGenerator; + private var maxContacts:int; + private var numContacts:int; + + public var timeStamp:uint; + + /** + * + * @param maxContacts максимально допустимое количество контактов + */ + public function RigidWorld(maxContacts:int = 100) { + this.maxContacts = maxContacts; + createContactsList(maxContacts); + } + + /** + * Создаёт список контактов заданной длины. + */ + private function createContactsList(num:int):void { + contacts = new Contact(0); + var last:Contact = contacts; + for (var i:int = 1; i < num; i++) { + last = last.next = new Contact(i); + } + } + + /** + * Добавляет телов симулятор. + * + * @param body добавляемое тело + */ + public function addBody(body:Body):void { + if (bodies == null) { + bodies = lastBody = body; + } else { + lastBody = lastBody.next = body; + } + } + + /** + * Удаляет тело из симулятора. + * + * @param body удаляемое тело + */ + public function removeBody(body:Body):Boolean { + if (body == null) { + return false; + } + + if (body == bodies) { + if (lastBody == bodies) { + lastBody = null; + } + bodies = bodies.next; + return true; + } + + var b:Body = bodies; + var prev:Body; + while (b != null && b != body) { + prev = b; + b = b.next; + } + if (b == null) { + return false; + } else { + if (lastBody == b) { + lastBody = prev; + } + prev.next = b.next; + return true; + } + } + + /** + * + * @param gen + * + */ + public function addContactGenerator(gen:ContactGenerator):void { + if (contactGenerators == null) { + contactGenerators = gen; + } else { + gen.next = contactGenerators; + contactGenerators = gen; + } + } + + /** + * Выполняет шаг симуляции. + * + * @param dt длительность шага симуляции + */ + public function runPhysics(dt:Number):void { + // Применяем все силы, действующие на тела + forceRegistry.updateForces(dt); + // Сохраняем состояние тела и считаем ускорения + var body:Body = bodies; + while (body != null) { + body.prevState.copy(body.state); + body.calcAccelerations(); + body = body.next; + } + // Создаём контакты. Контакты получаются из детектора столкновений и ограничений. + generateContacts(dt); + // Разрешение контактов. Изменяются скорости тел. + processContacts(dt, false); + // Интегрирование скоростей тел с откорректированными начальными условиями + integrateVelocity(dt); + // Разрешение оставшихся актуальных контактов, используя абсолютно неупругое взаимодействие + processContacts(dt, true); + // Shock propagation + shockPropagation(); + // Интегрирование положения тел, используя ранее полученные скорости + integratePosition(dt); + + timeStamp++; + } + + /** + * + * @param dt + */ + private function integrateVelocity(dt:Number):void { + var curr:Body = bodies; + while (curr != null) { + curr.integrateVelocity(dt); + curr = curr.next; + } + } + + /** + * + * @param dt + */ + private function integratePosition(dt:Number):void { + var curr:Body = bodies; + while (curr != null) { + curr.integratePosition(dt); + curr = curr.next; + } + } + + /** + * + */ + private function generateContacts(dt:Number):void { + var body:Body; + if (predictCollisions) { + // Для всех тел сохраняем текущее состояние и интегрируем его + body = bodies; + while (body != null) { + body.integrateFull(dt); + body.calcDerivedData(); + body = body.next; + } + } + + var gen:ContactGenerator = contactGenerators; + var contact:Contact = contacts; + while (gen != null && contact != null) { + contact = gen.addContacts(contact); + gen = gen.next; + } + numContacts = contact == null ? maxContacts : contact.index; + + if (predictCollisions) { + // Восстанавливаем сохранённое состояние + body = bodies; + while (body != null) { + body.state.copy(body.prevState); + body = body.next; + } + } + } + + /** + * + * @param inelastic + */ + private function processContacts(dt:Number, forceInelastic:Boolean):void { + if (numContacts == 0) { + return; + } + + // Вычисление дополнительных скоростей разделения в точках контактов + var contact:Contact = contacts; + while (contact != null && contact.index < numContacts) { + var time:Number = penResolutionSteps*dt; + if (contact.penetration > maxPenetration) { + var minSepVel:Number = (contact.penetration - maxPenetration)/time; + contact.minSepVelocity = minSepVel < maxSepVelocity ? minSepVel : maxSepVelocity; + } else { + contact.minSepVelocity = 0; + } + contact = contact.next; + } + + // Итерационное применение импульсов к контактам + var iterations:int = conllisionIterations*numContacts; + trace("timeStamp", timeStamp); + for (var iter:int = 0; iter < iterations; iter++) { + contact = contacts; + var max:Number = 0; + var bestContact:Contact = null; + while (contact != null && contact.index < numContacts) { + if (contact.getSepVelocity() <= contact.minSepVelocity) { + if (contact.getSepVelocity() < max) { + max = contact.getSepVelocity(); + bestContact = contact; + } + } + contact = contact.next; + } + if (bestContact == null) { + return; + } +// trace(timeStamp, "bestContact", bestContact.index); + trace(bestContact.index, "before", bestContact.getSepVelocity(), max); + bestContact.resolve(forceInelastic, dt); + trace(bestContact.index, "after", bestContact.getSepVelocity(), bestContact.minSepVelocity); + } + contact = contacts; + while (contact != null && contact.index < numContacts) { + trace(contact.getSepVelocity()); + contact = contact.next; + } + } + + /** + * + */ + private function shockPropagation():void { + + } + + } +} \ No newline at end of file diff --git a/0.1.New/src/alternativa/physics/rigid/generators/.svn/all-wcprops b/0.1.New/src/alternativa/physics/rigid/generators/.svn/all-wcprops new file mode 100644 index 0000000..209c472 --- /dev/null +++ b/0.1.New/src/alternativa/physics/rigid/generators/.svn/all-wcprops @@ -0,0 +1,17 @@ +K 25 +svn:wc:ra_dav:version-url +V 122 +/!svn/ver/9697/platform/clients/fp9/libraries/AlternativaPhysics/branches/0.1.New/src/alternativa/physics/rigid/generators +END +SphereWithPlaneContactGenerator.as +K 25 +svn:wc:ra_dav:version-url +V 157 +/!svn/ver/9697/platform/clients/fp9/libraries/AlternativaPhysics/branches/0.1.New/src/alternativa/physics/rigid/generators/SphereWithPlaneContactGenerator.as +END +BoxWidthPlaneContactGenerator.as +K 25 +svn:wc:ra_dav:version-url +V 155 +/!svn/ver/9562/platform/clients/fp9/libraries/AlternativaPhysics/branches/0.1.New/src/alternativa/physics/rigid/generators/BoxWidthPlaneContactGenerator.as +END diff --git a/0.1.New/src/alternativa/physics/rigid/generators/.svn/entries b/0.1.New/src/alternativa/physics/rigid/generators/.svn/entries new file mode 100644 index 0000000..783d236 --- /dev/null +++ b/0.1.New/src/alternativa/physics/rigid/generators/.svn/entries @@ -0,0 +1,52 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp9/libraries/AlternativaPhysics/branches/0.1.New/src/alternativa/physics/rigid/generators +http://svndev.alternativaplatform.com + + + +2009-03-23T09:25:39.308058Z +9697 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +SphereWithPlaneContactGenerator.as +file + + + + +2010-10-28T04:34:31.000000Z +7e5977753b895937d3a1daac53468898 +2009-03-23T09:25:39.308058Z +9697 +mike + +BoxWidthPlaneContactGenerator.as +file + + + + +2010-10-28T04:34:31.000000Z +e984454178b466380fc35aaf5067ddf3 +2009-03-18T12:14:56.932351Z +9562 +mike + diff --git a/0.1.New/src/alternativa/physics/rigid/generators/.svn/format b/0.1.New/src/alternativa/physics/rigid/generators/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.1.New/src/alternativa/physics/rigid/generators/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.1.New/src/alternativa/physics/rigid/generators/.svn/text-base/BoxWidthPlaneContactGenerator.as.svn-base b/0.1.New/src/alternativa/physics/rigid/generators/.svn/text-base/BoxWidthPlaneContactGenerator.as.svn-base new file mode 100644 index 0000000..1de29c6 --- /dev/null +++ b/0.1.New/src/alternativa/physics/rigid/generators/.svn/text-base/BoxWidthPlaneContactGenerator.as.svn-base @@ -0,0 +1,70 @@ +package alternativa.physics.rigid.generators { + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactGenerator; + import alternativa.physics.rigid.primitives.RigidBox; + import alternativa.types.Point3D; + + public class BoxWidthPlaneContactGenerator extends ContactGenerator { + + private var box:RigidBox; + private var normal:Point3D; + private var offset:Number; + + private var coeffs:Array = [-1, 1]; + + public function BoxWidthPlaneContactGenerator(box:RigidBox, normal:Point3D, offset:Number) { + super(); + this.box = box; + this.normal = normal.clone(); + this.offset = offset; + } + + private var xAxis:Point3D = new Point3D(); + private var yAxis:Point3D = new Point3D(); + private var zAxis:Point3D = new Point3D(); + + private var vertex:Point3D = new Point3D(); + + override public function addContacts(contact:Contact):Contact { + box.baseMatrix.getAxis(0, xAxis); + box.baseMatrix.getAxis(1, yAxis); + box.baseMatrix.getAxis(2, zAxis); + + var kx:Number; + var ky:Number; + var kz:Number; + + for (var xIdx:int = 0; xIdx < 2; xIdx++) { + kx = coeffs[xIdx]; + for (var yIdx:int = 0; yIdx < 2; yIdx++) { + ky = coeffs[yIdx]; + for (var zIdx:int = 0; zIdx < 2; zIdx++) { + kz = coeffs[zIdx]; + vertex.x = xAxis.x*kx*box.halfSize.x + yAxis.x*ky*box.halfSize.y + zAxis.x*kz*box.halfSize.z + box.state.pos.x; + vertex.y = xAxis.y*kx*box.halfSize.x + yAxis.y*ky*box.halfSize.y + zAxis.y*kz*box.halfSize.z + box.state.pos.y; + vertex.z = xAxis.z*kx*box.halfSize.x + yAxis.z*ky*box.halfSize.y + zAxis.z*kz*box.halfSize.z + box.state.pos.z; + contact = addContactForVertex(vertex, contact); + if (contact == null) { + return null; + } + } + } + } + return contact; + } + + private function addContactForVertex(v:Point3D, contact:Contact):Contact { + var d:Number = v.dot(normal) - offset; + if (d < 0) { + contact.body1 = box; + contact.body2 = null; + contact.penetration = -d; + contact.pos.copy(v); + contact.normal.copy(normal); + return contact.next; + } + return contact; + } + + } +} \ No newline at end of file diff --git a/0.1.New/src/alternativa/physics/rigid/generators/.svn/text-base/SphereWithPlaneContactGenerator.as.svn-base b/0.1.New/src/alternativa/physics/rigid/generators/.svn/text-base/SphereWithPlaneContactGenerator.as.svn-base new file mode 100644 index 0000000..ca0b324 --- /dev/null +++ b/0.1.New/src/alternativa/physics/rigid/generators/.svn/text-base/SphereWithPlaneContactGenerator.as.svn-base @@ -0,0 +1,36 @@ +package alternativa.physics.rigid.generators { + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactGenerator; + import alternativa.physics.rigid.primitives.RigidSphere; + import alternativa.types.Point3D; + + public class SphereWithPlaneContactGenerator extends ContactGenerator { + + private var sphere:RigidSphere; + private var normal:Point3D; + private var offset:Number; + + public function SphereWithPlaneContactGenerator(sphere:RigidSphere, normal:Point3D, offset:Number) { + super(); + this.sphere = sphere; + this.normal = normal.clone(); + this.offset = offset; + } + + override public function addContacts(contact:Contact):Contact { + var d:Number = sphere.state.pos.dot(normal) - offset - sphere.r; + if (d < 0) { + contact.body1 = sphere; + contact.body2 = null; + contact.penetration = -d; + contact.normal.copy(normal); + contact.pos.copy(normal); + contact.pos.multiply(-sphere.r); + contact.pos.add(sphere.state.pos); + return contact.next; + } + return contact; + } + + } +} \ No newline at end of file diff --git a/0.1.New/src/alternativa/physics/rigid/generators/BoxWidthPlaneContactGenerator.as b/0.1.New/src/alternativa/physics/rigid/generators/BoxWidthPlaneContactGenerator.as new file mode 100644 index 0000000..1de29c6 --- /dev/null +++ b/0.1.New/src/alternativa/physics/rigid/generators/BoxWidthPlaneContactGenerator.as @@ -0,0 +1,70 @@ +package alternativa.physics.rigid.generators { + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactGenerator; + import alternativa.physics.rigid.primitives.RigidBox; + import alternativa.types.Point3D; + + public class BoxWidthPlaneContactGenerator extends ContactGenerator { + + private var box:RigidBox; + private var normal:Point3D; + private var offset:Number; + + private var coeffs:Array = [-1, 1]; + + public function BoxWidthPlaneContactGenerator(box:RigidBox, normal:Point3D, offset:Number) { + super(); + this.box = box; + this.normal = normal.clone(); + this.offset = offset; + } + + private var xAxis:Point3D = new Point3D(); + private var yAxis:Point3D = new Point3D(); + private var zAxis:Point3D = new Point3D(); + + private var vertex:Point3D = new Point3D(); + + override public function addContacts(contact:Contact):Contact { + box.baseMatrix.getAxis(0, xAxis); + box.baseMatrix.getAxis(1, yAxis); + box.baseMatrix.getAxis(2, zAxis); + + var kx:Number; + var ky:Number; + var kz:Number; + + for (var xIdx:int = 0; xIdx < 2; xIdx++) { + kx = coeffs[xIdx]; + for (var yIdx:int = 0; yIdx < 2; yIdx++) { + ky = coeffs[yIdx]; + for (var zIdx:int = 0; zIdx < 2; zIdx++) { + kz = coeffs[zIdx]; + vertex.x = xAxis.x*kx*box.halfSize.x + yAxis.x*ky*box.halfSize.y + zAxis.x*kz*box.halfSize.z + box.state.pos.x; + vertex.y = xAxis.y*kx*box.halfSize.x + yAxis.y*ky*box.halfSize.y + zAxis.y*kz*box.halfSize.z + box.state.pos.y; + vertex.z = xAxis.z*kx*box.halfSize.x + yAxis.z*ky*box.halfSize.y + zAxis.z*kz*box.halfSize.z + box.state.pos.z; + contact = addContactForVertex(vertex, contact); + if (contact == null) { + return null; + } + } + } + } + return contact; + } + + private function addContactForVertex(v:Point3D, contact:Contact):Contact { + var d:Number = v.dot(normal) - offset; + if (d < 0) { + contact.body1 = box; + contact.body2 = null; + contact.penetration = -d; + contact.pos.copy(v); + contact.normal.copy(normal); + return contact.next; + } + return contact; + } + + } +} \ No newline at end of file diff --git a/0.1.New/src/alternativa/physics/rigid/generators/SphereWithPlaneContactGenerator.as b/0.1.New/src/alternativa/physics/rigid/generators/SphereWithPlaneContactGenerator.as new file mode 100644 index 0000000..ca0b324 --- /dev/null +++ b/0.1.New/src/alternativa/physics/rigid/generators/SphereWithPlaneContactGenerator.as @@ -0,0 +1,36 @@ +package alternativa.physics.rigid.generators { + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactGenerator; + import alternativa.physics.rigid.primitives.RigidSphere; + import alternativa.types.Point3D; + + public class SphereWithPlaneContactGenerator extends ContactGenerator { + + private var sphere:RigidSphere; + private var normal:Point3D; + private var offset:Number; + + public function SphereWithPlaneContactGenerator(sphere:RigidSphere, normal:Point3D, offset:Number) { + super(); + this.sphere = sphere; + this.normal = normal.clone(); + this.offset = offset; + } + + override public function addContacts(contact:Contact):Contact { + var d:Number = sphere.state.pos.dot(normal) - offset - sphere.r; + if (d < 0) { + contact.body1 = sphere; + contact.body2 = null; + contact.penetration = -d; + contact.normal.copy(normal); + contact.pos.copy(normal); + contact.pos.multiply(-sphere.r); + contact.pos.add(sphere.state.pos); + return contact.next; + } + return contact; + } + + } +} \ No newline at end of file diff --git a/0.1.New/src/alternativa/physics/rigid/primitives/.svn/all-wcprops b/0.1.New/src/alternativa/physics/rigid/primitives/.svn/all-wcprops new file mode 100644 index 0000000..48d7514 --- /dev/null +++ b/0.1.New/src/alternativa/physics/rigid/primitives/.svn/all-wcprops @@ -0,0 +1,17 @@ +K 25 +svn:wc:ra_dav:version-url +V 122 +/!svn/ver/9697/platform/clients/fp9/libraries/AlternativaPhysics/branches/0.1.New/src/alternativa/physics/rigid/primitives +END +RigidSphere.as +K 25 +svn:wc:ra_dav:version-url +V 137 +/!svn/ver/9697/platform/clients/fp9/libraries/AlternativaPhysics/branches/0.1.New/src/alternativa/physics/rigid/primitives/RigidSphere.as +END +RigidBox.as +K 25 +svn:wc:ra_dav:version-url +V 134 +/!svn/ver/9562/platform/clients/fp9/libraries/AlternativaPhysics/branches/0.1.New/src/alternativa/physics/rigid/primitives/RigidBox.as +END diff --git a/0.1.New/src/alternativa/physics/rigid/primitives/.svn/entries b/0.1.New/src/alternativa/physics/rigid/primitives/.svn/entries new file mode 100644 index 0000000..e806a75 --- /dev/null +++ b/0.1.New/src/alternativa/physics/rigid/primitives/.svn/entries @@ -0,0 +1,52 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp9/libraries/AlternativaPhysics/branches/0.1.New/src/alternativa/physics/rigid/primitives +http://svndev.alternativaplatform.com + + + +2009-03-23T09:25:39.308058Z +9697 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +RigidSphere.as +file + + + + +2010-10-28T04:34:31.000000Z +2c115ec275128d231b06cd47070ff29c +2009-03-23T09:25:39.308058Z +9697 +mike + +RigidBox.as +file + + + + +2010-10-28T04:34:31.000000Z +bf65fdafb7769491d3342558175276fe +2009-03-18T12:14:56.932351Z +9562 +mike + diff --git a/0.1.New/src/alternativa/physics/rigid/primitives/.svn/format b/0.1.New/src/alternativa/physics/rigid/primitives/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.1.New/src/alternativa/physics/rigid/primitives/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.1.New/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidBox.as.svn-base b/0.1.New/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidBox.as.svn-base new file mode 100644 index 0000000..287be4c --- /dev/null +++ b/0.1.New/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidBox.as.svn-base @@ -0,0 +1,25 @@ +package alternativa.physics.rigid.primitives { + import alternativa.physics.rigid.Body; + import alternativa.types.Matrix3D; + import alternativa.types.Point3D; + + public class RigidBox extends Body { + + public var halfSize:Point3D = new Point3D(); + + public function RigidBox(halfSize:Point3D, invMass:Number) { + super(); + this.halfSize.copy(halfSize); + + var m:Matrix3D = new Matrix3D(); + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + m.a = 12*invMass/(yy + zz); + m.f = 12*invMass/(zz + xx); + m.k = 12*invMass/(xx + yy); + setInvParams(invMass, m); + } + + } +} \ No newline at end of file diff --git a/0.1.New/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidSphere.as.svn-base b/0.1.New/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidSphere.as.svn-base new file mode 100644 index 0000000..0fa408e --- /dev/null +++ b/0.1.New/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidSphere.as.svn-base @@ -0,0 +1,19 @@ +package alternativa.physics.rigid.primitives { + import alternativa.physics.rigid.Body; + import alternativa.types.Matrix3D; + + public class RigidSphere extends Body { + + public var r:Number; + + public function RigidSphere(radius:Number, invMass:Number) { + super(); + r = radius; + var m:Matrix3D = new Matrix3D(); + var k:Number = 2.5*invMass/(r*r); + m.a = m.f = m.k = k; + setInvParams(invMass, m); + } + + } +} \ No newline at end of file diff --git a/0.1.New/src/alternativa/physics/rigid/primitives/RigidBox.as b/0.1.New/src/alternativa/physics/rigid/primitives/RigidBox.as new file mode 100644 index 0000000..287be4c --- /dev/null +++ b/0.1.New/src/alternativa/physics/rigid/primitives/RigidBox.as @@ -0,0 +1,25 @@ +package alternativa.physics.rigid.primitives { + import alternativa.physics.rigid.Body; + import alternativa.types.Matrix3D; + import alternativa.types.Point3D; + + public class RigidBox extends Body { + + public var halfSize:Point3D = new Point3D(); + + public function RigidBox(halfSize:Point3D, invMass:Number) { + super(); + this.halfSize.copy(halfSize); + + var m:Matrix3D = new Matrix3D(); + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + m.a = 12*invMass/(yy + zz); + m.f = 12*invMass/(zz + xx); + m.k = 12*invMass/(xx + yy); + setInvParams(invMass, m); + } + + } +} \ No newline at end of file diff --git a/0.1.New/src/alternativa/physics/rigid/primitives/RigidSphere.as b/0.1.New/src/alternativa/physics/rigid/primitives/RigidSphere.as new file mode 100644 index 0000000..0fa408e --- /dev/null +++ b/0.1.New/src/alternativa/physics/rigid/primitives/RigidSphere.as @@ -0,0 +1,19 @@ +package alternativa.physics.rigid.primitives { + import alternativa.physics.rigid.Body; + import alternativa.types.Matrix3D; + + public class RigidSphere extends Body { + + public var r:Number; + + public function RigidSphere(radius:Number, invMass:Number) { + super(); + r = radius; + var m:Matrix3D = new Matrix3D(); + var k:Number = 2.5*invMass/(r*r); + m.a = m.f = m.k = k; + setInvParams(invMass, m); + } + + } +} \ No newline at end of file diff --git a/0.1.SingleStep/.actionScriptProperties b/0.1.SingleStep/.actionScriptProperties new file mode 100644 index 0000000..f73adc8 --- /dev/null +++ b/0.1.SingleStep/.actionScriptProperties @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/0.1.SingleStep/.flexLibProperties b/0.1.SingleStep/.flexLibProperties new file mode 100644 index 0000000..708a638 --- /dev/null +++ b/0.1.SingleStep/.flexLibProperties @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/0.1.SingleStep/.project b/0.1.SingleStep/.project new file mode 100644 index 0000000..9c0c3d6 --- /dev/null +++ b/0.1.SingleStep/.project @@ -0,0 +1,18 @@ + + + AlternativaPhysics + + + + + + com.adobe.flexbuilder.project.flexbuilder + + + + + + com.adobe.flexbuilder.project.flexlibnature + com.adobe.flexbuilder.project.actionscriptnature + + diff --git a/0.1.SingleStep/.settings/.svn/all-wcprops b/0.1.SingleStep/.settings/.svn/all-wcprops new file mode 100644 index 0000000..f771948 --- /dev/null +++ b/0.1.SingleStep/.settings/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 100 +/!svn/ver/10588/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.SingleStep/.settings +END +org.eclipse.core.resources.prefs +K 25 +svn:wc:ra_dav:version-url +V 133 +/!svn/ver/10588/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.SingleStep/.settings/org.eclipse.core.resources.prefs +END diff --git a/0.1.SingleStep/.settings/.svn/entries b/0.1.SingleStep/.settings/.svn/entries new file mode 100644 index 0000000..e88b658 --- /dev/null +++ b/0.1.SingleStep/.settings/.svn/entries @@ -0,0 +1,40 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.SingleStep/.settings +http://svndev.alternativaplatform.com + + + +2009-04-01T12:25:29.638016Z +10301 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +org.eclipse.core.resources.prefs +file + + + + +2010-10-28T04:32:39.000000Z +4f70b476f3e5075e399505021df2f89b +2009-04-01T12:25:29.638016Z +10301 +mike + diff --git a/0.1.SingleStep/.settings/.svn/format b/0.1.SingleStep/.settings/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.1.SingleStep/.settings/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.1.SingleStep/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base b/0.1.SingleStep/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base new file mode 100644 index 0000000..b3d0d49 --- /dev/null +++ b/0.1.SingleStep/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base @@ -0,0 +1,3 @@ +#Fri Mar 20 00:19:33 YEKT 2009 +eclipse.preferences.version=1 +encoding/=UTF-8 diff --git a/0.1.SingleStep/.settings/org.eclipse.core.resources.prefs b/0.1.SingleStep/.settings/org.eclipse.core.resources.prefs new file mode 100644 index 0000000..b3d0d49 --- /dev/null +++ b/0.1.SingleStep/.settings/org.eclipse.core.resources.prefs @@ -0,0 +1,3 @@ +#Fri Mar 20 00:19:33 YEKT 2009 +eclipse.preferences.version=1 +encoding/=UTF-8 diff --git a/0.1.SingleStep/.svn/all-wcprops b/0.1.SingleStep/.svn/all-wcprops new file mode 100644 index 0000000..7735cd1 --- /dev/null +++ b/0.1.SingleStep/.svn/all-wcprops @@ -0,0 +1,23 @@ +K 25 +svn:wc:ra_dav:version-url +V 90 +/!svn/ver/10598/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.SingleStep +END +.flexLibProperties +K 25 +svn:wc:ra_dav:version-url +V 109 +/!svn/ver/10588/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.SingleStep/.flexLibProperties +END +.project +K 25 +svn:wc:ra_dav:version-url +V 99 +/!svn/ver/10588/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.SingleStep/.project +END +.actionScriptProperties +K 25 +svn:wc:ra_dav:version-url +V 114 +/!svn/ver/10588/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.SingleStep/.actionScriptProperties +END diff --git a/0.1.SingleStep/.svn/entries b/0.1.SingleStep/.svn/entries new file mode 100644 index 0000000..4603bad --- /dev/null +++ b/0.1.SingleStep/.svn/entries @@ -0,0 +1,70 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.SingleStep +http://svndev.alternativaplatform.com + + + +2009-04-07T09:35:01.456741Z +10598 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +.flexLibProperties +file + + + + +2010-10-28T04:32:39.000000Z +78f8d8688f24288c4e8e3122231df756 +2009-04-07T08:34:06.342994Z +10587 +mike + +.project +file + + + + +2010-10-28T04:32:39.000000Z +c456c99a28bbc231cb9b01e28168d072 +2009-04-01T12:25:29.638016Z +10301 +mike + +src +dir + +.actionScriptProperties +file + + + + +2010-10-28T04:32:39.000000Z +b8656f882fff2baedd57a92db86fcf8c +2009-04-01T12:25:29.638016Z +10301 +mike + +.settings +dir + diff --git a/0.1.SingleStep/.svn/format b/0.1.SingleStep/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.1.SingleStep/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.1.SingleStep/.svn/text-base/.actionScriptProperties.svn-base b/0.1.SingleStep/.svn/text-base/.actionScriptProperties.svn-base new file mode 100644 index 0000000..f73adc8 --- /dev/null +++ b/0.1.SingleStep/.svn/text-base/.actionScriptProperties.svn-base @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/0.1.SingleStep/.svn/text-base/.flexLibProperties.svn-base b/0.1.SingleStep/.svn/text-base/.flexLibProperties.svn-base new file mode 100644 index 0000000..708a638 --- /dev/null +++ b/0.1.SingleStep/.svn/text-base/.flexLibProperties.svn-base @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/0.1.SingleStep/.svn/text-base/.project.svn-base b/0.1.SingleStep/.svn/text-base/.project.svn-base new file mode 100644 index 0000000..9c0c3d6 --- /dev/null +++ b/0.1.SingleStep/.svn/text-base/.project.svn-base @@ -0,0 +1,18 @@ + + + AlternativaPhysics + + + + + + com.adobe.flexbuilder.project.flexbuilder + + + + + + com.adobe.flexbuilder.project.flexlibnature + com.adobe.flexbuilder.project.actionscriptnature + + diff --git a/0.1.SingleStep/src/.svn/all-wcprops b/0.1.SingleStep/src/.svn/all-wcprops new file mode 100644 index 0000000..a5c07fe --- /dev/null +++ b/0.1.SingleStep/src/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 94 +/!svn/ver/10598/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.SingleStep/src +END diff --git a/0.1.SingleStep/src/.svn/entries b/0.1.SingleStep/src/.svn/entries new file mode 100644 index 0000000..5ac82be --- /dev/null +++ b/0.1.SingleStep/src/.svn/entries @@ -0,0 +1,31 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.SingleStep/src +http://svndev.alternativaplatform.com + + + +2009-04-07T09:35:01.456741Z +10598 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +alternativa +dir + diff --git a/0.1.SingleStep/src/.svn/format b/0.1.SingleStep/src/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.1.SingleStep/src/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.1.SingleStep/src/alternativa/.svn/all-wcprops b/0.1.SingleStep/src/alternativa/.svn/all-wcprops new file mode 100644 index 0000000..c78bbbe --- /dev/null +++ b/0.1.SingleStep/src/alternativa/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 106 +/!svn/ver/10598/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.SingleStep/src/alternativa +END diff --git a/0.1.SingleStep/src/alternativa/.svn/entries b/0.1.SingleStep/src/alternativa/.svn/entries new file mode 100644 index 0000000..08ac37e --- /dev/null +++ b/0.1.SingleStep/src/alternativa/.svn/entries @@ -0,0 +1,31 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.SingleStep/src/alternativa +http://svndev.alternativaplatform.com + + + +2009-04-07T09:35:01.456741Z +10598 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +physics +dir + diff --git a/0.1.SingleStep/src/alternativa/.svn/format b/0.1.SingleStep/src/alternativa/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.1.SingleStep/src/alternativa/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.1.SingleStep/src/alternativa/physics/.svn/all-wcprops b/0.1.SingleStep/src/alternativa/physics/.svn/all-wcprops new file mode 100644 index 0000000..973311d --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 114 +/!svn/ver/10598/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.SingleStep/src/alternativa/physics +END diff --git a/0.1.SingleStep/src/alternativa/physics/.svn/entries b/0.1.SingleStep/src/alternativa/physics/.svn/entries new file mode 100644 index 0000000..553f5e8 --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/.svn/entries @@ -0,0 +1,37 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.SingleStep/src/alternativa/physics +http://svndev.alternativaplatform.com + + + +2009-04-07T09:35:01.456741Z +10598 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +types +dir + +collision +dir + +rigid +dir + diff --git a/0.1.SingleStep/src/alternativa/physics/.svn/format b/0.1.SingleStep/src/alternativa/physics/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.1.SingleStep/src/alternativa/physics/collision/.svn/all-wcprops b/0.1.SingleStep/src/alternativa/physics/collision/.svn/all-wcprops new file mode 100644 index 0000000..ca80117 --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/collision/.svn/all-wcprops @@ -0,0 +1,59 @@ +K 25 +svn:wc:ra_dav:version-url +V 124 +/!svn/ver/10588/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.SingleStep/src/alternativa/physics/collision +END +BoxSphereCollider.as +K 25 +svn:wc:ra_dav:version-url +V 145 +/!svn/ver/10588/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.SingleStep/src/alternativa/physics/collision/BoxSphereCollider.as +END +BoxBoxCollider.as +K 25 +svn:wc:ra_dav:version-url +V 142 +/!svn/ver/10588/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.SingleStep/src/alternativa/physics/collision/BoxBoxCollider.as +END +SpherePlaneCollider.as +K 25 +svn:wc:ra_dav:version-url +V 147 +/!svn/ver/10588/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.SingleStep/src/alternativa/physics/collision/SpherePlaneCollider.as +END +CollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 145 +/!svn/ver/10588/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.SingleStep/src/alternativa/physics/collision/CollisionDetector.as +END +CollisionPoint.as +K 25 +svn:wc:ra_dav:version-url +V 142 +/!svn/ver/10588/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.SingleStep/src/alternativa/physics/collision/CollisionPoint.as +END +BoxPlaneCollider.as +K 25 +svn:wc:ra_dav:version-url +V 144 +/!svn/ver/10588/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.SingleStep/src/alternativa/physics/collision/BoxPlaneCollider.as +END +ICollider.as +K 25 +svn:wc:ra_dav:version-url +V 137 +/!svn/ver/10588/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.SingleStep/src/alternativa/physics/collision/ICollider.as +END +CollisionInfo.as +K 25 +svn:wc:ra_dav:version-url +V 141 +/!svn/ver/10588/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.SingleStep/src/alternativa/physics/collision/CollisionInfo.as +END +SphereSphereCollider.as +K 25 +svn:wc:ra_dav:version-url +V 148 +/!svn/ver/10588/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.SingleStep/src/alternativa/physics/collision/SphereSphereCollider.as +END diff --git a/0.1.SingleStep/src/alternativa/physics/collision/.svn/entries b/0.1.SingleStep/src/alternativa/physics/collision/.svn/entries new file mode 100644 index 0000000..7b1c938 --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/collision/.svn/entries @@ -0,0 +1,136 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.SingleStep/src/alternativa/physics/collision +http://svndev.alternativaplatform.com + + + +2009-04-07T08:34:06.342994Z +10587 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +BoxSphereCollider.as +file + + + + +2010-10-28T04:32:39.000000Z +eee045dc3e58ad515d4b46a36d9dcc7c +2009-04-01T12:25:29.638016Z +10301 +mike + +BoxBoxCollider.as +file + + + + +2010-10-28T04:32:39.000000Z +bff3779d82b8cb11443685bafb7e725d +2009-04-07T08:34:06.342994Z +10587 +mike + +SpherePlaneCollider.as +file + + + + +2010-10-28T04:32:39.000000Z +a37d06dbb7feedfb467dce3bb80295aa +2009-04-07T08:34:06.342994Z +10587 +mike + +CollisionDetector.as +file + + + + +2010-10-28T04:32:39.000000Z +1ff741f830daa2bb3591a0b57c67b77a +2009-04-07T08:34:06.342994Z +10587 +mike + +CollisionPoint.as +file + + + + +2010-10-28T04:32:39.000000Z +138377f8af17805ecd5f417d8f076922 +2009-04-07T08:34:06.342994Z +10587 +mike + +BoxPlaneCollider.as +file + + + + +2010-10-28T04:32:39.000000Z +f6e15576593e8f77dbe43a9307d594ad +2009-04-07T08:34:06.342994Z +10587 +mike + +ICollider.as +file + + + + +2010-10-28T04:32:39.000000Z +f396118b41d8a9400a9eee83814384cd +2009-04-01T12:25:29.638016Z +10301 +mike + +CollisionInfo.as +file + + + + +2010-10-28T04:32:39.000000Z +585b169da636d8cdb0c064ec58d4b5f9 +2009-04-07T08:34:06.342994Z +10587 +mike + +SphereSphereCollider.as +file + + + + +2010-10-28T04:32:39.000000Z +4b9803a0fc860e40ea65c2f84157f4bb +2009-04-07T08:34:06.342994Z +10587 +mike + diff --git a/0.1.SingleStep/src/alternativa/physics/collision/.svn/format b/0.1.SingleStep/src/alternativa/physics/collision/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/collision/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.1.SingleStep/src/alternativa/physics/collision/.svn/text-base/BoxBoxCollider.as.svn-base b/0.1.SingleStep/src/alternativa/physics/collision/.svn/text-base/BoxBoxCollider.as.svn-base new file mode 100644 index 0000000..093b6f3 --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/collision/.svn/text-base/BoxBoxCollider.as.svn-base @@ -0,0 +1,721 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.primitives.RigidBox; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + + /** + * + */ + public class BoxBoxCollider implements ICollider { + + private var tolerance:Number = 0.001; + + private var vectorToBox1:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var colAxis:Vector3 = new Vector3(); + private var tmpAxis:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxBoxCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + /** + * + * @param body1 + * @param body2 + * @param colInfo + * @return + */ + public function collide(body1:Body, body2:Body, colInfo:CollisionInfo):Boolean { + minOverlap = 1e10; + var box1:RigidBox; + var box2:RigidBox; + if (body1.id < body2.id) { + box1 = body1 as RigidBox; + box2 = body2 as RigidBox; + } else { + box1 = body2 as RigidBox; + box2 = body1 as RigidBox; + } + + // Вектор из центра второго бокса в центр первого + vectorToBox1.diff(box1.state.pos, box2.state.pos); + + // Проверка пересечения по основным осям + box1.transform.getAxis(0, axis10); + if (!testAxis(box1, box2, axis10, 0, vectorToBox1)) return false; + box1.transform.getAxis(1, axis11); + if (!testAxis(box1, box2, axis11, 1, vectorToBox1)) return false; + box1.transform.getAxis(2, axis12); + if (!testAxis(box1, box2, axis12, 2, vectorToBox1)) return false; + + box2.transform.getAxis(0, axis20); + if (!testAxis(box1, box2, axis20, 3, vectorToBox1)) return false; + box2.transform.getAxis(1, axis21); + if (!testAxis(box1, box2, axis21, 4, vectorToBox1)) return false; + box2.transform.getAxis(2, axis22); + if (!testAxis(box1, box2, axis22, 5, vectorToBox1)) return false; + + // Проверка производных осей + if (!testAxis(box1, box2, axis.cross2(axis10, axis20), 6, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.cross2(axis10, axis21), 7, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.cross2(axis10, axis22), 8, vectorToBox1)) return false; + + if (!testAxis(box1, box2, axis.cross2(axis11, axis20), 9, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.cross2(axis11, axis21), 10, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.cross2(axis11, axis22), 11, vectorToBox1)) return false; + + if (!testAxis(box1, box2, axis.cross2(axis12, axis20), 12, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.cross2(axis12, axis21), 13, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.cross2(axis12, axis22), 14, vectorToBox1)) return false; + + + if (bestAxisIndex < 6) { + // Контакт грань-(грань|ребро|вершина) + findFaceContactPoints(box1, box2, vectorToBox1, bestAxisIndex, colInfo); + } else { + // Контакт ребро-ребро + bestAxisIndex -= 6; + findEdgesIntersection(box1, box2, vectorToBox1, int(bestAxisIndex/3), bestAxisIndex%3, colInfo); + } + colInfo.body1 = box1; + colInfo.body2 = box2; + + return true; + } + + /** + * Выполняет поиск точек контакта грани одного бокса с гранью/ребром/вершиной другого. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param faceAxisIdx индекс оси первого бокса, перпендикулярной грани, с которой произошло столкновение + * @param colInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box1:RigidBox, box2:RigidBox, vectorToBox1:Vector3, faceAxisIdx:int, colInfo:CollisionInfo):void { + var swapNormal:Boolean = false; + if (faceAxisIdx > 2) { + // Столкновение с гранью второго бокса. Для дальнейших расчётов боксы меняются местами, + // но нормаль контакта всё равно должна быть направлена в сторону первоначального box1 + var tmpBox:RigidBox = box1; + box1 = box2; + box2 = tmpBox; + vectorToBox1.reverse(); + faceAxisIdx -= 3; + swapNormal = true; + } + box1.transform.getAxis(faceAxisIdx, colAxis); + var faceReversed:Boolean = colAxis.dot(vectorToBox1) > 0; + if (!faceReversed) colAxis.reverse(); + // Ищем ось второго бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + box2.transform.getAxis(axisIdx, axis); + var dot:Number = axis.dot(colAxis); + if (dot < 0) dot = -dot; + if (dot > maxDot) { + maxDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Получаем список вершин грани второго бокса, переводим их в систему координат первого бокса и выполняем обрезку + // по грани первого бокса. Таким образом получается список потенциальных точек контакта. + box2.transform.getAxis(incFaceAxisIdx, axis); + getFaceVertsByAxis(box2, incFaceAxisIdx, axis.dot(colAxis) < 0, verts1); + box2.transform.transformVectors(verts1, verts2); + box1.transform.transformVectorsInverse(verts2, verts1); + var pnum:int = clip(box1.halfSize, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + colInfo.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + if ((pen = getPointBoxPenetration(box1.halfSize, verts1[i], faceAxisIdx, faceReversed)) > -tolerance) { + var cp:CollisionPoint = colInfo.points[colInfo.pcount++]; + box1.transform.transformVector(verts1[i], cp.pos); + cp.penetration = pen; + } + } + colInfo.normal.copy(colAxis); + if (swapNormal) colInfo.normal.reverse(); + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + private function getFaceVertsByAxis(box:RigidBox, axisIdx:int, reverse:Boolean, result:Vector.):void { + var hs:Vector3 = box.halfSize; + switch (axisIdx) { + case 0: + if (reverse) { + (result[0] as Vector3).reset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).reset(-hs.x, -hs.y, -hs.z); + (result[2] as Vector3).reset(-hs.x, -hs.y, hs.z); + (result[3] as Vector3).reset(-hs.x, hs.y, hs.z); + } else { + (result[0] as Vector3).reset(hs.x, -hs.y, -hs.z); + (result[1] as Vector3).reset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).reset(hs.x, hs.y, hs.z); + (result[3] as Vector3).reset(hs.x, -hs.y, hs.z); + } + break; + case 1: + if (reverse) { + (result[0] as Vector3).reset(-hs.x, -hs.y, -hs.z); + (result[1] as Vector3).reset(hs.x, -hs.y, -hs.z); + (result[2] as Vector3).reset(hs.x, -hs.y, hs.z); + (result[3] as Vector3).reset(-hs.x, -hs.y, hs.z); + } else { + (result[0] as Vector3).reset(hs.x, hs.y, -hs.z); + (result[1] as Vector3).reset(-hs.x, hs.y, -hs.z); + (result[2] as Vector3).reset(-hs.x, hs.y, hs.z); + (result[3] as Vector3).reset(hs.x, hs.y, hs.z); + } + break; + case 2: + if (reverse) { + (result[0] as Vector3).reset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).reset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).reset(hs.x, -hs.y, -hs.z); + (result[3] as Vector3).reset(-hs.x, -hs.y, -hs.z); + } else { + (result[0] as Vector3).reset(-hs.x, -hs.y, hs.z); + (result[1] as Vector3).reset(hs.x, -hs.y, hs.z); + (result[2] as Vector3).reset(hs.x, hs.y, hs.z); + (result[3] as Vector3).reset(-hs.x, hs.y, hs.z); + } + break; + } + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clip(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + return clipHighX(hs.x, pnum, verts2, verts1); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + } + return 0; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + (result[num++] as Vector3).copy(p1); + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).reset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).reset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + (result[num++] as Vector3).copy(p1); + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).reset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).reset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + (result[num++] as Vector3).copy(p1); + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).reset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).reset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + (result[num++] as Vector3).copy(p1); + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).reset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).reset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + (result[num++] as Vector3).copy(p1); + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).reset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).reset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + (result[num++] as Vector3).copy(p1); + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).reset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).reset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param colInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box1:RigidBox, box2:RigidBox, vectorToBox1:Vector3, axisIdx1:int, axisIdx2:int, colInfo:CollisionInfo):void { + box1.transform.getAxis(axisIdx1, axis10); + box2.transform.getAxis(axisIdx2, axis20); + colAxis.cross2(axis10, axis20).normalize(); + // Разворот оси в сторону первого бокса + if (colAxis.dot(vectorToBox1) < 0) colAxis.reverse(); + + /* Кодирование рёбер + бит 0: 1 (тип контакта ребро-ребро) + биты 1-2: индекс направляющей оси ребра + бит 3: + бит 4: + бит 5: + */ + var edgeCode1:int = 1; + var edgeCode2:int = 1; + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + point1.copy(box1.halfSize); + point2.copy(box2.halfSize); + // x + if (axisIdx1 == 0) { + point1.x = 0; + halfLen1 = box1.halfSize.x; + } else { + box1.transform.getAxis(0, tmpAxis); + if (tmpAxis.dot(colAxis) > 0) { + point1.x = -point1.x; + edgeCode1 |= 8; // 1 << 3 + } + } + if (axisIdx2 == 0) { + point2.x = 0; + halfLen2 = box2.halfSize.x; + } else { + box2.transform.getAxis(0, tmpAxis); + if (tmpAxis.dot(colAxis) < 0) { + point2.x = -point2.x; + edgeCode2 |= 8; // 1 << 3 + } + } + // y + if (axisIdx1 == 1) { + point1.y = 0; + halfLen1 = box1.halfSize.y; + edgeCode1 |= 2; // 1 << 1 + } else { + box1.transform.getAxis(1, tmpAxis); + if (tmpAxis.dot(colAxis) > 0) { + point1.y = -point1.y; + edgeCode1 |= 16; // 1 << 4 + } + } + if (axisIdx2 == 1) { + point2.y = 0; + halfLen2 = box2.halfSize.y; + edgeCode2 |= 2; // 1 << 1 + } else { + box2.transform.getAxis(1, tmpAxis); + if (tmpAxis.dot(colAxis) < 0) { + point2.y = -point2.y; + edgeCode2 |= 16; // 1 << 4 + } + } + // z + if (axisIdx1 == 2) { + point1.z = 0; + halfLen1 = box1.halfSize.z; + edgeCode1 |= 4; // 2 << 1 + } else { + box1.transform.getAxis(2, tmpAxis); + if (tmpAxis.dot(colAxis) > 0) { + point1.z = -point1.z; + edgeCode1 |= 32; // 1 << 5 + } + } + if (axisIdx2 == 2) { + point2.z = 0; + halfLen2 = box2.halfSize.z; + edgeCode2 |= 4; // 2 << 1 + } else { + box2.transform.getAxis(2, tmpAxis); + if (tmpAxis.dot(colAxis) < 0) { + point2.z = -point2.z; + edgeCode2 |= 32; // 1 << 5 + } + } + // Получаем глобальные координаты средних точек рёбер + point1.transformBy4(box1.transform); + point2.transformBy4(box2.transform); + // Находим точку пересечения рёбер, решая систему уравнений + var k:Number = axis10.dot(axis20); + var det:Number = k*k - 1; + vector.diff(point2, point1); + var c1:Number = axis10.dot(vector); + var c2:Number = axis20.dot(vector); + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + colInfo.normal.copy(colAxis); + colInfo.pcount = 1; + var cp:CollisionPoint = colInfo.points[0]; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(point1.x + axis10.x*t1 + point2.x + axis20.x*t2); + cp.pos.y = 0.5*(point1.y + axis10.y*t1 + point2.y + axis20.y*t2); + cp.pos.z = 0.5*(point1.z + axis10.z*t1 + point2.z + axis20.z*t2); + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение боксов вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box1 + * @param box2 + * @param axis + * @param axisIndex + * @param vectorToBox1 + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testAxis(box1:RigidBox, box2:RigidBox, axis:Vector3, axisIndex:int, vectorToBox1:Vector3):Boolean { + if (axis.lengthSqr() < 0.0001) { + return true; + } + axis.normalize(); + + var overlap:Number = overlapOnAxis(box1, box2, axis, vectorToBox1); + if (overlap < -tolerance) return false; + if (overlap + tolerance < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия двух боксов вдоль заданной оси. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param axis ось + * @param vectorToBox1 вектор, соединяющий центр второго бокса с центром первого + * @return величина перекрытия боксов вдоль оси + */ + public function overlapOnAxis(box1:RigidBox, box2:RigidBox, axis:Vector3, vectorToBox1:Vector3):Number { + var m:Matrix4 = box1.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box1.halfSize.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box1.halfSize.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box1.halfSize.z; + if (d < 0) d = -d; + projection += d; + + m = box2.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box2.halfSize.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box2.halfSize.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box2.halfSize.z; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox1.x*axis.x + vectorToBox1.y*axis.y + vectorToBox1.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} + import alternativa.physics.types.Vector3; + + class CollisionPointTmp { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + } diff --git a/0.1.SingleStep/src/alternativa/physics/collision/.svn/text-base/BoxPlaneCollider.as.svn-base b/0.1.SingleStep/src/alternativa/physics/collision/.svn/text-base/BoxPlaneCollider.as.svn-base new file mode 100644 index 0000000..4b19470 --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/collision/.svn/text-base/BoxPlaneCollider.as.svn-base @@ -0,0 +1,77 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.primitives.RigidBox; + import alternativa.physics.rigid.primitives.RigidPlane; + import alternativa.physics.types.Vector3; + + public class BoxPlaneCollider implements ICollider { + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var normal:Vector3 = new Vector3(); + + public function BoxPlaneCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + public function collide(body1:Body, body2:Body, collisionInfo:CollisionInfo):Boolean { + var box:RigidBox = body1 as RigidBox; + var plane:RigidPlane; + if (box == null) { + box = body2 as RigidBox; + plane = body1 as RigidPlane; + } else { + plane = body2 as RigidPlane; + } + + // Вычисляем глобальные координаты вершин бокса + var sx:Number = box.halfSize.x; + var sy:Number = box.halfSize.y; + var sz:Number = box.halfSize.z; + (verts1[0] as Vector3).reset(-sx, -sy, -sz); + (verts1[1] as Vector3).reset(sx, -sy, -sz); + (verts1[2] as Vector3).reset(sx, sy, -sz); + (verts1[3] as Vector3).reset(-sx, sy, -sz); + (verts1[4] as Vector3).reset(-sx, -sy, sz); + (verts1[5] as Vector3).reset(sx, -sy, sz); + (verts1[6] as Vector3).reset(sx, sy, sz); + (verts1[7] as Vector3).reset(-sx, sy, sz); + + box.transform.transformVectors(verts1, verts2); + // Вычисляем глобальные нормаль и смещение плоскости + plane.baseMatrix.transformVector(plane.normal, normal); + var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; + // Проверяем наличие столкновений с каждой вершиной + collisionInfo.pcount = 0; + for (var i:int = 0; i < 8; i++) { + // Вершина добавляется в список точек столкновения, если лежит под плоскостью + var dist:Number = (verts2[i] as Vector3).dot(normal); + if (dist < offset) { + var cp:CollisionPoint; + if (collisionInfo.pcount == collisionInfo.points.length) { + cp = new CollisionPoint(); + collisionInfo.points[collisionInfo.pcount] = cp; + } else { + cp = collisionInfo.points[collisionInfo.pcount]; + } + cp.pos.copy(verts2[i]); + cp.penetration = offset - dist; + collisionInfo.pcount++; + } + } + if (collisionInfo.pcount > 0) { + collisionInfo.body1 = box; + collisionInfo.body2 = plane; + collisionInfo.normal.copy(normal); + return true; + } + return false; + } + + } +} \ No newline at end of file diff --git a/0.1.SingleStep/src/alternativa/physics/collision/.svn/text-base/BoxSphereCollider.as.svn-base b/0.1.SingleStep/src/alternativa/physics/collision/.svn/text-base/BoxSphereCollider.as.svn-base new file mode 100644 index 0000000..65d8e40 --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/collision/.svn/text-base/BoxSphereCollider.as.svn-base @@ -0,0 +1,94 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.primitives.RigidBox; + import alternativa.physics.rigid.primitives.RigidSphere; + import alternativa.physics.types.Vector3; + + /** + * + */ + public class BoxSphereCollider implements ICollider { + + private var center:Vector3 = new Vector3(); + private var closestPt:Vector3 = new Vector3(); + + /** + * + */ + public function BoxSphereCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function collide(body1:Body, body2:Body, collisionInfo:CollisionInfo):Boolean { + var box:RigidBox = body1 as RigidBox; + var sphere:RigidSphere; + if (box == null) { + box = body2 as RigidBox; + sphere = body1 as RigidSphere; + } else { + sphere = body2 as RigidSphere; + } + // Трансформируем центр сферы в систему коорлинат бокса + box.transform.transformVectorInverse(sphere.state.pos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.halfSize; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.subtract(closestPt).lengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + // Зафиксированно столкновение + collisionInfo.body1 = box; + collisionInfo.body2 = sphere; + collisionInfo.normal.copy(closestPt).transformBy4(box.transform).subtract(sphere.state.pos).normalize(); + collisionInfo.pcount = 1; + + var cp:CollisionPoint = collisionInfo.points[0]; + cp.penetration = sphere.r - Math.sqrt(distSqr); + cp.pos.copy(collisionInfo.normal).scale(sphere.r).add(sphere.state.pos); + + return true; + } + + } +} \ No newline at end of file diff --git a/0.1.SingleStep/src/alternativa/physics/collision/.svn/text-base/CollisionDetector.as.svn-base b/0.1.SingleStep/src/alternativa/physics/collision/.svn/text-base/CollisionDetector.as.svn-base new file mode 100644 index 0000000..31b24a9 --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/collision/.svn/text-base/CollisionDetector.as.svn-base @@ -0,0 +1,45 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Body; + + /** + * + */ + public class CollisionDetector implements ICollider { + + private var colliders:Object = {}; + + /** + * + */ + public function CollisionDetector() { + addCollider(Body.BOX, Body.BOX, new BoxBoxCollider()); + addCollider(Body.BOX, Body.PLANE, new BoxPlaneCollider()); + addCollider(Body.BOX, Body.SPHERE, new BoxSphereCollider()); + addCollider(Body.SPHERE, Body.PLANE, new SpherePlaneCollider()); + addCollider(Body.SPHERE, Body.SPHERE, new SphereSphereCollider()); + } + + /** + * + * @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 body1 + * @param body2 + * @param contactInfo + * @return + */ + public function collide(body1:Body, body2:Body, contactInfo:CollisionInfo):Boolean { + var collider:ICollider = colliders[body1.type <= body2.type ? (body1.type << 16) | body2.type : (body2.type << 16) | body1.type] as ICollider; + return collider == null ? false : collider.collide(body1, body2, contactInfo); + } + + } +} \ No newline at end of file diff --git a/0.1.SingleStep/src/alternativa/physics/collision/.svn/text-base/CollisionInfo.as.svn-base b/0.1.SingleStep/src/alternativa/physics/collision/.svn/text-base/CollisionInfo.as.svn-base new file mode 100644 index 0000000..7429d25 --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/collision/.svn/text-base/CollisionInfo.as.svn-base @@ -0,0 +1,44 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Vector3; + + /** + * + */ + public class CollisionInfo { + + private static var pool:Vector. = new Vector.(); + + public static function create():CollisionInfo { + if (pool.length > 0) { + return pool.pop(); + } + return new CollisionInfo(); + } + + public static function destroy(collInfo:CollisionInfo):void { + collInfo.body1 = collInfo.body2 = null; + pool.push(collInfo); + } + + private const N:int = 8; + + public var body1:Body; + public var body2:Body; + // Коэффициент отскока + public var restitution:Number; + // Коэффициент трения + public var friction:Number; + + public var normal:Vector3 = new Vector3(); + public var points:Vector. = new Vector.(N, true); + public var pcount:int; + + public function CollisionInfo() { + for (var i:int = 0; i < N; i++) points[i] = new CollisionPoint(); + } + + } +} \ No newline at end of file diff --git a/0.1.SingleStep/src/alternativa/physics/collision/.svn/text-base/CollisionPoint.as.svn-base b/0.1.SingleStep/src/alternativa/physics/collision/.svn/text-base/CollisionPoint.as.svn-base new file mode 100644 index 0000000..6b6881e --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/collision/.svn/text-base/CollisionPoint.as.svn-base @@ -0,0 +1,33 @@ +package alternativa.physics.collision { + import alternativa.physics.types.Vector3; + + public class CollisionPoint { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + + // Величины, расчитываемые перед началом фазы решения контактов + + // Требуемая проекция конечной скорости на нормаль для упругого контакта + public var normalVel:Number; + // Минимальная скорость разделения неупругого контакта + public var minSepVel:Number; + // Изменение проекции скорости на единицу нормального импульса + public var velByUnitImpulseN:Number; + + // Радиус-вектор точки контакта относительно центра первого тела + public var r1:Vector3 = new Vector3(); + // Радиус-вектор точки контакта относительно центра второго тела + public var r2:Vector3 = new Vector3(); + + // Величины, накапливаемые во время фазы решения контактов + + // Накопленный импульс, применяемый для получения требуемой относительной скорости в точке контакта + public var accumImpulseN:Number; + // Накопленный импульс, применяемый для разделения тел в точке контакта. Не создаёт момента. +// public var accumSepImpulse:Number; + } +} \ No newline at end of file diff --git a/0.1.SingleStep/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base b/0.1.SingleStep/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base new file mode 100644 index 0000000..8474b8d --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Body; + + public interface ICollider { + function collide(body1:Body, body2:Body, collisionInfo:CollisionInfo):Boolean; + } +} \ No newline at end of file diff --git a/0.1.SingleStep/src/alternativa/physics/collision/.svn/text-base/SpherePlaneCollider.as.svn-base b/0.1.SingleStep/src/alternativa/physics/collision/.svn/text-base/SpherePlaneCollider.as.svn-base new file mode 100644 index 0000000..7686fa1 --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/collision/.svn/text-base/SpherePlaneCollider.as.svn-base @@ -0,0 +1,59 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.primitives.RigidPlane; + import alternativa.physics.rigid.primitives.RigidSphere; + import alternativa.physics.types.Vector3; + + import flash.utils.getTimer; + + /** + * + */ + public class SpherePlaneCollider implements ICollider { + + private var normal:Vector3 = new Vector3(); + + /** + * + */ + public function SpherePlaneCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function collide(body1:Body, body2:Body, collisionInfo:CollisionInfo):Boolean { + var sphere:RigidSphere = body1 as RigidSphere; + var plane:RigidPlane; + if (sphere == null) { + sphere = body2 as RigidSphere; + plane = body1 as RigidPlane; + } else { + plane = body2 as RigidPlane; + } + + // Вычисляем глобальные нормаль и смещение плоскости + plane.baseMatrix.transformVector(plane.normal, normal); + var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; + + var dist:Number = sphere.state.pos.dot(normal) - offset; + if (dist > sphere.r) return false; + + collisionInfo.body1 = sphere; + collisionInfo.body2 = plane; + collisionInfo.normal.copy(normal); + collisionInfo.pcount = 1; + + var cp:CollisionPoint = collisionInfo.points[0]; + cp.penetration = sphere.r - dist; + cp.pos.copy(normal).reverse().scale(sphere.r).add(sphere.state.pos); + + return true; + } + + } +} \ No newline at end of file diff --git a/0.1.SingleStep/src/alternativa/physics/collision/.svn/text-base/SphereSphereCollider.as.svn-base b/0.1.SingleStep/src/alternativa/physics/collision/.svn/text-base/SphereSphereCollider.as.svn-base new file mode 100644 index 0000000..a9702a4 --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/collision/.svn/text-base/SphereSphereCollider.as.svn-base @@ -0,0 +1,51 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.primitives.RigidSphere; + import alternativa.physics.types.Vector3; + + public class SphereSphereCollider implements ICollider { + + public function SphereSphereCollider() { + } + + public function collide(body1:Body, body2:Body, colInfo:CollisionInfo):Boolean { + var s1:RigidSphere; + var s2:RigidSphere; + if (body1.id < body2.id) { + s1 = body1 as RigidSphere; + s2 = body2 as RigidSphere; + } else { + s1 = body2 as RigidSphere; + s2 = body1 as RigidSphere; + } + + var p1:Vector3 = s1.state.pos; + var p2:Vector3 = s2.state.pos; + var dx:Number = p1.x - p2.x; + var dy:Number = p1.y - p2.y; + var dz:Number = p1.z - p2.z; + var len:Number = dx*dx + dy*dy + dz*dz; + var sum:Number = s1.r + s2.r; + if (len > sum*sum) return false; + len = Math.sqrt(len); + dx /= len; + dy /= len; + dz /= len; + + colInfo.body1 = s1; + colInfo.body2 = s2; + colInfo.normal.x = dx; + colInfo.normal.y = dy; + colInfo.normal.z = dz; + colInfo.pcount = 1; + var cp:CollisionPoint = colInfo.points[0]; + cp.penetration = sum - len; + cp.pos.x = p1.x - dx*s1.r; + cp.pos.y = p1.y - dy*s1.r; + cp.pos.z = p1.z - dz*s1.r; + + return true; + } + + } +} \ No newline at end of file diff --git a/0.1.SingleStep/src/alternativa/physics/collision/BoxBoxCollider.as b/0.1.SingleStep/src/alternativa/physics/collision/BoxBoxCollider.as new file mode 100644 index 0000000..093b6f3 --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/collision/BoxBoxCollider.as @@ -0,0 +1,721 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.primitives.RigidBox; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + + /** + * + */ + public class BoxBoxCollider implements ICollider { + + private var tolerance:Number = 0.001; + + private var vectorToBox1:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var colAxis:Vector3 = new Vector3(); + private var tmpAxis:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxBoxCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + /** + * + * @param body1 + * @param body2 + * @param colInfo + * @return + */ + public function collide(body1:Body, body2:Body, colInfo:CollisionInfo):Boolean { + minOverlap = 1e10; + var box1:RigidBox; + var box2:RigidBox; + if (body1.id < body2.id) { + box1 = body1 as RigidBox; + box2 = body2 as RigidBox; + } else { + box1 = body2 as RigidBox; + box2 = body1 as RigidBox; + } + + // Вектор из центра второго бокса в центр первого + vectorToBox1.diff(box1.state.pos, box2.state.pos); + + // Проверка пересечения по основным осям + box1.transform.getAxis(0, axis10); + if (!testAxis(box1, box2, axis10, 0, vectorToBox1)) return false; + box1.transform.getAxis(1, axis11); + if (!testAxis(box1, box2, axis11, 1, vectorToBox1)) return false; + box1.transform.getAxis(2, axis12); + if (!testAxis(box1, box2, axis12, 2, vectorToBox1)) return false; + + box2.transform.getAxis(0, axis20); + if (!testAxis(box1, box2, axis20, 3, vectorToBox1)) return false; + box2.transform.getAxis(1, axis21); + if (!testAxis(box1, box2, axis21, 4, vectorToBox1)) return false; + box2.transform.getAxis(2, axis22); + if (!testAxis(box1, box2, axis22, 5, vectorToBox1)) return false; + + // Проверка производных осей + if (!testAxis(box1, box2, axis.cross2(axis10, axis20), 6, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.cross2(axis10, axis21), 7, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.cross2(axis10, axis22), 8, vectorToBox1)) return false; + + if (!testAxis(box1, box2, axis.cross2(axis11, axis20), 9, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.cross2(axis11, axis21), 10, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.cross2(axis11, axis22), 11, vectorToBox1)) return false; + + if (!testAxis(box1, box2, axis.cross2(axis12, axis20), 12, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.cross2(axis12, axis21), 13, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.cross2(axis12, axis22), 14, vectorToBox1)) return false; + + + if (bestAxisIndex < 6) { + // Контакт грань-(грань|ребро|вершина) + findFaceContactPoints(box1, box2, vectorToBox1, bestAxisIndex, colInfo); + } else { + // Контакт ребро-ребро + bestAxisIndex -= 6; + findEdgesIntersection(box1, box2, vectorToBox1, int(bestAxisIndex/3), bestAxisIndex%3, colInfo); + } + colInfo.body1 = box1; + colInfo.body2 = box2; + + return true; + } + + /** + * Выполняет поиск точек контакта грани одного бокса с гранью/ребром/вершиной другого. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param faceAxisIdx индекс оси первого бокса, перпендикулярной грани, с которой произошло столкновение + * @param colInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box1:RigidBox, box2:RigidBox, vectorToBox1:Vector3, faceAxisIdx:int, colInfo:CollisionInfo):void { + var swapNormal:Boolean = false; + if (faceAxisIdx > 2) { + // Столкновение с гранью второго бокса. Для дальнейших расчётов боксы меняются местами, + // но нормаль контакта всё равно должна быть направлена в сторону первоначального box1 + var tmpBox:RigidBox = box1; + box1 = box2; + box2 = tmpBox; + vectorToBox1.reverse(); + faceAxisIdx -= 3; + swapNormal = true; + } + box1.transform.getAxis(faceAxisIdx, colAxis); + var faceReversed:Boolean = colAxis.dot(vectorToBox1) > 0; + if (!faceReversed) colAxis.reverse(); + // Ищем ось второго бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + box2.transform.getAxis(axisIdx, axis); + var dot:Number = axis.dot(colAxis); + if (dot < 0) dot = -dot; + if (dot > maxDot) { + maxDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Получаем список вершин грани второго бокса, переводим их в систему координат первого бокса и выполняем обрезку + // по грани первого бокса. Таким образом получается список потенциальных точек контакта. + box2.transform.getAxis(incFaceAxisIdx, axis); + getFaceVertsByAxis(box2, incFaceAxisIdx, axis.dot(colAxis) < 0, verts1); + box2.transform.transformVectors(verts1, verts2); + box1.transform.transformVectorsInverse(verts2, verts1); + var pnum:int = clip(box1.halfSize, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + colInfo.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + if ((pen = getPointBoxPenetration(box1.halfSize, verts1[i], faceAxisIdx, faceReversed)) > -tolerance) { + var cp:CollisionPoint = colInfo.points[colInfo.pcount++]; + box1.transform.transformVector(verts1[i], cp.pos); + cp.penetration = pen; + } + } + colInfo.normal.copy(colAxis); + if (swapNormal) colInfo.normal.reverse(); + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + private function getFaceVertsByAxis(box:RigidBox, axisIdx:int, reverse:Boolean, result:Vector.):void { + var hs:Vector3 = box.halfSize; + switch (axisIdx) { + case 0: + if (reverse) { + (result[0] as Vector3).reset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).reset(-hs.x, -hs.y, -hs.z); + (result[2] as Vector3).reset(-hs.x, -hs.y, hs.z); + (result[3] as Vector3).reset(-hs.x, hs.y, hs.z); + } else { + (result[0] as Vector3).reset(hs.x, -hs.y, -hs.z); + (result[1] as Vector3).reset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).reset(hs.x, hs.y, hs.z); + (result[3] as Vector3).reset(hs.x, -hs.y, hs.z); + } + break; + case 1: + if (reverse) { + (result[0] as Vector3).reset(-hs.x, -hs.y, -hs.z); + (result[1] as Vector3).reset(hs.x, -hs.y, -hs.z); + (result[2] as Vector3).reset(hs.x, -hs.y, hs.z); + (result[3] as Vector3).reset(-hs.x, -hs.y, hs.z); + } else { + (result[0] as Vector3).reset(hs.x, hs.y, -hs.z); + (result[1] as Vector3).reset(-hs.x, hs.y, -hs.z); + (result[2] as Vector3).reset(-hs.x, hs.y, hs.z); + (result[3] as Vector3).reset(hs.x, hs.y, hs.z); + } + break; + case 2: + if (reverse) { + (result[0] as Vector3).reset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).reset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).reset(hs.x, -hs.y, -hs.z); + (result[3] as Vector3).reset(-hs.x, -hs.y, -hs.z); + } else { + (result[0] as Vector3).reset(-hs.x, -hs.y, hs.z); + (result[1] as Vector3).reset(hs.x, -hs.y, hs.z); + (result[2] as Vector3).reset(hs.x, hs.y, hs.z); + (result[3] as Vector3).reset(-hs.x, hs.y, hs.z); + } + break; + } + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clip(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + return clipHighX(hs.x, pnum, verts2, verts1); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + } + return 0; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + (result[num++] as Vector3).copy(p1); + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).reset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).reset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + (result[num++] as Vector3).copy(p1); + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).reset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).reset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + (result[num++] as Vector3).copy(p1); + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).reset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).reset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + (result[num++] as Vector3).copy(p1); + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).reset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).reset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + (result[num++] as Vector3).copy(p1); + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).reset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).reset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + (result[num++] as Vector3).copy(p1); + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).reset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).reset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param colInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box1:RigidBox, box2:RigidBox, vectorToBox1:Vector3, axisIdx1:int, axisIdx2:int, colInfo:CollisionInfo):void { + box1.transform.getAxis(axisIdx1, axis10); + box2.transform.getAxis(axisIdx2, axis20); + colAxis.cross2(axis10, axis20).normalize(); + // Разворот оси в сторону первого бокса + if (colAxis.dot(vectorToBox1) < 0) colAxis.reverse(); + + /* Кодирование рёбер + бит 0: 1 (тип контакта ребро-ребро) + биты 1-2: индекс направляющей оси ребра + бит 3: + бит 4: + бит 5: + */ + var edgeCode1:int = 1; + var edgeCode2:int = 1; + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + point1.copy(box1.halfSize); + point2.copy(box2.halfSize); + // x + if (axisIdx1 == 0) { + point1.x = 0; + halfLen1 = box1.halfSize.x; + } else { + box1.transform.getAxis(0, tmpAxis); + if (tmpAxis.dot(colAxis) > 0) { + point1.x = -point1.x; + edgeCode1 |= 8; // 1 << 3 + } + } + if (axisIdx2 == 0) { + point2.x = 0; + halfLen2 = box2.halfSize.x; + } else { + box2.transform.getAxis(0, tmpAxis); + if (tmpAxis.dot(colAxis) < 0) { + point2.x = -point2.x; + edgeCode2 |= 8; // 1 << 3 + } + } + // y + if (axisIdx1 == 1) { + point1.y = 0; + halfLen1 = box1.halfSize.y; + edgeCode1 |= 2; // 1 << 1 + } else { + box1.transform.getAxis(1, tmpAxis); + if (tmpAxis.dot(colAxis) > 0) { + point1.y = -point1.y; + edgeCode1 |= 16; // 1 << 4 + } + } + if (axisIdx2 == 1) { + point2.y = 0; + halfLen2 = box2.halfSize.y; + edgeCode2 |= 2; // 1 << 1 + } else { + box2.transform.getAxis(1, tmpAxis); + if (tmpAxis.dot(colAxis) < 0) { + point2.y = -point2.y; + edgeCode2 |= 16; // 1 << 4 + } + } + // z + if (axisIdx1 == 2) { + point1.z = 0; + halfLen1 = box1.halfSize.z; + edgeCode1 |= 4; // 2 << 1 + } else { + box1.transform.getAxis(2, tmpAxis); + if (tmpAxis.dot(colAxis) > 0) { + point1.z = -point1.z; + edgeCode1 |= 32; // 1 << 5 + } + } + if (axisIdx2 == 2) { + point2.z = 0; + halfLen2 = box2.halfSize.z; + edgeCode2 |= 4; // 2 << 1 + } else { + box2.transform.getAxis(2, tmpAxis); + if (tmpAxis.dot(colAxis) < 0) { + point2.z = -point2.z; + edgeCode2 |= 32; // 1 << 5 + } + } + // Получаем глобальные координаты средних точек рёбер + point1.transformBy4(box1.transform); + point2.transformBy4(box2.transform); + // Находим точку пересечения рёбер, решая систему уравнений + var k:Number = axis10.dot(axis20); + var det:Number = k*k - 1; + vector.diff(point2, point1); + var c1:Number = axis10.dot(vector); + var c2:Number = axis20.dot(vector); + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + colInfo.normal.copy(colAxis); + colInfo.pcount = 1; + var cp:CollisionPoint = colInfo.points[0]; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(point1.x + axis10.x*t1 + point2.x + axis20.x*t2); + cp.pos.y = 0.5*(point1.y + axis10.y*t1 + point2.y + axis20.y*t2); + cp.pos.z = 0.5*(point1.z + axis10.z*t1 + point2.z + axis20.z*t2); + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение боксов вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box1 + * @param box2 + * @param axis + * @param axisIndex + * @param vectorToBox1 + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testAxis(box1:RigidBox, box2:RigidBox, axis:Vector3, axisIndex:int, vectorToBox1:Vector3):Boolean { + if (axis.lengthSqr() < 0.0001) { + return true; + } + axis.normalize(); + + var overlap:Number = overlapOnAxis(box1, box2, axis, vectorToBox1); + if (overlap < -tolerance) return false; + if (overlap + tolerance < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия двух боксов вдоль заданной оси. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param axis ось + * @param vectorToBox1 вектор, соединяющий центр второго бокса с центром первого + * @return величина перекрытия боксов вдоль оси + */ + public function overlapOnAxis(box1:RigidBox, box2:RigidBox, axis:Vector3, vectorToBox1:Vector3):Number { + var m:Matrix4 = box1.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box1.halfSize.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box1.halfSize.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box1.halfSize.z; + if (d < 0) d = -d; + projection += d; + + m = box2.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box2.halfSize.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box2.halfSize.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box2.halfSize.z; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox1.x*axis.x + vectorToBox1.y*axis.y + vectorToBox1.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} + import alternativa.physics.types.Vector3; + + class CollisionPointTmp { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + } diff --git a/0.1.SingleStep/src/alternativa/physics/collision/BoxPlaneCollider.as b/0.1.SingleStep/src/alternativa/physics/collision/BoxPlaneCollider.as new file mode 100644 index 0000000..4b19470 --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/collision/BoxPlaneCollider.as @@ -0,0 +1,77 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.primitives.RigidBox; + import alternativa.physics.rigid.primitives.RigidPlane; + import alternativa.physics.types.Vector3; + + public class BoxPlaneCollider implements ICollider { + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var normal:Vector3 = new Vector3(); + + public function BoxPlaneCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + public function collide(body1:Body, body2:Body, collisionInfo:CollisionInfo):Boolean { + var box:RigidBox = body1 as RigidBox; + var plane:RigidPlane; + if (box == null) { + box = body2 as RigidBox; + plane = body1 as RigidPlane; + } else { + plane = body2 as RigidPlane; + } + + // Вычисляем глобальные координаты вершин бокса + var sx:Number = box.halfSize.x; + var sy:Number = box.halfSize.y; + var sz:Number = box.halfSize.z; + (verts1[0] as Vector3).reset(-sx, -sy, -sz); + (verts1[1] as Vector3).reset(sx, -sy, -sz); + (verts1[2] as Vector3).reset(sx, sy, -sz); + (verts1[3] as Vector3).reset(-sx, sy, -sz); + (verts1[4] as Vector3).reset(-sx, -sy, sz); + (verts1[5] as Vector3).reset(sx, -sy, sz); + (verts1[6] as Vector3).reset(sx, sy, sz); + (verts1[7] as Vector3).reset(-sx, sy, sz); + + box.transform.transformVectors(verts1, verts2); + // Вычисляем глобальные нормаль и смещение плоскости + plane.baseMatrix.transformVector(plane.normal, normal); + var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; + // Проверяем наличие столкновений с каждой вершиной + collisionInfo.pcount = 0; + for (var i:int = 0; i < 8; i++) { + // Вершина добавляется в список точек столкновения, если лежит под плоскостью + var dist:Number = (verts2[i] as Vector3).dot(normal); + if (dist < offset) { + var cp:CollisionPoint; + if (collisionInfo.pcount == collisionInfo.points.length) { + cp = new CollisionPoint(); + collisionInfo.points[collisionInfo.pcount] = cp; + } else { + cp = collisionInfo.points[collisionInfo.pcount]; + } + cp.pos.copy(verts2[i]); + cp.penetration = offset - dist; + collisionInfo.pcount++; + } + } + if (collisionInfo.pcount > 0) { + collisionInfo.body1 = box; + collisionInfo.body2 = plane; + collisionInfo.normal.copy(normal); + return true; + } + return false; + } + + } +} \ No newline at end of file diff --git a/0.1.SingleStep/src/alternativa/physics/collision/BoxSphereCollider.as b/0.1.SingleStep/src/alternativa/physics/collision/BoxSphereCollider.as new file mode 100644 index 0000000..65d8e40 --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/collision/BoxSphereCollider.as @@ -0,0 +1,94 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.primitives.RigidBox; + import alternativa.physics.rigid.primitives.RigidSphere; + import alternativa.physics.types.Vector3; + + /** + * + */ + public class BoxSphereCollider implements ICollider { + + private var center:Vector3 = new Vector3(); + private var closestPt:Vector3 = new Vector3(); + + /** + * + */ + public function BoxSphereCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function collide(body1:Body, body2:Body, collisionInfo:CollisionInfo):Boolean { + var box:RigidBox = body1 as RigidBox; + var sphere:RigidSphere; + if (box == null) { + box = body2 as RigidBox; + sphere = body1 as RigidSphere; + } else { + sphere = body2 as RigidSphere; + } + // Трансформируем центр сферы в систему коорлинат бокса + box.transform.transformVectorInverse(sphere.state.pos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.halfSize; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.subtract(closestPt).lengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + // Зафиксированно столкновение + collisionInfo.body1 = box; + collisionInfo.body2 = sphere; + collisionInfo.normal.copy(closestPt).transformBy4(box.transform).subtract(sphere.state.pos).normalize(); + collisionInfo.pcount = 1; + + var cp:CollisionPoint = collisionInfo.points[0]; + cp.penetration = sphere.r - Math.sqrt(distSqr); + cp.pos.copy(collisionInfo.normal).scale(sphere.r).add(sphere.state.pos); + + return true; + } + + } +} \ No newline at end of file diff --git a/0.1.SingleStep/src/alternativa/physics/collision/CollisionDetector.as b/0.1.SingleStep/src/alternativa/physics/collision/CollisionDetector.as new file mode 100644 index 0000000..31b24a9 --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/collision/CollisionDetector.as @@ -0,0 +1,45 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Body; + + /** + * + */ + public class CollisionDetector implements ICollider { + + private var colliders:Object = {}; + + /** + * + */ + public function CollisionDetector() { + addCollider(Body.BOX, Body.BOX, new BoxBoxCollider()); + addCollider(Body.BOX, Body.PLANE, new BoxPlaneCollider()); + addCollider(Body.BOX, Body.SPHERE, new BoxSphereCollider()); + addCollider(Body.SPHERE, Body.PLANE, new SpherePlaneCollider()); + addCollider(Body.SPHERE, Body.SPHERE, new SphereSphereCollider()); + } + + /** + * + * @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 body1 + * @param body2 + * @param contactInfo + * @return + */ + public function collide(body1:Body, body2:Body, contactInfo:CollisionInfo):Boolean { + var collider:ICollider = colliders[body1.type <= body2.type ? (body1.type << 16) | body2.type : (body2.type << 16) | body1.type] as ICollider; + return collider == null ? false : collider.collide(body1, body2, contactInfo); + } + + } +} \ No newline at end of file diff --git a/0.1.SingleStep/src/alternativa/physics/collision/CollisionInfo.as b/0.1.SingleStep/src/alternativa/physics/collision/CollisionInfo.as new file mode 100644 index 0000000..7429d25 --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/collision/CollisionInfo.as @@ -0,0 +1,44 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Vector3; + + /** + * + */ + public class CollisionInfo { + + private static var pool:Vector. = new Vector.(); + + public static function create():CollisionInfo { + if (pool.length > 0) { + return pool.pop(); + } + return new CollisionInfo(); + } + + public static function destroy(collInfo:CollisionInfo):void { + collInfo.body1 = collInfo.body2 = null; + pool.push(collInfo); + } + + private const N:int = 8; + + public var body1:Body; + public var body2:Body; + // Коэффициент отскока + public var restitution:Number; + // Коэффициент трения + public var friction:Number; + + public var normal:Vector3 = new Vector3(); + public var points:Vector. = new Vector.(N, true); + public var pcount:int; + + public function CollisionInfo() { + for (var i:int = 0; i < N; i++) points[i] = new CollisionPoint(); + } + + } +} \ No newline at end of file diff --git a/0.1.SingleStep/src/alternativa/physics/collision/CollisionPoint.as b/0.1.SingleStep/src/alternativa/physics/collision/CollisionPoint.as new file mode 100644 index 0000000..6b6881e --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/collision/CollisionPoint.as @@ -0,0 +1,33 @@ +package alternativa.physics.collision { + import alternativa.physics.types.Vector3; + + public class CollisionPoint { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + + // Величины, расчитываемые перед началом фазы решения контактов + + // Требуемая проекция конечной скорости на нормаль для упругого контакта + public var normalVel:Number; + // Минимальная скорость разделения неупругого контакта + public var minSepVel:Number; + // Изменение проекции скорости на единицу нормального импульса + public var velByUnitImpulseN:Number; + + // Радиус-вектор точки контакта относительно центра первого тела + public var r1:Vector3 = new Vector3(); + // Радиус-вектор точки контакта относительно центра второго тела + public var r2:Vector3 = new Vector3(); + + // Величины, накапливаемые во время фазы решения контактов + + // Накопленный импульс, применяемый для получения требуемой относительной скорости в точке контакта + public var accumImpulseN:Number; + // Накопленный импульс, применяемый для разделения тел в точке контакта. Не создаёт момента. +// public var accumSepImpulse:Number; + } +} \ No newline at end of file diff --git a/0.1.SingleStep/src/alternativa/physics/collision/ICollider.as b/0.1.SingleStep/src/alternativa/physics/collision/ICollider.as new file mode 100644 index 0000000..8474b8d --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/collision/ICollider.as @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Body; + + public interface ICollider { + function collide(body1:Body, body2:Body, collisionInfo:CollisionInfo):Boolean; + } +} \ No newline at end of file diff --git a/0.1.SingleStep/src/alternativa/physics/collision/SpherePlaneCollider.as b/0.1.SingleStep/src/alternativa/physics/collision/SpherePlaneCollider.as new file mode 100644 index 0000000..7686fa1 --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/collision/SpherePlaneCollider.as @@ -0,0 +1,59 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.primitives.RigidPlane; + import alternativa.physics.rigid.primitives.RigidSphere; + import alternativa.physics.types.Vector3; + + import flash.utils.getTimer; + + /** + * + */ + public class SpherePlaneCollider implements ICollider { + + private var normal:Vector3 = new Vector3(); + + /** + * + */ + public function SpherePlaneCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function collide(body1:Body, body2:Body, collisionInfo:CollisionInfo):Boolean { + var sphere:RigidSphere = body1 as RigidSphere; + var plane:RigidPlane; + if (sphere == null) { + sphere = body2 as RigidSphere; + plane = body1 as RigidPlane; + } else { + plane = body2 as RigidPlane; + } + + // Вычисляем глобальные нормаль и смещение плоскости + plane.baseMatrix.transformVector(plane.normal, normal); + var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; + + var dist:Number = sphere.state.pos.dot(normal) - offset; + if (dist > sphere.r) return false; + + collisionInfo.body1 = sphere; + collisionInfo.body2 = plane; + collisionInfo.normal.copy(normal); + collisionInfo.pcount = 1; + + var cp:CollisionPoint = collisionInfo.points[0]; + cp.penetration = sphere.r - dist; + cp.pos.copy(normal).reverse().scale(sphere.r).add(sphere.state.pos); + + return true; + } + + } +} \ No newline at end of file diff --git a/0.1.SingleStep/src/alternativa/physics/collision/SphereSphereCollider.as b/0.1.SingleStep/src/alternativa/physics/collision/SphereSphereCollider.as new file mode 100644 index 0000000..a9702a4 --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/collision/SphereSphereCollider.as @@ -0,0 +1,51 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.primitives.RigidSphere; + import alternativa.physics.types.Vector3; + + public class SphereSphereCollider implements ICollider { + + public function SphereSphereCollider() { + } + + public function collide(body1:Body, body2:Body, colInfo:CollisionInfo):Boolean { + var s1:RigidSphere; + var s2:RigidSphere; + if (body1.id < body2.id) { + s1 = body1 as RigidSphere; + s2 = body2 as RigidSphere; + } else { + s1 = body2 as RigidSphere; + s2 = body1 as RigidSphere; + } + + var p1:Vector3 = s1.state.pos; + var p2:Vector3 = s2.state.pos; + var dx:Number = p1.x - p2.x; + var dy:Number = p1.y - p2.y; + var dz:Number = p1.z - p2.z; + var len:Number = dx*dx + dy*dy + dz*dz; + var sum:Number = s1.r + s2.r; + if (len > sum*sum) return false; + len = Math.sqrt(len); + dx /= len; + dy /= len; + dz /= len; + + colInfo.body1 = s1; + colInfo.body2 = s2; + colInfo.normal.x = dx; + colInfo.normal.y = dy; + colInfo.normal.z = dz; + colInfo.pcount = 1; + var cp:CollisionPoint = colInfo.points[0]; + cp.penetration = sum - len; + cp.pos.x = p1.x - dx*s1.r; + cp.pos.y = p1.y - dy*s1.r; + cp.pos.z = p1.z - dz*s1.r; + + return true; + } + + } +} \ No newline at end of file diff --git a/0.1.SingleStep/src/alternativa/physics/rigid/.svn/all-wcprops b/0.1.SingleStep/src/alternativa/physics/rigid/.svn/all-wcprops new file mode 100644 index 0000000..1156555 --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/rigid/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 120 +/!svn/ver/10598/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.SingleStep/src/alternativa/physics/rigid +END +Body.as +K 25 +svn:wc:ra_dav:version-url +V 128 +/!svn/ver/10588/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.SingleStep/src/alternativa/physics/rigid/Body.as +END +BodyState.as +K 25 +svn:wc:ra_dav:version-url +V 133 +/!svn/ver/10588/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.SingleStep/src/alternativa/physics/rigid/BodyState.as +END +RigidWorld.as +K 25 +svn:wc:ra_dav:version-url +V 134 +/!svn/ver/10598/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.SingleStep/src/alternativa/physics/rigid/RigidWorld.as +END +BodyMaterial.as +K 25 +svn:wc:ra_dav:version-url +V 136 +/!svn/ver/10588/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.SingleStep/src/alternativa/physics/rigid/BodyMaterial.as +END diff --git a/0.1.SingleStep/src/alternativa/physics/rigid/.svn/entries b/0.1.SingleStep/src/alternativa/physics/rigid/.svn/entries new file mode 100644 index 0000000..db4c37d --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/rigid/.svn/entries @@ -0,0 +1,79 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.SingleStep/src/alternativa/physics/rigid +http://svndev.alternativaplatform.com + + + +2009-04-07T09:35:01.456741Z +10598 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Body.as +file + + + + +2010-10-28T04:32:39.000000Z +054ce48aa4a6b58d010a61fca1448f64 +2009-04-07T08:34:06.342994Z +10587 +mike + +BodyState.as +file + + + + +2010-10-28T04:32:39.000000Z +39e60c552e4b47f3a7bb578912d14e77 +2009-04-01T12:25:29.638016Z +10301 +mike + +RigidWorld.as +file + + + + +2010-10-28T04:32:39.000000Z +49077d27546da688098f07063affb1f5 +2009-04-07T09:35:01.456741Z +10598 +mike + +primitives +dir + +BodyMaterial.as +file + + + + +2010-10-28T04:32:39.000000Z +49c7a55b5b26fdb03a60f24bac460f53 +2009-04-07T08:34:06.342994Z +10587 +mike + diff --git a/0.1.SingleStep/src/alternativa/physics/rigid/.svn/format b/0.1.SingleStep/src/alternativa/physics/rigid/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/rigid/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.1.SingleStep/src/alternativa/physics/rigid/.svn/text-base/Body.as.svn-base b/0.1.SingleStep/src/alternativa/physics/rigid/.svn/text-base/Body.as.svn-base new file mode 100644 index 0000000..2571652 --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/rigid/.svn/text-base/Body.as.svn-base @@ -0,0 +1,211 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Quaternion; + import alternativa.physics.types.Vector3; + + + public class Body { + + public static const BOX:int = 1; + public static const PLANE:int = 2; + public static const SPHERE:int = 3; + + public static var linDamping:Number = 0.995; + public static var rotDamping:Number = 0.995; + + public var id:int; + public var type:int; + public var name:String; + // Текущее и предыдущее состояние тела. Промежуточное состояние вычисляется линейной интерполяцией. + public var state:BodyState = new BodyState(); + public var prevState:BodyState = new BodyState(); + // Линейное и угловое ускорение тела на текущем шаге симуляции + public var accel:Vector3 = new Vector3(); + public var angleAccel:Vector3 = new Vector3(); + // Материал тела + public var material:BodyMaterial = new BodyMaterial(); + + public var invMass:Number = 1; + public var invInertia:Matrix3 = new Matrix3(); + public var invInertiaWorld:Matrix3 = new Matrix3(); + public var baseMatrix:Matrix3 = new Matrix3(); + public var transform:Matrix4 = new Matrix4(); + + private var forceAccum:Vector3 = new Vector3(); + private var torqueAccum:Vector3 = new Vector3(); + + /** + * + * @param type + * @param invMass + * @param invInertia + */ + public function Body(type:int, invMass:Number, invInertia:Matrix3) { + this.type = type; + this.invMass = invMass; + this.invInertia.copy(invInertia); + } + + /** + * + */ + public function clearAccumulators():void { + forceAccum.reset(); + torqueAccum.reset(); + } + + /** + * + * @param f + */ + public function addForce(f:Vector3):void { + forceAccum.add(f); + } + + /** + * + * @param t + */ + public function addTorque(t:Vector3):void { + torqueAccum.add(t); + } + + /** + * + */ + public function calcAccelerations():void { + calcDerivedData(); + + accel.x = forceAccum.x*invMass; + accel.y = forceAccum.y*invMass; + accel.z = forceAccum.z*invMass; + + invInertiaWorld.transformVector(torqueAccum, angleAccel); + } + + /** + * Вычисление вспомогательной информации. + */ + public function calcDerivedData():void { + // Вычисление базисной матрицы и обратного тензора инерции в мировых координатах + state.orientation.toMatrix3(baseMatrix); + invInertiaWorld.copy(invInertia).append(baseMatrix).prepend(baseMatrix.transpose()); + transform.setFromMatrix3(baseMatrix.transpose(), state.pos); + } + + /** + * + * @param t + * @param result + */ + public function interpolate(t:Number, pos:Vector3, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.w = prevState.orientation.w*t1 + state.orientation.w*t; + orientation.x = prevState.orientation.x*t1 + state.orientation.x*t; + orientation.y = prevState.orientation.y*t1 + state.orientation.y*t; + orientation.z = prevState.orientation.z*t1 + state.orientation.z*t; + } + + /** + * + */ + public function saveState():void { + prevState.copy(state); + } + + /** + * + */ + public function restoreState():void { + state.copy(prevState); + } + + /** + * + * @param dt + */ + public function integrateVelocity(dt:Number):void { + // v = v + a*t + state.velocity.x += accel.x*dt; + state.velocity.y += accel.y*dt; + state.velocity.z += accel.z*dt; + // rot = rot + eps*t + state.rotation.x += angleAccel.x*dt; + state.rotation.y += angleAccel.y*dt; + state.rotation.z += angleAccel.z*dt; + + state.velocity.x *= linDamping; + state.velocity.y *= linDamping; + state.velocity.z *= linDamping; + + state.rotation.x *= rotDamping; + state.rotation.y *= rotDamping; + state.rotation.z *= rotDamping; + } + + /** + * + */ + public function integratePosition(dt:Number):void { + // pos = pos + v*t + state.pos.x += state.velocity.x*dt; + state.pos.y += state.velocity.y*dt; + state.pos.z += state.velocity.z*dt; + // q = q + 0.5*rot*q + state.orientation.addScaledVector(state.rotation, dt); + } + + public function setPosition(pos:Vector3):void { + state.pos.copy(pos); + } + + public function setPositionXYZ(x:Number, y:Number, z:Number):void { + state.pos.reset(x, y, z); + } + + public function setVelocity(vel:Vector3):void { + state.velocity.copy(vel); + } + + public function setVelocityXYZ(x:Number, y:Number, z:Number):void { + state.velocity.reset(x, y, z); + } + + public function setRotation(rot:Vector3):void { + state.rotation.copy(rot); + } + + public function setRotationXYZ(x:Number, y:Number, z:Number):void { + state.rotation.reset(x, y, z); + } + + public function setOrientation(q:Quaternion):void { + state.orientation.copy(q); + } + + private var _v:Vector3 = new Vector3(); + public function applyWorldImpulse(r:Vector3, dir:Vector3, magnitude:Number):void { + var d:Number = magnitude*invMass; + // Линейная часть + state.velocity.x += d*dir.x; + state.velocity.y += d*dir.y; + state.velocity.z += d*dir.z; + // Вращательная часть + var rx:Number = r.x - state.pos.x; + var ry:Number = r.y - state.pos.y; + var rz:Number = r.z - state.pos.z; + _v.x = (ry*dir.z - rz*dir.y)*magnitude; + _v.y = (rz*dir.x - rx*dir.z)*magnitude; + _v.z = (rx*dir.y - ry*dir.x)*magnitude; + _v.transformBy3(invInertiaWorld); + state.rotation.x += _v.x; + state.rotation.y += _v.y; + state.rotation.z += _v.z; + } + + } +} \ No newline at end of file diff --git a/0.1.SingleStep/src/alternativa/physics/rigid/.svn/text-base/BodyMaterial.as.svn-base b/0.1.SingleStep/src/alternativa/physics/rigid/.svn/text-base/BodyMaterial.as.svn-base new file mode 100644 index 0000000..61d4382 --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/rigid/.svn/text-base/BodyMaterial.as.svn-base @@ -0,0 +1,11 @@ +package alternativa.physics.rigid { + + public class BodyMaterial { + + public var restitution:Number = 0; + public var friction:Number = 0.3; +// public var dynamicFriction:Number = 0.2; +// public var dynamicFriction:Number = 0.2; + + } +} \ No newline at end of file diff --git a/0.1.SingleStep/src/alternativa/physics/rigid/.svn/text-base/BodyState.as.svn-base b/0.1.SingleStep/src/alternativa/physics/rigid/.svn/text-base/BodyState.as.svn-base new file mode 100644 index 0000000..f0c95e9 --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/rigid/.svn/text-base/BodyState.as.svn-base @@ -0,0 +1,39 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Quaternion; + import alternativa.physics.types.Vector3; + + /** + * Класс описывает состояние твёрдого тела. + */ + public class BodyState { + /** + * Положение тела. + */ + public var pos:Vector3 = new Vector3(); + /** + * Ориентация тела. + */ + public var orientation:Quaternion = new Quaternion(); + /** + * Скорость тела. + */ + public var velocity:Vector3 = new Vector3(); + /** + * Угловая скорость тела. + */ + public var rotation:Vector3 = new Vector3(); + + /** + * Копирует значение указанного объекта. + * + * @param state + */ + public function copy(state:BodyState):void { + pos.copy(state.pos); + orientation.copy(state.orientation); + velocity.copy(state.velocity); + rotation.copy(state.rotation); + } + + } +} \ No newline at end of file diff --git a/0.1.SingleStep/src/alternativa/physics/rigid/.svn/text-base/RigidWorld.as.svn-base b/0.1.SingleStep/src/alternativa/physics/rigid/.svn/text-base/RigidWorld.as.svn-base new file mode 100644 index 0000000..6384697 --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/rigid/.svn/text-base/RigidWorld.as.svn-base @@ -0,0 +1,267 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.CollisionDetector; + import alternativa.physics.collision.CollisionInfo; + import alternativa.physics.collision.CollisionPoint; + import alternativa.physics.collision.ICollider; + import alternativa.physics.types.Vector3; + + + public class RigidWorld { + + private static var lastBodyId:int; + + public var penResolutionSteps:int = 10; + public var allowedPenetration:Number = 0.1; + public var maxPenResolutionSpeed:Number = 0.5; + public var collisionIterations:int = 5; + public var contactIterations:int = 5; + public var usePrediction:Boolean = false; + public var gravity:Vector3 = new Vector3(0, 0, -9.8); + + private var collider:ICollider = new CollisionDetector(); + + private var bodies:Vector. = new Vector.(); + private var bodyCount:int; + + public const MAX_COLLISIONS:int = 200; + public var collisions:Vector. = new Vector.(MAX_COLLISIONS, true); + public var colNum:int; + + /** + * + */ + public function RigidWorld() { + for (var i:int = 0; i < MAX_COLLISIONS; i++) collisions[i] = new CollisionInfo(); + } + + /** + * + * @param body + */ + public function addBody(body:Body):void { + bodies[bodyCount++] = body; + body.id = lastBodyId++; + } + + /** + * + * @param dt + */ + private function applyForces(dt:Number):void { + for (var i:int = 0; i < bodyCount; i++) { + var body:Body = bodies[i]; + body.calcAccelerations(); + if (body.invMass != 0) body.accel.add(gravity); + } + } + + /** + * + * @param dt + */ + private function detectCollisions(dt:Number):void { + var i:int; + var j:int; + var body:Body; + + for (i = 0; i < bodyCount; i++) { + body = bodies[i]; + body.saveState(); + if (usePrediction) { + body.integrateVelocity(dt); + body.integratePosition(dt); + body.calcDerivedData(); + } + } + + colNum = 0; + for (i = 0; i < bodyCount; i++) { + for (j = i + 1; j < bodyCount; j++) { + if (collider.collide(bodies[i], bodies[j], collisions[colNum])) colNum++; + } + } + + if (usePrediction) { + for (i = 0; i < bodyCount; i++) { + body = bodies[i]; + body.restoreState(); + body.calcDerivedData(); + } + } + } + + private var _r:Vector3 = new Vector3(); + private var _t:Vector3 = new Vector3(); + private var _v:Vector3 = new Vector3(); + private var _v1:Vector3 = new Vector3(); + private var _v2:Vector3 = new Vector3(); + /** + * + */ + private function preProcessCollisions(dt:Number):void { + for (var i:int = 0; i < colNum; i++) { + var colInfo:CollisionInfo = collisions[i]; + var b1:Body = colInfo.body1; + var b2:Body = colInfo.body2; + colInfo.restitution = b1.material.restitution < b2.material.restitution ? b1.material.restitution : b2.material.restitution; + colInfo.friction = b1.material.friction < b2.material.friction ? b1.material.friction : b2.material.friction; + for (var j:int = 0; j < colInfo.pcount; j++) { + var cp:CollisionPoint = colInfo.points[j]; + cp.accumImpulseN = 0; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + cp.velByUnitImpulseN = 0; + if (b1.invMass != 0) { + cp.r1.diff(cp.pos, b1.state.pos); + _v.cross2(cp.r1, colInfo.normal).transformBy3(b1.invInertiaWorld).cross(cp.r1); + cp.velByUnitImpulseN += b1.invMass + _v.dot(colInfo.normal); + } + if (b2.invMass != 0) { + cp.r2.diff(cp.pos, b2.state.pos); + _v.cross2(cp.r2, colInfo.normal).transformBy3(b2.invInertiaWorld).cross(cp.r2); + cp.velByUnitImpulseN += b2.invMass + _v.dot(colInfo.normal); + } + // Расчёт требуемой конечной скорости для упругого контакта + calcSepVelocity(b1, b2, cp, _v); + cp.normalVel = _v.dot(colInfo.normal); + if (cp.normalVel < 0) cp.normalVel = - colInfo.restitution*cp.normalVel; + // Скорость разделения неупругого контакта + cp.minSepVel = cp.penetration > allowedPenetration ? (cp.penetration - allowedPenetration)/(penResolutionSteps*dt) : 0; + if (cp.minSepVel > maxPenResolutionSpeed) cp.minSepVel = maxPenResolutionSpeed; + } + } + } + + /** + * + * @param dt + * @param forceInelastic + */ + private function processContacts(dt:Number, forceInelastic:Boolean):void { + var iterNum:int = forceInelastic ? contactIterations : collisionIterations; + + for (var iter:int = 0; iter < iterNum; iter++) { + for (var i:int = 0; i < colNum; i++) resolveCollision(collisions[i], forceInelastic); + } + + } + + /** + * + */ + private function resolveCollision(colInfo:CollisionInfo, forceInelastic:Boolean):void { + var b1:Body = colInfo.body1; + var b2:Body = colInfo.body2; + var normal:Vector3 = colInfo.normal; + var restitution:Number = forceInelastic ? 0 : colInfo.restitution; + + for (var i:int = 0; i < colInfo.pcount; i++) { + var cp:CollisionPoint = colInfo.points[i]; + var newVel:Number = 0; + calcSepVelocity(b1, b2, cp, _v); + var sepVel:Number = _v.dot(colInfo.normal); + + newVel = cp.normalVel + cp.minSepVel; + trace(cp.normalVel, cp.minSepVel); + +// if (forceInelastic) { +// if (sepVel < cp.minSepVel) newVel = cp.minSepVel; +// else newVel = 0; +// } else { +// newVel = cp.normalVel; +// } + var deltaVel:Number = newVel - sepVel; + var impulse:Number = deltaVel/cp.velByUnitImpulseN; + var accumImpulse:Number = cp.accumImpulseN + impulse; + if (accumImpulse < 0) accumImpulse = 0; + var deltaImpulse:Number = accumImpulse - cp.accumImpulseN; + cp.accumImpulseN = accumImpulse; + // Применяем импульс к телам + if (b1.invMass != 0) b1.applyWorldImpulse(cp.pos, normal, deltaImpulse); + if (b2.invMass != 0) b2.applyWorldImpulse(cp.pos, normal, -deltaImpulse); + + // Учёт силы трения + calcSepVelocity(b1, b2, cp, _v); + // Расчитываем изменение касательной скорости на единицу касательного импульса + var tanSpeedByUnitImpulse:Number = 0; + _v.addScaled(-_v.dot(colInfo.normal), colInfo.normal); + var tanSpeed:Number = _v.length(); + if (tanSpeed < 0.001) continue; + _t.copy(_v).normalize().reverse(); + // dV = b.invMass + ((invI * (r % t)) % r) * t + if (b1.invMass != 0) { + _v.cross2(cp.r1, _t).transformBy3(b1.invInertiaWorld).cross(cp.r1); + tanSpeedByUnitImpulse += b1.invMass + _v.dot(_t); + } + if (b2.invMass != 0) { + _v.cross2(cp.r2, _t).transformBy3(b2.invInertiaWorld).cross(cp.r2); + tanSpeedByUnitImpulse += b2.invMass + _v.dot(_t); + } + + var tanImpulse:Number = tanSpeed/tanSpeedByUnitImpulse; + var max:Number = colInfo.friction*cp.accumImpulseN; + if (max < 0) { + if (tanImpulse < max) tanImpulse = max; + } else { + if (tanImpulse > max) tanImpulse = max; + } + // Применяем импульс к телам + if (b1.invMass != 0) b1.applyWorldImpulse(cp.pos, _t, tanImpulse); + if (b2.invMass != 0) b2.applyWorldImpulse(cp.pos, _t, -tanImpulse); + + } + } + + /** + * + * @param cp + * @param normal + * @return + */ + private function calcSepVelocity(body1:Body, body2:Body, cp:CollisionPoint, result:Vector3):void { + // sepVel = (V1 - V2)*normal + result.x = result.y = result.z = 0; + // V1 = V1_c + w1%r1 + if (body1.invMass != 0) result.add(body1.state.velocity).add(_v1.cross2(body1.state.rotation, cp.r1)); + // V2 = V2_c + w2%r2 + if (body2.invMass != 0) result.subtract(body2.state.velocity).subtract(_v2.cross2(body2.state.rotation, cp.r2)); + } + + /** + * + * @param dt + */ + private function intergateVelocities(dt:Number):void { + for (var i:int = 0; i < bodyCount; i++) { + (bodies[i] as Body).integrateVelocity(dt); + } + } + + /** + * + * @param dt + */ + private function integratePositions(dt:Number):void { + for (var i:int = 0; i < bodyCount; i++) { + (bodies[i] as Body).integratePosition(dt); + } + } + + /** + * + * @param dt + */ + public function runPhysics(dt:Number):void { + applyForces(dt); + detectCollisions(dt); + preProcessCollisions(dt); + processContacts(dt, false); + intergateVelocities(dt); +// processContacts(dt, true); + integratePositions(dt); + } + + } +} \ No newline at end of file diff --git a/0.1.SingleStep/src/alternativa/physics/rigid/Body.as b/0.1.SingleStep/src/alternativa/physics/rigid/Body.as new file mode 100644 index 0000000..2571652 --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/rigid/Body.as @@ -0,0 +1,211 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Quaternion; + import alternativa.physics.types.Vector3; + + + public class Body { + + public static const BOX:int = 1; + public static const PLANE:int = 2; + public static const SPHERE:int = 3; + + public static var linDamping:Number = 0.995; + public static var rotDamping:Number = 0.995; + + public var id:int; + public var type:int; + public var name:String; + // Текущее и предыдущее состояние тела. Промежуточное состояние вычисляется линейной интерполяцией. + public var state:BodyState = new BodyState(); + public var prevState:BodyState = new BodyState(); + // Линейное и угловое ускорение тела на текущем шаге симуляции + public var accel:Vector3 = new Vector3(); + public var angleAccel:Vector3 = new Vector3(); + // Материал тела + public var material:BodyMaterial = new BodyMaterial(); + + public var invMass:Number = 1; + public var invInertia:Matrix3 = new Matrix3(); + public var invInertiaWorld:Matrix3 = new Matrix3(); + public var baseMatrix:Matrix3 = new Matrix3(); + public var transform:Matrix4 = new Matrix4(); + + private var forceAccum:Vector3 = new Vector3(); + private var torqueAccum:Vector3 = new Vector3(); + + /** + * + * @param type + * @param invMass + * @param invInertia + */ + public function Body(type:int, invMass:Number, invInertia:Matrix3) { + this.type = type; + this.invMass = invMass; + this.invInertia.copy(invInertia); + } + + /** + * + */ + public function clearAccumulators():void { + forceAccum.reset(); + torqueAccum.reset(); + } + + /** + * + * @param f + */ + public function addForce(f:Vector3):void { + forceAccum.add(f); + } + + /** + * + * @param t + */ + public function addTorque(t:Vector3):void { + torqueAccum.add(t); + } + + /** + * + */ + public function calcAccelerations():void { + calcDerivedData(); + + accel.x = forceAccum.x*invMass; + accel.y = forceAccum.y*invMass; + accel.z = forceAccum.z*invMass; + + invInertiaWorld.transformVector(torqueAccum, angleAccel); + } + + /** + * Вычисление вспомогательной информации. + */ + public function calcDerivedData():void { + // Вычисление базисной матрицы и обратного тензора инерции в мировых координатах + state.orientation.toMatrix3(baseMatrix); + invInertiaWorld.copy(invInertia).append(baseMatrix).prepend(baseMatrix.transpose()); + transform.setFromMatrix3(baseMatrix.transpose(), state.pos); + } + + /** + * + * @param t + * @param result + */ + public function interpolate(t:Number, pos:Vector3, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.w = prevState.orientation.w*t1 + state.orientation.w*t; + orientation.x = prevState.orientation.x*t1 + state.orientation.x*t; + orientation.y = prevState.orientation.y*t1 + state.orientation.y*t; + orientation.z = prevState.orientation.z*t1 + state.orientation.z*t; + } + + /** + * + */ + public function saveState():void { + prevState.copy(state); + } + + /** + * + */ + public function restoreState():void { + state.copy(prevState); + } + + /** + * + * @param dt + */ + public function integrateVelocity(dt:Number):void { + // v = v + a*t + state.velocity.x += accel.x*dt; + state.velocity.y += accel.y*dt; + state.velocity.z += accel.z*dt; + // rot = rot + eps*t + state.rotation.x += angleAccel.x*dt; + state.rotation.y += angleAccel.y*dt; + state.rotation.z += angleAccel.z*dt; + + state.velocity.x *= linDamping; + state.velocity.y *= linDamping; + state.velocity.z *= linDamping; + + state.rotation.x *= rotDamping; + state.rotation.y *= rotDamping; + state.rotation.z *= rotDamping; + } + + /** + * + */ + public function integratePosition(dt:Number):void { + // pos = pos + v*t + state.pos.x += state.velocity.x*dt; + state.pos.y += state.velocity.y*dt; + state.pos.z += state.velocity.z*dt; + // q = q + 0.5*rot*q + state.orientation.addScaledVector(state.rotation, dt); + } + + public function setPosition(pos:Vector3):void { + state.pos.copy(pos); + } + + public function setPositionXYZ(x:Number, y:Number, z:Number):void { + state.pos.reset(x, y, z); + } + + public function setVelocity(vel:Vector3):void { + state.velocity.copy(vel); + } + + public function setVelocityXYZ(x:Number, y:Number, z:Number):void { + state.velocity.reset(x, y, z); + } + + public function setRotation(rot:Vector3):void { + state.rotation.copy(rot); + } + + public function setRotationXYZ(x:Number, y:Number, z:Number):void { + state.rotation.reset(x, y, z); + } + + public function setOrientation(q:Quaternion):void { + state.orientation.copy(q); + } + + private var _v:Vector3 = new Vector3(); + public function applyWorldImpulse(r:Vector3, dir:Vector3, magnitude:Number):void { + var d:Number = magnitude*invMass; + // Линейная часть + state.velocity.x += d*dir.x; + state.velocity.y += d*dir.y; + state.velocity.z += d*dir.z; + // Вращательная часть + var rx:Number = r.x - state.pos.x; + var ry:Number = r.y - state.pos.y; + var rz:Number = r.z - state.pos.z; + _v.x = (ry*dir.z - rz*dir.y)*magnitude; + _v.y = (rz*dir.x - rx*dir.z)*magnitude; + _v.z = (rx*dir.y - ry*dir.x)*magnitude; + _v.transformBy3(invInertiaWorld); + state.rotation.x += _v.x; + state.rotation.y += _v.y; + state.rotation.z += _v.z; + } + + } +} \ No newline at end of file diff --git a/0.1.SingleStep/src/alternativa/physics/rigid/BodyMaterial.as b/0.1.SingleStep/src/alternativa/physics/rigid/BodyMaterial.as new file mode 100644 index 0000000..61d4382 --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/rigid/BodyMaterial.as @@ -0,0 +1,11 @@ +package alternativa.physics.rigid { + + public class BodyMaterial { + + public var restitution:Number = 0; + public var friction:Number = 0.3; +// public var dynamicFriction:Number = 0.2; +// public var dynamicFriction:Number = 0.2; + + } +} \ No newline at end of file diff --git a/0.1.SingleStep/src/alternativa/physics/rigid/BodyState.as b/0.1.SingleStep/src/alternativa/physics/rigid/BodyState.as new file mode 100644 index 0000000..f0c95e9 --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/rigid/BodyState.as @@ -0,0 +1,39 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Quaternion; + import alternativa.physics.types.Vector3; + + /** + * Класс описывает состояние твёрдого тела. + */ + public class BodyState { + /** + * Положение тела. + */ + public var pos:Vector3 = new Vector3(); + /** + * Ориентация тела. + */ + public var orientation:Quaternion = new Quaternion(); + /** + * Скорость тела. + */ + public var velocity:Vector3 = new Vector3(); + /** + * Угловая скорость тела. + */ + public var rotation:Vector3 = new Vector3(); + + /** + * Копирует значение указанного объекта. + * + * @param state + */ + public function copy(state:BodyState):void { + pos.copy(state.pos); + orientation.copy(state.orientation); + velocity.copy(state.velocity); + rotation.copy(state.rotation); + } + + } +} \ No newline at end of file diff --git a/0.1.SingleStep/src/alternativa/physics/rigid/RigidWorld.as b/0.1.SingleStep/src/alternativa/physics/rigid/RigidWorld.as new file mode 100644 index 0000000..6384697 --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/rigid/RigidWorld.as @@ -0,0 +1,267 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.CollisionDetector; + import alternativa.physics.collision.CollisionInfo; + import alternativa.physics.collision.CollisionPoint; + import alternativa.physics.collision.ICollider; + import alternativa.physics.types.Vector3; + + + public class RigidWorld { + + private static var lastBodyId:int; + + public var penResolutionSteps:int = 10; + public var allowedPenetration:Number = 0.1; + public var maxPenResolutionSpeed:Number = 0.5; + public var collisionIterations:int = 5; + public var contactIterations:int = 5; + public var usePrediction:Boolean = false; + public var gravity:Vector3 = new Vector3(0, 0, -9.8); + + private var collider:ICollider = new CollisionDetector(); + + private var bodies:Vector. = new Vector.(); + private var bodyCount:int; + + public const MAX_COLLISIONS:int = 200; + public var collisions:Vector. = new Vector.(MAX_COLLISIONS, true); + public var colNum:int; + + /** + * + */ + public function RigidWorld() { + for (var i:int = 0; i < MAX_COLLISIONS; i++) collisions[i] = new CollisionInfo(); + } + + /** + * + * @param body + */ + public function addBody(body:Body):void { + bodies[bodyCount++] = body; + body.id = lastBodyId++; + } + + /** + * + * @param dt + */ + private function applyForces(dt:Number):void { + for (var i:int = 0; i < bodyCount; i++) { + var body:Body = bodies[i]; + body.calcAccelerations(); + if (body.invMass != 0) body.accel.add(gravity); + } + } + + /** + * + * @param dt + */ + private function detectCollisions(dt:Number):void { + var i:int; + var j:int; + var body:Body; + + for (i = 0; i < bodyCount; i++) { + body = bodies[i]; + body.saveState(); + if (usePrediction) { + body.integrateVelocity(dt); + body.integratePosition(dt); + body.calcDerivedData(); + } + } + + colNum = 0; + for (i = 0; i < bodyCount; i++) { + for (j = i + 1; j < bodyCount; j++) { + if (collider.collide(bodies[i], bodies[j], collisions[colNum])) colNum++; + } + } + + if (usePrediction) { + for (i = 0; i < bodyCount; i++) { + body = bodies[i]; + body.restoreState(); + body.calcDerivedData(); + } + } + } + + private var _r:Vector3 = new Vector3(); + private var _t:Vector3 = new Vector3(); + private var _v:Vector3 = new Vector3(); + private var _v1:Vector3 = new Vector3(); + private var _v2:Vector3 = new Vector3(); + /** + * + */ + private function preProcessCollisions(dt:Number):void { + for (var i:int = 0; i < colNum; i++) { + var colInfo:CollisionInfo = collisions[i]; + var b1:Body = colInfo.body1; + var b2:Body = colInfo.body2; + colInfo.restitution = b1.material.restitution < b2.material.restitution ? b1.material.restitution : b2.material.restitution; + colInfo.friction = b1.material.friction < b2.material.friction ? b1.material.friction : b2.material.friction; + for (var j:int = 0; j < colInfo.pcount; j++) { + var cp:CollisionPoint = colInfo.points[j]; + cp.accumImpulseN = 0; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + cp.velByUnitImpulseN = 0; + if (b1.invMass != 0) { + cp.r1.diff(cp.pos, b1.state.pos); + _v.cross2(cp.r1, colInfo.normal).transformBy3(b1.invInertiaWorld).cross(cp.r1); + cp.velByUnitImpulseN += b1.invMass + _v.dot(colInfo.normal); + } + if (b2.invMass != 0) { + cp.r2.diff(cp.pos, b2.state.pos); + _v.cross2(cp.r2, colInfo.normal).transformBy3(b2.invInertiaWorld).cross(cp.r2); + cp.velByUnitImpulseN += b2.invMass + _v.dot(colInfo.normal); + } + // Расчёт требуемой конечной скорости для упругого контакта + calcSepVelocity(b1, b2, cp, _v); + cp.normalVel = _v.dot(colInfo.normal); + if (cp.normalVel < 0) cp.normalVel = - colInfo.restitution*cp.normalVel; + // Скорость разделения неупругого контакта + cp.minSepVel = cp.penetration > allowedPenetration ? (cp.penetration - allowedPenetration)/(penResolutionSteps*dt) : 0; + if (cp.minSepVel > maxPenResolutionSpeed) cp.minSepVel = maxPenResolutionSpeed; + } + } + } + + /** + * + * @param dt + * @param forceInelastic + */ + private function processContacts(dt:Number, forceInelastic:Boolean):void { + var iterNum:int = forceInelastic ? contactIterations : collisionIterations; + + for (var iter:int = 0; iter < iterNum; iter++) { + for (var i:int = 0; i < colNum; i++) resolveCollision(collisions[i], forceInelastic); + } + + } + + /** + * + */ + private function resolveCollision(colInfo:CollisionInfo, forceInelastic:Boolean):void { + var b1:Body = colInfo.body1; + var b2:Body = colInfo.body2; + var normal:Vector3 = colInfo.normal; + var restitution:Number = forceInelastic ? 0 : colInfo.restitution; + + for (var i:int = 0; i < colInfo.pcount; i++) { + var cp:CollisionPoint = colInfo.points[i]; + var newVel:Number = 0; + calcSepVelocity(b1, b2, cp, _v); + var sepVel:Number = _v.dot(colInfo.normal); + + newVel = cp.normalVel + cp.minSepVel; + trace(cp.normalVel, cp.minSepVel); + +// if (forceInelastic) { +// if (sepVel < cp.minSepVel) newVel = cp.minSepVel; +// else newVel = 0; +// } else { +// newVel = cp.normalVel; +// } + var deltaVel:Number = newVel - sepVel; + var impulse:Number = deltaVel/cp.velByUnitImpulseN; + var accumImpulse:Number = cp.accumImpulseN + impulse; + if (accumImpulse < 0) accumImpulse = 0; + var deltaImpulse:Number = accumImpulse - cp.accumImpulseN; + cp.accumImpulseN = accumImpulse; + // Применяем импульс к телам + if (b1.invMass != 0) b1.applyWorldImpulse(cp.pos, normal, deltaImpulse); + if (b2.invMass != 0) b2.applyWorldImpulse(cp.pos, normal, -deltaImpulse); + + // Учёт силы трения + calcSepVelocity(b1, b2, cp, _v); + // Расчитываем изменение касательной скорости на единицу касательного импульса + var tanSpeedByUnitImpulse:Number = 0; + _v.addScaled(-_v.dot(colInfo.normal), colInfo.normal); + var tanSpeed:Number = _v.length(); + if (tanSpeed < 0.001) continue; + _t.copy(_v).normalize().reverse(); + // dV = b.invMass + ((invI * (r % t)) % r) * t + if (b1.invMass != 0) { + _v.cross2(cp.r1, _t).transformBy3(b1.invInertiaWorld).cross(cp.r1); + tanSpeedByUnitImpulse += b1.invMass + _v.dot(_t); + } + if (b2.invMass != 0) { + _v.cross2(cp.r2, _t).transformBy3(b2.invInertiaWorld).cross(cp.r2); + tanSpeedByUnitImpulse += b2.invMass + _v.dot(_t); + } + + var tanImpulse:Number = tanSpeed/tanSpeedByUnitImpulse; + var max:Number = colInfo.friction*cp.accumImpulseN; + if (max < 0) { + if (tanImpulse < max) tanImpulse = max; + } else { + if (tanImpulse > max) tanImpulse = max; + } + // Применяем импульс к телам + if (b1.invMass != 0) b1.applyWorldImpulse(cp.pos, _t, tanImpulse); + if (b2.invMass != 0) b2.applyWorldImpulse(cp.pos, _t, -tanImpulse); + + } + } + + /** + * + * @param cp + * @param normal + * @return + */ + private function calcSepVelocity(body1:Body, body2:Body, cp:CollisionPoint, result:Vector3):void { + // sepVel = (V1 - V2)*normal + result.x = result.y = result.z = 0; + // V1 = V1_c + w1%r1 + if (body1.invMass != 0) result.add(body1.state.velocity).add(_v1.cross2(body1.state.rotation, cp.r1)); + // V2 = V2_c + w2%r2 + if (body2.invMass != 0) result.subtract(body2.state.velocity).subtract(_v2.cross2(body2.state.rotation, cp.r2)); + } + + /** + * + * @param dt + */ + private function intergateVelocities(dt:Number):void { + for (var i:int = 0; i < bodyCount; i++) { + (bodies[i] as Body).integrateVelocity(dt); + } + } + + /** + * + * @param dt + */ + private function integratePositions(dt:Number):void { + for (var i:int = 0; i < bodyCount; i++) { + (bodies[i] as Body).integratePosition(dt); + } + } + + /** + * + * @param dt + */ + public function runPhysics(dt:Number):void { + applyForces(dt); + detectCollisions(dt); + preProcessCollisions(dt); + processContacts(dt, false); + intergateVelocities(dt); +// processContacts(dt, true); + integratePositions(dt); + } + + } +} \ No newline at end of file diff --git a/0.1.SingleStep/src/alternativa/physics/rigid/primitives/.svn/all-wcprops b/0.1.SingleStep/src/alternativa/physics/rigid/primitives/.svn/all-wcprops new file mode 100644 index 0000000..9fd34af --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/rigid/primitives/.svn/all-wcprops @@ -0,0 +1,23 @@ +K 25 +svn:wc:ra_dav:version-url +V 131 +/!svn/ver/10588/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.SingleStep/src/alternativa/physics/rigid/primitives +END +RigidPlane.as +K 25 +svn:wc:ra_dav:version-url +V 145 +/!svn/ver/10588/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.SingleStep/src/alternativa/physics/rigid/primitives/RigidPlane.as +END +RigidSphere.as +K 25 +svn:wc:ra_dav:version-url +V 146 +/!svn/ver/10588/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.SingleStep/src/alternativa/physics/rigid/primitives/RigidSphere.as +END +RigidBox.as +K 25 +svn:wc:ra_dav:version-url +V 143 +/!svn/ver/10588/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.SingleStep/src/alternativa/physics/rigid/primitives/RigidBox.as +END diff --git a/0.1.SingleStep/src/alternativa/physics/rigid/primitives/.svn/entries b/0.1.SingleStep/src/alternativa/physics/rigid/primitives/.svn/entries new file mode 100644 index 0000000..4c3157f --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/rigid/primitives/.svn/entries @@ -0,0 +1,64 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.SingleStep/src/alternativa/physics/rigid/primitives +http://svndev.alternativaplatform.com + + + +2009-04-07T08:34:06.342994Z +10587 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +RigidPlane.as +file + + + + +2010-10-28T04:32:39.000000Z +a6ee47f62583bb1b5de0309b227c5453 +2009-04-07T08:34:06.342994Z +10587 +mike + +RigidSphere.as +file + + + + +2010-10-28T04:32:39.000000Z +ff3d32c51595a8db028510bd35de8a3a +2009-04-07T08:34:06.342994Z +10587 +mike + +RigidBox.as +file + + + + +2010-10-28T04:32:39.000000Z +fc3ec43555fab0ecbeb2ec651e62d78b +2009-04-01T12:25:29.638016Z +10301 +mike + diff --git a/0.1.SingleStep/src/alternativa/physics/rigid/primitives/.svn/format b/0.1.SingleStep/src/alternativa/physics/rigid/primitives/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/rigid/primitives/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.1.SingleStep/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidBox.as.svn-base b/0.1.SingleStep/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidBox.as.svn-base new file mode 100644 index 0000000..123c43d --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidBox.as.svn-base @@ -0,0 +1,42 @@ +package alternativa.physics.rigid.primitives { + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + public class RigidBox extends Body { + + public var halfSize:Vector3 = new Vector3(); + + /** + * + * @param halfSize + * @param mass + */ + public function RigidBox(halfSize:Vector3, mass:Number) { + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + var m:Matrix3 = new Matrix3(); + if (mass == Infinity) { + mass = 0; + m.a = m.f = m.k = 0; + } else { + mass = 1/mass; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + m.a = 3*mass/(yy + zz); + m.f = 3*mass/(zz + xx); + m.k = 3*mass/(xx + yy); + } + super(Body.BOX, mass, m); + this.halfSize.copy(halfSize); + } + + } +} \ No newline at end of file diff --git a/0.1.SingleStep/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidPlane.as.svn-base b/0.1.SingleStep/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidPlane.as.svn-base new file mode 100644 index 0000000..50a375e --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidPlane.as.svn-base @@ -0,0 +1,18 @@ +package alternativa.physics.rigid.primitives { + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + public class RigidPlane extends Body { + + public var normal:Vector3 = new Vector3(); + public var offset:Number; + + public function RigidPlane() { + super(Body.PLANE, 0, Matrix3.ZERO); + this.normal.copy(Vector3.Z_AXIS); + this.offset = 0; + } + + } +} \ No newline at end of file diff --git a/0.1.SingleStep/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidSphere.as.svn-base b/0.1.SingleStep/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidSphere.as.svn-base new file mode 100644 index 0000000..f79cf5c --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidSphere.as.svn-base @@ -0,0 +1,25 @@ +package alternativa.physics.rigid.primitives { + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + + public class RigidSphere extends Body { + + public var r:Number; + + public function RigidSphere(radius:Number, mass:Number) { + var m:Matrix3; + if (mass == Infinity) { + mass = 0; + m = Matrix3.ZERO; + } else { + mass = 1/mass; + var r2:Number = radius*radius; + m = new Matrix3(); + m.a = m.f = m.k = 2.5*mass/r2; + } + super(Body.SPHERE, mass, m); + r = radius; + } + + } +} \ No newline at end of file diff --git a/0.1.SingleStep/src/alternativa/physics/rigid/primitives/RigidBox.as b/0.1.SingleStep/src/alternativa/physics/rigid/primitives/RigidBox.as new file mode 100644 index 0000000..123c43d --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/rigid/primitives/RigidBox.as @@ -0,0 +1,42 @@ +package alternativa.physics.rigid.primitives { + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + public class RigidBox extends Body { + + public var halfSize:Vector3 = new Vector3(); + + /** + * + * @param halfSize + * @param mass + */ + public function RigidBox(halfSize:Vector3, mass:Number) { + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + var m:Matrix3 = new Matrix3(); + if (mass == Infinity) { + mass = 0; + m.a = m.f = m.k = 0; + } else { + mass = 1/mass; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + m.a = 3*mass/(yy + zz); + m.f = 3*mass/(zz + xx); + m.k = 3*mass/(xx + yy); + } + super(Body.BOX, mass, m); + this.halfSize.copy(halfSize); + } + + } +} \ No newline at end of file diff --git a/0.1.SingleStep/src/alternativa/physics/rigid/primitives/RigidPlane.as b/0.1.SingleStep/src/alternativa/physics/rigid/primitives/RigidPlane.as new file mode 100644 index 0000000..50a375e --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/rigid/primitives/RigidPlane.as @@ -0,0 +1,18 @@ +package alternativa.physics.rigid.primitives { + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + public class RigidPlane extends Body { + + public var normal:Vector3 = new Vector3(); + public var offset:Number; + + public function RigidPlane() { + super(Body.PLANE, 0, Matrix3.ZERO); + this.normal.copy(Vector3.Z_AXIS); + this.offset = 0; + } + + } +} \ No newline at end of file diff --git a/0.1.SingleStep/src/alternativa/physics/rigid/primitives/RigidSphere.as b/0.1.SingleStep/src/alternativa/physics/rigid/primitives/RigidSphere.as new file mode 100644 index 0000000..f79cf5c --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/rigid/primitives/RigidSphere.as @@ -0,0 +1,25 @@ +package alternativa.physics.rigid.primitives { + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + + public class RigidSphere extends Body { + + public var r:Number; + + public function RigidSphere(radius:Number, mass:Number) { + var m:Matrix3; + if (mass == Infinity) { + mass = 0; + m = Matrix3.ZERO; + } else { + mass = 1/mass; + var r2:Number = radius*radius; + m = new Matrix3(); + m.a = m.f = m.k = 2.5*mass/r2; + } + super(Body.SPHERE, mass, m); + r = radius; + } + + } +} \ No newline at end of file diff --git a/0.1.SingleStep/src/alternativa/physics/types/.svn/all-wcprops b/0.1.SingleStep/src/alternativa/physics/types/.svn/all-wcprops new file mode 100644 index 0000000..7e1378f --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/types/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 120 +/!svn/ver/10588/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.SingleStep/src/alternativa/physics/types +END +Quaternion.as +K 25 +svn:wc:ra_dav:version-url +V 134 +/!svn/ver/10588/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.SingleStep/src/alternativa/physics/types/Quaternion.as +END +Vector3.as +K 25 +svn:wc:ra_dav:version-url +V 131 +/!svn/ver/10588/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.SingleStep/src/alternativa/physics/types/Vector3.as +END +Matrix3.as +K 25 +svn:wc:ra_dav:version-url +V 131 +/!svn/ver/10588/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.SingleStep/src/alternativa/physics/types/Matrix3.as +END +Matrix4.as +K 25 +svn:wc:ra_dav:version-url +V 131 +/!svn/ver/10588/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.SingleStep/src/alternativa/physics/types/Matrix4.as +END diff --git a/0.1.SingleStep/src/alternativa/physics/types/.svn/entries b/0.1.SingleStep/src/alternativa/physics/types/.svn/entries new file mode 100644 index 0000000..1619b2d --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/types/.svn/entries @@ -0,0 +1,76 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.SingleStep/src/alternativa/physics/types +http://svndev.alternativaplatform.com + + + +2009-04-07T08:34:06.342994Z +10587 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Quaternion.as +file + + + + +2010-10-28T04:32:39.000000Z +883ddfa95ffe7b085d4ced5e156f33e1 +2009-04-01T12:25:29.638016Z +10301 +mike + +Vector3.as +file + + + + +2010-10-28T04:32:39.000000Z +6371d17e0f6073f1cbc03312107117a6 +2009-04-07T08:34:06.342994Z +10587 +mike + +Matrix3.as +file + + + + +2010-10-28T04:32:39.000000Z +e74af4acaa4ea7e6b93a759e99c8457a +2009-04-07T08:34:06.342994Z +10587 +mike + +Matrix4.as +file + + + + +2010-10-28T04:32:39.000000Z +79dbaa2aae7d2741dd1c084bab6114dd +2009-04-07T08:34:06.342994Z +10587 +mike + diff --git a/0.1.SingleStep/src/alternativa/physics/types/.svn/format b/0.1.SingleStep/src/alternativa/physics/types/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/types/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.1.SingleStep/src/alternativa/physics/types/.svn/text-base/Matrix3.as.svn-base b/0.1.SingleStep/src/alternativa/physics/types/.svn/text-base/Matrix3.as.svn-base new file mode 100644 index 0000000..690c0f2 --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/types/.svn/text-base/Matrix3.as.svn-base @@ -0,0 +1,285 @@ +package alternativa.physics.types { + + /** + * + */ + public class Matrix3 { + + public static const ZERO:Matrix3 = new Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0); + public static const IDENTITY:Matrix3 = new Matrix3(); + + public var a:Number; + public var b:Number; + public var c:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix3(a:Number = 1, b:Number = 0, c:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1) { + this.a = a; + this.b = b; + this.c = c; + + this.e = e; + this.f = f; + this.g = g; + + this.i = i; + this.j = j; + this.k = k; + } + + /** + * + * @return + */ + public function toIdentity():Matrix3 { + a = f = k = 1; + b = c = e = g = i = j = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + var det:Number = 1/(-cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk); + + a = (ff*kk - gg*jj)*det; + b = (cc*jj - bb*kk)*det; + c = (bb*gg - cc*ff)*det; + e = (gg*ii - ee*kk)*det; + f = (aa*kk - cc*ii)*det; + g = (cc*ee - aa*gg)*det; + i = (ee*jj - ff*ii)*det; + j = (bb*ii - aa*jj)*det; + k = (aa*ff - bb*ee)*det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix3):Matrix3 { + a += m.a; + b += m.b; + c += m.c; + e += m.e; + f += m.f; + g += m.g; + i += m.i; + j += m.j; + k += m.k; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix3):Matrix3 { + a -= m.a; + b -= m.b; + c -= m.c; + e -= m.e; + f -= m.f; + g -= m.g; + i -= m.i; + j -= m.j; + k -= m.k; + + return this; + } + + /** + * + * @return + */ + public function transpose():Matrix3 { + var tmp:Number = b; + b = e; + e = tmp; + tmp = c; + c = i; + i = tmp; + tmp = g; + g = j; + j = tmp; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * + * @param matrix + */ + public function createSkewSymmetric(v:Vector3):Matrix3 { + a = f = k = 0; + b = -v.z; + c = v.y; + e = v.z; + g = -v.x; + i = -v.y; + j = v.x; + return this; + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(i:int, axis:Vector3):void { + switch (i) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + } + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix3):Matrix3 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix3 { + return new Matrix3(a, b, c, e, f, g, i, j, k); + } + + public function toString():String { + return "[Matrix3 (" + a + ", " + b + ", " + c + "), (" + e + ", " + f + ", " + g + "), (" + i + ", " + j + ", " + k + ")]"; + } + + } +} \ No newline at end of file diff --git a/0.1.SingleStep/src/alternativa/physics/types/.svn/text-base/Matrix4.as.svn-base b/0.1.SingleStep/src/alternativa/physics/types/.svn/text-base/Matrix4.as.svn-base new file mode 100644 index 0000000..9e873ef --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/types/.svn/text-base/Matrix4.as.svn-base @@ -0,0 +1,375 @@ +package alternativa.physics.types { + import __AS3__.vec.Vector; + + /** + * + */ + public class Matrix4 { + + public var a:Number; + public var b:Number; + public var c:Number; + public var d:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + public var h:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + public var l:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix4(a:Number = 1, b:Number = 0, c:Number = 0, d:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, h:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1, l:Number = 0) { + this.a = a; + this.b = b; + this.c = c; + this.d = h; + + this.e = e; + this.f = f; + this.g = g; + this.h = h; + + this.i = i; + this.j = j; + this.k = k; + this.l = l; + } + + /** + * + * @return + */ + public function toIdentity():Matrix4 { + a = f = k = 1; + b = c = e = g = i = j = d = h = l = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + var det:Number = -cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk; + + a = (-gg*jj + ff*kk)/det; + b = (cc*jj - bb*kk)/det; + c = (-cc*ff + bb*gg)/det; + d = (dd*gg*jj - cc*hh*jj - dd*ff*kk + bb*hh*kk + cc*ff*ll - bb*gg*ll)/det; + e = (gg*ii - ee*kk)/det; + f = (-cc*ii + aa*kk)/det; + g = (cc*ee - aa*gg)/det; + h = (cc*hh*ii - dd*gg*ii + dd*ee*kk - aa*hh*kk - cc*ee*ll + aa*gg*ll)/det; + i = (-ff*ii + ee*jj)/det; + j = (bb*ii - aa*jj)/det; + k = (-bb*ee + aa*ff)/det; + l = (dd*ff*ii - bb*hh*ii - dd*ee*jj + aa*hh*jj + bb*ee*ll - aa*ff*ll)/det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function prepend(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + d = m.a*dd + m.b*hh + m.c*ll + m.d; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + h = m.e*dd + m.f*hh + m.g*ll + m.h; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + l = m.i*dd + m.j*hh + m.k*ll + m.l; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function append(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + d = aa*m.d + bb*m.h + cc*m.l + dd; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + h = ee*m.d + ff*m.h + gg*m.l + hh; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + l = ii*m.d + jj*m.h + kk*m.l + ll; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix4):Matrix4 { + a += m.a; + b += m.b; + c += m.c; + d += m.d; + e += m.e; + f += m.f; + g += m.g; + h += m.h; + i += m.i; + j += m.j; + k += m.k; + l += m.l; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix4):Matrix4 { + a -= m.a; + b -= m.b; + c -= m.c; + d -= m.d; + e -= m.e; + f -= m.f; + g -= m.g; + h -= m.h; + i -= m.i; + j -= m.j; + k -= m.k; + l -= m.l; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectors(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverse(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(idx:int, axis:Vector3):void { + switch (idx) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + case 3: + axis.x = d; + axis.y = h; + axis.z = l; + return; + } + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix4):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = m.d; + e = m.e; + f = m.f; + g = m.g; + h = m.h; + i = m.i; + j = m.j; + k = m.k; + l = m.l; + + return this; + } + + public function setFromMatrix3(m:Matrix3, offset:Vector3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = offset.x; + e = m.e; + f = m.f; + g = m.g; + h = offset.y; + i = m.i; + j = m.j; + k = m.k; + l = offset.z; + + return this; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix4 { + return new Matrix4(a, b, c, d, e, f, g, h, i, j, k, l); + } + + public function toString():String { + return "[Matrix4 [" + a.toFixed(3) + " " + b.toFixed(3) + " " + c.toFixed(3) + " " + d.toFixed(3) + "] [" + e.toFixed(3) + " " + f.toFixed(3) + " " + g.toFixed(3) + " " + h.toFixed(3) + "] [" + i.toFixed(3) + " " + j.toFixed(3) + " " + k.toFixed(3) + " " + l.toFixed(3) + "]]"; + } + + } +} \ No newline at end of file diff --git a/0.1.SingleStep/src/alternativa/physics/types/.svn/text-base/Quaternion.as.svn-base b/0.1.SingleStep/src/alternativa/physics/types/.svn/text-base/Quaternion.as.svn-base new file mode 100644 index 0000000..c1574ce --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/types/.svn/text-base/Quaternion.as.svn-base @@ -0,0 +1,386 @@ +package alternativa.physics.types { + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + + public class Quaternion { + + public var w:Number; + public var x:Number; + public var y:Number; + public var z:Number; + + public static function multiply(q1:Quaternion, q2:Quaternion, result:Quaternion):void { + result.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z; + result.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y; + result.y = q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z; + result.z = q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x; + } + + public static function createFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngle(axis, angle); + return q; + } + + public static function createFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngleComponents(x, y, z, angle); + return q; + } + + public function Quaternion(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0) { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + } + + public function reset(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0):Quaternion { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + return this; + } + + public function normalize():Quaternion { + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * Умножает на указанный кватернион слева: this * q + * + * @param q множитель + */ + public function prepend(q:Quaternion):Quaternion { + var ww:Number = w*q.w - x*q.x - y*q.y - z*q.z; + var xx:Number = w*q.x + x*q.w + y*q.z - z*q.y; + var yy:Number = w*q.y + y*q.w + z*q.x - x*q.z; + var zz:Number = w*q.z + z*q.w + x*q.y - y*q.x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Умножает на указанный кватернион справа: q * this + * + * @param q множитель + */ + public function append(q:Quaternion):Quaternion { + var ww:Number = q.w*w - q.x*x - q.y*y - q.z*z; + var xx:Number = q.w*x + q.x*w + q.y*z - q.z*y; + var yy:Number = q.w*y + q.y*w + q.z*x - q.x*z; + var zz:Number = q.w*z + q.z*w + q.x*y - q.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * + * @param vector + */ + public function rotateByVector(v:Vector3):Quaternion { + var ww:Number = -v.x*x - v.y*y - v.z*z; + var xx:Number = v.x*w + v.y*z - v.z*y; + var yy:Number = v.y*w + v.z*x - v.x*z; + var zz:Number = v.z*w + v.x*y - v.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Добавляет вращение, приданное вектором угловой скорости за указанное время. + * + * @param v + * @param scale + */ + public function addScaledVector(v:Vector3, scale:Number):Quaternion { + var vx:Number = v.x*scale; + var vy:Number = v.y*scale; + var vz:Number = v.z*scale; + var ww:Number = -x*vx - y*vy - z*vz; + var xx:Number = vx*w + vy*z - vz*y; + var yy:Number = vy*w + vz*x - vx*z; + var zz:Number = vz*w + vx*y - vy*x; + w += 0.5*ww; + x += 0.5*xx; + y += 0.5*yy; + z += 0.5*zz; + // inlined normalize + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param m + * @return + */ + public function toMatrix3(m:Matrix3):Quaternion { + var qi2:Number = 2*x*x; + var qj2:Number = 2*y*y; + var qk2:Number = 2*z*z; + var qij:Number = 2*x*y; + var qjk:Number = 2*y*z; + var qki:Number = 2*z*x; + var qri:Number = 2*w*x; + var qrj:Number = 2*w*y; + var qrk:Number = 2*w*z; + + m.a = 1 - qj2 - qk2; + m.b = qij - qrk; + m.c = qki + qrj; + + m.e = qij + qrk; + m.f = 1 - qi2 - qk2; + m.g = qjk - qri; + + m.i = qki - qrj; + m.j = qjk + qri; + m.k = 1 - qi2 - qj2; + return this; + } + + /** + * + * @return + */ + public function length():Number { + return Math.sqrt(w*w + x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function lengthSqr():Number { + return w*w + x*x + y*y + z*z; + } + + /** + * + * @param axis + * @param angle + * @return + */ + public function setFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z); + x = axis.x*k; + y = axis.y*k; + z = axis.z*k; + return this; + } + + /** + * + * @param x + * @param y + * @param z + * @param angle + */ + public function setFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(x*x + y*y + z*z); + this.x = x*k; + this.y = y*k; + this.z = z*k; + return this; + } + + /** + * + * @param vector + */ + public function toAxisVector(v:Vector3 = null):Vector3 { + if (w < -1 || w > 1) { + normalize(); + } + if (v == null) { + v = new Vector3(); + } + if (w > -1 && w < 1) { + if (w == 0) { + v.x = x; + v.y = y; + v.z = z; + } else { + var angle:Number = 2*Math.acos(w); + var coeff:Number = 1/Math.sqrt(1 - w*w); + v.x = x*coeff*angle; + v.y = y*coeff*angle; + v.z = z*coeff*angle; + } + } else { + v.x = 0; + v.y = 0; + v.z = 0; + } + return v; + } + + /** + * + * @param rotations + */ + public function getEulerAngles(angles:Vector3):Vector3 { + var qi2:Number = 2*x*x; + var qj2:Number = 2*y*y; + var qk2:Number = 2*z*z; + var qij:Number = 2*x*y; + var qjk:Number = 2*y*z; + var qki:Number = 2*z*x; + var qri:Number = 2*w*x; + var qrj:Number = 2*w*y; + var qrk:Number = 2*w*z; + + var aa:Number = 1 - qj2 - qk2; + var bb:Number = qij - qrk; + var ee:Number = qij + qrk; + var ff:Number = 1 - qi2 - qk2; + var ii:Number = qki - qrj; + var jj:Number = qjk + qri; + var kk:Number = 1 - qi2 - qj2; + + if (-1 < ii && ii < 1) { + if (angles == null) { + angles = new Vector3(Math.atan2(jj, kk), -Math.asin(ii), Math.atan2(ee, aa)); + } else { + angles.x = Math.atan2(jj, kk); + angles.y = -Math.asin(ii); + angles.z = Math.atan2(ee, aa); + } + } else { + if (angles == null) { + angles = new Vector3(0, 0.5*((ii <= -1) ? Math.PI : -Math.PI), Math.atan2(-bb, ff)); + } else { + angles.x = 0; + angles.y = 0.5*((ii <= -1) ? Math.PI : -Math.PI); + angles.z = Math.atan2(-bb, ff); + } + } + return angles; + } + + /** + * + */ + public function conjugate():void { + x = -x; + y = -y; + z = -z; + } + + /** + * Выполняет линейную интерполяцию. + * + * @param q1 начало отрезка + * @param q2 конец отрезка + * @param t время, обычно задаётся в интервале [0, 1] + */ + public function nlerp(q1:Quaternion, q2:Quaternion, t:Number):Quaternion { + var d:Number = 1 - t; + w = q1.w*d + q2.w*t; + x = q1.x*d + q2.x*t; + y = q1.y*d + q2.y*t; + z = q1.z*d + q2.z*t; + // inlined normalize + d = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param q + * @return + */ + public function subtract(q:Quaternion):Quaternion { + w -= q.w; + x -= q.x; + y -= q.y; + z -= q.z; + return this; + } + + /** + * + * @param q1 + * @param q2 + * @return + */ + public function diff(q1:Quaternion, q2:Quaternion):Quaternion { + w = q2.w - q1.w; + x = q2.x - q1.x; + y = q2.y - q1.y; + z = q2.z - q1.z; + return this; + } + + /** + * + * @param q + * @return + */ + public function copy(q:Quaternion):Quaternion { + w = q.w; + x = q.x; + y = q.y; + z = q.z; + return this; + } + + /** + * + * @return + */ + public function clone():Quaternion { + return new Quaternion(w, x, y, z); + } + + /** + * + * @return + */ + public function toString():String { + return "[" + w + ", " + x + ", " + y + ", " + z + "]"; + } + } +} diff --git a/0.1.SingleStep/src/alternativa/physics/types/.svn/text-base/Vector3.as.svn-base b/0.1.SingleStep/src/alternativa/physics/types/.svn/text-base/Vector3.as.svn-base new file mode 100644 index 0000000..1ffca5e --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/types/.svn/text-base/Vector3.as.svn-base @@ -0,0 +1,313 @@ +package alternativa.physics.types { + + /** + * + */ + public class Vector3 { + + public static const ZERO:Vector3 = new Vector3(0, 0, 0); + public static const X_AXIS:Vector3 = new Vector3(1, 0, 0); + public static const Y_AXIS:Vector3 = new Vector3(0, 1, 0); + public static const Z_AXIS:Vector3 = new Vector3(0, 0, 1); + + public var x:Number; + public var y:Number; + public var z:Number; + + /** + * + * @param x + * @param y + * @param z + */ + public function Vector3(x:Number = 0, y:Number = 0, z:Number = 0) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * + * @return + */ + public function length():Number { + return Math.sqrt(x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function lengthSqr():Number { + return x*x + y*y + z*z; + } + + /** + * + * @param length + * @return + */ + public function setLength(length:Number):Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = length; + } else { + var k:Number = length/Math.sqrt(x*x + y*y + z*z); + x *= k; + y *= k; + z *= k; + } + return this; + } + + /** + * + * @return + */ + public function normalize():Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = 1; + } else { + var k:Number = Math.sqrt(d); + x /= k; + y /= k; + z /= k; + } + return this; + } + + /** + * Сложение координат. + * + * @param v точка, координаты которой прибавляются к собственным + */ + public function add(v:Vector3):Vector3 { + x += v.x; + y += v.y; + z += v.z; + return this; + } + + /** + * + * @param k + * @param v + * @return + */ + public function addScaled(k:Number, v:Vector3):Vector3 { + x += k*v.x; + y += k*v.y; + z += k*v.z; + return this; + } + + /** + * Вычитание координат. + * + * @param v точка, координаты которой вычитаются из собственных + */ + public function subtract(v:Vector3):Vector3 { + x -= v.x; + y -= v.y; + z -= v.z; + return this; + } + + /** + * Вычисляет сумму векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + * @return this + */ + public function sum(a:Vector3, b:Vector3):Vector3 { + x = a.x + b.x; + y = a.y + b.y; + z = a.z + b.z; + return this; + } + + /** + * Вычисление разности векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + */ + public function diff(a:Vector3, b:Vector3):Vector3 { + x = a.x - b.x; + y = a.y - b.y; + z = a.z - b.z; + return this; + } + + /** + * Умножение на скаляр. + * + * @param k число, на которое умножаются координаты + */ + public function scale(k:Number):Vector3 { + x *= k; + y *= k; + z *= k; + return this; + } + + /** + * Инвертирование вектора. + */ + public function reverse():Vector3 { + x = -x; + y = -y; + z = -z; + return this; + } + + /** + * + * @param v + * @return + */ + public function dot(v:Vector3):Number { + return x*v.x + y*v.y + z*v.z; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function cross(v:Vector3):Vector3 { + var xx:Number = y*v.z - z*v.y; + var yy:Number = z*v.x - x*v.z; + var zz:Number = x*v.y - y*v.x; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function cross2(a:Vector3, b:Vector3):Vector3 { + x = a.y*b.z - a.z*b.y; + y = a.z*b.x - a.x*b.z; + z = a.x*b.y - a.y*b.x; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function transformBy3(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * + * @param matrix + */ + public function transformBy3Tr(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function transformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz + m.d; + y = m.e*xx + m.f*yy + m.g*zz + m.h; + z = m.i*xx + m.j*yy + m.k*zz + m.l; + return this; + } + + /** + * + * @param m + * @return + */ + public function transformInverseBy4(m:Matrix4):Vector3 { + var xx:Number = x - m.d; + var yy:Number = y - m.h; + var zz:Number = z - m.l; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * + * @param m + */ + public function deltaTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * Установка координат. + */ + public function reset(x:Number = 0, y:Number = 0, z:Number = 0):Vector3 { + this.x = x; + this.y = y; + this.z = z; + return this; + } + + /** + * Копирование координат точки. + * + * @param v точка, координаты которой копируются + */ + public function copy(v:Vector3):Vector3 { + x = v.x; + y = v.y; + z = v.z; + return this; + } + + /** + * Клонирование точки. + * + * @return клонированная точка + */ + public function clone():Vector3 { + return new Vector3(x, y, z); + } + + public function toString():String { + return "[Vector (" + x.toFixed(3) + ", " + y.toFixed(3) + ", " + z.toFixed(3) + ")]"; + } + + } +} \ No newline at end of file diff --git a/0.1.SingleStep/src/alternativa/physics/types/Matrix3.as b/0.1.SingleStep/src/alternativa/physics/types/Matrix3.as new file mode 100644 index 0000000..690c0f2 --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/types/Matrix3.as @@ -0,0 +1,285 @@ +package alternativa.physics.types { + + /** + * + */ + public class Matrix3 { + + public static const ZERO:Matrix3 = new Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0); + public static const IDENTITY:Matrix3 = new Matrix3(); + + public var a:Number; + public var b:Number; + public var c:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix3(a:Number = 1, b:Number = 0, c:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1) { + this.a = a; + this.b = b; + this.c = c; + + this.e = e; + this.f = f; + this.g = g; + + this.i = i; + this.j = j; + this.k = k; + } + + /** + * + * @return + */ + public function toIdentity():Matrix3 { + a = f = k = 1; + b = c = e = g = i = j = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + var det:Number = 1/(-cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk); + + a = (ff*kk - gg*jj)*det; + b = (cc*jj - bb*kk)*det; + c = (bb*gg - cc*ff)*det; + e = (gg*ii - ee*kk)*det; + f = (aa*kk - cc*ii)*det; + g = (cc*ee - aa*gg)*det; + i = (ee*jj - ff*ii)*det; + j = (bb*ii - aa*jj)*det; + k = (aa*ff - bb*ee)*det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix3):Matrix3 { + a += m.a; + b += m.b; + c += m.c; + e += m.e; + f += m.f; + g += m.g; + i += m.i; + j += m.j; + k += m.k; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix3):Matrix3 { + a -= m.a; + b -= m.b; + c -= m.c; + e -= m.e; + f -= m.f; + g -= m.g; + i -= m.i; + j -= m.j; + k -= m.k; + + return this; + } + + /** + * + * @return + */ + public function transpose():Matrix3 { + var tmp:Number = b; + b = e; + e = tmp; + tmp = c; + c = i; + i = tmp; + tmp = g; + g = j; + j = tmp; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * + * @param matrix + */ + public function createSkewSymmetric(v:Vector3):Matrix3 { + a = f = k = 0; + b = -v.z; + c = v.y; + e = v.z; + g = -v.x; + i = -v.y; + j = v.x; + return this; + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(i:int, axis:Vector3):void { + switch (i) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + } + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix3):Matrix3 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix3 { + return new Matrix3(a, b, c, e, f, g, i, j, k); + } + + public function toString():String { + return "[Matrix3 (" + a + ", " + b + ", " + c + "), (" + e + ", " + f + ", " + g + "), (" + i + ", " + j + ", " + k + ")]"; + } + + } +} \ No newline at end of file diff --git a/0.1.SingleStep/src/alternativa/physics/types/Matrix4.as b/0.1.SingleStep/src/alternativa/physics/types/Matrix4.as new file mode 100644 index 0000000..9e873ef --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/types/Matrix4.as @@ -0,0 +1,375 @@ +package alternativa.physics.types { + import __AS3__.vec.Vector; + + /** + * + */ + public class Matrix4 { + + public var a:Number; + public var b:Number; + public var c:Number; + public var d:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + public var h:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + public var l:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix4(a:Number = 1, b:Number = 0, c:Number = 0, d:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, h:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1, l:Number = 0) { + this.a = a; + this.b = b; + this.c = c; + this.d = h; + + this.e = e; + this.f = f; + this.g = g; + this.h = h; + + this.i = i; + this.j = j; + this.k = k; + this.l = l; + } + + /** + * + * @return + */ + public function toIdentity():Matrix4 { + a = f = k = 1; + b = c = e = g = i = j = d = h = l = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + var det:Number = -cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk; + + a = (-gg*jj + ff*kk)/det; + b = (cc*jj - bb*kk)/det; + c = (-cc*ff + bb*gg)/det; + d = (dd*gg*jj - cc*hh*jj - dd*ff*kk + bb*hh*kk + cc*ff*ll - bb*gg*ll)/det; + e = (gg*ii - ee*kk)/det; + f = (-cc*ii + aa*kk)/det; + g = (cc*ee - aa*gg)/det; + h = (cc*hh*ii - dd*gg*ii + dd*ee*kk - aa*hh*kk - cc*ee*ll + aa*gg*ll)/det; + i = (-ff*ii + ee*jj)/det; + j = (bb*ii - aa*jj)/det; + k = (-bb*ee + aa*ff)/det; + l = (dd*ff*ii - bb*hh*ii - dd*ee*jj + aa*hh*jj + bb*ee*ll - aa*ff*ll)/det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function prepend(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + d = m.a*dd + m.b*hh + m.c*ll + m.d; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + h = m.e*dd + m.f*hh + m.g*ll + m.h; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + l = m.i*dd + m.j*hh + m.k*ll + m.l; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function append(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + d = aa*m.d + bb*m.h + cc*m.l + dd; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + h = ee*m.d + ff*m.h + gg*m.l + hh; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + l = ii*m.d + jj*m.h + kk*m.l + ll; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix4):Matrix4 { + a += m.a; + b += m.b; + c += m.c; + d += m.d; + e += m.e; + f += m.f; + g += m.g; + h += m.h; + i += m.i; + j += m.j; + k += m.k; + l += m.l; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix4):Matrix4 { + a -= m.a; + b -= m.b; + c -= m.c; + d -= m.d; + e -= m.e; + f -= m.f; + g -= m.g; + h -= m.h; + i -= m.i; + j -= m.j; + k -= m.k; + l -= m.l; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectors(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverse(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(idx:int, axis:Vector3):void { + switch (idx) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + case 3: + axis.x = d; + axis.y = h; + axis.z = l; + return; + } + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix4):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = m.d; + e = m.e; + f = m.f; + g = m.g; + h = m.h; + i = m.i; + j = m.j; + k = m.k; + l = m.l; + + return this; + } + + public function setFromMatrix3(m:Matrix3, offset:Vector3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = offset.x; + e = m.e; + f = m.f; + g = m.g; + h = offset.y; + i = m.i; + j = m.j; + k = m.k; + l = offset.z; + + return this; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix4 { + return new Matrix4(a, b, c, d, e, f, g, h, i, j, k, l); + } + + public function toString():String { + return "[Matrix4 [" + a.toFixed(3) + " " + b.toFixed(3) + " " + c.toFixed(3) + " " + d.toFixed(3) + "] [" + e.toFixed(3) + " " + f.toFixed(3) + " " + g.toFixed(3) + " " + h.toFixed(3) + "] [" + i.toFixed(3) + " " + j.toFixed(3) + " " + k.toFixed(3) + " " + l.toFixed(3) + "]]"; + } + + } +} \ No newline at end of file diff --git a/0.1.SingleStep/src/alternativa/physics/types/Quaternion.as b/0.1.SingleStep/src/alternativa/physics/types/Quaternion.as new file mode 100644 index 0000000..c1574ce --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/types/Quaternion.as @@ -0,0 +1,386 @@ +package alternativa.physics.types { + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + + public class Quaternion { + + public var w:Number; + public var x:Number; + public var y:Number; + public var z:Number; + + public static function multiply(q1:Quaternion, q2:Quaternion, result:Quaternion):void { + result.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z; + result.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y; + result.y = q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z; + result.z = q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x; + } + + public static function createFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngle(axis, angle); + return q; + } + + public static function createFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngleComponents(x, y, z, angle); + return q; + } + + public function Quaternion(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0) { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + } + + public function reset(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0):Quaternion { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + return this; + } + + public function normalize():Quaternion { + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * Умножает на указанный кватернион слева: this * q + * + * @param q множитель + */ + public function prepend(q:Quaternion):Quaternion { + var ww:Number = w*q.w - x*q.x - y*q.y - z*q.z; + var xx:Number = w*q.x + x*q.w + y*q.z - z*q.y; + var yy:Number = w*q.y + y*q.w + z*q.x - x*q.z; + var zz:Number = w*q.z + z*q.w + x*q.y - y*q.x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Умножает на указанный кватернион справа: q * this + * + * @param q множитель + */ + public function append(q:Quaternion):Quaternion { + var ww:Number = q.w*w - q.x*x - q.y*y - q.z*z; + var xx:Number = q.w*x + q.x*w + q.y*z - q.z*y; + var yy:Number = q.w*y + q.y*w + q.z*x - q.x*z; + var zz:Number = q.w*z + q.z*w + q.x*y - q.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * + * @param vector + */ + public function rotateByVector(v:Vector3):Quaternion { + var ww:Number = -v.x*x - v.y*y - v.z*z; + var xx:Number = v.x*w + v.y*z - v.z*y; + var yy:Number = v.y*w + v.z*x - v.x*z; + var zz:Number = v.z*w + v.x*y - v.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Добавляет вращение, приданное вектором угловой скорости за указанное время. + * + * @param v + * @param scale + */ + public function addScaledVector(v:Vector3, scale:Number):Quaternion { + var vx:Number = v.x*scale; + var vy:Number = v.y*scale; + var vz:Number = v.z*scale; + var ww:Number = -x*vx - y*vy - z*vz; + var xx:Number = vx*w + vy*z - vz*y; + var yy:Number = vy*w + vz*x - vx*z; + var zz:Number = vz*w + vx*y - vy*x; + w += 0.5*ww; + x += 0.5*xx; + y += 0.5*yy; + z += 0.5*zz; + // inlined normalize + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param m + * @return + */ + public function toMatrix3(m:Matrix3):Quaternion { + var qi2:Number = 2*x*x; + var qj2:Number = 2*y*y; + var qk2:Number = 2*z*z; + var qij:Number = 2*x*y; + var qjk:Number = 2*y*z; + var qki:Number = 2*z*x; + var qri:Number = 2*w*x; + var qrj:Number = 2*w*y; + var qrk:Number = 2*w*z; + + m.a = 1 - qj2 - qk2; + m.b = qij - qrk; + m.c = qki + qrj; + + m.e = qij + qrk; + m.f = 1 - qi2 - qk2; + m.g = qjk - qri; + + m.i = qki - qrj; + m.j = qjk + qri; + m.k = 1 - qi2 - qj2; + return this; + } + + /** + * + * @return + */ + public function length():Number { + return Math.sqrt(w*w + x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function lengthSqr():Number { + return w*w + x*x + y*y + z*z; + } + + /** + * + * @param axis + * @param angle + * @return + */ + public function setFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z); + x = axis.x*k; + y = axis.y*k; + z = axis.z*k; + return this; + } + + /** + * + * @param x + * @param y + * @param z + * @param angle + */ + public function setFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(x*x + y*y + z*z); + this.x = x*k; + this.y = y*k; + this.z = z*k; + return this; + } + + /** + * + * @param vector + */ + public function toAxisVector(v:Vector3 = null):Vector3 { + if (w < -1 || w > 1) { + normalize(); + } + if (v == null) { + v = new Vector3(); + } + if (w > -1 && w < 1) { + if (w == 0) { + v.x = x; + v.y = y; + v.z = z; + } else { + var angle:Number = 2*Math.acos(w); + var coeff:Number = 1/Math.sqrt(1 - w*w); + v.x = x*coeff*angle; + v.y = y*coeff*angle; + v.z = z*coeff*angle; + } + } else { + v.x = 0; + v.y = 0; + v.z = 0; + } + return v; + } + + /** + * + * @param rotations + */ + public function getEulerAngles(angles:Vector3):Vector3 { + var qi2:Number = 2*x*x; + var qj2:Number = 2*y*y; + var qk2:Number = 2*z*z; + var qij:Number = 2*x*y; + var qjk:Number = 2*y*z; + var qki:Number = 2*z*x; + var qri:Number = 2*w*x; + var qrj:Number = 2*w*y; + var qrk:Number = 2*w*z; + + var aa:Number = 1 - qj2 - qk2; + var bb:Number = qij - qrk; + var ee:Number = qij + qrk; + var ff:Number = 1 - qi2 - qk2; + var ii:Number = qki - qrj; + var jj:Number = qjk + qri; + var kk:Number = 1 - qi2 - qj2; + + if (-1 < ii && ii < 1) { + if (angles == null) { + angles = new Vector3(Math.atan2(jj, kk), -Math.asin(ii), Math.atan2(ee, aa)); + } else { + angles.x = Math.atan2(jj, kk); + angles.y = -Math.asin(ii); + angles.z = Math.atan2(ee, aa); + } + } else { + if (angles == null) { + angles = new Vector3(0, 0.5*((ii <= -1) ? Math.PI : -Math.PI), Math.atan2(-bb, ff)); + } else { + angles.x = 0; + angles.y = 0.5*((ii <= -1) ? Math.PI : -Math.PI); + angles.z = Math.atan2(-bb, ff); + } + } + return angles; + } + + /** + * + */ + public function conjugate():void { + x = -x; + y = -y; + z = -z; + } + + /** + * Выполняет линейную интерполяцию. + * + * @param q1 начало отрезка + * @param q2 конец отрезка + * @param t время, обычно задаётся в интервале [0, 1] + */ + public function nlerp(q1:Quaternion, q2:Quaternion, t:Number):Quaternion { + var d:Number = 1 - t; + w = q1.w*d + q2.w*t; + x = q1.x*d + q2.x*t; + y = q1.y*d + q2.y*t; + z = q1.z*d + q2.z*t; + // inlined normalize + d = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param q + * @return + */ + public function subtract(q:Quaternion):Quaternion { + w -= q.w; + x -= q.x; + y -= q.y; + z -= q.z; + return this; + } + + /** + * + * @param q1 + * @param q2 + * @return + */ + public function diff(q1:Quaternion, q2:Quaternion):Quaternion { + w = q2.w - q1.w; + x = q2.x - q1.x; + y = q2.y - q1.y; + z = q2.z - q1.z; + return this; + } + + /** + * + * @param q + * @return + */ + public function copy(q:Quaternion):Quaternion { + w = q.w; + x = q.x; + y = q.y; + z = q.z; + return this; + } + + /** + * + * @return + */ + public function clone():Quaternion { + return new Quaternion(w, x, y, z); + } + + /** + * + * @return + */ + public function toString():String { + return "[" + w + ", " + x + ", " + y + ", " + z + "]"; + } + } +} diff --git a/0.1.SingleStep/src/alternativa/physics/types/Vector3.as b/0.1.SingleStep/src/alternativa/physics/types/Vector3.as new file mode 100644 index 0000000..1ffca5e --- /dev/null +++ b/0.1.SingleStep/src/alternativa/physics/types/Vector3.as @@ -0,0 +1,313 @@ +package alternativa.physics.types { + + /** + * + */ + public class Vector3 { + + public static const ZERO:Vector3 = new Vector3(0, 0, 0); + public static const X_AXIS:Vector3 = new Vector3(1, 0, 0); + public static const Y_AXIS:Vector3 = new Vector3(0, 1, 0); + public static const Z_AXIS:Vector3 = new Vector3(0, 0, 1); + + public var x:Number; + public var y:Number; + public var z:Number; + + /** + * + * @param x + * @param y + * @param z + */ + public function Vector3(x:Number = 0, y:Number = 0, z:Number = 0) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * + * @return + */ + public function length():Number { + return Math.sqrt(x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function lengthSqr():Number { + return x*x + y*y + z*z; + } + + /** + * + * @param length + * @return + */ + public function setLength(length:Number):Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = length; + } else { + var k:Number = length/Math.sqrt(x*x + y*y + z*z); + x *= k; + y *= k; + z *= k; + } + return this; + } + + /** + * + * @return + */ + public function normalize():Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = 1; + } else { + var k:Number = Math.sqrt(d); + x /= k; + y /= k; + z /= k; + } + return this; + } + + /** + * Сложение координат. + * + * @param v точка, координаты которой прибавляются к собственным + */ + public function add(v:Vector3):Vector3 { + x += v.x; + y += v.y; + z += v.z; + return this; + } + + /** + * + * @param k + * @param v + * @return + */ + public function addScaled(k:Number, v:Vector3):Vector3 { + x += k*v.x; + y += k*v.y; + z += k*v.z; + return this; + } + + /** + * Вычитание координат. + * + * @param v точка, координаты которой вычитаются из собственных + */ + public function subtract(v:Vector3):Vector3 { + x -= v.x; + y -= v.y; + z -= v.z; + return this; + } + + /** + * Вычисляет сумму векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + * @return this + */ + public function sum(a:Vector3, b:Vector3):Vector3 { + x = a.x + b.x; + y = a.y + b.y; + z = a.z + b.z; + return this; + } + + /** + * Вычисление разности векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + */ + public function diff(a:Vector3, b:Vector3):Vector3 { + x = a.x - b.x; + y = a.y - b.y; + z = a.z - b.z; + return this; + } + + /** + * Умножение на скаляр. + * + * @param k число, на которое умножаются координаты + */ + public function scale(k:Number):Vector3 { + x *= k; + y *= k; + z *= k; + return this; + } + + /** + * Инвертирование вектора. + */ + public function reverse():Vector3 { + x = -x; + y = -y; + z = -z; + return this; + } + + /** + * + * @param v + * @return + */ + public function dot(v:Vector3):Number { + return x*v.x + y*v.y + z*v.z; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function cross(v:Vector3):Vector3 { + var xx:Number = y*v.z - z*v.y; + var yy:Number = z*v.x - x*v.z; + var zz:Number = x*v.y - y*v.x; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function cross2(a:Vector3, b:Vector3):Vector3 { + x = a.y*b.z - a.z*b.y; + y = a.z*b.x - a.x*b.z; + z = a.x*b.y - a.y*b.x; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function transformBy3(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * + * @param matrix + */ + public function transformBy3Tr(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function transformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz + m.d; + y = m.e*xx + m.f*yy + m.g*zz + m.h; + z = m.i*xx + m.j*yy + m.k*zz + m.l; + return this; + } + + /** + * + * @param m + * @return + */ + public function transformInverseBy4(m:Matrix4):Vector3 { + var xx:Number = x - m.d; + var yy:Number = y - m.h; + var zz:Number = z - m.l; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * + * @param m + */ + public function deltaTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * Установка координат. + */ + public function reset(x:Number = 0, y:Number = 0, z:Number = 0):Vector3 { + this.x = x; + this.y = y; + this.z = z; + return this; + } + + /** + * Копирование координат точки. + * + * @param v точка, координаты которой копируются + */ + public function copy(v:Vector3):Vector3 { + x = v.x; + y = v.y; + z = v.z; + return this; + } + + /** + * Клонирование точки. + * + * @return клонированная точка + */ + public function clone():Vector3 { + return new Vector3(x, y, z); + } + + public function toString():String { + return "[Vector (" + x.toFixed(3) + ", " + y.toFixed(3) + ", " + z.toFixed(3) + ")]"; + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/.actionScriptProperties b/0.1.Vector3D/.actionScriptProperties new file mode 100644 index 0000000..f73adc8 --- /dev/null +++ b/0.1.Vector3D/.actionScriptProperties @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/0.1.Vector3D/.flexLibProperties b/0.1.Vector3D/.flexLibProperties new file mode 100644 index 0000000..2083d0b --- /dev/null +++ b/0.1.Vector3D/.flexLibProperties @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/0.1.Vector3D/.project b/0.1.Vector3D/.project new file mode 100644 index 0000000..1f1425f --- /dev/null +++ b/0.1.Vector3D/.project @@ -0,0 +1,19 @@ + + + AlternativaPhysics + + + + + + com.adobe.flexbuilder.project.flexbuilder + + + + + + org.maven.ide.eclipse.maven2Nature + com.adobe.flexbuilder.project.flexlibnature + com.adobe.flexbuilder.project.actionscriptnature + + diff --git a/0.1.Vector3D/.settings/.svn/all-wcprops b/0.1.Vector3D/.settings/.svn/all-wcprops new file mode 100644 index 0000000..f47d15b --- /dev/null +++ b/0.1.Vector3D/.settings/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 98 +/!svn/ver/13892/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/.settings +END +org.eclipse.core.resources.prefs +K 25 +svn:wc:ra_dav:version-url +V 131 +/!svn/ver/13892/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/.settings/org.eclipse.core.resources.prefs +END diff --git a/0.1.Vector3D/.settings/.svn/entries b/0.1.Vector3D/.settings/.svn/entries new file mode 100644 index 0000000..f1d9be9 --- /dev/null +++ b/0.1.Vector3D/.settings/.svn/entries @@ -0,0 +1,40 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/.settings +http://svndev.alternativaplatform.com + + + +2009-04-01T12:25:29.638016Z +10301 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +org.eclipse.core.resources.prefs +file + + + + +2010-10-28T04:32:39.000000Z +4f70b476f3e5075e399505021df2f89b +2009-04-01T12:25:29.638016Z +10301 +mike + diff --git a/0.1.Vector3D/.settings/.svn/format b/0.1.Vector3D/.settings/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.1.Vector3D/.settings/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.1.Vector3D/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base b/0.1.Vector3D/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base new file mode 100644 index 0000000..b3d0d49 --- /dev/null +++ b/0.1.Vector3D/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base @@ -0,0 +1,3 @@ +#Fri Mar 20 00:19:33 YEKT 2009 +eclipse.preferences.version=1 +encoding/=UTF-8 diff --git a/0.1.Vector3D/.settings/org.eclipse.core.resources.prefs b/0.1.Vector3D/.settings/org.eclipse.core.resources.prefs new file mode 100644 index 0000000..b3d0d49 --- /dev/null +++ b/0.1.Vector3D/.settings/org.eclipse.core.resources.prefs @@ -0,0 +1,3 @@ +#Fri Mar 20 00:19:33 YEKT 2009 +eclipse.preferences.version=1 +encoding/=UTF-8 diff --git a/0.1.Vector3D/.svn/all-wcprops b/0.1.Vector3D/.svn/all-wcprops new file mode 100644 index 0000000..fb081f1 --- /dev/null +++ b/0.1.Vector3D/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 88 +/!svn/ver/13893/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D +END +.flexLibProperties +K 25 +svn:wc:ra_dav:version-url +V 107 +/!svn/ver/13892/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/.flexLibProperties +END +.project +K 25 +svn:wc:ra_dav:version-url +V 97 +/!svn/ver/13892/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/.project +END +pom.xml +K 25 +svn:wc:ra_dav:version-url +V 96 +/!svn/ver/13892/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/pom.xml +END +.actionScriptProperties +K 25 +svn:wc:ra_dav:version-url +V 112 +/!svn/ver/13892/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/.actionScriptProperties +END diff --git a/0.1.Vector3D/.svn/entries b/0.1.Vector3D/.svn/entries new file mode 100644 index 0000000..0cf008f --- /dev/null +++ b/0.1.Vector3D/.svn/entries @@ -0,0 +1,85 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D +http://svndev.alternativaplatform.com + + + +2009-06-03T09:16:50.564913Z +13893 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +META-INF +dir + +.flexLibProperties +file + + + + +2010-10-28T04:32:39.000000Z +fcef7ca7fe61a3459fb00858c28adec1 +2009-05-26T08:41:33.471001Z +13389 +mike + +.project +file + + + + +2010-10-28T04:32:39.000000Z +1e90cc68b52b93173b2b5de88eb5c3a3 +2009-04-20T17:15:55.260110Z +11541 +mike + +src +dir + +pom.xml +file + + + + +2010-10-28T04:32:39.000000Z +b59717ccb49cb1b40abb158289fdc507 +2009-04-20T17:15:55.260110Z +11541 +mike + +.actionScriptProperties +file + + + + +2010-10-28T04:32:39.000000Z +b8656f882fff2baedd57a92db86fcf8c +2009-04-01T12:25:29.638016Z +10301 +mike + +.settings +dir + diff --git a/0.1.Vector3D/.svn/format b/0.1.Vector3D/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.1.Vector3D/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.1.Vector3D/.svn/text-base/.actionScriptProperties.svn-base b/0.1.Vector3D/.svn/text-base/.actionScriptProperties.svn-base new file mode 100644 index 0000000..f73adc8 --- /dev/null +++ b/0.1.Vector3D/.svn/text-base/.actionScriptProperties.svn-base @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/0.1.Vector3D/.svn/text-base/.flexLibProperties.svn-base b/0.1.Vector3D/.svn/text-base/.flexLibProperties.svn-base new file mode 100644 index 0000000..2083d0b --- /dev/null +++ b/0.1.Vector3D/.svn/text-base/.flexLibProperties.svn-base @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/0.1.Vector3D/.svn/text-base/.project.svn-base b/0.1.Vector3D/.svn/text-base/.project.svn-base new file mode 100644 index 0000000..1f1425f --- /dev/null +++ b/0.1.Vector3D/.svn/text-base/.project.svn-base @@ -0,0 +1,19 @@ + + + AlternativaPhysics + + + + + + com.adobe.flexbuilder.project.flexbuilder + + + + + + org.maven.ide.eclipse.maven2Nature + com.adobe.flexbuilder.project.flexlibnature + com.adobe.flexbuilder.project.actionscriptnature + + diff --git a/0.1.Vector3D/.svn/text-base/pom.xml.svn-base b/0.1.Vector3D/.svn/text-base/pom.xml.svn-base new file mode 100644 index 0000000..20baf0b --- /dev/null +++ b/0.1.Vector3D/.svn/text-base/pom.xml.svn-base @@ -0,0 +1,17 @@ + + 4.0.0 + platform.clients.fp10.libraries + AlternativaPhysics + swc + 0.0.1.0-SNAPSHOT + + platform.tools.maven + FlashBasePom + 1.0 + + + scm:svn:http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/trunk + + + + \ No newline at end of file diff --git a/0.1.Vector3D/META-INF/.svn/all-wcprops b/0.1.Vector3D/META-INF/.svn/all-wcprops new file mode 100644 index 0000000..5bf54ff --- /dev/null +++ b/0.1.Vector3D/META-INF/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 97 +/!svn/ver/13892/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/META-INF +END +MANIFEST.MF +K 25 +svn:wc:ra_dav:version-url +V 109 +/!svn/ver/13892/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/META-INF/MANIFEST.MF +END diff --git a/0.1.Vector3D/META-INF/.svn/entries b/0.1.Vector3D/META-INF/.svn/entries new file mode 100644 index 0000000..3a43f11 --- /dev/null +++ b/0.1.Vector3D/META-INF/.svn/entries @@ -0,0 +1,40 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/META-INF +http://svndev.alternativaplatform.com + + + +2009-04-20T17:15:55.260110Z +11541 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +MANIFEST.MF +file + + + + +2010-10-28T04:32:38.000000Z +7b64dceb50a6905850ff00a0bfbe77eb +2009-04-20T17:15:55.260110Z +11541 +mike + diff --git a/0.1.Vector3D/META-INF/.svn/format b/0.1.Vector3D/META-INF/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.1.Vector3D/META-INF/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.1.Vector3D/META-INF/.svn/text-base/MANIFEST.MF.svn-base b/0.1.Vector3D/META-INF/.svn/text-base/MANIFEST.MF.svn-base new file mode 100644 index 0000000..5066a55 --- /dev/null +++ b/0.1.Vector3D/META-INF/.svn/text-base/MANIFEST.MF.svn-base @@ -0,0 +1 @@ +Bundle-Name: platform.clients.fp10.libraries.AlternativaPhysics diff --git a/0.1.Vector3D/META-INF/MANIFEST.MF b/0.1.Vector3D/META-INF/MANIFEST.MF new file mode 100644 index 0000000..5066a55 --- /dev/null +++ b/0.1.Vector3D/META-INF/MANIFEST.MF @@ -0,0 +1 @@ +Bundle-Name: platform.clients.fp10.libraries.AlternativaPhysics diff --git a/0.1.Vector3D/pom.xml b/0.1.Vector3D/pom.xml new file mode 100644 index 0000000..20baf0b --- /dev/null +++ b/0.1.Vector3D/pom.xml @@ -0,0 +1,17 @@ + + 4.0.0 + platform.clients.fp10.libraries + AlternativaPhysics + swc + 0.0.1.0-SNAPSHOT + + platform.tools.maven + FlashBasePom + 1.0 + + + scm:svn:http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/trunk + + + + \ No newline at end of file diff --git a/0.1.Vector3D/src/.svn/all-wcprops b/0.1.Vector3D/src/.svn/all-wcprops new file mode 100644 index 0000000..911ae7e --- /dev/null +++ b/0.1.Vector3D/src/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 92 +/!svn/ver/13893/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src +END diff --git a/0.1.Vector3D/src/.svn/entries b/0.1.Vector3D/src/.svn/entries new file mode 100644 index 0000000..a3a435b --- /dev/null +++ b/0.1.Vector3D/src/.svn/entries @@ -0,0 +1,31 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src +http://svndev.alternativaplatform.com + + + +2009-06-03T09:16:50.564913Z +13893 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +alternativa +dir + diff --git a/0.1.Vector3D/src/.svn/format b/0.1.Vector3D/src/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.1.Vector3D/src/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.1.Vector3D/src/alternativa/.svn/all-wcprops b/0.1.Vector3D/src/alternativa/.svn/all-wcprops new file mode 100644 index 0000000..72db65f --- /dev/null +++ b/0.1.Vector3D/src/alternativa/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 104 +/!svn/ver/13893/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa +END diff --git a/0.1.Vector3D/src/alternativa/.svn/entries b/0.1.Vector3D/src/alternativa/.svn/entries new file mode 100644 index 0000000..181f448 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/.svn/entries @@ -0,0 +1,31 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa +http://svndev.alternativaplatform.com + + + +2009-06-03T09:16:50.564913Z +13893 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +physics +dir + diff --git a/0.1.Vector3D/src/alternativa/.svn/format b/0.1.Vector3D/src/alternativa/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.1.Vector3D/src/alternativa/physics/.svn/all-wcprops b/0.1.Vector3D/src/alternativa/physics/.svn/all-wcprops new file mode 100644 index 0000000..3f64638 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 112 +/!svn/ver/13893/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics +END +altphysics.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/13892/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/altphysics.as +END diff --git a/0.1.Vector3D/src/alternativa/physics/.svn/entries b/0.1.Vector3D/src/alternativa/physics/.svn/entries new file mode 100644 index 0000000..503bef0 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/.svn/entries @@ -0,0 +1,49 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics +http://svndev.alternativaplatform.com + + + +2009-06-03T09:16:50.564913Z +13893 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +types +dir + +altphysics.as +file + + + + +2010-10-28T04:32:39.000000Z +04fbe40a27502dbbd0dba02a76b2df50 +2009-04-20T20:09:14.715403Z +11579 +mike + +collision +dir + +rigid +dir + diff --git a/0.1.Vector3D/src/alternativa/physics/.svn/format b/0.1.Vector3D/src/alternativa/physics/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.1.Vector3D/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base b/0.1.Vector3D/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base new file mode 100644 index 0000000..2715307 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/.svn/text-base/altphysics.as.svn-base @@ -0,0 +1,3 @@ +package alternativa.physics { + public namespace altphysics = "http://alternativaplatform.com/en/altphysics"; +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/altphysics.as b/0.1.Vector3D/src/alternativa/physics/altphysics.as new file mode 100644 index 0000000..2715307 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/altphysics.as @@ -0,0 +1,3 @@ +package alternativa.physics { + public namespace altphysics = "http://alternativaplatform.com/en/altphysics"; +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/collision/.svn/all-wcprops b/0.1.Vector3D/src/alternativa/physics/collision/.svn/all-wcprops new file mode 100644 index 0000000..21a04cf --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/.svn/all-wcprops @@ -0,0 +1,95 @@ +K 25 +svn:wc:ra_dav:version-url +V 122 +/!svn/ver/13893/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/collision +END +BoxSphereCollider.as +K 25 +svn:wc:ra_dav:version-url +V 143 +/!svn/ver/13893/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/collision/BoxSphereCollider.as +END +BoxBoxCollider.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/13893/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/collision/BoxBoxCollider.as +END +CollisionKdNode.as +K 25 +svn:wc:ra_dav:version-url +V 141 +/!svn/ver/13892/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/collision/CollisionKdNode.as +END +SpherePlaneCollider.as +K 25 +svn:wc:ra_dav:version-url +V 145 +/!svn/ver/13892/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/collision/SpherePlaneCollider.as +END +KdTreeCollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 149 +/!svn/ver/13892/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/collision/KdTreeCollisionDetector.as +END +BoxRectCollider.as +K 25 +svn:wc:ra_dav:version-url +V 141 +/!svn/ver/13893/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/collision/BoxRectCollider.as +END +ICollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 145 +/!svn/ver/13892/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/collision/ICollisionPredicate.as +END +BoxPlaneCollider.as +K 25 +svn:wc:ra_dav:version-url +V 142 +/!svn/ver/13892/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/collision/BoxPlaneCollider.as +END +CollisionKdTree.as +K 25 +svn:wc:ra_dav:version-url +V 141 +/!svn/ver/13892/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/collision/CollisionKdTree.as +END +BoxTriangleCollider.as +K 25 +svn:wc:ra_dav:version-url +V 145 +/!svn/ver/13892/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/collision/BoxTriangleCollider.as +END +ICollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 144 +/!svn/ver/13892/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/collision/ICollisionDetector.as +END +IRayCollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 148 +/!svn/ver/13892/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/collision/IRayCollisionPredicate.as +END +ICollider.as +K 25 +svn:wc:ra_dav:version-url +V 135 +/!svn/ver/13892/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/collision/ICollider.as +END +BruteForceCollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 153 +/!svn/ver/13892/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/collision/BruteForceCollisionDetector.as +END +SphereSphereCollider.as +K 25 +svn:wc:ra_dav:version-url +V 146 +/!svn/ver/13893/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/collision/SphereSphereCollider.as +END diff --git a/0.1.Vector3D/src/alternativa/physics/collision/.svn/entries b/0.1.Vector3D/src/alternativa/physics/collision/.svn/entries new file mode 100644 index 0000000..609dc92 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/.svn/entries @@ -0,0 +1,214 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/collision +http://svndev.alternativaplatform.com + + + +2009-06-03T09:16:50.564913Z +13893 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +BoxSphereCollider.as +file + + + + +2010-10-28T04:32:39.000000Z +ad1dc1849019ac159d22cd93b1ba38d3 +2009-06-03T09:16:50.564913Z +13893 +mike + +BoxBoxCollider.as +file + + + + +2010-10-28T04:32:39.000000Z +e4eb46370e3c7098fa35d0da4e346d09 +2009-06-03T09:16:50.564913Z +13893 +mike + +CollisionKdNode.as +file + + + + +2010-10-28T04:32:39.000000Z +7952384e785be1fca93657d94121b246 +2009-04-17T17:39:21.367569Z +11341 +mike + +BoxRectCollider.as +file + + + + +2010-10-28T04:32:39.000000Z +8554254b88a8ecc31bf005fdee676a67 +2009-06-03T09:16:50.564913Z +13893 +mike + +KdTreeCollisionDetector.as +file + + + + +2010-10-28T04:32:39.000000Z +579f909f9a1c0398918d2c9a16d3150e +2009-05-18T16:37:02.465501Z +13087 +mike + +SpherePlaneCollider.as +file + + + + +2010-10-28T04:32:39.000000Z +7fbbf70eef5caa9a09ce679819429874 +2009-04-13T03:07:43.422558Z +11019 +mike + +ICollisionPredicate.as +file + + + + +2010-10-28T04:32:39.000000Z +866471ea582e95e46e94eb7ae437c827 +2009-04-26T15:39:06.673206Z +11995 +mike + +BoxPlaneCollider.as +file + + + + +2010-10-28T04:32:39.000000Z +4344df47c2b9c1873d2dc711815b4737 +2009-04-13T03:07:43.422558Z +11019 +mike + +primitives +dir + +CollisionKdTree.as +file + + + + +2010-10-28T04:32:39.000000Z +92511d7533028a942e299ca3a4251da8 +2009-05-18T16:37:02.465501Z +13087 +mike + +BoxTriangleCollider.as +file + + + + +2010-10-28T04:32:39.000000Z +f95da4166903174a2c92c1ab262e719c +2009-05-26T08:41:33.471001Z +13389 +mike + +ICollisionDetector.as +file + + + + +2010-10-28T04:32:39.000000Z +6a1d7d33a702880c21d76093341a3516 +2009-04-26T15:39:06.673206Z +11995 +mike + +IRayCollisionPredicate.as +file + + + + +2010-10-28T04:32:39.000000Z +f7443fb728cf4e6157dcfb4f39665889 +2009-04-26T15:39:06.673206Z +11995 +mike + +types +dir + +BruteForceCollisionDetector.as +file + + + + +2010-10-28T04:32:39.000000Z +3f28452009040d0b8f69b4970321eec2 +2009-04-26T15:39:06.673206Z +11995 +mike + +ICollider.as +file + + + + +2010-10-28T04:32:39.000000Z +bc3785279b9b7810a0c84cb4fe6084fc +2009-04-13T03:07:43.422558Z +11019 +mike + +SphereSphereCollider.as +file + + + + +2010-10-28T04:32:39.000000Z +fd6e8cf39dcf007125384bc91b347a00 +2009-06-03T09:16:50.564913Z +13893 +mike + diff --git a/0.1.Vector3D/src/alternativa/physics/collision/.svn/format b/0.1.Vector3D/src/alternativa/physics/collision/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/BoxBoxCollider.as.svn-base b/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/BoxBoxCollider.as.svn-base new file mode 100644 index 0000000..2893b49 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/BoxBoxCollider.as.svn-base @@ -0,0 +1,791 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Расчитывает точки контакта двух боксов. Нормаль контакта направляется в сторону бокса с меньшим ID. + */ + public class BoxBoxCollider implements ICollider { + + private var tolerance:Number = 0.001; + + private var pos1:Vector3 = new Vector3(); + private var pos2:Vector3 = new Vector3(); + private var vectorToBox1:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var colAxis:Vector3 = new Vector3(); + private var tmpAxis:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var tmpPoints:Vector. = new Vector.(8, true); + private var pcount:int; + + /** + * + */ + public function BoxBoxCollider() { + for (var i:int = 0; i < 8; i++) { + tmpPoints[i] = new ContactPoint(); + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + /** + * + * @param body1 + * @param body2 + * @param contactInfo + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + minOverlap = 1e10; + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + // Вектор из центра второго бокса в центр первого + pos1.x = transform1.d; pos1.y = transform1.h; pos1.z = transform1.l; + pos2.x = transform2.d; pos2.y = transform2.h; pos2.z = transform2.l; + vectorToBox1.x = pos1.x - pos2.x; + vectorToBox1.y = pos1.y - pos2.y; + vectorToBox1.z = pos1.z - pos2.z; + + // Проверка пересечения по основным осям +// box1.transform.getAxis(0, axis10); + axis10.x = transform1.a; axis10.y = transform1.e; axis10.z = transform1.i; + if (!testAxis(box1, box2, axis10, 0, vectorToBox1)) return false; +// box1.transform.getAxis(1, axis11); + axis11.x = transform1.b; axis11.y = transform1.f; axis11.z = transform1.j; + if (!testAxis(box1, box2, axis11, 1, vectorToBox1)) return false; +// box1.transform.getAxis(2, axis12); + axis12.x = transform1.c; axis12.y = transform1.g; axis12.z = transform1.k; + if (!testAxis(box1, box2, axis12, 2, vectorToBox1)) return false; + +// box2.transform.getAxis(0, axis20); + axis20.x = transform2.a; axis20.y = transform2.e; axis20.z = transform2.i; + if (!testAxis(box1, box2, axis20, 3, vectorToBox1)) return false; +// box2.transform.getAxis(1, axis21); + axis21.x = transform2.b; axis21.y = transform2.f; axis21.z = transform2.j; + if (!testAxis(box1, box2, axis21, 4, vectorToBox1)) return false; +// box2.transform.getAxis(2, axis22); + axis22.x = transform2.c; axis22.y = transform2.g; axis22.z = transform2.k; + if (!testAxis(box1, box2, axis22, 5, vectorToBox1)) return false; + + // Проверка производных осей + if (!testAxis(box1, box2, axis.vCross2(axis10, axis20), 6, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis10, axis21), 7, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis10, axis22), 8, vectorToBox1)) return false; + + if (!testAxis(box1, box2, axis.vCross2(axis11, axis20), 9, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis11, axis21), 10, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis11, axis22), 11, vectorToBox1)) return false; + + if (!testAxis(box1, box2, axis.vCross2(axis12, axis20), 12, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis12, axis21), 13, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis12, axis22), 14, vectorToBox1)) return false; + + if (bestAxisIndex < 6) { + // Контакт грань-(грань|ребро|вершина) + findFaceContactPoints(box1, box2, vectorToBox1, bestAxisIndex, contact); + } else { + // Контакт ребро-ребро + bestAxisIndex -= 6; + findEdgesIntersection(box1, box2, vectorToBox1, int(bestAxisIndex/3), bestAxisIndex%3, contact); + } + contact.body1 = box1.body; + contact.body2 = box2.body; + + return true; + } + + /** + * Выполняет поиск точек контакта грани одного бокса с гранью/ребром/вершиной другого. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param faceAxisIdx индекс оси первого бокса, перпендикулярной грани, с которой произошло столкновение + * @param contactInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, faceAxisIdx:int, contactInfo:Contact):void { + var swapNormal:Boolean = false; + if (faceAxisIdx > 2) { + // Столкновение с гранью второго бокса. Для дальнейших расчётов боксы меняются местами, + // но нормаль контакта всё равно должна быть направлена в сторону первоначального box1 + var tmpBox:CollisionBox = box1; + box1 = box2; + box2 = tmpBox; + vectorToBox1.vReverse(); + faceAxisIdx -= 3; + swapNormal = true; + } + box1.transform.getAxis(faceAxisIdx, colAxis); + var faceReversed:Boolean = colAxis.vDot(vectorToBox1) > 0; + if (!faceReversed) colAxis.vReverse(); + // Ищем ось второго бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + box2.transform.getAxis(axisIdx, axis); + var dot:Number = axis.vDot(colAxis); + if (dot < 0) dot = -dot; + if (dot > maxDot) { + maxDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Получаем список вершин грани второго бокса, переводим их в систему координат первого бокса и выполняем обрезку + // по грани первого бокса. Таким образом получается список потенциальных точек контакта. + box2.transform.getAxis(incFaceAxisIdx, axis); + getFaceVertsByAxis(box2, incFaceAxisIdx, axis.vDot(colAxis) < 0, verts1); + box2.transform.transformVectors(verts1, verts2); + box1.transform.transformVectorsInverse(verts2, verts1); + var pnum:int = clip(box1.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + pcount = 0; + for (var i:int = 0; i < pnum; i++) { + if ((pen = getPointBoxPenetration(box1.hs, verts1[i], faceAxisIdx, faceReversed)) > -tolerance) { + var cp:ContactPoint = tmpPoints[pcount++]; + box1.transform.transformVector(verts1[i], cp.pos); + cp.r1.vDiff(cp.pos, pos1); + cp.r2.vDiff(cp.pos, pos2); + cp.penetration = pen; + } + } + contactInfo.normal.vCopy(colAxis); + if (swapNormal) contactInfo.normal.vReverse(); + + if (pcount > 4) reducePoints(); + for (i = 0; i < pcount; i++) (contactInfo.points[i] as ContactPoint).copyFrom(tmpPoints[i]); + contactInfo.pcount = pcount; + } + + /** + * + * @param contactInfo + */ + private function reducePoints():void { + var i:int; + var minIdx:int; + var cp1:ContactPoint; + var cp2:ContactPoint; + while (pcount > 4) { + var minLen:Number = 1e10; + var p1:Vector3 = (tmpPoints[pcount - 1] as ContactPoint).pos; + var p2:Vector3; + for (i = 0; i < pcount; i++) { + p2 = (tmpPoints[i] as ContactPoint).pos; + var dx:Number = p2.x - p1.x; + var dy:Number = p2.y - p1.y; + var dz:Number = p2.z - p1.z; + var len:Number = dx*dx + dy*dy + dz*dz; + if (len < minLen) { + minLen = len; + minIdx = i; + } + p1 = p2; + } + cp1 = tmpPoints[minIdx == 0 ? (pcount - 1) : (minIdx - 1)]; + cp2 = tmpPoints[minIdx]; + p1 = cp1.pos; + p2 = cp2.pos; + p2.x = 0.5*(p1.x + p2.x); + p2.y = 0.5*(p1.y + p2.y); + p2.z = 0.5*(p1.z + p2.z); + cp2.penetration = 0.5*(cp1.penetration + cp2.penetration); + if (minIdx > 0) { + for (i = minIdx; i < pcount; i++) tmpPoints[i - 1] = tmpPoints[i]; + tmpPoints[pcount - 1] = cp1; + } + pcount--; + } + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + private function getFaceVertsByAxis(box:CollisionBox, axisIdx:int, reverse:Boolean, result:Vector.):void { + var hs:Vector3 = box.hs; + switch (axisIdx) { + case 0: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, -hs.y, hs.z); + } + break; + case 1: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, hs.y, hs.z); + } + break; + case 2: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + } else { + (result[0] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } + break; + } + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clip(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + return clipHighX(hs.x, pnum, verts2, verts1); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + } + return 0; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param contactInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):void { + box1.transform.getAxis(axisIdx1, axis10); + box2.transform.getAxis(axisIdx2, axis20); + colAxis.vCross2(axis10, axis20).vNormalize(); + // Разворот оси в сторону первого бокса + if (colAxis.vDot(vectorToBox1) < 0) colAxis.vReverse(); + + /* Кодирование рёбер + бит 0: 1 (тип контакта ребро-ребро) + биты 1-2: индекс направляющей оси ребра + бит 3: + бит 4: + бит 5: + */ + var edgeCode1:int = 1; + var edgeCode2:int = 1; + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + point1.vCopy(box1.hs); + point2.vCopy(box2.hs); + // x + if (axisIdx1 == 0) { + point1.x = 0; + halfLen1 = box1.hs.x; + } else { + box1.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.x = -point1.x; + edgeCode1 |= 8; // 1 << 3 + } + } + if (axisIdx2 == 0) { + point2.x = 0; + halfLen2 = box2.hs.x; + } else { + box2.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.x = -point2.x; + edgeCode2 |= 8; // 1 << 3 + } + } + // y + if (axisIdx1 == 1) { + point1.y = 0; + halfLen1 = box1.hs.y; + edgeCode1 |= 2; // 1 << 1 + } else { + box1.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.y = -point1.y; + edgeCode1 |= 16; // 1 << 4 + } + } + if (axisIdx2 == 1) { + point2.y = 0; + halfLen2 = box2.hs.y; + edgeCode2 |= 2; // 1 << 1 + } else { + box2.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.y = -point2.y; + edgeCode2 |= 16; // 1 << 4 + } + } + // z + if (axisIdx1 == 2) { + point1.z = 0; + halfLen1 = box1.hs.z; + edgeCode1 |= 4; // 2 << 1 + } else { + box1.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.z = -point1.z; + edgeCode1 |= 32; // 1 << 5 + } + } + if (axisIdx2 == 2) { + point2.z = 0; + halfLen2 = box2.hs.z; + edgeCode2 |= 4; // 2 << 1 + } else { + box2.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.z = -point2.z; + edgeCode2 |= 32; // 1 << 5 + } + } + // Получаем глобальные координаты средних точек рёбер + point1.vTransformBy4(box1.transform); + point2.vTransformBy4(box2.transform); + // Находим точку пересечения рёбер, решая систему уравнений + var k:Number = axis10.vDot(axis20); + var det:Number = k*k - 1; + vector.vDiff(point2, point1); + var c1:Number = axis10.vDot(vector); + var c2:Number = axis20.vDot(vector); + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.normal.vCopy(colAxis); + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(point1.x + axis10.x*t1 + point2.x + axis20.x*t2); + cp.pos.y = 0.5*(point1.y + axis10.y*t1 + point2.y + axis20.y*t2); + cp.pos.z = 0.5*(point1.z + axis10.z*t1 + point2.z + axis20.z*t2); + cp.r1.vDiff(cp.pos, pos1); + cp.r2.vDiff(cp.pos, pos2); + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение боксов вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box1 + * @param box2 + * @param axis + * @param axisIndex + * @param vectorToBox1 + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, axisIndex:int, vectorToBox1:Vector3):Boolean { + if (axis.vLengthSqr() < 0.0001) { + return true; + } + axis.vNormalize(); + + var overlap:Number = overlapOnAxis(box1, box2, axis, vectorToBox1); + if (overlap < -tolerance) return false; + if (overlap + tolerance < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия двух боксов вдоль заданной оси. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param axis ось + * @param vectorToBox1 вектор, соединяющий центр второго бокса с центром первого + * @return величина перекрытия боксов вдоль оси + */ + public function overlapOnAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, vectorToBox1:Vector3):Number { + var m:Matrix4 = box1.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box1.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box1.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box1.hs.z; + if (d < 0) d = -d; + projection += d; + + m = box2.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box2.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box2.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box2.hs.z; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox1.x*axis.x + vectorToBox1.y*axis.y + vectorToBox1.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} + import alternativa.physics.types.Vector3; + + class CollisionPointTmp { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + } diff --git a/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/BoxPlaneCollider.as.svn-base b/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/BoxPlaneCollider.as.svn-base new file mode 100644 index 0000000..8568dc7 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/BoxPlaneCollider.as.svn-base @@ -0,0 +1,78 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + public class BoxPlaneCollider implements ICollider { + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var normal:Vector3 = new Vector3(); + + public function BoxPlaneCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var box:RigidBox = body1 as RigidBox; +// var plane:RigidPlane; +// if (box == null) { +// box = body2 as RigidBox; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные координаты вершин бокса +// var sx:Number = box.halfSize.x; +// var sy:Number = box.halfSize.y; +// var sz:Number = box.halfSize.z; +// (verts1[0] as Vector3).reset(-sx, -sy, -sz); +// (verts1[1] as Vector3).reset(sx, -sy, -sz); +// (verts1[2] as Vector3).reset(sx, sy, -sz); +// (verts1[3] as Vector3).reset(-sx, sy, -sz); +// (verts1[4] as Vector3).reset(-sx, -sy, sz); +// (verts1[5] as Vector3).reset(sx, -sy, sz); +// (verts1[6] as Vector3).reset(sx, sy, sz); +// (verts1[7] as Vector3).reset(-sx, sy, sz); +// +// box.transform.transformVectors(verts1, verts2); +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// // Проверяем наличие столкновений с каждой вершиной +// collisionInfo.pcount = 0; +// for (var i:int = 0; i < 8; i++) { +// // Вершина добавляется в список точек столкновения, если лежит под плоскостью +// var dist:Number = (verts2[i] as Vector3).dot(normal); +// if (dist < offset) { +// var cp:ContactPoint; +// if (collisionInfo.pcount == collisionInfo.points.length) { +// cp = new ContactPoint(); +// collisionInfo.points[collisionInfo.pcount] = cp; +// } else { +// cp = collisionInfo.points[collisionInfo.pcount]; +// } +// cp.pos.copy(verts2[i]); +// cp.r1.diff(cp.pos, box.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); +// cp.penetration = offset - dist; +// collisionInfo.pcount++; +// } +// } +// if (collisionInfo.pcount > 0) { +// collisionInfo.body1 = box; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// return true; +// } + return false; + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/BoxRectCollider.as.svn-base b/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/BoxRectCollider.as.svn-base new file mode 100644 index 0000000..b5287f3 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/BoxRectCollider.as.svn-base @@ -0,0 +1,734 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + public class BoxRectCollider implements ICollider { + + private var tolerance:Number = 0.001; + + private var bPos:Vector3 = new Vector3(); + private var rPos:Vector3 = new Vector3(); + + private var vectorToBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var colAxis:Vector3 = new Vector3(); + private var tmpAxis:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxRectCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + /** + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + minOverlap = 1e10; + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + + // Вектор из центра прямоугольника в центр бокса + box.transform.getAxis(3, bPos); + rect.transform.getAxis(3, rPos); + vectorToBox.vDiff(bPos, rPos); + + // Проверка пересечения по нормали прямоугольника + rect.transform.getAxis(2, axis22); + if (!testAxis(box, rect, axis22, 0, vectorToBox)) return false; + + // Проверка пересечения по основным осям бокса + box.transform.getAxis(0, axis10); + if (!testAxis(box, rect, axis10, 1, vectorToBox)) return false; + box.transform.getAxis(1, axis11); + if (!testAxis(box, rect, axis11, 2, vectorToBox)) return false; + box.transform.getAxis(2, axis12); + if (!testAxis(box, rect, axis12, 3, vectorToBox)) return false; + + // Получаем направляющие рёбер прямоугольника + rect.transform.getAxis(0, axis20); + rect.transform.getAxis(1, axis21); + + // Проверка производных осей + if (!testAxis(box, rect, axis.vCross2(axis10, axis20), 4, vectorToBox)) return false; + if (!testAxis(box, rect, axis.vCross2(axis10, axis21), 5, vectorToBox)) return false; + + if (!testAxis(box, rect, axis.vCross2(axis11, axis20), 6, vectorToBox)) return false; + if (!testAxis(box, rect, axis.vCross2(axis11, axis21), 7, vectorToBox)) return false; + + if (!testAxis(box, rect, axis.vCross2(axis12, axis20), 8, vectorToBox)) return false; + if (!testAxis(box, rect, axis.vCross2(axis12, axis21), 9, vectorToBox)) return false; + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, rect, vectorToBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + findEdgesIntersection(box, rect, vectorToBox, int(bestAxisIndex/2), bestAxisIndex%2, contact); + } + contact.body1 = box.body; + contact.body2 = rect.body; + + return true; + } + + /** + * Выполняет поиск точек контакта бокса с прямоугольником. + * + * @param box бокс + * @param rect прямоугольник + * @param vectorToBox вектор, направленный из центра прямоугольника в центр бокса + * @param faceAxisIdx индекс оси, идентифицирующей полскость столкновения (грань бокса или полскость прямоугольника) + * @param colInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, faceAxisIdx:int, colInfo:Contact):Boolean { + var pnum:int, i:int, v:Vector3, cp:ContactPoint; + if (faceAxisIdx == 0) { + // Столкновение с плоскостью прямоугольника + + // Проверим положение бокса относительно плоскости прямоугольника + rect.transform.getAxis(2, colAxis); + var offset:Number = colAxis.vDot(rPos); + if (bPos.vDot(colAxis) < offset) return false; + + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + box.transform.getAxis(axisIdx, axis); + var dot:Number = axis.vDot(colAxis); + if (dot < 0) dot = -dot; + if (dot > maxDot) { + maxDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Получаем список вершин грани бокса, переводим их в систему координат прямоугольника и выполняем обрезку + // по прямоугольнику. Таким образом получается список потенциальных точек контакта. + box.transform.getAxis(incFaceAxisIdx, axis); + getFaceVertsByAxis(box.hs, incFaceAxisIdx, axis.vDot(colAxis) > 0, verts1); + box.transform.transformVectors(verts1, verts2); + rect.transform.transformVectorsInverse(verts2, verts1); + pnum = clipByRect(rect.hs); + // Проверяем каждую потенциальную точку на принадлежность нижней полуплоскости прямоугольника и добавляем такие точки в список контактов + colInfo.pcount = 0; + for (i = 0; i < pnum; i++) { + v = verts1[i]; + if (v.z < tolerance) { + cp = colInfo.points[colInfo.pcount++]; + rect.transform.transformVector(v, cp.pos); + cp.r1.vDiff(cp.pos, bPos); + cp.r2.vDiff(cp.pos, rPos); + cp.penetration = -v.z; + } + } + colInfo.normal.vCopy(colAxis); + } else { + // Столкновение с гранью бокса + faceAxisIdx--; + box.transform.getAxis(faceAxisIdx, colAxis); + var faceReversed:Boolean = colAxis.vDot(vectorToBox) > 0; + if (!faceReversed) colAxis.vReverse(); + + rect.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) return false; + + getFaceVertsByAxis(rect.hs, 2, false, verts1); + rect.transform.transformVectors(verts1, verts2); + box.transform.transformVectorsInverse(verts2, verts1); + pnum = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + colInfo.pcount = 0; + for (i = 0; i < pnum; i++) { + if ((pen = getPointBoxPenetration(box.hs, verts1[i], faceAxisIdx, faceReversed)) > -tolerance) { + cp = colInfo.points[colInfo.pcount++]; + box.transform.transformVector(verts1[i], cp.pos); + cp.r1.vDiff(cp.pos, bPos); + cp.r2.vDiff(cp.pos, rPos); + cp.penetration = pen; + } + } + colInfo.normal.vCopy(colAxis); + } + return true; + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + private function getFaceVertsByAxis(hs:Vector3, axisIdx:int, reverse:Boolean, result:Vector.):void { + switch (axisIdx) { + case 0: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, -hs.y, hs.z); + } + break; + case 1: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, hs.y, hs.z); + } + break; + case 2: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + } else { + (result[0] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } + break; + } + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + return clipHighX(hs.x, pnum, verts2, verts1); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + } + return 0; + } + + /** + * + * @param hs + * @return + */ + private function clipByRect(hs:Vector3):int { + var pnum:int = 4; + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box первый бокс + * @param rect второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param colInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, axisIdx1:int, axisIdx2:int, colInfo:Contact):void { + box.transform.getAxis(axisIdx1, axis10); + rect.transform.getAxis(axisIdx2, axis20); + colAxis.vCross2(axis10, axis20).vNormalize(); + // Разворот оси в сторону бокса + if (colAxis.vDot(vectorToBox) < 0) colAxis.vReverse(); + + /* Кодирование рёбер + бит 0: 1 (тип контакта ребро-ребро) + биты 1-2: индекс направляющей оси ребра + бит 3: + бит 4: + бит 5: + */ + var edgeCode1:int = 1; + var edgeCode2:int = 1; + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + point1.vCopy(box.hs); + point2.vCopy(rect.hs); + // x + if (axisIdx1 == 0) { + point1.x = 0; + halfLen1 = box.hs.x; + } else { + box.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.x = -point1.x; + edgeCode1 |= 8; // 1 << 3 + } + } + if (axisIdx2 == 0) { + point2.x = 0; + halfLen2 = rect.hs.x; + } else { + rect.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.x = -point2.x; + edgeCode2 |= 8; // 1 << 3 + } + } + // y + if (axisIdx1 == 1) { + point1.y = 0; + halfLen1 = box.hs.y; + edgeCode1 |= 2; // 1 << 1 + } else { + box.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.y = -point1.y; + edgeCode1 |= 16; // 1 << 4 + } + } + if (axisIdx2 == 1) { + point2.y = 0; + halfLen2 = rect.hs.y; + edgeCode2 |= 2; // 1 << 1 + } else { + rect.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.y = -point2.y; + edgeCode2 |= 16; // 1 << 4 + } + } + // z + if (axisIdx1 == 2) { + point1.z = 0; + halfLen1 = box.hs.z; + edgeCode1 |= 4; // 2 << 1 + } else { + box.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.z = -point1.z; + edgeCode1 |= 32; // 1 << 5 + } + } + // Получаем глобальные координаты средних точек рёбер + point1.vTransformBy4(box.transform); + point2.vTransformBy4(rect.transform); + // Находим точку пересечения рёбер, решая систему уравнений + var k:Number = axis10.vDot(axis20); + var det:Number = k*k - 1; + vector.vDiff(point2, point1); + var c1:Number = axis10.vDot(vector); + var c2:Number = axis20.vDot(vector); + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + colInfo.normal.vCopy(colAxis); + colInfo.pcount = 1; + var cp:ContactPoint = colInfo.points[0]; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(point1.x + axis10.x*t1 + point2.x + axis20.x*t2); + cp.pos.y = 0.5*(point1.y + axis10.y*t1 + point2.y + axis20.y*t2); + cp.pos.z = 0.5*(point1.z + axis10.z*t1 + point2.z + axis20.z*t2); + cp.r1.vDiff(cp.pos, bPos); + cp.r2.vDiff(cp.pos, rPos); + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box + * @param rect + * @param axis + * @param axisIndex + * @param vectorToBox + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + if (axis.vLengthSqr() < 0.0001) return true; + axis.vNormalize(); + + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -tolerance) return false; + if (overlap + tolerance < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия вдоль заданной оси. + * + * @param box бокс + * @param rect прямоугольник + * @param axis ось + * @param vectorToBox вектор, соединяющий центр прямоугольника с центром бокса + * @return величина перекрытия вдоль оси + */ + public function overlapOnAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, vectorToBox:Vector3):Number { + var m:Matrix4 = box.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box.hs.z; + if (d < 0) d = -d; + projection += d; + + m = rect.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*rect.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*rect.hs.y; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox.x*axis.x + vectorToBox.y*axis.y + vectorToBox.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/BoxSphereCollider.as.svn-base b/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/BoxSphereCollider.as.svn-base new file mode 100644 index 0000000..2b166f3 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/BoxSphereCollider.as.svn-base @@ -0,0 +1,104 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + use namespace altphysics; + /** + * + */ + public class BoxSphereCollider implements ICollider { + + private var center:Vector3 = new Vector3(); + private var closestPt:Vector3 = new Vector3(); + + private var bPos:Vector3 = new Vector3(); + private var sPos:Vector3 = new Vector3(); + + /** + * + */ + public function BoxSphereCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + // Зафиксированно столкновение + contact.body1 = sphere.body; + contact.body2 = box.body; + contact.normal.vCopy(closestPt).vTransformBy4(box.transform).vSubtract(sPos).vNormalize().vReverse(); + contact.pcount = 1; + + var cp:ContactPoint = contact.points[0]; + cp.penetration = sphere.r - Math.sqrt(distSqr); + cp.pos.vCopy(contact.normal).vScale(-sphere.r).vAdd(sPos); + cp.r1.vDiff(cp.pos, sPos); + cp.r2.vDiff(cp.pos, bPos); + + return true; + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/BoxTriangleCollider.as.svn-base b/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/BoxTriangleCollider.as.svn-base new file mode 100644 index 0000000..63c6efd --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/BoxTriangleCollider.as.svn-base @@ -0,0 +1,14 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Contact; + import alternativa.physics.collision.primitives.CollisionPrimitive; + + public class BoxTriangleCollider implements ICollider { + public function BoxTriangleCollider() { + } + + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/BruteForceCollisionDetector.as.svn-base b/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/BruteForceCollisionDetector.as.svn-base new file mode 100644 index 0000000..d5160ef --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/BruteForceCollisionDetector.as.svn-base @@ -0,0 +1,33 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + public class BruteForceCollisionDetector implements ICollisionDetector { + public function BruteForceCollisionDetector() { + } + + public function addPrimitive(primitive:CollisionPrimitive, isStatic:Boolean=true):Boolean { + return false; + } + + public function removePrimitive(primitive:CollisionPrimitive, isStatic:Boolean=true):Boolean { + return false; + } + + public function init():void { + } + + public function getAllCollisions(contacts:Vector.):int { + return 0; + } + + public function intersectRay(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, intersection:RayIntersection):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base b/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base new file mode 100644 index 0000000..e1a1657 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base @@ -0,0 +1,16 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.types.BoundBox; + + public class CollisionKdNode { + public var objects:Vector.; + public var boundBox:BoundBox; + public var parent:CollisionKdNode; + + public var axis:int = -1; // 0 - x, 1 - y, 2 - z + public var coord:Number; + public var positiveNode:CollisionKdNode; + public var negativeNode:CollisionKdNode; + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base b/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base new file mode 100644 index 0000000..6c62b79 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base @@ -0,0 +1,259 @@ +package alternativa.physics.collision { + + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + + /** + * @author mike + */ + public class CollisionKdTree { + + public var threshold:Number = 0.01; + public var minPrimitivesPerNode:int = 1; + + public var rootNode:CollisionKdNode; + public var staticChildren:Vector. = new Vector.(); + public var numStaticChildren:int; + private var staticBoundBoxes:Vector. = new Vector.(); + + /** + * @param primitive + */ + public function addStaticPrimitive(primitive:CollisionPrimitive):void { + staticChildren[numStaticChildren++] = primitive; + } + + /** + * @param primitive + * @return + */ + public function removeStaticPrimitive(primitive:CollisionPrimitive):Boolean { + var idx:int = staticChildren.indexOf(primitive); + if (idx < 0) return false; + staticChildren.splice(idx, 1); + numStaticChildren--; + return true; + } + + /** + * @param boundBox + */ + public function createTree(boundBox:BoundBox = null):void { + if (numStaticChildren == 0) return; + // Создаём корневую ноду + rootNode = new CollisionKdNode(); + rootNode.objects = new Vector.(); + // Расчитываем баунды объектов и рутовой ноды + var rootNodeBoundBox:BoundBox = rootNode.boundBox = (boundBox != null) ? boundBox : new BoundBox(); + for (var i:int = 0; i < numStaticChildren; i++) { + var child:CollisionPrimitive = staticChildren[i]; + var childBoundBox:BoundBox = staticBoundBoxes[i] = child.calculateAABB(); + rootNodeBoundBox.addBoundBox(childBoundBox); + rootNode.objects[i] = i; + } + staticBoundBoxes.length = numStaticChildren; + // Разделяем рутовую ноду + splitNode(rootNode); + } + + private var splitAxis:int; + private var splitCoord:Number; + private var splitCost:Number; + static private const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + static private const splitCoordsX:Vector. = new Vector.(); + static private const splitCoordsY:Vector. = new Vector.(); + static private const splitCoordsZ:Vector. = new Vector.(); + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + if (node.objects.length <= minPrimitivesPerNode) return; + + var objects:Vector. = node.objects; + var i:int, j:int, k:int, length:int = objects.length, c1:Number, c2:Number, ct1:Number, ct2:Number, area:Number, areaNegative:Number, areaPositive:Number, numNegative:int, numPositive:int, conflict:Boolean, cost:Number; + + var nodeBoundBox:BoundBox = node.boundBox; + + // Подготовка баунда с погрешностями + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + // Собираем опорные координаты + var numSplitCoordsX:int = 0, numSplitCoordsY:int = 0, numSplitCoordsZ:int = 0; + for (i = 0; i < length; i++) { + var boundBox:BoundBox = staticBoundBoxes[objects[i]]; + if (boundBox.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = boundBox.minX; + if (boundBox.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = boundBox.maxX; + + if (boundBox.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = boundBox.minY; + if (boundBox.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = boundBox.maxY; + + if (boundBox.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.minZ; + if (boundBox.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.maxZ; + } + + // Убираем дубликаты координат, ищем наилучший сплит + splitAxis = -1; splitCost = 1e308; + i = 0; area = (nodeBoundBox.maxY - nodeBoundBox.minY)*(nodeBoundBox.maxZ - nodeBoundBox.minZ); + while (i < numSplitCoordsX) { + if (isNaN(c1 = splitCoordsX[i++])) continue; + ct1 = c1 - threshold; + ct2 = c1 + threshold; + areaNegative = area*(c1 - nodeBoundBox.minX); + areaPositive = area*(nodeBoundBox.maxX - c1); + numNegative = numPositive = 0; + conflict = false; + // Проверяем объекты + for (j = 0; j < length; j++) { + boundBox = staticBoundBoxes[objects[j]]; + if (boundBox.maxX <= ct2) { + if (boundBox.minX < ct1) numNegative++; + } else { + if (boundBox.minX >= ct1) numPositive++; else {conflict = true; break;} + } + } + // Если хороший сплит, сохраняем + if (!conflict && (cost = areaNegative*numNegative + areaPositive*numPositive) < splitCost) { + splitCost = cost; + splitAxis = 0; + splitCoord = c1; + } + j = i; + while (++j < numSplitCoordsX) if ((c2 = splitCoordsX[j]) >= c1 - threshold && c2 <= c1 + threshold) splitCoordsX[j] = NaN; + } + i = 0; area = (nodeBoundBox.maxX - nodeBoundBox.minX)*(nodeBoundBox.maxZ - nodeBoundBox.minZ); + while (i < numSplitCoordsY) { + if (isNaN(c1 = splitCoordsY[i++])) continue; + ct1 = c1 - threshold; + ct2 = c1 + threshold; + areaNegative = area*(c1 - nodeBoundBox.minY); + areaPositive = area*(nodeBoundBox.maxY - c1); + numNegative = numPositive = 0; + conflict = false; + // Проверяем объекты + for (j = 0; j < length; j++) { + boundBox = staticBoundBoxes[objects[j]]; + if (boundBox.maxY <= ct2) { + if (boundBox.minY < ct1) numNegative++; + } else { + if (boundBox.minY >= ct1) numPositive++; else {conflict = true; break;} + } + } + // Если хороший сплит, сохраняем + if (!conflict && (cost = areaNegative*numNegative + areaPositive*numPositive) < splitCost) { + splitCost = cost; + splitAxis = 1; + splitCoord = c1; + } + j = i; + while (++j < numSplitCoordsY) if ((c2 = splitCoordsY[j]) >= c1 - threshold && c2 <= c1 + threshold) splitCoordsY[j] = NaN; + } + i = 0; area = (nodeBoundBox.maxX - nodeBoundBox.minX)*(nodeBoundBox.maxY - nodeBoundBox.minY); + while (i < numSplitCoordsZ) { + if (isNaN(c1 = splitCoordsZ[i++])) continue; + ct1 = c1 - threshold; + ct2 = c1 + threshold; + areaNegative = area*(c1 - nodeBoundBox.minZ); + areaPositive = area*(nodeBoundBox.maxZ - c1); + numNegative = numPositive = 0; + conflict = false; + // Проверяем объекты + for (j = 0; j < length; j++) { + boundBox = staticBoundBoxes[objects[j]]; + if (boundBox.maxZ <= ct2) { + if (boundBox.minZ < ct1) numNegative++; + } else { + if (boundBox.minZ >= ct1) numPositive++; else {conflict = true; break;} + } + } + // Если хороший сплит, сохраняем + if (!conflict && (cost = areaNegative*numNegative + areaPositive*numPositive) < splitCost) { + splitCost = cost; + splitAxis = 2; + splitCoord = c1; + } + j = i; + while (++j < numSplitCoordsZ) if ((c2 = splitCoordsZ[j]) >= c1 - threshold && c2 <= c1 + threshold) splitCoordsZ[j] = NaN; + } + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Разделяем ноду + var axisX:Boolean = splitAxis == 0, axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.positiveNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.positiveNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + ct1 = splitCoord - threshold; ct2 = splitCoord + threshold; + for (i = 0; i < length; i++) { + boundBox = staticBoundBoxes[objects[i]]; + var min:Number = axisX ? boundBox.minX : (axisY ? boundBox.minY : boundBox.minZ); + var max:Number = axisX ? boundBox.maxX : (axisY ? boundBox.maxY : boundBox.maxZ); + if (max <= ct2) { + // Объект в негативной стороне + if (node.negativeNode.objects == null) node.negativeNode.objects = new Vector.(); + node.negativeNode.objects.push(objects[i]); + objects[i] = -1; + } else { + if (min >= ct1) { + // Объект в положительной стороне + if (node.positiveNode.objects == null) node.positiveNode.objects = new Vector.(); + node.positiveNode.objects.push(objects[i]); + objects[i] = -1; + } else { + // Распилился + } + } + } + + // Очистка списка объектов + j = 0; + for (i = 0; i < length; i++) { + if (objects[i] >= 0) objects[j++] = objects[i]; + } + if (j > 0) objects.length = j; else node.objects = null; + + // Разделение дочерних нод + if (node.negativeNode.objects != null) splitNode(node.negativeNode); + if (node.positiveNode.objects != null) splitNode(node.positiveNode); + } + + /** + * + */ + public function traceTree():void { + traceNode("", rootNode); + } + + /** + * @param str + * @param node + */ + private function traceNode(str:String, node:CollisionKdNode):void { + if (node == null) return; + trace(str, node.axis == -1 ? "end" : ((node.axis == 0) ? "X" : ((node.axis == 1) ? "Y" : "Z")), "splitCoord=" + splitCoord, "bound", node.boundBox, "objs:", node.objects); + traceNode(str + "-", node.negativeNode); + traceNode(str + "+", node.positiveNode); + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base b/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base new file mode 100644 index 0000000..733231b --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base @@ -0,0 +1,12 @@ +package alternativa.physics.collision { + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + + /** + * Интерфейс определителя столкновений между двумя примитивами. + */ + public interface ICollider { + function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base b/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base new file mode 100644 index 0000000..b555d17 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base @@ -0,0 +1,59 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + /** + * Интерфейс детектора столкновений. + */ + public interface ICollisionDetector { + + /** + * Добавляет физический примитив в коллайдер. + * + * @param primitive добавляемый примитив + * @param isStatic указывает тип примитива: статический или динамический + * @return true если примитив был успешно добавлен, иначе false + */ + function addPrimitive(primitive:CollisionPrimitive, isStatic:Boolean = true):Boolean; + + /** + * Удаляет физический примитив из коллайдера. + * + * @param primitive удаляемый примитив + * @param isStatic указывает тип примитива: статический или динамический + * @return true если примитив был успшено удалён, иначе false + */ + function removePrimitive(primitive:CollisionPrimitive, isStatic:Boolean = true):Boolean; + + /** + * Выполняет инициализацию детектора после обновления списка примитивов. + */ + function init():void; + + /** + * Получает все столкновения в текущей конфигурации физической геометрии. + * + * @param contacts список контактов, в кторые будет записана информация о столкновении + * @return количество найденных столкновений + */ + function getAllCollisions(contacts:Vector.):int; + + /** + * Тестирует луч на пересечение с физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча. + * + * @param origin + * @param dir + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента + * @param predicate + * @param intersection переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRay(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, intersection:RayIntersection):Boolean; + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base b/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base new file mode 100644 index 0000000..bb1b756 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base @@ -0,0 +1,9 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + + public interface ICollisionPredicate { + + function considerCollision(primitive:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base b/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base new file mode 100644 index 0000000..0f3d929 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Body; + + public interface IRayCollisionPredicate { + function considerBody(body:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base b/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base new file mode 100644 index 0000000..1134885 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base @@ -0,0 +1,412 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Детектор, хранящий статическую геометрию в kD-дереве и использующий дерево для ускорения тестов на пересечения. + */ + public class KdTreeCollisionDetector implements ICollisionDetector { + + altphysics var tree:CollisionKdTree; + altphysics var dynamicPrimitives:Vector.; + altphysics var dynamicPrimitivesNum:int; + altphysics var threshold:Number = 0.0001; + private var colliders:Object = {}; + + private var gtMax:Number = 0; + private var gtMin:Number = 0; + 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.(); + + addCollider(CollisionPrimitive.BOX, CollisionPrimitive.BOX, new BoxBoxCollider()); +// addCollider(CollisionPrimitive.BOX, CollisionPrimitive.PLANE, new BoxPlaneCollider()); + addCollider(CollisionPrimitive.BOX, CollisionPrimitive.SPHERE, new BoxSphereCollider()); + addCollider(CollisionPrimitive.BOX, CollisionPrimitive.RECT, new BoxRectCollider()); + +// addCollider(CollisionPrimitive.SPHERE, CollisionPrimitive.PLANE, new SpherePlaneCollider()); + addCollider(CollisionPrimitive.SPHERE, CollisionPrimitive.SPHERE, new SphereSphereCollider()); + } + + /** + * + * @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 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():void { + tree.createTree(); +// tree.traceTree(); + } + + /** + * + * @param contacts + * @return + */ + public function getAllCollisions(contacts:Vector.):int { + var colNum:int = 0; + for (var i:int = 0; i < dynamicPrimitivesNum; i++) { + var primitive:CollisionPrimitive = dynamicPrimitives[i]; + primitive.calculateAABB(); + colNum += getPrimitiveNodeCollisions(tree.rootNode, primitive, contacts, colNum); + + // Столкновения динамических примитивов между собой + // TODO: Попробовать различные оптимизации (сортировка по баундам, встраивание в дерево) + for (var j:int = i + 1; j < dynamicPrimitivesNum; j++) { + if (collide(primitive, dynamicPrimitives[j], contacts[colNum])) colNum++; + } + } + + return colNum; + } + + /** + * Выполняет поиск столкновений динамического примитива с примитивами узла дерева. + * + * @param node + * @param primitive + * @param contacts + * @param startIndex + * @return + */ + private function getPrimitiveNodeCollisions(node:CollisionKdNode, primitive:CollisionPrimitive, contacts:Vector., startIndex:int):int { + var colNum:int = 0; + if (node.objects != null) { + // Поиск столкновений со статическими примитивами узла + var primitives:Vector. = tree.staticChildren; + var indices:Vector. = node.objects; + for (var i:int = indices.length - 1; i >= 0; i--) + if (collide(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; + } + + /** + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + public function collide(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.collide(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 = (contact.points[0] as ContactPoint).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 origin + * @param dir + * @param collisionGroup + * @param maxTime + * @param result + * @return + */ + public function intersectRay(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean { + var hasStatIntersection:Boolean = intersectRayWithStatic(origin, dir, collisionGroup, maxTime, predicate, result); + var hasDynIntersection:Boolean = intersectRayWithDynamic(origin, dir, collisionGroup, maxTime, predicate, _dynamicIntersection); + + if (!(hasDynIntersection || hasStatIntersection)) return false; + if (hasDynIntersection && hasStatIntersection) { + if (result.t < _dynamicIntersection.t) return result.t < maxTime; + else { + if (_dynamicIntersection.t < maxTime) { + result.copy(_dynamicIntersection); + return true; + } else return false; + } + } + if (hasStatIntersection) return result.t < maxTime; + else { + if (_dynamicIntersection.t < maxTime) { + result.copy(_dynamicIntersection); + return true; + } else return false; + } + } + + /** + * Тест пересечения луча с динамическими примитивами. + * + * @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 minTime:Number = maxTime + 1; + for (var i:int = 0; i < dynamicPrimitivesNum; i++) { + var primitive:CollisionPrimitive = dynamicPrimitives[i]; + if ((primitive.collisionGroup & collisionGroup) == 0) continue; + if (predicate != null && primitive.body != null && !predicate.considerBody(primitive.body)) continue; + var t:Number = primitive.getSegmentIntersection(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; + } + + /** + * @param origin + * @param dir + * @param collisionGroup + * @param maxTime + * @param intersection + * @return + */ + private function intersectRayWithStatic(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean { + // Вычислим точки пересечения с корневм узлом + if (!getRayBoundBoxIntersection(origin, dir, tree.rootNode.boundBox)) return false; + if (gtMax < 0 || gtMin > maxTime) return false; + + if (gtMin < 0) { + gtMin = 0; + _o.x = origin.x; + _o.y = origin.y; + _o.z = origin.z; + } + else { + _o.x = origin.x + gtMin*dir.x; + _o.y = origin.y + gtMin*dir.y; + _o.z = origin.z + gtMin*dir.z; + } + if (gtMax > maxTime) gtMax = maxTime; + + return testRayAgainstNode(tree.rootNode, origin, _o, dir, collisionGroup, gtMin, gtMax, predicate, result); + } + + /** + * @param ray + * @param boundBox + */ + private function getRayBoundBoxIntersection(origin:Vector3, dir:Vector3, bb:BoundBox):Boolean { + gtMin = -1; + gtMax = 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 > gtMin) gtMin = t1; + if (t2 < gtMax) gtMax = t2; + } else { + if (t2 > gtMin) gtMin = t2; + if (t1 < gtMax) gtMax = t1; + } + if (gtMax < gtMin) 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.objects != null && getRayNodeIntersection(origin, dir, collisionGroup, tree.staticChildren, node.objects, 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., indices:Vector., 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.getSegmentIntersection(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; + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/SpherePlaneCollider.as.svn-base b/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/SpherePlaneCollider.as.svn-base new file mode 100644 index 0000000..9b527d8 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/SpherePlaneCollider.as.svn-base @@ -0,0 +1,58 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + /** + * + */ + public class SpherePlaneCollider implements ICollider { + + private var normal:Vector3 = new Vector3(); + + /** + * + */ + public function SpherePlaneCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var sphere:RigidSphere = body1 as RigidSphere; +// var plane:RigidPlane; +// if (sphere == null) { +// sphere = body2 as RigidSphere; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// +// var dist:Number = sphere.state.pos.dot(normal) - offset; +// if (dist > sphere.r) return false; +// +// collisionInfo.body1 = sphere; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// collisionInfo.pcount = 1; +// +// var cp:ContactPoint = collisionInfo.points[0]; +// cp.penetration = sphere.r - dist; +// cp.pos.copy(normal).reverse().scale(sphere.r).add(sphere.state.pos); +// cp.r1.diff(cp.pos, sphere.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); + + return true; + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/SphereSphereCollider.as.svn-base b/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/SphereSphereCollider.as.svn-base new file mode 100644 index 0000000..9111a84 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/.svn/text-base/SphereSphereCollider.as.svn-base @@ -0,0 +1,60 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + public class SphereSphereCollider implements ICollider { + + private var p1:Vector3 = new Vector3(); + private var p2:Vector3 = new Vector3(); + + public function SphereSphereCollider() { + } + + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var s1:CollisionSphere; + var s2:CollisionSphere; + if (prim1.body != null) { + s1 = prim1 as CollisionSphere; + s2 = prim2 as CollisionSphere; + } else { + s1 = prim2 as CollisionSphere; + s2 = prim1 as CollisionSphere; + } + + s1.transform.getAxis(3, p1); + s2.transform.getAxis(3, p2); + var dx:Number = p1.x - p2.x; + var dy:Number = p1.y - p2.y; + var dz:Number = p1.z - p2.z; + var len:Number = dx*dx + dy*dy + dz*dz; + var sum:Number = s1.r + s2.r; + if (len > sum*sum) return false; + len = Math.sqrt(len); + dx /= len; + dy /= len; + dz /= len; + + contact.body1 = s1.body; + contact.body2 = s2.body; + contact.normal.x = dx; + contact.normal.y = dy; + contact.normal.z = dz; + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = sum - len; + cp.pos.x = p1.x - dx*s1.r; + cp.pos.y = p1.y - dy*s1.r; + cp.pos.z = p1.z - dz*s1.r; + cp.r1.vDiff(cp.pos, p1); + cp.r2.vDiff(cp.pos, p2); + + return true; + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/collision/BoxBoxCollider.as b/0.1.Vector3D/src/alternativa/physics/collision/BoxBoxCollider.as new file mode 100644 index 0000000..2893b49 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/BoxBoxCollider.as @@ -0,0 +1,791 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Расчитывает точки контакта двух боксов. Нормаль контакта направляется в сторону бокса с меньшим ID. + */ + public class BoxBoxCollider implements ICollider { + + private var tolerance:Number = 0.001; + + private var pos1:Vector3 = new Vector3(); + private var pos2:Vector3 = new Vector3(); + private var vectorToBox1:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var colAxis:Vector3 = new Vector3(); + private var tmpAxis:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var tmpPoints:Vector. = new Vector.(8, true); + private var pcount:int; + + /** + * + */ + public function BoxBoxCollider() { + for (var i:int = 0; i < 8; i++) { + tmpPoints[i] = new ContactPoint(); + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + /** + * + * @param body1 + * @param body2 + * @param contactInfo + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + minOverlap = 1e10; + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + // Вектор из центра второго бокса в центр первого + pos1.x = transform1.d; pos1.y = transform1.h; pos1.z = transform1.l; + pos2.x = transform2.d; pos2.y = transform2.h; pos2.z = transform2.l; + vectorToBox1.x = pos1.x - pos2.x; + vectorToBox1.y = pos1.y - pos2.y; + vectorToBox1.z = pos1.z - pos2.z; + + // Проверка пересечения по основным осям +// box1.transform.getAxis(0, axis10); + axis10.x = transform1.a; axis10.y = transform1.e; axis10.z = transform1.i; + if (!testAxis(box1, box2, axis10, 0, vectorToBox1)) return false; +// box1.transform.getAxis(1, axis11); + axis11.x = transform1.b; axis11.y = transform1.f; axis11.z = transform1.j; + if (!testAxis(box1, box2, axis11, 1, vectorToBox1)) return false; +// box1.transform.getAxis(2, axis12); + axis12.x = transform1.c; axis12.y = transform1.g; axis12.z = transform1.k; + if (!testAxis(box1, box2, axis12, 2, vectorToBox1)) return false; + +// box2.transform.getAxis(0, axis20); + axis20.x = transform2.a; axis20.y = transform2.e; axis20.z = transform2.i; + if (!testAxis(box1, box2, axis20, 3, vectorToBox1)) return false; +// box2.transform.getAxis(1, axis21); + axis21.x = transform2.b; axis21.y = transform2.f; axis21.z = transform2.j; + if (!testAxis(box1, box2, axis21, 4, vectorToBox1)) return false; +// box2.transform.getAxis(2, axis22); + axis22.x = transform2.c; axis22.y = transform2.g; axis22.z = transform2.k; + if (!testAxis(box1, box2, axis22, 5, vectorToBox1)) return false; + + // Проверка производных осей + if (!testAxis(box1, box2, axis.vCross2(axis10, axis20), 6, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis10, axis21), 7, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis10, axis22), 8, vectorToBox1)) return false; + + if (!testAxis(box1, box2, axis.vCross2(axis11, axis20), 9, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis11, axis21), 10, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis11, axis22), 11, vectorToBox1)) return false; + + if (!testAxis(box1, box2, axis.vCross2(axis12, axis20), 12, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis12, axis21), 13, vectorToBox1)) return false; + if (!testAxis(box1, box2, axis.vCross2(axis12, axis22), 14, vectorToBox1)) return false; + + if (bestAxisIndex < 6) { + // Контакт грань-(грань|ребро|вершина) + findFaceContactPoints(box1, box2, vectorToBox1, bestAxisIndex, contact); + } else { + // Контакт ребро-ребро + bestAxisIndex -= 6; + findEdgesIntersection(box1, box2, vectorToBox1, int(bestAxisIndex/3), bestAxisIndex%3, contact); + } + contact.body1 = box1.body; + contact.body2 = box2.body; + + return true; + } + + /** + * Выполняет поиск точек контакта грани одного бокса с гранью/ребром/вершиной другого. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param faceAxisIdx индекс оси первого бокса, перпендикулярной грани, с которой произошло столкновение + * @param contactInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, faceAxisIdx:int, contactInfo:Contact):void { + var swapNormal:Boolean = false; + if (faceAxisIdx > 2) { + // Столкновение с гранью второго бокса. Для дальнейших расчётов боксы меняются местами, + // но нормаль контакта всё равно должна быть направлена в сторону первоначального box1 + var tmpBox:CollisionBox = box1; + box1 = box2; + box2 = tmpBox; + vectorToBox1.vReverse(); + faceAxisIdx -= 3; + swapNormal = true; + } + box1.transform.getAxis(faceAxisIdx, colAxis); + var faceReversed:Boolean = colAxis.vDot(vectorToBox1) > 0; + if (!faceReversed) colAxis.vReverse(); + // Ищем ось второго бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + box2.transform.getAxis(axisIdx, axis); + var dot:Number = axis.vDot(colAxis); + if (dot < 0) dot = -dot; + if (dot > maxDot) { + maxDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Получаем список вершин грани второго бокса, переводим их в систему координат первого бокса и выполняем обрезку + // по грани первого бокса. Таким образом получается список потенциальных точек контакта. + box2.transform.getAxis(incFaceAxisIdx, axis); + getFaceVertsByAxis(box2, incFaceAxisIdx, axis.vDot(colAxis) < 0, verts1); + box2.transform.transformVectors(verts1, verts2); + box1.transform.transformVectorsInverse(verts2, verts1); + var pnum:int = clip(box1.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + pcount = 0; + for (var i:int = 0; i < pnum; i++) { + if ((pen = getPointBoxPenetration(box1.hs, verts1[i], faceAxisIdx, faceReversed)) > -tolerance) { + var cp:ContactPoint = tmpPoints[pcount++]; + box1.transform.transformVector(verts1[i], cp.pos); + cp.r1.vDiff(cp.pos, pos1); + cp.r2.vDiff(cp.pos, pos2); + cp.penetration = pen; + } + } + contactInfo.normal.vCopy(colAxis); + if (swapNormal) contactInfo.normal.vReverse(); + + if (pcount > 4) reducePoints(); + for (i = 0; i < pcount; i++) (contactInfo.points[i] as ContactPoint).copyFrom(tmpPoints[i]); + contactInfo.pcount = pcount; + } + + /** + * + * @param contactInfo + */ + private function reducePoints():void { + var i:int; + var minIdx:int; + var cp1:ContactPoint; + var cp2:ContactPoint; + while (pcount > 4) { + var minLen:Number = 1e10; + var p1:Vector3 = (tmpPoints[pcount - 1] as ContactPoint).pos; + var p2:Vector3; + for (i = 0; i < pcount; i++) { + p2 = (tmpPoints[i] as ContactPoint).pos; + var dx:Number = p2.x - p1.x; + var dy:Number = p2.y - p1.y; + var dz:Number = p2.z - p1.z; + var len:Number = dx*dx + dy*dy + dz*dz; + if (len < minLen) { + minLen = len; + minIdx = i; + } + p1 = p2; + } + cp1 = tmpPoints[minIdx == 0 ? (pcount - 1) : (minIdx - 1)]; + cp2 = tmpPoints[minIdx]; + p1 = cp1.pos; + p2 = cp2.pos; + p2.x = 0.5*(p1.x + p2.x); + p2.y = 0.5*(p1.y + p2.y); + p2.z = 0.5*(p1.z + p2.z); + cp2.penetration = 0.5*(cp1.penetration + cp2.penetration); + if (minIdx > 0) { + for (i = minIdx; i < pcount; i++) tmpPoints[i - 1] = tmpPoints[i]; + tmpPoints[pcount - 1] = cp1; + } + pcount--; + } + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + private function getFaceVertsByAxis(box:CollisionBox, axisIdx:int, reverse:Boolean, result:Vector.):void { + var hs:Vector3 = box.hs; + switch (axisIdx) { + case 0: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, -hs.y, hs.z); + } + break; + case 1: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, hs.y, hs.z); + } + break; + case 2: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + } else { + (result[0] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } + break; + } + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clip(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + return clipHighX(hs.x, pnum, verts2, verts1); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + } + return 0; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param contactInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):void { + box1.transform.getAxis(axisIdx1, axis10); + box2.transform.getAxis(axisIdx2, axis20); + colAxis.vCross2(axis10, axis20).vNormalize(); + // Разворот оси в сторону первого бокса + if (colAxis.vDot(vectorToBox1) < 0) colAxis.vReverse(); + + /* Кодирование рёбер + бит 0: 1 (тип контакта ребро-ребро) + биты 1-2: индекс направляющей оси ребра + бит 3: + бит 4: + бит 5: + */ + var edgeCode1:int = 1; + var edgeCode2:int = 1; + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + point1.vCopy(box1.hs); + point2.vCopy(box2.hs); + // x + if (axisIdx1 == 0) { + point1.x = 0; + halfLen1 = box1.hs.x; + } else { + box1.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.x = -point1.x; + edgeCode1 |= 8; // 1 << 3 + } + } + if (axisIdx2 == 0) { + point2.x = 0; + halfLen2 = box2.hs.x; + } else { + box2.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.x = -point2.x; + edgeCode2 |= 8; // 1 << 3 + } + } + // y + if (axisIdx1 == 1) { + point1.y = 0; + halfLen1 = box1.hs.y; + edgeCode1 |= 2; // 1 << 1 + } else { + box1.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.y = -point1.y; + edgeCode1 |= 16; // 1 << 4 + } + } + if (axisIdx2 == 1) { + point2.y = 0; + halfLen2 = box2.hs.y; + edgeCode2 |= 2; // 1 << 1 + } else { + box2.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.y = -point2.y; + edgeCode2 |= 16; // 1 << 4 + } + } + // z + if (axisIdx1 == 2) { + point1.z = 0; + halfLen1 = box1.hs.z; + edgeCode1 |= 4; // 2 << 1 + } else { + box1.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.z = -point1.z; + edgeCode1 |= 32; // 1 << 5 + } + } + if (axisIdx2 == 2) { + point2.z = 0; + halfLen2 = box2.hs.z; + edgeCode2 |= 4; // 2 << 1 + } else { + box2.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.z = -point2.z; + edgeCode2 |= 32; // 1 << 5 + } + } + // Получаем глобальные координаты средних точек рёбер + point1.vTransformBy4(box1.transform); + point2.vTransformBy4(box2.transform); + // Находим точку пересечения рёбер, решая систему уравнений + var k:Number = axis10.vDot(axis20); + var det:Number = k*k - 1; + vector.vDiff(point2, point1); + var c1:Number = axis10.vDot(vector); + var c2:Number = axis20.vDot(vector); + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.normal.vCopy(colAxis); + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(point1.x + axis10.x*t1 + point2.x + axis20.x*t2); + cp.pos.y = 0.5*(point1.y + axis10.y*t1 + point2.y + axis20.y*t2); + cp.pos.z = 0.5*(point1.z + axis10.z*t1 + point2.z + axis20.z*t2); + cp.r1.vDiff(cp.pos, pos1); + cp.r2.vDiff(cp.pos, pos2); + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение боксов вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box1 + * @param box2 + * @param axis + * @param axisIndex + * @param vectorToBox1 + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, axisIndex:int, vectorToBox1:Vector3):Boolean { + if (axis.vLengthSqr() < 0.0001) { + return true; + } + axis.vNormalize(); + + var overlap:Number = overlapOnAxis(box1, box2, axis, vectorToBox1); + if (overlap < -tolerance) return false; + if (overlap + tolerance < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия двух боксов вдоль заданной оси. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param axis ось + * @param vectorToBox1 вектор, соединяющий центр второго бокса с центром первого + * @return величина перекрытия боксов вдоль оси + */ + public function overlapOnAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, vectorToBox1:Vector3):Number { + var m:Matrix4 = box1.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box1.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box1.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box1.hs.z; + if (d < 0) d = -d; + projection += d; + + m = box2.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box2.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box2.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box2.hs.z; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox1.x*axis.x + vectorToBox1.y*axis.y + vectorToBox1.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} + import alternativa.physics.types.Vector3; + + class CollisionPointTmp { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + } diff --git a/0.1.Vector3D/src/alternativa/physics/collision/BoxPlaneCollider.as b/0.1.Vector3D/src/alternativa/physics/collision/BoxPlaneCollider.as new file mode 100644 index 0000000..8568dc7 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/BoxPlaneCollider.as @@ -0,0 +1,78 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + public class BoxPlaneCollider implements ICollider { + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var normal:Vector3 = new Vector3(); + + public function BoxPlaneCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var box:RigidBox = body1 as RigidBox; +// var plane:RigidPlane; +// if (box == null) { +// box = body2 as RigidBox; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные координаты вершин бокса +// var sx:Number = box.halfSize.x; +// var sy:Number = box.halfSize.y; +// var sz:Number = box.halfSize.z; +// (verts1[0] as Vector3).reset(-sx, -sy, -sz); +// (verts1[1] as Vector3).reset(sx, -sy, -sz); +// (verts1[2] as Vector3).reset(sx, sy, -sz); +// (verts1[3] as Vector3).reset(-sx, sy, -sz); +// (verts1[4] as Vector3).reset(-sx, -sy, sz); +// (verts1[5] as Vector3).reset(sx, -sy, sz); +// (verts1[6] as Vector3).reset(sx, sy, sz); +// (verts1[7] as Vector3).reset(-sx, sy, sz); +// +// box.transform.transformVectors(verts1, verts2); +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// // Проверяем наличие столкновений с каждой вершиной +// collisionInfo.pcount = 0; +// for (var i:int = 0; i < 8; i++) { +// // Вершина добавляется в список точек столкновения, если лежит под плоскостью +// var dist:Number = (verts2[i] as Vector3).dot(normal); +// if (dist < offset) { +// var cp:ContactPoint; +// if (collisionInfo.pcount == collisionInfo.points.length) { +// cp = new ContactPoint(); +// collisionInfo.points[collisionInfo.pcount] = cp; +// } else { +// cp = collisionInfo.points[collisionInfo.pcount]; +// } +// cp.pos.copy(verts2[i]); +// cp.r1.diff(cp.pos, box.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); +// cp.penetration = offset - dist; +// collisionInfo.pcount++; +// } +// } +// if (collisionInfo.pcount > 0) { +// collisionInfo.body1 = box; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// return true; +// } + return false; + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/collision/BoxRectCollider.as b/0.1.Vector3D/src/alternativa/physics/collision/BoxRectCollider.as new file mode 100644 index 0000000..b5287f3 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/BoxRectCollider.as @@ -0,0 +1,734 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + public class BoxRectCollider implements ICollider { + + private var tolerance:Number = 0.001; + + private var bPos:Vector3 = new Vector3(); + private var rPos:Vector3 = new Vector3(); + + private var vectorToBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var colAxis:Vector3 = new Vector3(); + private var tmpAxis:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxRectCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + /** + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + minOverlap = 1e10; + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + + // Вектор из центра прямоугольника в центр бокса + box.transform.getAxis(3, bPos); + rect.transform.getAxis(3, rPos); + vectorToBox.vDiff(bPos, rPos); + + // Проверка пересечения по нормали прямоугольника + rect.transform.getAxis(2, axis22); + if (!testAxis(box, rect, axis22, 0, vectorToBox)) return false; + + // Проверка пересечения по основным осям бокса + box.transform.getAxis(0, axis10); + if (!testAxis(box, rect, axis10, 1, vectorToBox)) return false; + box.transform.getAxis(1, axis11); + if (!testAxis(box, rect, axis11, 2, vectorToBox)) return false; + box.transform.getAxis(2, axis12); + if (!testAxis(box, rect, axis12, 3, vectorToBox)) return false; + + // Получаем направляющие рёбер прямоугольника + rect.transform.getAxis(0, axis20); + rect.transform.getAxis(1, axis21); + + // Проверка производных осей + if (!testAxis(box, rect, axis.vCross2(axis10, axis20), 4, vectorToBox)) return false; + if (!testAxis(box, rect, axis.vCross2(axis10, axis21), 5, vectorToBox)) return false; + + if (!testAxis(box, rect, axis.vCross2(axis11, axis20), 6, vectorToBox)) return false; + if (!testAxis(box, rect, axis.vCross2(axis11, axis21), 7, vectorToBox)) return false; + + if (!testAxis(box, rect, axis.vCross2(axis12, axis20), 8, vectorToBox)) return false; + if (!testAxis(box, rect, axis.vCross2(axis12, axis21), 9, vectorToBox)) return false; + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, rect, vectorToBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + findEdgesIntersection(box, rect, vectorToBox, int(bestAxisIndex/2), bestAxisIndex%2, contact); + } + contact.body1 = box.body; + contact.body2 = rect.body; + + return true; + } + + /** + * Выполняет поиск точек контакта бокса с прямоугольником. + * + * @param box бокс + * @param rect прямоугольник + * @param vectorToBox вектор, направленный из центра прямоугольника в центр бокса + * @param faceAxisIdx индекс оси, идентифицирующей полскость столкновения (грань бокса или полскость прямоугольника) + * @param colInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, faceAxisIdx:int, colInfo:Contact):Boolean { + var pnum:int, i:int, v:Vector3, cp:ContactPoint; + if (faceAxisIdx == 0) { + // Столкновение с плоскостью прямоугольника + + // Проверим положение бокса относительно плоскости прямоугольника + rect.transform.getAxis(2, colAxis); + var offset:Number = colAxis.vDot(rPos); + if (bPos.vDot(colAxis) < offset) return false; + + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + box.transform.getAxis(axisIdx, axis); + var dot:Number = axis.vDot(colAxis); + if (dot < 0) dot = -dot; + if (dot > maxDot) { + maxDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Получаем список вершин грани бокса, переводим их в систему координат прямоугольника и выполняем обрезку + // по прямоугольнику. Таким образом получается список потенциальных точек контакта. + box.transform.getAxis(incFaceAxisIdx, axis); + getFaceVertsByAxis(box.hs, incFaceAxisIdx, axis.vDot(colAxis) > 0, verts1); + box.transform.transformVectors(verts1, verts2); + rect.transform.transformVectorsInverse(verts2, verts1); + pnum = clipByRect(rect.hs); + // Проверяем каждую потенциальную точку на принадлежность нижней полуплоскости прямоугольника и добавляем такие точки в список контактов + colInfo.pcount = 0; + for (i = 0; i < pnum; i++) { + v = verts1[i]; + if (v.z < tolerance) { + cp = colInfo.points[colInfo.pcount++]; + rect.transform.transformVector(v, cp.pos); + cp.r1.vDiff(cp.pos, bPos); + cp.r2.vDiff(cp.pos, rPos); + cp.penetration = -v.z; + } + } + colInfo.normal.vCopy(colAxis); + } else { + // Столкновение с гранью бокса + faceAxisIdx--; + box.transform.getAxis(faceAxisIdx, colAxis); + var faceReversed:Boolean = colAxis.vDot(vectorToBox) > 0; + if (!faceReversed) colAxis.vReverse(); + + rect.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) return false; + + getFaceVertsByAxis(rect.hs, 2, false, verts1); + rect.transform.transformVectors(verts1, verts2); + box.transform.transformVectorsInverse(verts2, verts1); + pnum = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + colInfo.pcount = 0; + for (i = 0; i < pnum; i++) { + if ((pen = getPointBoxPenetration(box.hs, verts1[i], faceAxisIdx, faceReversed)) > -tolerance) { + cp = colInfo.points[colInfo.pcount++]; + box.transform.transformVector(verts1[i], cp.pos); + cp.r1.vDiff(cp.pos, bPos); + cp.r2.vDiff(cp.pos, rPos); + cp.penetration = pen; + } + } + colInfo.normal.vCopy(colAxis); + } + return true; + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + private function getFaceVertsByAxis(hs:Vector3, axisIdx:int, reverse:Boolean, result:Vector.):void { + switch (axisIdx) { + case 0: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, -hs.y, hs.z); + } + break; + case 1: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, hs.z); + } else { + (result[0] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(-hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(hs.x, hs.y, hs.z); + } + break; + case 2: + if (reverse) { + (result[0] as Vector3).vReset(-hs.x, hs.y, -hs.z); + (result[1] as Vector3).vReset(hs.x, hs.y, -hs.z); + (result[2] as Vector3).vReset(hs.x, -hs.y, -hs.z); + (result[3] as Vector3).vReset(-hs.x, -hs.y, -hs.z); + } else { + (result[0] as Vector3).vReset(-hs.x, -hs.y, hs.z); + (result[1] as Vector3).vReset(hs.x, -hs.y, hs.z); + (result[2] as Vector3).vReset(hs.x, hs.y, hs.z); + (result[3] as Vector3).vReset(-hs.x, hs.y, hs.z); + } + break; + } + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + return clipHighX(hs.x, pnum, verts2, verts1); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + } + return 0; + } + + /** + * + * @param hs + * @return + */ + private function clipByRect(hs:Vector3):int { + var pnum:int = 4; + if ((pnum = clipLowX(-hs.x, pnum, verts1, verts2)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, verts2, verts1)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, verts1, verts2)) == 0) return 0; + return clipHighY(hs.y, pnum, verts2, verts1); + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighX(x:Number, pnum:int, points:Vector., result:Vector.):int { + var x1:Number = x + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighY(y:Number, pnum:int, points:Vector., result:Vector.):int { + var y1:Number = y + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z - tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + private function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector.):int { + var z1:Number = z + tolerance; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + (result[num++] as Vector3).vCopy(p1); + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + (result[num++] as Vector3).vReset(p1.x + t*dx, p1.y + t*dy, p1.z + t*dz); + } + p1 = p2; + } + return num; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box первый бокс + * @param rect второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param colInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, axisIdx1:int, axisIdx2:int, colInfo:Contact):void { + box.transform.getAxis(axisIdx1, axis10); + rect.transform.getAxis(axisIdx2, axis20); + colAxis.vCross2(axis10, axis20).vNormalize(); + // Разворот оси в сторону бокса + if (colAxis.vDot(vectorToBox) < 0) colAxis.vReverse(); + + /* Кодирование рёбер + бит 0: 1 (тип контакта ребро-ребро) + биты 1-2: индекс направляющей оси ребра + бит 3: + бит 4: + бит 5: + */ + var edgeCode1:int = 1; + var edgeCode2:int = 1; + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + point1.vCopy(box.hs); + point2.vCopy(rect.hs); + // x + if (axisIdx1 == 0) { + point1.x = 0; + halfLen1 = box.hs.x; + } else { + box.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.x = -point1.x; + edgeCode1 |= 8; // 1 << 3 + } + } + if (axisIdx2 == 0) { + point2.x = 0; + halfLen2 = rect.hs.x; + } else { + rect.transform.getAxis(0, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.x = -point2.x; + edgeCode2 |= 8; // 1 << 3 + } + } + // y + if (axisIdx1 == 1) { + point1.y = 0; + halfLen1 = box.hs.y; + edgeCode1 |= 2; // 1 << 1 + } else { + box.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.y = -point1.y; + edgeCode1 |= 16; // 1 << 4 + } + } + if (axisIdx2 == 1) { + point2.y = 0; + halfLen2 = rect.hs.y; + edgeCode2 |= 2; // 1 << 1 + } else { + rect.transform.getAxis(1, tmpAxis); + if (tmpAxis.vDot(colAxis) < 0) { + point2.y = -point2.y; + edgeCode2 |= 16; // 1 << 4 + } + } + // z + if (axisIdx1 == 2) { + point1.z = 0; + halfLen1 = box.hs.z; + edgeCode1 |= 4; // 2 << 1 + } else { + box.transform.getAxis(2, tmpAxis); + if (tmpAxis.vDot(colAxis) > 0) { + point1.z = -point1.z; + edgeCode1 |= 32; // 1 << 5 + } + } + // Получаем глобальные координаты средних точек рёбер + point1.vTransformBy4(box.transform); + point2.vTransformBy4(rect.transform); + // Находим точку пересечения рёбер, решая систему уравнений + var k:Number = axis10.vDot(axis20); + var det:Number = k*k - 1; + vector.vDiff(point2, point1); + var c1:Number = axis10.vDot(vector); + var c2:Number = axis20.vDot(vector); + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + colInfo.normal.vCopy(colAxis); + colInfo.pcount = 1; + var cp:ContactPoint = colInfo.points[0]; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(point1.x + axis10.x*t1 + point2.x + axis20.x*t2); + cp.pos.y = 0.5*(point1.y + axis10.y*t1 + point2.y + axis20.y*t2); + cp.pos.z = 0.5*(point1.z + axis10.z*t1 + point2.z + axis20.z*t2); + cp.r1.vDiff(cp.pos, bPos); + cp.r2.vDiff(cp.pos, rPos); + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box + * @param rect + * @param axis + * @param axisIndex + * @param vectorToBox + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + if (axis.vLengthSqr() < 0.0001) return true; + axis.vNormalize(); + + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -tolerance) return false; + if (overlap + tolerance < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия вдоль заданной оси. + * + * @param box бокс + * @param rect прямоугольник + * @param axis ось + * @param vectorToBox вектор, соединяющий центр прямоугольника с центром бокса + * @return величина перекрытия вдоль оси + */ + public function overlapOnAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, vectorToBox:Vector3):Number { + var m:Matrix4 = box.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box.hs.z; + if (d < 0) d = -d; + projection += d; + + m = rect.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*rect.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*rect.hs.y; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox.x*axis.x + vectorToBox.y*axis.y + vectorToBox.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/collision/BoxSphereCollider.as b/0.1.Vector3D/src/alternativa/physics/collision/BoxSphereCollider.as new file mode 100644 index 0000000..2b166f3 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/BoxSphereCollider.as @@ -0,0 +1,104 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + use namespace altphysics; + /** + * + */ + public class BoxSphereCollider implements ICollider { + + private var center:Vector3 = new Vector3(); + private var closestPt:Vector3 = new Vector3(); + + private var bPos:Vector3 = new Vector3(); + private var sPos:Vector3 = new Vector3(); + + /** + * + */ + public function BoxSphereCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.vSubtract(closestPt).vLengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + // Зафиксированно столкновение + contact.body1 = sphere.body; + contact.body2 = box.body; + contact.normal.vCopy(closestPt).vTransformBy4(box.transform).vSubtract(sPos).vNormalize().vReverse(); + contact.pcount = 1; + + var cp:ContactPoint = contact.points[0]; + cp.penetration = sphere.r - Math.sqrt(distSqr); + cp.pos.vCopy(contact.normal).vScale(-sphere.r).vAdd(sPos); + cp.r1.vDiff(cp.pos, sPos); + cp.r2.vDiff(cp.pos, bPos); + + return true; + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/collision/BoxTriangleCollider.as b/0.1.Vector3D/src/alternativa/physics/collision/BoxTriangleCollider.as new file mode 100644 index 0000000..63c6efd --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/BoxTriangleCollider.as @@ -0,0 +1,14 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Contact; + import alternativa.physics.collision.primitives.CollisionPrimitive; + + public class BoxTriangleCollider implements ICollider { + public function BoxTriangleCollider() { + } + + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/collision/BruteForceCollisionDetector.as b/0.1.Vector3D/src/alternativa/physics/collision/BruteForceCollisionDetector.as new file mode 100644 index 0000000..d5160ef --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/BruteForceCollisionDetector.as @@ -0,0 +1,33 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + public class BruteForceCollisionDetector implements ICollisionDetector { + public function BruteForceCollisionDetector() { + } + + public function addPrimitive(primitive:CollisionPrimitive, isStatic:Boolean=true):Boolean { + return false; + } + + public function removePrimitive(primitive:CollisionPrimitive, isStatic:Boolean=true):Boolean { + return false; + } + + public function init():void { + } + + public function getAllCollisions(contacts:Vector.):int { + return 0; + } + + public function intersectRay(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, intersection:RayIntersection):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/collision/CollisionKdNode.as b/0.1.Vector3D/src/alternativa/physics/collision/CollisionKdNode.as new file mode 100644 index 0000000..e1a1657 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/CollisionKdNode.as @@ -0,0 +1,16 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.collision.types.BoundBox; + + public class CollisionKdNode { + public var objects:Vector.; + public var boundBox:BoundBox; + public var parent:CollisionKdNode; + + public var axis:int = -1; // 0 - x, 1 - y, 2 - z + public var coord:Number; + public var positiveNode:CollisionKdNode; + public var negativeNode:CollisionKdNode; + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/collision/CollisionKdTree.as b/0.1.Vector3D/src/alternativa/physics/collision/CollisionKdTree.as new file mode 100644 index 0000000..6c62b79 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/CollisionKdTree.as @@ -0,0 +1,259 @@ +package alternativa.physics.collision { + + import __AS3__.vec.Vector; + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + + /** + * @author mike + */ + public class CollisionKdTree { + + public var threshold:Number = 0.01; + public var minPrimitivesPerNode:int = 1; + + public var rootNode:CollisionKdNode; + public var staticChildren:Vector. = new Vector.(); + public var numStaticChildren:int; + private var staticBoundBoxes:Vector. = new Vector.(); + + /** + * @param primitive + */ + public function addStaticPrimitive(primitive:CollisionPrimitive):void { + staticChildren[numStaticChildren++] = primitive; + } + + /** + * @param primitive + * @return + */ + public function removeStaticPrimitive(primitive:CollisionPrimitive):Boolean { + var idx:int = staticChildren.indexOf(primitive); + if (idx < 0) return false; + staticChildren.splice(idx, 1); + numStaticChildren--; + return true; + } + + /** + * @param boundBox + */ + public function createTree(boundBox:BoundBox = null):void { + if (numStaticChildren == 0) return; + // Создаём корневую ноду + rootNode = new CollisionKdNode(); + rootNode.objects = new Vector.(); + // Расчитываем баунды объектов и рутовой ноды + var rootNodeBoundBox:BoundBox = rootNode.boundBox = (boundBox != null) ? boundBox : new BoundBox(); + for (var i:int = 0; i < numStaticChildren; i++) { + var child:CollisionPrimitive = staticChildren[i]; + var childBoundBox:BoundBox = staticBoundBoxes[i] = child.calculateAABB(); + rootNodeBoundBox.addBoundBox(childBoundBox); + rootNode.objects[i] = i; + } + staticBoundBoxes.length = numStaticChildren; + // Разделяем рутовую ноду + splitNode(rootNode); + } + + private var splitAxis:int; + private var splitCoord:Number; + private var splitCost:Number; + static private const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + static private const splitCoordsX:Vector. = new Vector.(); + static private const splitCoordsY:Vector. = new Vector.(); + static private const splitCoordsZ:Vector. = new Vector.(); + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + if (node.objects.length <= minPrimitivesPerNode) return; + + var objects:Vector. = node.objects; + var i:int, j:int, k:int, length:int = objects.length, c1:Number, c2:Number, ct1:Number, ct2:Number, area:Number, areaNegative:Number, areaPositive:Number, numNegative:int, numPositive:int, conflict:Boolean, cost:Number; + + var nodeBoundBox:BoundBox = node.boundBox; + + // Подготовка баунда с погрешностями + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + // Собираем опорные координаты + var numSplitCoordsX:int = 0, numSplitCoordsY:int = 0, numSplitCoordsZ:int = 0; + for (i = 0; i < length; i++) { + var boundBox:BoundBox = staticBoundBoxes[objects[i]]; + if (boundBox.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = boundBox.minX; + if (boundBox.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = boundBox.maxX; + + if (boundBox.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = boundBox.minY; + if (boundBox.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = boundBox.maxY; + + if (boundBox.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.minZ; + if (boundBox.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.maxZ; + } + + // Убираем дубликаты координат, ищем наилучший сплит + splitAxis = -1; splitCost = 1e308; + i = 0; area = (nodeBoundBox.maxY - nodeBoundBox.minY)*(nodeBoundBox.maxZ - nodeBoundBox.minZ); + while (i < numSplitCoordsX) { + if (isNaN(c1 = splitCoordsX[i++])) continue; + ct1 = c1 - threshold; + ct2 = c1 + threshold; + areaNegative = area*(c1 - nodeBoundBox.minX); + areaPositive = area*(nodeBoundBox.maxX - c1); + numNegative = numPositive = 0; + conflict = false; + // Проверяем объекты + for (j = 0; j < length; j++) { + boundBox = staticBoundBoxes[objects[j]]; + if (boundBox.maxX <= ct2) { + if (boundBox.minX < ct1) numNegative++; + } else { + if (boundBox.minX >= ct1) numPositive++; else {conflict = true; break;} + } + } + // Если хороший сплит, сохраняем + if (!conflict && (cost = areaNegative*numNegative + areaPositive*numPositive) < splitCost) { + splitCost = cost; + splitAxis = 0; + splitCoord = c1; + } + j = i; + while (++j < numSplitCoordsX) if ((c2 = splitCoordsX[j]) >= c1 - threshold && c2 <= c1 + threshold) splitCoordsX[j] = NaN; + } + i = 0; area = (nodeBoundBox.maxX - nodeBoundBox.minX)*(nodeBoundBox.maxZ - nodeBoundBox.minZ); + while (i < numSplitCoordsY) { + if (isNaN(c1 = splitCoordsY[i++])) continue; + ct1 = c1 - threshold; + ct2 = c1 + threshold; + areaNegative = area*(c1 - nodeBoundBox.minY); + areaPositive = area*(nodeBoundBox.maxY - c1); + numNegative = numPositive = 0; + conflict = false; + // Проверяем объекты + for (j = 0; j < length; j++) { + boundBox = staticBoundBoxes[objects[j]]; + if (boundBox.maxY <= ct2) { + if (boundBox.minY < ct1) numNegative++; + } else { + if (boundBox.minY >= ct1) numPositive++; else {conflict = true; break;} + } + } + // Если хороший сплит, сохраняем + if (!conflict && (cost = areaNegative*numNegative + areaPositive*numPositive) < splitCost) { + splitCost = cost; + splitAxis = 1; + splitCoord = c1; + } + j = i; + while (++j < numSplitCoordsY) if ((c2 = splitCoordsY[j]) >= c1 - threshold && c2 <= c1 + threshold) splitCoordsY[j] = NaN; + } + i = 0; area = (nodeBoundBox.maxX - nodeBoundBox.minX)*(nodeBoundBox.maxY - nodeBoundBox.minY); + while (i < numSplitCoordsZ) { + if (isNaN(c1 = splitCoordsZ[i++])) continue; + ct1 = c1 - threshold; + ct2 = c1 + threshold; + areaNegative = area*(c1 - nodeBoundBox.minZ); + areaPositive = area*(nodeBoundBox.maxZ - c1); + numNegative = numPositive = 0; + conflict = false; + // Проверяем объекты + for (j = 0; j < length; j++) { + boundBox = staticBoundBoxes[objects[j]]; + if (boundBox.maxZ <= ct2) { + if (boundBox.minZ < ct1) numNegative++; + } else { + if (boundBox.minZ >= ct1) numPositive++; else {conflict = true; break;} + } + } + // Если хороший сплит, сохраняем + if (!conflict && (cost = areaNegative*numNegative + areaPositive*numPositive) < splitCost) { + splitCost = cost; + splitAxis = 2; + splitCoord = c1; + } + j = i; + while (++j < numSplitCoordsZ) if ((c2 = splitCoordsZ[j]) >= c1 - threshold && c2 <= c1 + threshold) splitCoordsZ[j] = NaN; + } + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Разделяем ноду + var axisX:Boolean = splitAxis == 0, axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.positiveNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.positiveNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + ct1 = splitCoord - threshold; ct2 = splitCoord + threshold; + for (i = 0; i < length; i++) { + boundBox = staticBoundBoxes[objects[i]]; + var min:Number = axisX ? boundBox.minX : (axisY ? boundBox.minY : boundBox.minZ); + var max:Number = axisX ? boundBox.maxX : (axisY ? boundBox.maxY : boundBox.maxZ); + if (max <= ct2) { + // Объект в негативной стороне + if (node.negativeNode.objects == null) node.negativeNode.objects = new Vector.(); + node.negativeNode.objects.push(objects[i]); + objects[i] = -1; + } else { + if (min >= ct1) { + // Объект в положительной стороне + if (node.positiveNode.objects == null) node.positiveNode.objects = new Vector.(); + node.positiveNode.objects.push(objects[i]); + objects[i] = -1; + } else { + // Распилился + } + } + } + + // Очистка списка объектов + j = 0; + for (i = 0; i < length; i++) { + if (objects[i] >= 0) objects[j++] = objects[i]; + } + if (j > 0) objects.length = j; else node.objects = null; + + // Разделение дочерних нод + if (node.negativeNode.objects != null) splitNode(node.negativeNode); + if (node.positiveNode.objects != null) splitNode(node.positiveNode); + } + + /** + * + */ + public function traceTree():void { + traceNode("", rootNode); + } + + /** + * @param str + * @param node + */ + private function traceNode(str:String, node:CollisionKdNode):void { + if (node == null) return; + trace(str, node.axis == -1 ? "end" : ((node.axis == 0) ? "X" : ((node.axis == 1) ? "Y" : "Z")), "splitCoord=" + splitCoord, "bound", node.boundBox, "objs:", node.objects); + traceNode(str + "-", node.negativeNode); + traceNode(str + "+", node.positiveNode); + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/collision/ICollider.as b/0.1.Vector3D/src/alternativa/physics/collision/ICollider.as new file mode 100644 index 0000000..733231b --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/ICollider.as @@ -0,0 +1,12 @@ +package alternativa.physics.collision { + + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + + /** + * Интерфейс определителя столкновений между двумя примитивами. + */ + public interface ICollider { + function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/collision/ICollisionDetector.as b/0.1.Vector3D/src/alternativa/physics/collision/ICollisionDetector.as new file mode 100644 index 0000000..b555d17 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/ICollisionDetector.as @@ -0,0 +1,59 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + /** + * Интерфейс детектора столкновений. + */ + public interface ICollisionDetector { + + /** + * Добавляет физический примитив в коллайдер. + * + * @param primitive добавляемый примитив + * @param isStatic указывает тип примитива: статический или динамический + * @return true если примитив был успешно добавлен, иначе false + */ + function addPrimitive(primitive:CollisionPrimitive, isStatic:Boolean = true):Boolean; + + /** + * Удаляет физический примитив из коллайдера. + * + * @param primitive удаляемый примитив + * @param isStatic указывает тип примитива: статический или динамический + * @return true если примитив был успшено удалён, иначе false + */ + function removePrimitive(primitive:CollisionPrimitive, isStatic:Boolean = true):Boolean; + + /** + * Выполняет инициализацию детектора после обновления списка примитивов. + */ + function init():void; + + /** + * Получает все столкновения в текущей конфигурации физической геометрии. + * + * @param contacts список контактов, в кторые будет записана информация о столкновении + * @return количество найденных столкновений + */ + function getAllCollisions(contacts:Vector.):int; + + /** + * Тестирует луч на пересечение с физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча. + * + * @param origin + * @param dir + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента + * @param predicate + * @param intersection переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function intersectRay(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, intersection:RayIntersection):Boolean; + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/collision/ICollisionPredicate.as b/0.1.Vector3D/src/alternativa/physics/collision/ICollisionPredicate.as new file mode 100644 index 0000000..bb1b756 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/ICollisionPredicate.as @@ -0,0 +1,9 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + + public interface ICollisionPredicate { + + function considerCollision(primitive:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/collision/IRayCollisionPredicate.as b/0.1.Vector3D/src/alternativa/physics/collision/IRayCollisionPredicate.as new file mode 100644 index 0000000..0f3d929 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/IRayCollisionPredicate.as @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.rigid.Body; + + public interface IRayCollisionPredicate { + function considerBody(body:Body):Boolean; + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/collision/KdTreeCollisionDetector.as b/0.1.Vector3D/src/alternativa/physics/collision/KdTreeCollisionDetector.as new file mode 100644 index 0000000..1134885 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/KdTreeCollisionDetector.as @@ -0,0 +1,412 @@ +package alternativa.physics.collision { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.collision.types.RayIntersection; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Детектор, хранящий статическую геометрию в kD-дереве и использующий дерево для ускорения тестов на пересечения. + */ + public class KdTreeCollisionDetector implements ICollisionDetector { + + altphysics var tree:CollisionKdTree; + altphysics var dynamicPrimitives:Vector.; + altphysics var dynamicPrimitivesNum:int; + altphysics var threshold:Number = 0.0001; + private var colliders:Object = {}; + + private var gtMax:Number = 0; + private var gtMin:Number = 0; + 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.(); + + addCollider(CollisionPrimitive.BOX, CollisionPrimitive.BOX, new BoxBoxCollider()); +// addCollider(CollisionPrimitive.BOX, CollisionPrimitive.PLANE, new BoxPlaneCollider()); + addCollider(CollisionPrimitive.BOX, CollisionPrimitive.SPHERE, new BoxSphereCollider()); + addCollider(CollisionPrimitive.BOX, CollisionPrimitive.RECT, new BoxRectCollider()); + +// addCollider(CollisionPrimitive.SPHERE, CollisionPrimitive.PLANE, new SpherePlaneCollider()); + addCollider(CollisionPrimitive.SPHERE, CollisionPrimitive.SPHERE, new SphereSphereCollider()); + } + + /** + * + * @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 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():void { + tree.createTree(); +// tree.traceTree(); + } + + /** + * + * @param contacts + * @return + */ + public function getAllCollisions(contacts:Vector.):int { + var colNum:int = 0; + for (var i:int = 0; i < dynamicPrimitivesNum; i++) { + var primitive:CollisionPrimitive = dynamicPrimitives[i]; + primitive.calculateAABB(); + colNum += getPrimitiveNodeCollisions(tree.rootNode, primitive, contacts, colNum); + + // Столкновения динамических примитивов между собой + // TODO: Попробовать различные оптимизации (сортировка по баундам, встраивание в дерево) + for (var j:int = i + 1; j < dynamicPrimitivesNum; j++) { + if (collide(primitive, dynamicPrimitives[j], contacts[colNum])) colNum++; + } + } + + return colNum; + } + + /** + * Выполняет поиск столкновений динамического примитива с примитивами узла дерева. + * + * @param node + * @param primitive + * @param contacts + * @param startIndex + * @return + */ + private function getPrimitiveNodeCollisions(node:CollisionKdNode, primitive:CollisionPrimitive, contacts:Vector., startIndex:int):int { + var colNum:int = 0; + if (node.objects != null) { + // Поиск столкновений со статическими примитивами узла + var primitives:Vector. = tree.staticChildren; + var indices:Vector. = node.objects; + for (var i:int = indices.length - 1; i >= 0; i--) + if (collide(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; + } + + /** + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + public function collide(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.collide(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 = (contact.points[0] as ContactPoint).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 origin + * @param dir + * @param collisionGroup + * @param maxTime + * @param result + * @return + */ + public function intersectRay(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean { + var hasStatIntersection:Boolean = intersectRayWithStatic(origin, dir, collisionGroup, maxTime, predicate, result); + var hasDynIntersection:Boolean = intersectRayWithDynamic(origin, dir, collisionGroup, maxTime, predicate, _dynamicIntersection); + + if (!(hasDynIntersection || hasStatIntersection)) return false; + if (hasDynIntersection && hasStatIntersection) { + if (result.t < _dynamicIntersection.t) return result.t < maxTime; + else { + if (_dynamicIntersection.t < maxTime) { + result.copy(_dynamicIntersection); + return true; + } else return false; + } + } + if (hasStatIntersection) return result.t < maxTime; + else { + if (_dynamicIntersection.t < maxTime) { + result.copy(_dynamicIntersection); + return true; + } else return false; + } + } + + /** + * Тест пересечения луча с динамическими примитивами. + * + * @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 minTime:Number = maxTime + 1; + for (var i:int = 0; i < dynamicPrimitivesNum; i++) { + var primitive:CollisionPrimitive = dynamicPrimitives[i]; + if ((primitive.collisionGroup & collisionGroup) == 0) continue; + if (predicate != null && primitive.body != null && !predicate.considerBody(primitive.body)) continue; + var t:Number = primitive.getSegmentIntersection(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; + } + + /** + * @param origin + * @param dir + * @param collisionGroup + * @param maxTime + * @param intersection + * @return + */ + private function intersectRayWithStatic(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayIntersection):Boolean { + // Вычислим точки пересечения с корневм узлом + if (!getRayBoundBoxIntersection(origin, dir, tree.rootNode.boundBox)) return false; + if (gtMax < 0 || gtMin > maxTime) return false; + + if (gtMin < 0) { + gtMin = 0; + _o.x = origin.x; + _o.y = origin.y; + _o.z = origin.z; + } + else { + _o.x = origin.x + gtMin*dir.x; + _o.y = origin.y + gtMin*dir.y; + _o.z = origin.z + gtMin*dir.z; + } + if (gtMax > maxTime) gtMax = maxTime; + + return testRayAgainstNode(tree.rootNode, origin, _o, dir, collisionGroup, gtMin, gtMax, predicate, result); + } + + /** + * @param ray + * @param boundBox + */ + private function getRayBoundBoxIntersection(origin:Vector3, dir:Vector3, bb:BoundBox):Boolean { + gtMin = -1; + gtMax = 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 > gtMin) gtMin = t1; + if (t2 < gtMax) gtMax = t2; + } else { + if (t2 > gtMin) gtMin = t2; + if (t1 < gtMax) gtMax = t1; + } + if (gtMax < gtMin) 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.objects != null && getRayNodeIntersection(origin, dir, collisionGroup, tree.staticChildren, node.objects, 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., indices:Vector., 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.getSegmentIntersection(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; + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/collision/SpherePlaneCollider.as b/0.1.Vector3D/src/alternativa/physics/collision/SpherePlaneCollider.as new file mode 100644 index 0000000..9b527d8 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/SpherePlaneCollider.as @@ -0,0 +1,58 @@ +package alternativa.physics.collision { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.rigid.Contact; + import alternativa.physics.types.Vector3; + + /** + * + */ + public class SpherePlaneCollider implements ICollider { + + private var normal:Vector3 = new Vector3(); + + /** + * + */ + public function SpherePlaneCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var sphere:RigidSphere = body1 as RigidSphere; +// var plane:RigidPlane; +// if (sphere == null) { +// sphere = body2 as RigidSphere; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// +// var dist:Number = sphere.state.pos.dot(normal) - offset; +// if (dist > sphere.r) return false; +// +// collisionInfo.body1 = sphere; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// collisionInfo.pcount = 1; +// +// var cp:ContactPoint = collisionInfo.points[0]; +// cp.penetration = sphere.r - dist; +// cp.pos.copy(normal).reverse().scale(sphere.r).add(sphere.state.pos); +// cp.r1.diff(cp.pos, sphere.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); + + return true; + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/collision/SphereSphereCollider.as b/0.1.Vector3D/src/alternativa/physics/collision/SphereSphereCollider.as new file mode 100644 index 0000000..9111a84 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/SphereSphereCollider.as @@ -0,0 +1,60 @@ +package alternativa.physics.collision { + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Contact; + import alternativa.physics.rigid.ContactPoint; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + public class SphereSphereCollider implements ICollider { + + private var p1:Vector3 = new Vector3(); + private var p2:Vector3 = new Vector3(); + + public function SphereSphereCollider() { + } + + public function collide(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var s1:CollisionSphere; + var s2:CollisionSphere; + if (prim1.body != null) { + s1 = prim1 as CollisionSphere; + s2 = prim2 as CollisionSphere; + } else { + s1 = prim2 as CollisionSphere; + s2 = prim1 as CollisionSphere; + } + + s1.transform.getAxis(3, p1); + s2.transform.getAxis(3, p2); + var dx:Number = p1.x - p2.x; + var dy:Number = p1.y - p2.y; + var dz:Number = p1.z - p2.z; + var len:Number = dx*dx + dy*dy + dz*dz; + var sum:Number = s1.r + s2.r; + if (len > sum*sum) return false; + len = Math.sqrt(len); + dx /= len; + dy /= len; + dz /= len; + + contact.body1 = s1.body; + contact.body2 = s2.body; + contact.normal.x = dx; + contact.normal.y = dy; + contact.normal.z = dz; + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = sum - len; + cp.pos.x = p1.x - dx*s1.r; + cp.pos.y = p1.y - dy*s1.r; + cp.pos.z = p1.z - dz*s1.r; + cp.r1.vDiff(cp.pos, p1); + cp.r2.vDiff(cp.pos, p2); + + return true; + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/collision/primitives/.svn/all-wcprops b/0.1.Vector3D/src/alternativa/physics/collision/primitives/.svn/all-wcprops new file mode 100644 index 0000000..3f58acb --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/primitives/.svn/all-wcprops @@ -0,0 +1,35 @@ +K 25 +svn:wc:ra_dav:version-url +V 133 +/!svn/ver/13893/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/collision/primitives +END +CollisionTriangle.as +K 25 +svn:wc:ra_dav:version-url +V 154 +/!svn/ver/13893/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/collision/primitives/CollisionTriangle.as +END +CollisionSphere.as +K 25 +svn:wc:ra_dav:version-url +V 152 +/!svn/ver/13892/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/collision/primitives/CollisionSphere.as +END +CollisionBox.as +K 25 +svn:wc:ra_dav:version-url +V 149 +/!svn/ver/13893/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/collision/primitives/CollisionBox.as +END +CollisionPrimitive.as +K 25 +svn:wc:ra_dav:version-url +V 155 +/!svn/ver/13892/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/collision/primitives/CollisionPrimitive.as +END +CollisionRect.as +K 25 +svn:wc:ra_dav:version-url +V 150 +/!svn/ver/13893/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/collision/primitives/CollisionRect.as +END diff --git a/0.1.Vector3D/src/alternativa/physics/collision/primitives/.svn/entries b/0.1.Vector3D/src/alternativa/physics/collision/primitives/.svn/entries new file mode 100644 index 0000000..1b81423 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/primitives/.svn/entries @@ -0,0 +1,88 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/collision/primitives +http://svndev.alternativaplatform.com + + + +2009-06-03T09:16:50.564913Z +13893 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +CollisionTriangle.as +file + + + + +2010-10-28T04:32:39.000000Z +a1ccd11eb25d172d2739fce7fef21ee2 +2009-06-03T09:16:50.564913Z +13893 +mike + +CollisionSphere.as +file + + + + +2010-10-28T04:32:39.000000Z +1ca67429cc8bf27435b6ec46f8d6f80a +2009-05-26T08:41:33.471001Z +13389 +mike + +CollisionBox.as +file + + + + +2010-10-28T04:32:39.000000Z +8612a8b83910ba60c39918849caf106a +2009-06-03T09:16:50.564913Z +13893 +mike + +CollisionPrimitive.as +file + + + + +2010-10-28T04:32:39.000000Z +0461146e5497c532c9420b49331d9884 +2009-05-26T08:41:33.471001Z +13389 +mike + +CollisionRect.as +file + + + + +2010-10-28T04:32:39.000000Z +5c866ac8da0f6951375b77f713d73f52 +2009-06-03T09:16:50.564913Z +13893 +mike + diff --git a/0.1.Vector3D/src/alternativa/physics/collision/primitives/.svn/format b/0.1.Vector3D/src/alternativa/physics/collision/primitives/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/primitives/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.1.Vector3D/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base b/0.1.Vector3D/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base new file mode 100644 index 0000000..589536e --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base @@ -0,0 +1,160 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + /** + * Ориентированный бокс. + */ + public class CollisionBox extends CollisionPrimitive { + + // Половинные размеры вдоль каждой из осей + public var hs:Vector3 = new Vector3(); + + // Используются в определении пересечения с лучом + private static var _o:Vector3 = new Vector3(); + private static var _v:Vector3 = new Vector3(); + + /** + * @param hs + * @param collisionGroup + */ + public function CollisionBox(hs:Vector3, collisionGroup:int) { + super(BOX, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + var t:Matrix4 = transform; + aabb.maxX = hs.x*(t.a < 0 ? -t.a : t.a) + hs.y*(t.b < 0 ? -t.b : t.b) + hs.z*(t.c < 0 ? -t.c : t.c); + aabb.minX = -aabb.maxX; + + aabb.maxY = hs.x*(t.e < 0 ? -t.e : t.e) + hs.y*(t.f < 0 ? -t.f : t.f) + hs.z*(t.g < 0 ? -t.g : t.g); + aabb.minY = -aabb.maxY; + + aabb.maxZ = hs.x*(t.i < 0 ? -t.i : t.i) + hs.y*(t.j < 0 ? -t.j : t.j) + hs.z*(t.k < 0 ? -t.k : t.k); + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var box:CollisionBox = source as CollisionBox; + if (box == null) return this; + super.copyFrom(box); + hs.vCopy(box.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionBox(hs, collisionGroup); + } + + /** + * @param origin + * @param vector + * @param threshold + * @param normal + * @return + */ + override public function getSegmentIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var tMin:Number = -1, tMax:Number = 1e308, t1:Number, t2:Number; + + transform.transformVectorInverse(origin, _o); + transform.deltaTransformVectorInverse(vector, _v); + // X + if (_v.x < threshold && _v.x > -threshold) { + if (_o.x < -hs.x || _o.x > hs.x) return -1; + } else { + t1 = (-hs.x - _o.x)/_v.x; + t2 = (hs.x - _o.x)/_v.x; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.x = -1; + normal.y = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.x = 1; + normal.y = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Y + if (_v.y < threshold && _v.y > -threshold) { + if (_o.y < -hs.y || _o.y > hs.y) return -1; + } else { + t1 = (-hs.y - _o.y)/_v.y; + t2 = (hs.y - _o.y)/_v.y; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.y = -1; + normal.x = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.y = 1; + normal.x = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Z + if (_v.z < threshold && _v.z > -threshold) { + if (_o.z < -hs.z || _o.z > hs.z) return -1; + } else { + t1 = (-hs.z - _o.z)/_v.z; + t2 = (hs.z - _o.z)/_v.z; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.z = -1; + normal.x = normal.y = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.z = 1; + normal.x = normal.y = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + normal.vDeltaTransformBy4(transform); + return tMin; + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionPrimitive.as.svn-base b/0.1.Vector3D/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionPrimitive.as.svn-base new file mode 100644 index 0000000..72d5c7a --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionPrimitive.as.svn-base @@ -0,0 +1,128 @@ +package alternativa.physics.collision.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.ICollisionPredicate; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Базовый класс для примитивов, использующихся детектором столкновений. + */ + public class CollisionPrimitive { + // Константы типов примитива + public static const BOX:int = 1; + public static const PLANE:int = 2; + public static const SPHERE:int = 3; + public static const RECT:int = 4; + public static const TRIANGLE:int = 5; + + // Тип примитива + public var type:int; + // Группы примитива. Каждая группа определяется установленным битом. Столкновения проверяются только для примитивов, + // имеющих хотя бы одну общую группу. + public var collisionGroup:int; + // Предикат примитива, вызывающийся детектором при нахождении столкновения. В зависимости от возвращённого + // предикатом результата столкновение либо регистрируется, либо игнорируется. Nullable. + public var postCollisionPredicate:ICollisionPredicate; + + // Тело, владеющее примитивом. Может быть null. + altphysics var body:Body; + // Трансформация примитива в системе координат тела, если оно указано. Не допускается масштабирование матрицы. + altphysics var localTransform:Matrix4 = new Matrix4(); + // Полная трансформация примитива. Не допускается масштабирование матрицы. + altphysics var transform:Matrix4 = new Matrix4(); + // AABB в мировой системе координат. Расчитывается системой вызовом функции calculateBoundBox(). + altphysics var aabb:BoundBox = new BoundBox(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param type тип примитива + * @param collisionGroup группа примитива + */ + public function CollisionPrimitive(type:int, collisionGroup:int) { + this.type = type; + this.collisionGroup = collisionGroup; + } + + /** + * Устанавливает тело, владеющее примитивом. + * + * @param body тело, к которому относится примитив. Если указано значение null, матрица локальной трансформации удаляется. + * @param localTransform трансформация примитива в системе координат тела. Указание значения null равносильно + * заданию единичной матрицы. + */ + public function setBody(body:Body, localTransform:Matrix4 = null):void { + if (this.body == body) return; + this.body = body; + if (body == null) this.localTransform = null; + else { + if (this.localTransform == null) this.localTransform = new Matrix4(); + if (localTransform != null) this.localTransform.copy(localTransform); + } + } + + /** + * Наследники должны переопределять этот метод, реализуя в нём корректный расчёт ограничивающего бокса, выравненного + * по осям мировой системы координат. + * + * @return ссылка на свой баунд бокс + */ + public function calculateAABB():BoundBox { + return aabb; + } + + /** + * Клонирует примитив. Переопределять не рекомендуется. Вместо этого переопределяются методы createPrimitive() и copyFrom(). + * + * @return клон примитива + */ + public function clone():CollisionPrimitive { + var p:CollisionPrimitive = createPrimitive(); + return p.copyFrom(this); + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + type = source.type; + transform.copy(source.transform); + collisionGroup = source.collisionGroup; + setBody(source.body, source.localTransform); + aabb.copyFrom(source.aabb); + return this; + } + + /** + * Создаёт новый экземпляр примитива соотвествующего типа. + * + * @return новый экземпляр примитива + */ + protected function createPrimitive():CollisionPrimitive { + return new CollisionPrimitive(type, collisionGroup); + } + + /** + * Метод должен вычислять параметры пересечения заданного сегмента с примитивом. + * + * @param origin начальная точка сегмента в мировых координатах + * @param vector вектор сегмента в мировых координатах + * @param threshold погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную + * погрешность, считается равной нулю. + * @param normal переменная, в которую записывается нормаль к примитиву в точке пересечения с сегментом + * @return в случае наличия пересечения возвращается время точки пересечения. В противном случае возвращается -1. + */ + public function getSegmentIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + return -1; + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base b/0.1.Vector3D/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base new file mode 100644 index 0000000..8f38466 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base @@ -0,0 +1,117 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + /** + * Ориентированный прямоугольник. Задаётся половинами размеров вдоль осей X (ширина) и Y (длина) локальной системы + * координат примитива. Таким образом, прямоугольник лежит в плоскости XY, его стороны параллельны осям этой + * плоскости, а нормаль направлена вдоль локальной оси Z. + * + * Прямоугольник может быть одно- или двусторонним. В случае одностороннего прямоугольника, + * столкновения с внутренней стороны не регистрируются. + */ + public class CollisionRect extends CollisionPrimitive { + // Половинные размеры прямоугольника вдоль осей X и Y. Нормаль направлена вдоль оси Z. + public var hs:Vector3 = new Vector3(); + // Флаг указывает, является примитив одно- или двусторонним + public var twoSided:Boolean = true; + + // Малое значение. Используется для указания фиктивной высоты примитива и в функции определения пересечения с сегментом. + private static const EPSILON:Number = 0.01; + // Переменные используются в функции поиска пересечения с лучом + private static var _o:Vector3 = new Vector3(); + private static var _v:Vector3 = new Vector3(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param hs половинные размерв прямоугольника вдоль осей X и Y. Значение z игнорируется. + * @param collisionGroup группа примитива + */ + public function CollisionRect(hs:Vector3, collisionGroup:int) { + super(RECT, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * Расчитывает ограничивающий бокс прямоугольника. Для избежания проблем высота примитива принимается равной + * не нулю, а малому значению. + * + * @return + */ + override public function calculateAABB():BoundBox { + // Баунд бокс прямоугольника имеет минимальную высоту, отличную от нуля во избежание проблем с построением kd-дерева + var t:Matrix4 = transform; + aabb.maxX = hs.x*(t.a < 0 ? -t.a : t.a) + hs.y*(t.b < 0 ? -t.b : t.b) + EPSILON*(t.c < 0 ? -t.c : t.c); + aabb.minX = -aabb.maxX; + + aabb.maxY = hs.x*(t.e < 0 ? -t.e : t.e) + hs.y*(t.f < 0 ? -t.f : t.f) + EPSILON*(t.g < 0 ? -t.g : t.g); + aabb.minY = -aabb.maxY; + + aabb.maxZ = hs.x*(t.i < 0 ? -t.i : t.i) + hs.y*(t.j < 0 ? -t.j : t.j) + EPSILON*(t.k < 0 ? -t.k : t.k); + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var rect:CollisionRect = source as CollisionRect; + if (rect == null) return this; + super.copyFrom(rect); + hs.vCopy(rect.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionRect(hs, collisionGroup); + } + + /** + * @param origin + * @param vector + * @param collisionGroup + * @param threshold + * @param normal + * @return + */ + override public function getSegmentIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + // Переводим параметры сегмента в систему координат примитива + transform.transformVectorInverse(origin, _o); + transform.deltaTransformVectorInverse(vector, _v); + + // Проверка параллельности сегмента и плоскости примитива + if (_v.z > -threshold && _v.z < threshold) return -1; + var t:Number = -_o.z/_v.z; + if (t < 0) return -1; + // Проверка вхождения точки пересечения в прямоугольник + _o.x += _v.x*t; + _o.y += _v.y*t; + _o.z = 0; + if (_o.x < (-hs.x - EPSILON) || _o.x > (hs.x + EPSILON) || _o.y < (-hs.y - EPSILON) || _o.y > (hs.y + EPSILON)) return -1; + + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + return t; + } + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base b/0.1.Vector3D/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base new file mode 100644 index 0000000..1a9e6d1 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base @@ -0,0 +1,80 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + + /** + * Сфера. + */ + public class CollisionSphere extends CollisionPrimitive { + + // Радиус сферы + public var r:Number = 0; + + /** + * + * @param r + * @param collisionGroup + */ + public function CollisionSphere(r:Number, collisionGroup:int) { + super(SPHERE, collisionGroup); + this.r = r; + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + aabb.maxX = transform.d + r; + aabb.minX = transform.d - r; + + aabb.maxY = transform.h + r; + aabb.minY = transform.h - r; + + aabb.maxZ = transform.l + r; + aabb.minZ = transform.l - r; + + return aabb; + } + + /** + * @param origin + * @param vector + * @param threshold + * @param normal + * @return + */ + override public function getSegmentIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var px:Number = origin.x - transform.d; + var py:Number = origin.y - transform.h; + var pz:Number = origin.z - transform.l; + var k:Number = vector.x*px + vector.y*py + vector.z*pz; + if (k > 0) return -1; + var a:Number = vector.x*vector.x + vector.y*vector.y + vector.z*vector.z; + var D:Number = k*k - a*(px*px + py*py + pz*pz - r*r); + if (D < 0) return -1; + return -(k + Math.sqrt(D))/a; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var sphere:CollisionSphere = source as CollisionSphere; + if (sphere == null) return this; + super.copyFrom(sphere); + r = sphere.r; + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionSphere(r, collisionGroup); + } + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base b/0.1.Vector3D/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base new file mode 100644 index 0000000..a48803a --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base @@ -0,0 +1,39 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + public class CollisionTriangle extends CollisionPrimitive { + + public var v0:Vector3 = new Vector3(); + public var v1:Vector3 = new Vector3(); + public var v2:Vector3 = new Vector3(); + public var e0:Vector3 = new Vector3(); + public var e1:Vector3 = new Vector3(); + public var e2:Vector3 = new Vector3(); + public var len0:Number; + public var len1:Number; + public var len2:Number; + + public function CollisionTriangle(v0:Vector3, v1:Vector3, v2:Vector3, collisionGroup:int) { + super(TRIANGLE, collisionGroup); + + this.v0.vCopy(v0); + this.v1.vCopy(v1); + this.v2.vCopy(v2); + + e0.vDiff(v1, v0); + len0 = e0.vLength(); + e0.vNormalize(); + + e1.vDiff(v2, v1); + len1 = e1.vLength(); + e1.vNormalize(); + + e2.vDiff(v0, v2); + len2 = e2.vLength(); + e2.vNormalize(); + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/collision/primitives/CollisionBox.as b/0.1.Vector3D/src/alternativa/physics/collision/primitives/CollisionBox.as new file mode 100644 index 0000000..589536e --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/primitives/CollisionBox.as @@ -0,0 +1,160 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + /** + * Ориентированный бокс. + */ + public class CollisionBox extends CollisionPrimitive { + + // Половинные размеры вдоль каждой из осей + public var hs:Vector3 = new Vector3(); + + // Используются в определении пересечения с лучом + private static var _o:Vector3 = new Vector3(); + private static var _v:Vector3 = new Vector3(); + + /** + * @param hs + * @param collisionGroup + */ + public function CollisionBox(hs:Vector3, collisionGroup:int) { + super(BOX, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + var t:Matrix4 = transform; + aabb.maxX = hs.x*(t.a < 0 ? -t.a : t.a) + hs.y*(t.b < 0 ? -t.b : t.b) + hs.z*(t.c < 0 ? -t.c : t.c); + aabb.minX = -aabb.maxX; + + aabb.maxY = hs.x*(t.e < 0 ? -t.e : t.e) + hs.y*(t.f < 0 ? -t.f : t.f) + hs.z*(t.g < 0 ? -t.g : t.g); + aabb.minY = -aabb.maxY; + + aabb.maxZ = hs.x*(t.i < 0 ? -t.i : t.i) + hs.y*(t.j < 0 ? -t.j : t.j) + hs.z*(t.k < 0 ? -t.k : t.k); + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var box:CollisionBox = source as CollisionBox; + if (box == null) return this; + super.copyFrom(box); + hs.vCopy(box.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionBox(hs, collisionGroup); + } + + /** + * @param origin + * @param vector + * @param threshold + * @param normal + * @return + */ + override public function getSegmentIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var tMin:Number = -1, tMax:Number = 1e308, t1:Number, t2:Number; + + transform.transformVectorInverse(origin, _o); + transform.deltaTransformVectorInverse(vector, _v); + // X + if (_v.x < threshold && _v.x > -threshold) { + if (_o.x < -hs.x || _o.x > hs.x) return -1; + } else { + t1 = (-hs.x - _o.x)/_v.x; + t2 = (hs.x - _o.x)/_v.x; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.x = -1; + normal.y = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.x = 1; + normal.y = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Y + if (_v.y < threshold && _v.y > -threshold) { + if (_o.y < -hs.y || _o.y > hs.y) return -1; + } else { + t1 = (-hs.y - _o.y)/_v.y; + t2 = (hs.y - _o.y)/_v.y; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.y = -1; + normal.x = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.y = 1; + normal.x = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Z + if (_v.z < threshold && _v.z > -threshold) { + if (_o.z < -hs.z || _o.z > hs.z) return -1; + } else { + t1 = (-hs.z - _o.z)/_v.z; + t2 = (hs.z - _o.z)/_v.z; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.z = -1; + normal.x = normal.y = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.z = 1; + normal.x = normal.y = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + normal.vDeltaTransformBy4(transform); + return tMin; + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/collision/primitives/CollisionPrimitive.as b/0.1.Vector3D/src/alternativa/physics/collision/primitives/CollisionPrimitive.as new file mode 100644 index 0000000..72d5c7a --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/primitives/CollisionPrimitive.as @@ -0,0 +1,128 @@ +package alternativa.physics.collision.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.ICollisionPredicate; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Базовый класс для примитивов, использующихся детектором столкновений. + */ + public class CollisionPrimitive { + // Константы типов примитива + public static const BOX:int = 1; + public static const PLANE:int = 2; + public static const SPHERE:int = 3; + public static const RECT:int = 4; + public static const TRIANGLE:int = 5; + + // Тип примитива + public var type:int; + // Группы примитива. Каждая группа определяется установленным битом. Столкновения проверяются только для примитивов, + // имеющих хотя бы одну общую группу. + public var collisionGroup:int; + // Предикат примитива, вызывающийся детектором при нахождении столкновения. В зависимости от возвращённого + // предикатом результата столкновение либо регистрируется, либо игнорируется. Nullable. + public var postCollisionPredicate:ICollisionPredicate; + + // Тело, владеющее примитивом. Может быть null. + altphysics var body:Body; + // Трансформация примитива в системе координат тела, если оно указано. Не допускается масштабирование матрицы. + altphysics var localTransform:Matrix4 = new Matrix4(); + // Полная трансформация примитива. Не допускается масштабирование матрицы. + altphysics var transform:Matrix4 = new Matrix4(); + // AABB в мировой системе координат. Расчитывается системой вызовом функции calculateBoundBox(). + altphysics var aabb:BoundBox = new BoundBox(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param type тип примитива + * @param collisionGroup группа примитива + */ + public function CollisionPrimitive(type:int, collisionGroup:int) { + this.type = type; + this.collisionGroup = collisionGroup; + } + + /** + * Устанавливает тело, владеющее примитивом. + * + * @param body тело, к которому относится примитив. Если указано значение null, матрица локальной трансформации удаляется. + * @param localTransform трансформация примитива в системе координат тела. Указание значения null равносильно + * заданию единичной матрицы. + */ + public function setBody(body:Body, localTransform:Matrix4 = null):void { + if (this.body == body) return; + this.body = body; + if (body == null) this.localTransform = null; + else { + if (this.localTransform == null) this.localTransform = new Matrix4(); + if (localTransform != null) this.localTransform.copy(localTransform); + } + } + + /** + * Наследники должны переопределять этот метод, реализуя в нём корректный расчёт ограничивающего бокса, выравненного + * по осям мировой системы координат. + * + * @return ссылка на свой баунд бокс + */ + public function calculateAABB():BoundBox { + return aabb; + } + + /** + * Клонирует примитив. Переопределять не рекомендуется. Вместо этого переопределяются методы createPrimitive() и copyFrom(). + * + * @return клон примитива + */ + public function clone():CollisionPrimitive { + var p:CollisionPrimitive = createPrimitive(); + return p.copyFrom(this); + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + type = source.type; + transform.copy(source.transform); + collisionGroup = source.collisionGroup; + setBody(source.body, source.localTransform); + aabb.copyFrom(source.aabb); + return this; + } + + /** + * Создаёт новый экземпляр примитива соотвествующего типа. + * + * @return новый экземпляр примитива + */ + protected function createPrimitive():CollisionPrimitive { + return new CollisionPrimitive(type, collisionGroup); + } + + /** + * Метод должен вычислять параметры пересечения заданного сегмента с примитивом. + * + * @param origin начальная точка сегмента в мировых координатах + * @param vector вектор сегмента в мировых координатах + * @param threshold погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную + * погрешность, считается равной нулю. + * @param normal переменная, в которую записывается нормаль к примитиву в точке пересечения с сегментом + * @return в случае наличия пересечения возвращается время точки пересечения. В противном случае возвращается -1. + */ + public function getSegmentIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + return -1; + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/collision/primitives/CollisionRect.as b/0.1.Vector3D/src/alternativa/physics/collision/primitives/CollisionRect.as new file mode 100644 index 0000000..8f38466 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/primitives/CollisionRect.as @@ -0,0 +1,117 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + /** + * Ориентированный прямоугольник. Задаётся половинами размеров вдоль осей X (ширина) и Y (длина) локальной системы + * координат примитива. Таким образом, прямоугольник лежит в плоскости XY, его стороны параллельны осям этой + * плоскости, а нормаль направлена вдоль локальной оси Z. + * + * Прямоугольник может быть одно- или двусторонним. В случае одностороннего прямоугольника, + * столкновения с внутренней стороны не регистрируются. + */ + public class CollisionRect extends CollisionPrimitive { + // Половинные размеры прямоугольника вдоль осей X и Y. Нормаль направлена вдоль оси Z. + public var hs:Vector3 = new Vector3(); + // Флаг указывает, является примитив одно- или двусторонним + public var twoSided:Boolean = true; + + // Малое значение. Используется для указания фиктивной высоты примитива и в функции определения пересечения с сегментом. + private static const EPSILON:Number = 0.01; + // Переменные используются в функции поиска пересечения с лучом + private static var _o:Vector3 = new Vector3(); + private static var _v:Vector3 = new Vector3(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param hs половинные размерв прямоугольника вдоль осей X и Y. Значение z игнорируется. + * @param collisionGroup группа примитива + */ + public function CollisionRect(hs:Vector3, collisionGroup:int) { + super(RECT, collisionGroup); + this.hs.vCopy(hs); + } + + /** + * Расчитывает ограничивающий бокс прямоугольника. Для избежания проблем высота примитива принимается равной + * не нулю, а малому значению. + * + * @return + */ + override public function calculateAABB():BoundBox { + // Баунд бокс прямоугольника имеет минимальную высоту, отличную от нуля во избежание проблем с построением kd-дерева + var t:Matrix4 = transform; + aabb.maxX = hs.x*(t.a < 0 ? -t.a : t.a) + hs.y*(t.b < 0 ? -t.b : t.b) + EPSILON*(t.c < 0 ? -t.c : t.c); + aabb.minX = -aabb.maxX; + + aabb.maxY = hs.x*(t.e < 0 ? -t.e : t.e) + hs.y*(t.f < 0 ? -t.f : t.f) + EPSILON*(t.g < 0 ? -t.g : t.g); + aabb.minY = -aabb.maxY; + + aabb.maxZ = hs.x*(t.i < 0 ? -t.i : t.i) + hs.y*(t.j < 0 ? -t.j : t.j) + EPSILON*(t.k < 0 ? -t.k : t.k); + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var rect:CollisionRect = source as CollisionRect; + if (rect == null) return this; + super.copyFrom(rect); + hs.vCopy(rect.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionRect(hs, collisionGroup); + } + + /** + * @param origin + * @param vector + * @param collisionGroup + * @param threshold + * @param normal + * @return + */ + override public function getSegmentIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + // Переводим параметры сегмента в систему координат примитива + transform.transformVectorInverse(origin, _o); + transform.deltaTransformVectorInverse(vector, _v); + + // Проверка параллельности сегмента и плоскости примитива + if (_v.z > -threshold && _v.z < threshold) return -1; + var t:Number = -_o.z/_v.z; + if (t < 0) return -1; + // Проверка вхождения точки пересечения в прямоугольник + _o.x += _v.x*t; + _o.y += _v.y*t; + _o.z = 0; + if (_o.x < (-hs.x - EPSILON) || _o.x > (hs.x + EPSILON) || _o.y < (-hs.y - EPSILON) || _o.y > (hs.y + EPSILON)) return -1; + + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + return t; + } + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/collision/primitives/CollisionSphere.as b/0.1.Vector3D/src/alternativa/physics/collision/primitives/CollisionSphere.as new file mode 100644 index 0000000..1a9e6d1 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/primitives/CollisionSphere.as @@ -0,0 +1,80 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.collision.types.BoundBox; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + + /** + * Сфера. + */ + public class CollisionSphere extends CollisionPrimitive { + + // Радиус сферы + public var r:Number = 0; + + /** + * + * @param r + * @param collisionGroup + */ + public function CollisionSphere(r:Number, collisionGroup:int) { + super(SPHERE, collisionGroup); + this.r = r; + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + aabb.maxX = transform.d + r; + aabb.minX = transform.d - r; + + aabb.maxY = transform.h + r; + aabb.minY = transform.h - r; + + aabb.maxZ = transform.l + r; + aabb.minZ = transform.l - r; + + return aabb; + } + + /** + * @param origin + * @param vector + * @param threshold + * @param normal + * @return + */ + override public function getSegmentIntersection(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var px:Number = origin.x - transform.d; + var py:Number = origin.y - transform.h; + var pz:Number = origin.z - transform.l; + var k:Number = vector.x*px + vector.y*py + vector.z*pz; + if (k > 0) return -1; + var a:Number = vector.x*vector.x + vector.y*vector.y + vector.z*vector.z; + var D:Number = k*k - a*(px*px + py*py + pz*pz - r*r); + if (D < 0) return -1; + return -(k + Math.sqrt(D))/a; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var sphere:CollisionSphere = source as CollisionSphere; + if (sphere == null) return this; + super.copyFrom(sphere); + r = sphere.r; + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionSphere(r, collisionGroup); + } + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/collision/primitives/CollisionTriangle.as b/0.1.Vector3D/src/alternativa/physics/collision/primitives/CollisionTriangle.as new file mode 100644 index 0000000..a48803a --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/primitives/CollisionTriangle.as @@ -0,0 +1,39 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.altphysics; + import alternativa.physics.types.Vector3; + use namespace altphysics; + + public class CollisionTriangle extends CollisionPrimitive { + + public var v0:Vector3 = new Vector3(); + public var v1:Vector3 = new Vector3(); + public var v2:Vector3 = new Vector3(); + public var e0:Vector3 = new Vector3(); + public var e1:Vector3 = new Vector3(); + public var e2:Vector3 = new Vector3(); + public var len0:Number; + public var len1:Number; + public var len2:Number; + + public function CollisionTriangle(v0:Vector3, v1:Vector3, v2:Vector3, collisionGroup:int) { + super(TRIANGLE, collisionGroup); + + this.v0.vCopy(v0); + this.v1.vCopy(v1); + this.v2.vCopy(v2); + + e0.vDiff(v1, v0); + len0 = e0.vLength(); + e0.vNormalize(); + + e1.vDiff(v2, v1); + len1 = e1.vLength(); + e1.vNormalize(); + + e2.vDiff(v0, v2); + len2 = e2.vLength(); + e2.vNormalize(); + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/collision/types/.svn/all-wcprops b/0.1.Vector3D/src/alternativa/physics/collision/types/.svn/all-wcprops new file mode 100644 index 0000000..b3c65d3 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/types/.svn/all-wcprops @@ -0,0 +1,23 @@ +K 25 +svn:wc:ra_dav:version-url +V 128 +/!svn/ver/13893/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/collision/types +END +BoundBox.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/13892/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/collision/types/BoundBox.as +END +RayIntersection.as +K 25 +svn:wc:ra_dav:version-url +V 147 +/!svn/ver/13893/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/collision/types/RayIntersection.as +END +Ray.as +K 25 +svn:wc:ra_dav:version-url +V 135 +/!svn/ver/13892/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/collision/types/Ray.as +END diff --git a/0.1.Vector3D/src/alternativa/physics/collision/types/.svn/entries b/0.1.Vector3D/src/alternativa/physics/collision/types/.svn/entries new file mode 100644 index 0000000..503c6bc --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/types/.svn/entries @@ -0,0 +1,64 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/collision/types +http://svndev.alternativaplatform.com + + + +2009-06-03T09:16:50.564913Z +13893 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +BoundBox.as +file + + + + +2010-10-28T04:32:39.000000Z +13aaaf1b1923de8a2579543651734ab7 +2009-05-18T16:37:02.465501Z +13087 +mike + +RayIntersection.as +file + + + + +2010-10-28T04:32:39.000000Z +a0a4cf3dd94468fdf44cd06734927c94 +2009-06-03T09:16:50.564913Z +13893 +mike + +Ray.as +file + + + + +2010-10-28T04:32:39.000000Z +55eeb1cf79a416bb863bee95f089dbcd +2009-04-17T17:39:21.367569Z +11341 +mike + diff --git a/0.1.Vector3D/src/alternativa/physics/collision/types/.svn/format b/0.1.Vector3D/src/alternativa/physics/collision/types/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/types/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.1.Vector3D/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base b/0.1.Vector3D/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base new file mode 100644 index 0000000..6efc097 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base @@ -0,0 +1,72 @@ +package alternativa.physics.collision.types { + + public class BoundBox { + + public var minX:Number = 1e308; + public var minY:Number = 1e308; + public var minZ:Number = 1e308; + public var maxX:Number = -1e308; + public var maxY:Number = -1e308; + public var maxZ:Number = -1e308; + + public function setSize(minX:Number, minY:Number, minZ:Number, maxX:Number, maxY:Number, maxZ:Number):void { + this.minX = minX; + this.minY = minY; + this.minZ = minZ; + this.maxX = maxX; + this.maxY = maxY; + this.maxZ = maxZ; + } + + public function addBoundBox(boundBox:BoundBox):void { + minX = (boundBox.minX < minX) ? boundBox.minX : minX; + minY = (boundBox.minY < minY) ? boundBox.minY : minY; + minZ = (boundBox.minZ < minZ) ? boundBox.minZ : minZ; + maxX = (boundBox.maxX > maxX) ? boundBox.maxX : maxX; + maxY = (boundBox.maxY > maxY) ? boundBox.maxY : maxY; + maxZ = (boundBox.maxZ > maxZ) ? boundBox.maxZ : maxZ; + } + + public function addPoint(x:Number, y:Number, z:Number):void { + if (x < minX) minX = x; + if (x > maxX) maxX = x; + if (y < minY) minY = y; + if (y > maxY) maxY = y; + if (z < minZ) minZ = z; + if (z > maxZ) maxZ = z; + } + + public function infinity():void { + minX = 1e308; + minY = 1e308; + minZ = 1e308; + maxX = -1e308; + maxY = -1e308; + maxZ = -1e308; + } + + public function intersects(bb:BoundBox, tolerance:Number):Boolean { + return !(minX > bb.maxX + tolerance || maxX < bb.minX - tolerance || minY > bb.maxY + tolerance || maxY < bb.minY - tolerance || minZ > bb.maxZ + tolerance || maxZ < bb.minZ - tolerance); + } + + public function copyFrom(boundBox:BoundBox):void { + minX = boundBox.minX; + minY = boundBox.minY; + minZ = boundBox.minZ; + maxX = boundBox.maxX; + maxY = boundBox.maxY; + maxZ = boundBox.maxZ; + } + + public function clone():BoundBox { + var clone:BoundBox = new BoundBox(); + clone.copyFrom(this); + return clone; + } + + public function toString():String { + return "BoundBox [" + minX + ", " + minY + ", " + minZ + " : " + maxX + ", " + maxY + ", " + maxZ + "]"; + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base b/0.1.Vector3D/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base new file mode 100644 index 0000000..f80620a --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base @@ -0,0 +1,13 @@ +package alternativa.physics.collision.types { + import alternativa.physics.types.Vector3; + + /** + * Структура, описывающая луч и сегмент прямой в пространстве. + */ + public class Ray { + // Начало луча. Начальная точка сегмента на прямой луча. + public var origin:Vector3 = new Vector3(); + // Направляющий вектор луча. Длина вектора задаёт длину сегмента на луче. Конечная точка сегмента вычисляется как origin + dir. + public var dir:Vector3 = new Vector3(); + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base b/0.1.Vector3D/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base new file mode 100644 index 0000000..8a89f75 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/types/.svn/text-base/RayIntersection.as.svn-base @@ -0,0 +1,25 @@ +package alternativa.physics.collision.types { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.types.Vector3; + + /** + * Структура описывает точку пересечения луча с физической геометрией. + */ + public class RayIntersection { + // Физический примитив, с которым пересекается луч + public var primitive:CollisionPrimitive; + // Положенеи точки пересечения + public var pos:Vector3 = new Vector3(); + // Нормаль поверхности физического примитива в точке пересечения + public var normal:Vector3 = new Vector3(); + // Время пересечения (|pos - ray.origin|/|ray.dir|) + public var t:Number = 0; + + public function copy(source:RayIntersection):void { + primitive = source.primitive; + pos.vCopy(source.pos); + normal.vCopy(source.normal); + t = source.t; + } + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/collision/types/BoundBox.as b/0.1.Vector3D/src/alternativa/physics/collision/types/BoundBox.as new file mode 100644 index 0000000..6efc097 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/types/BoundBox.as @@ -0,0 +1,72 @@ +package alternativa.physics.collision.types { + + public class BoundBox { + + public var minX:Number = 1e308; + public var minY:Number = 1e308; + public var minZ:Number = 1e308; + public var maxX:Number = -1e308; + public var maxY:Number = -1e308; + public var maxZ:Number = -1e308; + + public function setSize(minX:Number, minY:Number, minZ:Number, maxX:Number, maxY:Number, maxZ:Number):void { + this.minX = minX; + this.minY = minY; + this.minZ = minZ; + this.maxX = maxX; + this.maxY = maxY; + this.maxZ = maxZ; + } + + public function addBoundBox(boundBox:BoundBox):void { + minX = (boundBox.minX < minX) ? boundBox.minX : minX; + minY = (boundBox.minY < minY) ? boundBox.minY : minY; + minZ = (boundBox.minZ < minZ) ? boundBox.minZ : minZ; + maxX = (boundBox.maxX > maxX) ? boundBox.maxX : maxX; + maxY = (boundBox.maxY > maxY) ? boundBox.maxY : maxY; + maxZ = (boundBox.maxZ > maxZ) ? boundBox.maxZ : maxZ; + } + + public function addPoint(x:Number, y:Number, z:Number):void { + if (x < minX) minX = x; + if (x > maxX) maxX = x; + if (y < minY) minY = y; + if (y > maxY) maxY = y; + if (z < minZ) minZ = z; + if (z > maxZ) maxZ = z; + } + + public function infinity():void { + minX = 1e308; + minY = 1e308; + minZ = 1e308; + maxX = -1e308; + maxY = -1e308; + maxZ = -1e308; + } + + public function intersects(bb:BoundBox, tolerance:Number):Boolean { + return !(minX > bb.maxX + tolerance || maxX < bb.minX - tolerance || minY > bb.maxY + tolerance || maxY < bb.minY - tolerance || minZ > bb.maxZ + tolerance || maxZ < bb.minZ - tolerance); + } + + public function copyFrom(boundBox:BoundBox):void { + minX = boundBox.minX; + minY = boundBox.minY; + minZ = boundBox.minZ; + maxX = boundBox.maxX; + maxY = boundBox.maxY; + maxZ = boundBox.maxZ; + } + + public function clone():BoundBox { + var clone:BoundBox = new BoundBox(); + clone.copyFrom(this); + return clone; + } + + public function toString():String { + return "BoundBox [" + minX + ", " + minY + ", " + minZ + " : " + maxX + ", " + maxY + ", " + maxZ + "]"; + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/collision/types/Ray.as b/0.1.Vector3D/src/alternativa/physics/collision/types/Ray.as new file mode 100644 index 0000000..f80620a --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/types/Ray.as @@ -0,0 +1,13 @@ +package alternativa.physics.collision.types { + import alternativa.physics.types.Vector3; + + /** + * Структура, описывающая луч и сегмент прямой в пространстве. + */ + public class Ray { + // Начало луча. Начальная точка сегмента на прямой луча. + public var origin:Vector3 = new Vector3(); + // Направляющий вектор луча. Длина вектора задаёт длину сегмента на луче. Конечная точка сегмента вычисляется как origin + dir. + public var dir:Vector3 = new Vector3(); + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/collision/types/RayIntersection.as b/0.1.Vector3D/src/alternativa/physics/collision/types/RayIntersection.as new file mode 100644 index 0000000..8a89f75 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/collision/types/RayIntersection.as @@ -0,0 +1,25 @@ +package alternativa.physics.collision.types { + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.types.Vector3; + + /** + * Структура описывает точку пересечения луча с физической геометрией. + */ + public class RayIntersection { + // Физический примитив, с которым пересекается луч + public var primitive:CollisionPrimitive; + // Положенеи точки пересечения + public var pos:Vector3 = new Vector3(); + // Нормаль поверхности физического примитива в точке пересечения + public var normal:Vector3 = new Vector3(); + // Время пересечения (|pos - ray.origin|/|ray.dir|) + public var t:Number = 0; + + public function copy(source:RayIntersection):void { + primitive = source.primitive; + pos.vCopy(source.pos); + normal.vCopy(source.normal); + t = source.t; + } + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/rigid/.svn/all-wcprops b/0.1.Vector3D/src/alternativa/physics/rigid/.svn/all-wcprops new file mode 100644 index 0000000..a6b2c0c --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/rigid/.svn/all-wcprops @@ -0,0 +1,47 @@ +K 25 +svn:wc:ra_dav:version-url +V 118 +/!svn/ver/13893/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/rigid +END +Body.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/13893/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/rigid/Body.as +END +BodyState.as +K 25 +svn:wc:ra_dav:version-url +V 131 +/!svn/ver/13893/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/rigid/BodyState.as +END +PhysicsUtils.as +K 25 +svn:wc:ra_dav:version-url +V 134 +/!svn/ver/13892/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/rigid/PhysicsUtils.as +END +ContactPoint.as +K 25 +svn:wc:ra_dav:version-url +V 134 +/!svn/ver/13893/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/rigid/ContactPoint.as +END +RigidWorld.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/13893/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/rigid/RigidWorld.as +END +Contact.as +K 25 +svn:wc:ra_dav:version-url +V 129 +/!svn/ver/13892/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/rigid/Contact.as +END +BodyMaterial.as +K 25 +svn:wc:ra_dav:version-url +V 134 +/!svn/ver/13892/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/rigid/BodyMaterial.as +END diff --git a/0.1.Vector3D/src/alternativa/physics/rigid/.svn/entries b/0.1.Vector3D/src/alternativa/physics/rigid/.svn/entries new file mode 100644 index 0000000..5174585 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/rigid/.svn/entries @@ -0,0 +1,118 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/rigid +http://svndev.alternativaplatform.com + + + +2009-06-03T09:16:50.564913Z +13893 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Body.as +file + + + + +2010-10-28T04:32:39.000000Z +43a884d83ae7d8a1d25eb9f1d26eb755 +2009-06-03T09:16:50.564913Z +13893 +mike + +BodyState.as +file + + + + +2010-10-28T04:32:39.000000Z +8a089205beccac13a05eccfb9cfc6387 +2009-06-03T09:16:50.564913Z +13893 +mike + +PhysicsUtils.as +file + + + + +2010-10-28T04:32:39.000000Z +6d88004ddab2ae19e0d15f5b1b7c016a +2009-04-26T21:40:17.813142Z +12023 +mike + +constraints +dir + +ContactPoint.as +file + + + + +2010-10-28T04:32:39.000000Z +a94e73e3bfc85be2a79d6cc939512d91 +2009-06-03T09:16:50.564913Z +13893 +mike + +RigidWorld.as +file + + + + +2010-10-28T04:32:39.000000Z +ee25e8fe15553bfa5789bf0e99c1524b +2009-06-03T09:16:50.564913Z +13893 +mike + +primitives +dir + +Contact.as +file + + + + +2010-10-28T04:32:39.000000Z +d65ad3d488d4a06addcbfa4aaf958f10 +2009-04-13T03:04:08.898183Z +11018 +mike + +BodyMaterial.as +file + + + + +2010-10-28T04:32:39.000000Z +49c7a55b5b26fdb03a60f24bac460f53 +2009-04-07T08:34:06.342994Z +10587 +mike + diff --git a/0.1.Vector3D/src/alternativa/physics/rigid/.svn/format b/0.1.Vector3D/src/alternativa/physics/rigid/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/rigid/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.1.Vector3D/src/alternativa/physics/rigid/.svn/text-base/Body.as.svn-base b/0.1.Vector3D/src/alternativa/physics/rigid/.svn/text-base/Body.as.svn-base new file mode 100644 index 0000000..4e83bfe --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/rigid/.svn/text-base/Body.as.svn-base @@ -0,0 +1,329 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Quaternion; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class Body { + + public static var linDamping:Number = 0.997; + public static var rotDamping:Number = 0.997; + + public var name:String; + public var movable:Boolean = true; + + altphysics var id:int; + // Мир, в котором находится тело + altphysics var world:RigidWorld; + // Текущее и предыдущее состояние тела. Промежуточное состояние вычисляется линейной интерполяцией. + altphysics var state:BodyState = new BodyState(); + altphysics var prevState:BodyState = new BodyState(); + // Линейное и угловое ускорение тела на текущем шаге симуляции + altphysics var accel:Vector3 = new Vector3(); + altphysics var angleAccel:Vector3 = new Vector3(); + // Материал тела + altphysics var material:BodyMaterial = new BodyMaterial(); + + altphysics var invMass:Number = 1; + altphysics var invInertia:Matrix3 = new Matrix3(); + altphysics var invInertiaWorld:Matrix3 = new Matrix3(); + altphysics var baseMatrix:Matrix3 = new Matrix3(); + + altphysics const MAX_CONTACTS:int = 20; + altphysics var contacts:Vector. = new Vector.(MAX_CONTACTS); + altphysics var contactsNum:int; + + altphysics var collisionPrimitives:Vector.; + altphysics var collisionPrimitivesNum:int; + + // Аккумулятор сил + altphysics var forceAccum:Vector3 = new Vector3(); + // Аккумулятор моментов + altphysics var torqueAccum:Vector3 = new Vector3(); + + // Внутренние переменные для избежания создания экземпляров + private static var _r:Vector3 = new Vector3(); + private static var _f:Vector3 = new Vector3(); + + /** + * + * @param invMass + * @param invInertia + */ + public function Body(invMass:Number, invInertia:Matrix3) { + this.invMass = invMass; + this.invInertia.copy(invInertia); + } + + /** + * @param prim + */ + public function addCollisionPrimitive(primitive:CollisionPrimitive, localTransform:Matrix4 = null):void { + if (primitive == null) throw new ArgumentError("Primitive cannot be null"); + if (collisionPrimitives == null) collisionPrimitives = new Vector.(); + collisionPrimitives[collisionPrimitivesNum++] = primitive; + primitive.setBody(this, localTransform); + } + + /** + * @param t + * @param result + */ + public function interpolate(t:Number, pos:Vector3, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.w = prevState.orientation.w*t1 + state.orientation.w*t; + orientation.x = prevState.orientation.x*t1 + state.orientation.x*t; + orientation.y = prevState.orientation.y*t1 + state.orientation.y*t; + orientation.z = prevState.orientation.z*t1 + state.orientation.z*t; + } + + /** + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + state.pos.vCopy(pos); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setPositionXYZ(x:Number, y:Number, z:Number):void { + state.pos.vReset(x, y, z); + } + + /** + * + * @param vel + */ + public function setVelocity(vel:Vector3):void { + state.velocity.vCopy(vel); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setVelocityXYZ(x:Number, y:Number, z:Number):void { + state.velocity.vReset(x, y, z); + } + + /** + * + * @param rot + */ + public function setRotation(rot:Vector3):void { + state.rotation.vCopy(rot); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setRotationXYZ(x:Number, y:Number, z:Number):void { + state.rotation.vReset(x, y, z); + } + + /** + * + * @param q + */ + public function setOrientation(q:Quaternion):void { + state.orientation.copy(q); + } + + /** + * @param r + * @param dir + * @param magnitude + */ + public function applyRelPosWorldImpulse(r:Vector3, dir:Vector3, magnitude:Number):void { + var d:Number = magnitude*invMass; + // Линейная часть + state.velocity.x += d*dir.x; + state.velocity.y += d*dir.y; + state.velocity.z += d*dir.z; + + // Вращательная часть + var rx:Number = r.x; + var ry:Number = r.y; + var rz:Number = r.z; + _r.x = (ry*dir.z - rz*dir.y)*magnitude; + _r.y = (rz*dir.x - rx*dir.z)*magnitude; + _r.z = (rx*dir.y - ry*dir.x)*magnitude; + _r.vTransformBy3(invInertiaWorld); + state.rotation.x += _r.x; + state.rotation.y += _r.y; + state.rotation.z += _r.z; + } + + /** + * @param f + */ + public function addForce(f:Vector3):void { + forceAccum.vAdd(f); + } + + /** + * @param pos + * @param f + */ + public function addWorldForce(pos:Vector3, force:Vector3):void { + forceAccum.vAdd(force); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(force)); + } + + /** + * @param pos + * @param f + */ + public function addWorldForceScaled(pos:Vector3, force:Vector3, scale:Number):void { + _f.x = scale*force.x, _f.y = scale*force.y, _f.z = scale*force.z; + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(_f)); + } + + /** + * @param pos + * @param f + */ + public function addLocalForce(pos:Vector3, force:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(pos, _r); + // Трансформируем вектор силы в мировую систему + baseMatrix.transformVector(force, _f); + // Добавляем силу и момент + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vCross(_f)); + } + + /** + * + * @param localPos + * @param worldForce + */ + public function addWorldForceAtLocalPoint(localPos:Vector3, worldForce:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(localPos, _r); + // Добавляем силу и момент + forceAccum.vAdd(worldForce); + torqueAccum.vAdd(_r.vCross(worldForce)); + } + + /** + * @param dt + */ + public function addExternalForces(dt:Number):void { + } + + /** + * @param t + */ + public function addTorque(t:Vector3):void { + torqueAccum.vAdd(t); + } + + /** + * + */ + altphysics function clearAccumulators():void { + forceAccum.vReset(); + torqueAccum.vReset(); + } + + /** + * + */ + altphysics function calcAccelerations():void { + accel.x = forceAccum.x*invMass; + accel.y = forceAccum.y*invMass; + accel.z = forceAccum.z*invMass; + invInertiaWorld.transformVector(torqueAccum, angleAccel); + } + + /** + * Вычисляет производные данные. + */ + altphysics function calcDerivedData():void { + // Вычисление базисной матрицы и обратного тензора инерции в мировых координатах + state.orientation.toMatrix3(baseMatrix); + invInertiaWorld.copy(invInertia).append(baseMatrix).prepend(baseMatrix.transpose()); + baseMatrix.transpose(); + if (collisionPrimitives != null) { + for (var i:int = 0; i < collisionPrimitivesNum; i++) { + var primitive:CollisionPrimitive = collisionPrimitives[i]; + primitive.transform.setFromMatrix3(baseMatrix, state.pos); + if (primitive.localTransform != null) primitive.transform.prepend(primitive.localTransform); + } + } + } + + /** + * + */ + altphysics function saveState():void { + prevState.copy(state); + } + + /** + * + */ + altphysics function restoreState():void { + state.copy(prevState); + } + + /** + * @param dt + */ + altphysics function integrateVelocity(dt:Number):void { + // v = v + a*t + state.velocity.x += accel.x*dt; + state.velocity.y += accel.y*dt; + state.velocity.z += accel.z*dt; + // rot = rot + eps*t + state.rotation.x += angleAccel.x*dt; + state.rotation.y += angleAccel.y*dt; + state.rotation.z += angleAccel.z*dt; + + state.velocity.x *= linDamping; + state.velocity.y *= linDamping; + state.velocity.z *= linDamping; + + state.rotation.x *= rotDamping; + state.rotation.y *= rotDamping; + state.rotation.z *= rotDamping; + } + + /** + * + */ + altphysics function integratePosition(dt:Number):void { + // pos = pos + v*t + state.pos.x += state.velocity.x*dt; + state.pos.y += state.velocity.y*dt; + state.pos.z += state.velocity.z*dt; + // q = q + 0.5*rot*q + state.orientation.addScaledVector(state.rotation, dt); + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/rigid/.svn/text-base/BodyMaterial.as.svn-base b/0.1.Vector3D/src/alternativa/physics/rigid/.svn/text-base/BodyMaterial.as.svn-base new file mode 100644 index 0000000..61d4382 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/rigid/.svn/text-base/BodyMaterial.as.svn-base @@ -0,0 +1,11 @@ +package alternativa.physics.rigid { + + public class BodyMaterial { + + public var restitution:Number = 0; + public var friction:Number = 0.3; +// public var dynamicFriction:Number = 0.2; +// public var dynamicFriction:Number = 0.2; + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/rigid/.svn/text-base/BodyState.as.svn-base b/0.1.Vector3D/src/alternativa/physics/rigid/.svn/text-base/BodyState.as.svn-base new file mode 100644 index 0000000..408b0ea --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/rigid/.svn/text-base/BodyState.as.svn-base @@ -0,0 +1,39 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Quaternion; + import alternativa.physics.types.Vector3; + + /** + * Класс описывает состояние твёрдого тела. + */ + public class BodyState { + /** + * Положение тела. + */ + public var pos:Vector3 = new Vector3(); + /** + * Ориентация тела. + */ + public var orientation:Quaternion = new Quaternion(); + /** + * Скорость тела. + */ + public var velocity:Vector3 = new Vector3(); + /** + * Угловая скорость тела. + */ + public var rotation:Vector3 = new Vector3(); + + /** + * Копирует значение указанного объекта. + * + * @param state + */ + public function copy(state:BodyState):void { + pos.vCopy(state.pos); + orientation.copy(state.orientation); + velocity.vCopy(state.velocity); + rotation.vCopy(state.rotation); + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/rigid/.svn/text-base/Contact.as.svn-base b/0.1.Vector3D/src/alternativa/physics/rigid/.svn/text-base/Contact.as.svn-base new file mode 100644 index 0000000..b73fc55 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/rigid/.svn/text-base/Contact.as.svn-base @@ -0,0 +1,53 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.types.Vector3; + + /** + * Информация о контакте между двумя телами. Содержит нормаль, множество точек контакта и прочие величины. + */ + public class Contact { + +// private static var pool:Vector. = new Vector.(); +// +// public static function create():ContactInfo { +// if (pool.length > 0) { +// return pool.pop(); +// } +// return new ContactInfo(); +// } +// +// public static function destroy(collInfo:ContactInfo):void { +// collInfo.body1 = collInfo.body2 = null; +// pool.push(collInfo); +// } + + // Максимальное количество точек контакта + private const N:int = 8; + + public var body1:Body; + public var body2:Body; + // Взаимный коэффициент отскока + public var restitution:Number; + // Взаимный коэффициент трения + public var friction:Number; + // Нормаль контакта. Направлена от второго тела к первому. + public var normal:Vector3 = new Vector3(); + // Список точек контакта + public var points:Vector. = new Vector.(N, true); + // Количество точек контакта + public var pcount:int; + // Максимальная глубина пересечения тел + public var maxPenetration:Number = 0; + + public var satisfied:Boolean; + + /** + * + */ + public function Contact() { + for (var i:int = 0; i < N; i++) points[i] = new ContactPoint(); + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/rigid/.svn/text-base/ContactPoint.as.svn-base b/0.1.Vector3D/src/alternativa/physics/rigid/.svn/text-base/ContactPoint.as.svn-base new file mode 100644 index 0000000..c36f958 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/rigid/.svn/text-base/ContactPoint.as.svn-base @@ -0,0 +1,52 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Vector3; + + public class ContactPoint { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + + // Величины, расчитываемые перед началом фазы решения контактов + + // Требуемая проекция конечной скорости на нормаль для упругого контакта + public var normalVel:Number; + // Минимальная скорость разделения неупругого контакта + public var minSepVel:Number; + // Изменение проекции скорости на единицу нормального импульса + public var velByUnitImpulseN:Number; + + public var angularInertia1:Number; + public var angularInertia2:Number; + + // Радиус-вектор точки контакта относительно центра первого тела + public var r1:Vector3 = new Vector3(); + // Радиус-вектор точки контакта относительно центра второго тела + public var r2:Vector3 = new Vector3(); + + // Величины, накапливаемые во время фазы решения контактов + + // Накопленный импульс, применяемый для получения требуемой относительной скорости в точке контакта + public var accumImpulseN:Number; + // Накопленный импульс, применяемый для разделения тел в точке контакта. Не создаёт момента. +// public var accumSepImpulse:Number; + + public var satisfied:Boolean; + + /** + * + * @param cp + */ + public function copyFrom(cp:ContactPoint):void { + pos.vCopy(cp.pos); + penetration = cp.penetration; + feature1 = cp.feature1; + feature2 = cp.feature2; + r1.vCopy(cp.r1); + r2.vCopy(cp.r2); + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/rigid/.svn/text-base/PhysicsUtils.as.svn-base b/0.1.Vector3D/src/alternativa/physics/rigid/.svn/text-base/PhysicsUtils.as.svn-base new file mode 100644 index 0000000..a9aaa99 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/rigid/.svn/text-base/PhysicsUtils.as.svn-base @@ -0,0 +1,49 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + public class PhysicsUtils { + + /** + * @param mass + * @param halfSize + * @param result + */ + public static function getBoxInvInertia(mass:Number, halfSize:Vector3, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + result.a = 3/(mass*(yy + zz)); + result.f = 3/(mass*(zz + xx)); + result.k = 3/(mass*(xx + yy)); + } + + /** + * @param mass + * @param r + * @param h + * @param result + */ + public static function getCylinderInvInertia(mass:Number, r:Number, h:Number, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + + result.a = result.f = 1/(mass*(h*h/12 + r*r/4)); + result.k = 2/(mass*r*r); + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/rigid/.svn/text-base/RigidWorld.as.svn-base b/0.1.Vector3D/src/alternativa/physics/rigid/.svn/text-base/RigidWorld.as.svn-base new file mode 100644 index 0000000..fb3d9c7 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/rigid/.svn/text-base/RigidWorld.as.svn-base @@ -0,0 +1,603 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.ICollisionDetector; + import alternativa.physics.collision.KdTreeCollisionDetector; + import alternativa.physics.rigid.constraints.Constraint; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Класс реализует физическую симуляцию поведения твёрдых тел. + */ + public class RigidWorld { + + private static var lastBodyId:int; + + // Максимальное количество контактов + altphysics const MAX_CONTACTS:int = 1000; + + ////////////////////////////////////////////////////////////////// + // Настроечные параметры симуляции. Могут быть изменены в любой + // момент времени без нарушения корректной работы симуляции. + ////////////////////////////////////////////////////////////////// + // Количество шагов, за которое пересекающиеся тела должны разделиться + public var penResolutionSteps:int = 10; + // Величина допустимой глубины пересечения + public var allowedPenetration:Number = 0.1; + // Максимальная скорость, добавляемая с целью разделения тел + public var maxPenResolutionSpeed:Number = 0.5; + // Количество итераций для обработки упругих контактов + public var collisionIterations:int = 5; + // Количество итераций для обработки неупругих контактов + public var contactIterations:int = 5; + // Флаг использования предсказания состояний + public var usePrediction:Boolean = false; + + // Переменные для процедуры разделения тел путём непосредственного их перемещенеия. + // !!!!!!!!!!!!!!!!!!!!!!!! + // !!! Экспериментально !!! + // !!!!!!!!!!!!!!!!!!!!!!!! + public var staticSeparationIterations:int = 10; + public var staticSeparationSteps:int = 10; + public var maxAngleMove:Number = 10; + public var useStaticSeparation:Boolean = false; + + // Вектор гравитации + altphysics var _gravity:Vector3 = new Vector3(0, 0, -9.8); + // Использующийся детектор столкновений + altphysics var collisionDetector:ICollisionDetector; + // Список тел, участвующих в симуляции + altphysics var bodies:Vector. = new Vector.(); + // Количество тел, участвующих в симуляции + altphysics var bodiesNum:int; + // Список контактов на текущем шаге симуляции + altphysics var contacts:Vector. = new Vector.(MAX_CONTACTS, true); + // Количество контактов на текущем шаге симуляции + altphysics var contactsNum:int; + // Список ограничений + altphysics var constraints:Vector. = new Vector.(); + // Количество ограничений + altphysics var constraintsNum:int; + // Временная метка. Число прошедших шагов с начала симуляции. + altphysics var timeStamp:uint; + + // Временные переменные для избежания создания экземпляров + private var _r:Vector3 = new Vector3(); + private var _t:Vector3 = new Vector3(); + private var _v:Vector3 = new Vector3(); + private var _v1:Vector3 = new Vector3(); + private var _v2:Vector3 = new Vector3(); + + /** + * + */ + public function RigidWorld() { + for (var i:int = 0; i < MAX_CONTACTS; i++) contacts[i] = new Contact(); + collisionDetector = new KdTreeCollisionDetector(); + } + + /** + * Вектор гравитации. + */ + public function get gravity():Vector3 { + return _gravity.vClone(); + } + + /** + * @private + */ + public function set gravity(value:Vector3):void { + _gravity.vCopy(value); + } + + /** + * Добавляет тело в симуляцию. + * @param body + * @return + */ + public function addBody(body:Body, isStatic:Boolean):Boolean { + var i:int = bodies.indexOf(body); + if (i > -1) return false; + bodies[bodiesNum++] = body; + body.world = this; + body.id = lastBodyId++; + if (body.collisionPrimitives != null) { + for (i = 0; i < body.collisionPrimitivesNum; i++) collisionDetector.addPrimitive(body.collisionPrimitives[i], isStatic); + } + return true; + } + + /** + * Удаляет тело из симуляции. + * @param body + * @return + */ + public function removeBody(body:Body, isStatic:Boolean):Boolean { + var i:int = bodies.indexOf(body); + if (i == -1) return false; + bodies.splice(i, 1); + body.world = null; + if (body.collisionPrimitives != null) { + for (i = 0; i < body.collisionPrimitivesNum; i++) collisionDetector.removePrimitive(body.collisionPrimitives[i], isStatic); + } + bodiesNum--; + return true; + } + + /** + * Добавляет ограничение. + * @param c + */ + public function addConstraint(c:Constraint):void { + constraints[constraintsNum++] = c; + c.world = this; + } + + /** + * Удаляет ограничение. + * @param c + */ + public function removeConstraint(c:Constraint):Boolean { + var idx:int = constraints.indexOf(c); + if (idx < 0) return false; + constraints.splice(idx, 1) + constraintsNum--; + c.world = null; + return true; + } + + /** + * Применяет к телам действующте на них силы. Аккумуляторы сил и моментов тел очищаются после завершения шага + * симуляции, поэтому на момент вызова метода могут уже содержать некоторые значения. + * + * @param dt промежуток времени, в течении которого действуют силы + */ + private function applyForces(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i]; + body.addExternalForces(dt); + body.calcAccelerations(); + // Ускорение свободного падения применяется только к подвижным телам во избежание некорректного изменения + // фиктивной скорости неподвижных тел. + if (body.movable) body.accel.vAdd(_gravity); + } + } + + /** + * Определяет все столкновения на текущем шаге симуляции и заполняет список получившихся контактов. + * + * @param dt длительность шага симуляции + */ + private function detectCollisions(dt:Number):void { + var i:int; + var j:int; + var body:Body; + + for (i = 0; i < bodiesNum; i++) { + body = bodies[i]; + body.contactsNum = 0; + body.saveState(); + // При включённом режиме предсказания состояние тел интегрируется на один шаг вперёд + if (usePrediction) { + body.integrateVelocity(dt); + body.integratePosition(dt); + } + body.calcDerivedData(); + } + + contactsNum = collisionDetector.getAllCollisions(contacts); + + // Расчёт относительных векторов точки контакта вынесен сюда из-за необходимости учитывать + // положение тел в предсказанном состоянии + for (i = 0; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + var b1:Body = contact.body1; + var b2:Body = contact.body2; + for (j = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.r1.x = cp.pos.x - b1.state.pos.x; + cp.r1.y = cp.pos.y - b1.state.pos.y; + cp.r1.z = cp.pos.z - b1.state.pos.z; + if (b2 != null) { + cp.r2.x = cp.pos.x - b2.state.pos.x; + cp.r2.y = cp.pos.y - b2.state.pos.y; + cp.r2.z = cp.pos.z - b2.state.pos.z; + } + } + } + + // Восстановление состояния тел + if (usePrediction) { + for (i = 0; i < bodiesNum; i++) { + body = bodies[i]; + body.restoreState(); + body.calcDerivedData(); + } + } + } + + /** + * Подготваливает полученные из детектора столкновений контакты, расчитывая значения, не меняющиеся + * в ходе шага симуляции. + */ + private function preProcessContacts(dt:Number):void { + var i:int; + for (i = 0; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + var b1:Body = contact.body1; + var b2:Body = contact.body2; + contact.restitution = b1.material.restitution; + if (b2 != null && b2.material.restitution < contact.restitution) contact.restitution = b2.material.restitution; + contact.friction = b1.material.friction; + if (b2 != null && b2.material.friction < contact.friction) contact.friction = b2.material.friction; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.accumImpulseN = 0; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + cp.velByUnitImpulseN = 0; + if (b1.movable) { + cp.angularInertia1 = _v.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1).vDot(contact.normal); + cp.velByUnitImpulseN += b1.invMass + cp.angularInertia1; + } + if (b2 != null && b2.movable) { + cp.angularInertia2 = _v.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2).vDot(contact.normal); + cp.velByUnitImpulseN += b2.invMass + cp.angularInertia2; + } + // Расчёт требуемой конечной скорости для упругого контакта + calcSepVelocity(b1, b2, cp, _v); + cp.normalVel = _v.vDot(contact.normal); + if (cp.normalVel < 0) cp.normalVel = - contact.restitution*cp.normalVel; + // Скорость разделения неупругого контакта + cp.minSepVel = cp.penetration > allowedPenetration ? (cp.penetration - allowedPenetration)/(penResolutionSteps*dt) : 0; + if (cp.minSepVel > maxPenResolutionSpeed) cp.minSepVel = maxPenResolutionSpeed; + } + } + for (i = 0; i < constraintsNum; i++) (constraints[i] as Constraint).preProcess(dt); + } + + /** + * + * @param dt + * @param forceInelastic + */ + private function processContacts(dt:Number, forceInelastic:Boolean):void { + var iterNum:int = forceInelastic ? contactIterations : collisionIterations; + var i:int; + var forwardLoop:Boolean = false; + for (var iter:int = 0; iter < iterNum; iter++) { + forwardLoop = !forwardLoop; + for (i = 0; i < contactsNum; i++) resolveContact(contacts[i], forceInelastic, forwardLoop); + // Ограничения + for (i = 0; i < constraintsNum; i++) (constraints[i] as Constraint).apply(dt); + } + + // Разделение контактов путём непосредственного изменения координат и ориентации. Экспериментально. + if (forceInelastic && useStaticSeparation) performStaticSeparation(); + } + + /** + * + */ + private function resolveContact(contactInfo:Contact, forceInelastic:Boolean, forwardLoop:Boolean):void { + var b1:Body = contactInfo.body1; + var b2:Body = contactInfo.body2; + var normal:Vector3 = contactInfo.normal; + var i:int; + if (forwardLoop) { + for (i = 0; i < contactInfo.pcount; i++) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } else { + for (i = contactInfo.pcount - 1; i >= 0; i--) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } + } + + /** + * + * @param idx + * @param b1 + * @param b2 + * @param colInfo + * @param normal + * @param forceInelastic + */ + private function resolveContactPoint(idx:int, b1:Body, b2:Body, contact:Contact, normal:Vector3, forceInelastic:Boolean):void { + var cp:ContactPoint = contact.points[idx]; + if (!forceInelastic) cp.satisfied = true; + + var newVel:Number = 0; + calcSepVelocity(b1, b2, cp, _v); + var sepVel:Number = _v.vDot(contact.normal); + if (forceInelastic) { + var minSpeVel:Number = useStaticSeparation ? 0 : cp.minSepVel; + if (sepVel < minSpeVel) cp.satisfied = false; + else if (cp.satisfied) return; + newVel = minSpeVel; + } else { + newVel = cp.normalVel; + } + var deltaVel:Number = newVel - sepVel; + var impulse:Number = deltaVel/cp.velByUnitImpulseN; + var accumImpulse:Number = cp.accumImpulseN + impulse; + if (accumImpulse < 0) accumImpulse = 0; + var deltaImpulse:Number = accumImpulse - cp.accumImpulseN; + cp.accumImpulseN = accumImpulse; + // Применяем импульс к телам + if (b1.movable) b1.applyRelPosWorldImpulse(cp.r1, normal, deltaImpulse); + if (b2 != null && b2.movable) b2.applyRelPosWorldImpulse(cp.r2, normal, -deltaImpulse); + + // Учёт силы трения + calcSepVelocity(b1, b2, cp, _v); + // Расчитываем изменение касательной скорости на единицу касательного импульса + var tanSpeedByUnitImpulse:Number = 0; + _v.vAddScaled(-_v.vDot(contact.normal), contact.normal); + var tanSpeed:Number = _v.vLength(); + if (tanSpeed < 0.001) return; + _t.vCopy(_v).vNormalize().vReverse(); + // dV = b.invMass + ((invI * (r % t)) % r) * t + if (b1.movable) { + _v.vCross2(cp.r1, _t).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1); + tanSpeedByUnitImpulse += b1.invMass + _v.vDot(_t); + } + if (b2 != null && b2.movable) { + _v.vCross2(cp.r2, _t).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2); + tanSpeedByUnitImpulse += b2.invMass + _v.vDot(_t); + } + + var tanImpulse:Number = tanSpeed/tanSpeedByUnitImpulse; + var max:Number = contact.friction*cp.accumImpulseN; + if (max < 0) { + if (tanImpulse < max) tanImpulse = max; + } else { + if (tanImpulse > max) tanImpulse = max; + } + + // Применяем импульс к телам + if (b1.movable) b1.applyRelPosWorldImpulse(cp.r1, _t, tanImpulse); + if (b2 != null && b2.movable) b2.applyRelPosWorldImpulse(cp.r2, _t, -tanImpulse); + } + + /** + * + * @param cp + * @param normal + * @return + */ + private function calcSepVelocity(body1:Body, body2:Body, cp:ContactPoint, result:Vector3):void { + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 + result.vCopy(body1.state.velocity).vAdd(_v1.vCross2(body1.state.rotation, cp.r1)); + // V2 = V2_c + w2%r2 + if (body2 != null) result.vSubtract(body2.state.velocity).vSubtract(_v2.vCross2(body2.state.rotation, cp.r2)); + } + + /** + * + * @param dt + */ + private function intergateVelocities(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + (bodies[i] as Body).integrateVelocity(dt); + } + } + + /** + * + * @param dt + */ + private function integratePositions(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i] as Body; + if (body.movable) body.integratePosition(dt); + } + } + + /** + * + */ + private function performStaticSeparation():void { + var iterNum:int = staticSeparationIterations; +// iterNum = 100; + + // 1. В начале каждой итерации для всех контактов сбрасывается флаг satisfied с одновременным поиском наихудшего контакта, т.е. имеющего наибольшую величину пересечения тел. + // 2. Если найденный контакт имеет величину пересечения меньше предельно допустимой процедура прерывается, т.к. разделение контактов не требуется. + // 3. Если процедура продолжается, то выполняется разделение найденного контакта и установка его флага satisfied в true, после чего запускается внутренний цикл, состоящий + // из contactsNum - 1 итераций. На каждой итерации ищется наихудший контакт среди оставшихся (satisfied == false). Если такой контакт найден, то выполняется его разделение + // и переход к следующей итерации внутреннего цикла, иначе внутренний цикл прерывается и выполняется переход к следующей итерации внешнего цикла (шаг 1.). + for (var iter:int = 0; iter < iterNum; iter++) { + // Ищем контакт с максимальной величиной пересечения тел, одновременно сбрасывая флаги satisfied в false + var worstContact:Contact = contacts[0]; + var i:int; + for (i = 1; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + contact.satisfied = false; + if (contact.maxPenetration > worstContact.maxPenetration) worstContact = contact; + } + if (worstContact.maxPenetration <= allowedPenetration) return; + + resolveInterpenetration(worstContact); + // Внутренний цикл по оставшимся контактам + for (i = 1; i < contactsNum; i++) { + worstContact = getWorstContact(); + if (worstContact == null) break; + resolveInterpenetration(worstContact); + } + } + } + + /** + * + */ + private function getWorstContact():Contact { + var maxPen:Number = 0; + var worst:Contact = null; + for (var i:int = 0; i < contactsNum; i++) { + var c:Contact = contacts[i]; + if (!c.satisfied && c.maxPenetration > maxPen) { + worst = c; + maxPen = c.maxPenetration; + } + } + return maxPen > allowedPenetration ? worst : null; + } + + /** + * Разделяет указанный контакт, выполняя staticSeparationSteps итераций по списку точек контакта. + * 1. В начале каждой итерации ищется точка с наибольшим пересечением, одновременно сбрасываются флаги satisfied у точек. + * 2. Для найденной точки выполняется процедура разделения. + * 3. После выполняется pcount - 1 итерация по списку точек, каждый раз ищется наихудшая среди имеющих satisfied == false, для которой выполняется процедура разделения. + * Если наихудшая точка не найдена, то выполнется переход к следующей итерации внешнего цикла (шаг 1). + * + * В конце процедуры разделения обновляются значения пересечений для остальных точек контакта, а также для всех прочих контактов, относящихся к телам текущего. + * + * @param contact контакт для разделения + */ + private function resolveInterpenetration(contact:Contact):void { + contact.satisfied = true; + + for (var step:int = 0; step < staticSeparationSteps; step++) { + var worstCp:ContactPoint = contact.points[0]; + var cp:ContactPoint; + var i:int; + for (i = 1; i < contact.pcount; i++) { + cp = contact.points[i]; + cp.satisfied = false; + if (cp.penetration > worstCp.penetration) worstCp = cp; + } + if (worstCp.penetration <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + // Разделяем оставшиеся точки + var maxPen:Number = 0; + for (i = 1; i < contact.pcount; i++) { + // Поиск наихудшей точки + for (var j:int = 0; j < contact.pcount; j++) { + cp = contact.points[j]; + if (cp.satisfied) continue; + if (cp.penetration > maxPen) { + maxPen = cp.penetration; + worstCp = cp; + } + } + if (maxPen <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + } + } + } + + /** + * + * @param cp + * @param contact + */ + private function separateContactPoint(cp:ContactPoint, contact:Contact):void { + cp.satisfied = true; + + var b1:Body = contact.body1; + var b2:Body = contact.body2; + var totalMove:Number = cp.penetration - allowedPenetration; + var moveCoeff:Number = totalMove/cp.velByUnitImpulseN; + var linMove1:Number; + var angleMove1:Number; + if (b1.movable) { + linMove1 = b1.invMass*moveCoeff; + angleMove1 = cp.angularInertia1*moveCoeff; + if (angleMove1 > maxAngleMove) { + linMove1 += angleMove1 - maxAngleMove; + angleMove1 -= maxAngleMove; + } + b1.state.pos.vAddScaled(linMove1, contact.normal); + _v1.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vScale(angleMove1); + b1.state.orientation.addScaledVector(_v1, 1); + } + var linMove2:Number; + var angleMove2:Number; + if (b2 != null && b2.movable) { + linMove2 = b2.invMass*moveCoeff; + angleMove2 = cp.angularInertia2*moveCoeff; + if (angleMove2 > maxAngleMove) { + linMove2 += angleMove2 - maxAngleMove; + angleMove2 -= maxAngleMove; + } + b2.state.pos.vAddScaled(-linMove2, contact.normal); + _v2.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vScale(angleMove2); + _v2.vReverse(); + b2.state.orientation.addScaledVector(_v2, 1); + } + cp.penetration = allowedPenetration; + // Обновляем пересечения в других точках + var i:int; + for (i = 0; i < contact.pcount; i++) { + var cp1:ContactPoint = contact.points[i]; + if (cp1 == cp) continue; + var angularMove:Number; + if (b1.movable) { + angularMove = _v.vCross2(_v1, cp1.r1).vDot(contact.normal); + cp1.penetration -= linMove1 + angularMove; + } + if (b2 != null && b2.movable) { + angularMove = _v.vCross2(_v2, cp1.r2).vDot(contact.normal); + cp1.penetration -= linMove2 - angularMove; + } + // Обновление максимального значения для контакта + if (cp1.penetration > contact.maxPenetration) contact.maxPenetration = cp1.penetration; + } + // Обновляем пересечения для других контактов + var c:Contact; + var j:int; + if (b1.movable) { + for (i = 0; i < b1.contactsNum; i++) { + c = b1.contacts[i]; + if (c == contact) continue; + for (j = 0; j < c.pcount; j++) { + cp1 = c.points[j]; + if (b1 == c.body1) cp1.penetration -= linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r1).vDot(c.normal); + else cp1.penetration += linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r2).vDot(c.normal); + if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; + } + } + } + if (b2 != null && b2.movable) { + for (i = 0; i < b2.contactsNum; i++) { + c = b2.contacts[i]; + if (c == contact) continue; + for (j = 0; j < c.pcount; j++) { + cp1 = c.points[j]; + if (b2 == c.body1) cp1.penetration -= linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r1).vDot(c.normal); + else cp1.penetration += linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r2).vDot(c.normal); + if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; + } + } + } + } + + /** + * + */ + private function postPhysics():void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i]; + body.clearAccumulators(); + body.calcDerivedData(); + } + } + + /** + * + * @param dt + */ + public function runPhysics(dt:Number):void { + timeStamp++; + applyForces(dt); + detectCollisions(dt); + preProcessContacts(dt); + processContacts(dt, false); + intergateVelocities(dt); + processContacts(dt, true); + integratePositions(dt); + postPhysics(); + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/rigid/Body.as b/0.1.Vector3D/src/alternativa/physics/rigid/Body.as new file mode 100644 index 0000000..4e83bfe --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/rigid/Body.as @@ -0,0 +1,329 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionPrimitive; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Matrix4; + import alternativa.physics.types.Quaternion; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class Body { + + public static var linDamping:Number = 0.997; + public static var rotDamping:Number = 0.997; + + public var name:String; + public var movable:Boolean = true; + + altphysics var id:int; + // Мир, в котором находится тело + altphysics var world:RigidWorld; + // Текущее и предыдущее состояние тела. Промежуточное состояние вычисляется линейной интерполяцией. + altphysics var state:BodyState = new BodyState(); + altphysics var prevState:BodyState = new BodyState(); + // Линейное и угловое ускорение тела на текущем шаге симуляции + altphysics var accel:Vector3 = new Vector3(); + altphysics var angleAccel:Vector3 = new Vector3(); + // Материал тела + altphysics var material:BodyMaterial = new BodyMaterial(); + + altphysics var invMass:Number = 1; + altphysics var invInertia:Matrix3 = new Matrix3(); + altphysics var invInertiaWorld:Matrix3 = new Matrix3(); + altphysics var baseMatrix:Matrix3 = new Matrix3(); + + altphysics const MAX_CONTACTS:int = 20; + altphysics var contacts:Vector. = new Vector.(MAX_CONTACTS); + altphysics var contactsNum:int; + + altphysics var collisionPrimitives:Vector.; + altphysics var collisionPrimitivesNum:int; + + // Аккумулятор сил + altphysics var forceAccum:Vector3 = new Vector3(); + // Аккумулятор моментов + altphysics var torqueAccum:Vector3 = new Vector3(); + + // Внутренние переменные для избежания создания экземпляров + private static var _r:Vector3 = new Vector3(); + private static var _f:Vector3 = new Vector3(); + + /** + * + * @param invMass + * @param invInertia + */ + public function Body(invMass:Number, invInertia:Matrix3) { + this.invMass = invMass; + this.invInertia.copy(invInertia); + } + + /** + * @param prim + */ + public function addCollisionPrimitive(primitive:CollisionPrimitive, localTransform:Matrix4 = null):void { + if (primitive == null) throw new ArgumentError("Primitive cannot be null"); + if (collisionPrimitives == null) collisionPrimitives = new Vector.(); + collisionPrimitives[collisionPrimitivesNum++] = primitive; + primitive.setBody(this, localTransform); + } + + /** + * @param t + * @param result + */ + public function interpolate(t:Number, pos:Vector3, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.w = prevState.orientation.w*t1 + state.orientation.w*t; + orientation.x = prevState.orientation.x*t1 + state.orientation.x*t; + orientation.y = prevState.orientation.y*t1 + state.orientation.y*t; + orientation.z = prevState.orientation.z*t1 + state.orientation.z*t; + } + + /** + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + state.pos.vCopy(pos); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setPositionXYZ(x:Number, y:Number, z:Number):void { + state.pos.vReset(x, y, z); + } + + /** + * + * @param vel + */ + public function setVelocity(vel:Vector3):void { + state.velocity.vCopy(vel); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setVelocityXYZ(x:Number, y:Number, z:Number):void { + state.velocity.vReset(x, y, z); + } + + /** + * + * @param rot + */ + public function setRotation(rot:Vector3):void { + state.rotation.vCopy(rot); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setRotationXYZ(x:Number, y:Number, z:Number):void { + state.rotation.vReset(x, y, z); + } + + /** + * + * @param q + */ + public function setOrientation(q:Quaternion):void { + state.orientation.copy(q); + } + + /** + * @param r + * @param dir + * @param magnitude + */ + public function applyRelPosWorldImpulse(r:Vector3, dir:Vector3, magnitude:Number):void { + var d:Number = magnitude*invMass; + // Линейная часть + state.velocity.x += d*dir.x; + state.velocity.y += d*dir.y; + state.velocity.z += d*dir.z; + + // Вращательная часть + var rx:Number = r.x; + var ry:Number = r.y; + var rz:Number = r.z; + _r.x = (ry*dir.z - rz*dir.y)*magnitude; + _r.y = (rz*dir.x - rx*dir.z)*magnitude; + _r.z = (rx*dir.y - ry*dir.x)*magnitude; + _r.vTransformBy3(invInertiaWorld); + state.rotation.x += _r.x; + state.rotation.y += _r.y; + state.rotation.z += _r.z; + } + + /** + * @param f + */ + public function addForce(f:Vector3):void { + forceAccum.vAdd(f); + } + + /** + * @param pos + * @param f + */ + public function addWorldForce(pos:Vector3, force:Vector3):void { + forceAccum.vAdd(force); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(force)); + } + + /** + * @param pos + * @param f + */ + public function addWorldForceScaled(pos:Vector3, force:Vector3, scale:Number):void { + _f.x = scale*force.x, _f.y = scale*force.y, _f.z = scale*force.z; + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(_f)); + } + + /** + * @param pos + * @param f + */ + public function addLocalForce(pos:Vector3, force:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(pos, _r); + // Трансформируем вектор силы в мировую систему + baseMatrix.transformVector(force, _f); + // Добавляем силу и момент + forceAccum.vAdd(_f); + torqueAccum.vAdd(_r.vCross(_f)); + } + + /** + * + * @param localPos + * @param worldForce + */ + public function addWorldForceAtLocalPoint(localPos:Vector3, worldForce:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(localPos, _r); + // Добавляем силу и момент + forceAccum.vAdd(worldForce); + torqueAccum.vAdd(_r.vCross(worldForce)); + } + + /** + * @param dt + */ + public function addExternalForces(dt:Number):void { + } + + /** + * @param t + */ + public function addTorque(t:Vector3):void { + torqueAccum.vAdd(t); + } + + /** + * + */ + altphysics function clearAccumulators():void { + forceAccum.vReset(); + torqueAccum.vReset(); + } + + /** + * + */ + altphysics function calcAccelerations():void { + accel.x = forceAccum.x*invMass; + accel.y = forceAccum.y*invMass; + accel.z = forceAccum.z*invMass; + invInertiaWorld.transformVector(torqueAccum, angleAccel); + } + + /** + * Вычисляет производные данные. + */ + altphysics function calcDerivedData():void { + // Вычисление базисной матрицы и обратного тензора инерции в мировых координатах + state.orientation.toMatrix3(baseMatrix); + invInertiaWorld.copy(invInertia).append(baseMatrix).prepend(baseMatrix.transpose()); + baseMatrix.transpose(); + if (collisionPrimitives != null) { + for (var i:int = 0; i < collisionPrimitivesNum; i++) { + var primitive:CollisionPrimitive = collisionPrimitives[i]; + primitive.transform.setFromMatrix3(baseMatrix, state.pos); + if (primitive.localTransform != null) primitive.transform.prepend(primitive.localTransform); + } + } + } + + /** + * + */ + altphysics function saveState():void { + prevState.copy(state); + } + + /** + * + */ + altphysics function restoreState():void { + state.copy(prevState); + } + + /** + * @param dt + */ + altphysics function integrateVelocity(dt:Number):void { + // v = v + a*t + state.velocity.x += accel.x*dt; + state.velocity.y += accel.y*dt; + state.velocity.z += accel.z*dt; + // rot = rot + eps*t + state.rotation.x += angleAccel.x*dt; + state.rotation.y += angleAccel.y*dt; + state.rotation.z += angleAccel.z*dt; + + state.velocity.x *= linDamping; + state.velocity.y *= linDamping; + state.velocity.z *= linDamping; + + state.rotation.x *= rotDamping; + state.rotation.y *= rotDamping; + state.rotation.z *= rotDamping; + } + + /** + * + */ + altphysics function integratePosition(dt:Number):void { + // pos = pos + v*t + state.pos.x += state.velocity.x*dt; + state.pos.y += state.velocity.y*dt; + state.pos.z += state.velocity.z*dt; + // q = q + 0.5*rot*q + state.orientation.addScaledVector(state.rotation, dt); + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/rigid/BodyMaterial.as b/0.1.Vector3D/src/alternativa/physics/rigid/BodyMaterial.as new file mode 100644 index 0000000..61d4382 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/rigid/BodyMaterial.as @@ -0,0 +1,11 @@ +package alternativa.physics.rigid { + + public class BodyMaterial { + + public var restitution:Number = 0; + public var friction:Number = 0.3; +// public var dynamicFriction:Number = 0.2; +// public var dynamicFriction:Number = 0.2; + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/rigid/BodyState.as b/0.1.Vector3D/src/alternativa/physics/rigid/BodyState.as new file mode 100644 index 0000000..408b0ea --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/rigid/BodyState.as @@ -0,0 +1,39 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Quaternion; + import alternativa.physics.types.Vector3; + + /** + * Класс описывает состояние твёрдого тела. + */ + public class BodyState { + /** + * Положение тела. + */ + public var pos:Vector3 = new Vector3(); + /** + * Ориентация тела. + */ + public var orientation:Quaternion = new Quaternion(); + /** + * Скорость тела. + */ + public var velocity:Vector3 = new Vector3(); + /** + * Угловая скорость тела. + */ + public var rotation:Vector3 = new Vector3(); + + /** + * Копирует значение указанного объекта. + * + * @param state + */ + public function copy(state:BodyState):void { + pos.vCopy(state.pos); + orientation.copy(state.orientation); + velocity.vCopy(state.velocity); + rotation.vCopy(state.rotation); + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/rigid/Contact.as b/0.1.Vector3D/src/alternativa/physics/rigid/Contact.as new file mode 100644 index 0000000..b73fc55 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/rigid/Contact.as @@ -0,0 +1,53 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.types.Vector3; + + /** + * Информация о контакте между двумя телами. Содержит нормаль, множество точек контакта и прочие величины. + */ + public class Contact { + +// private static var pool:Vector. = new Vector.(); +// +// public static function create():ContactInfo { +// if (pool.length > 0) { +// return pool.pop(); +// } +// return new ContactInfo(); +// } +// +// public static function destroy(collInfo:ContactInfo):void { +// collInfo.body1 = collInfo.body2 = null; +// pool.push(collInfo); +// } + + // Максимальное количество точек контакта + private const N:int = 8; + + public var body1:Body; + public var body2:Body; + // Взаимный коэффициент отскока + public var restitution:Number; + // Взаимный коэффициент трения + public var friction:Number; + // Нормаль контакта. Направлена от второго тела к первому. + public var normal:Vector3 = new Vector3(); + // Список точек контакта + public var points:Vector. = new Vector.(N, true); + // Количество точек контакта + public var pcount:int; + // Максимальная глубина пересечения тел + public var maxPenetration:Number = 0; + + public var satisfied:Boolean; + + /** + * + */ + public function Contact() { + for (var i:int = 0; i < N; i++) points[i] = new ContactPoint(); + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/rigid/ContactPoint.as b/0.1.Vector3D/src/alternativa/physics/rigid/ContactPoint.as new file mode 100644 index 0000000..c36f958 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/rigid/ContactPoint.as @@ -0,0 +1,52 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Vector3; + + public class ContactPoint { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + + // Величины, расчитываемые перед началом фазы решения контактов + + // Требуемая проекция конечной скорости на нормаль для упругого контакта + public var normalVel:Number; + // Минимальная скорость разделения неупругого контакта + public var minSepVel:Number; + // Изменение проекции скорости на единицу нормального импульса + public var velByUnitImpulseN:Number; + + public var angularInertia1:Number; + public var angularInertia2:Number; + + // Радиус-вектор точки контакта относительно центра первого тела + public var r1:Vector3 = new Vector3(); + // Радиус-вектор точки контакта относительно центра второго тела + public var r2:Vector3 = new Vector3(); + + // Величины, накапливаемые во время фазы решения контактов + + // Накопленный импульс, применяемый для получения требуемой относительной скорости в точке контакта + public var accumImpulseN:Number; + // Накопленный импульс, применяемый для разделения тел в точке контакта. Не создаёт момента. +// public var accumSepImpulse:Number; + + public var satisfied:Boolean; + + /** + * + * @param cp + */ + public function copyFrom(cp:ContactPoint):void { + pos.vCopy(cp.pos); + penetration = cp.penetration; + feature1 = cp.feature1; + feature2 = cp.feature2; + r1.vCopy(cp.r1); + r2.vCopy(cp.r2); + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/rigid/PhysicsUtils.as b/0.1.Vector3D/src/alternativa/physics/rigid/PhysicsUtils.as new file mode 100644 index 0000000..a9aaa99 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/rigid/PhysicsUtils.as @@ -0,0 +1,49 @@ +package alternativa.physics.rigid { + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + public class PhysicsUtils { + + /** + * @param mass + * @param halfSize + * @param result + */ + public static function getBoxInvInertia(mass:Number, halfSize:Vector3, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + result.a = 3/(mass*(yy + zz)); + result.f = 3/(mass*(zz + xx)); + result.k = 3/(mass*(xx + yy)); + } + + /** + * @param mass + * @param r + * @param h + * @param result + */ + public static function getCylinderInvInertia(mass:Number, r:Number, h:Number, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + + result.a = result.f = 1/(mass*(h*h/12 + r*r/4)); + result.k = 2/(mass*r*r); + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/rigid/RigidWorld.as b/0.1.Vector3D/src/alternativa/physics/rigid/RigidWorld.as new file mode 100644 index 0000000..fb3d9c7 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/rigid/RigidWorld.as @@ -0,0 +1,603 @@ +package alternativa.physics.rigid { + import __AS3__.vec.Vector; + + import alternativa.physics.altphysics; + import alternativa.physics.collision.ICollisionDetector; + import alternativa.physics.collision.KdTreeCollisionDetector; + import alternativa.physics.rigid.constraints.Constraint; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * Класс реализует физическую симуляцию поведения твёрдых тел. + */ + public class RigidWorld { + + private static var lastBodyId:int; + + // Максимальное количество контактов + altphysics const MAX_CONTACTS:int = 1000; + + ////////////////////////////////////////////////////////////////// + // Настроечные параметры симуляции. Могут быть изменены в любой + // момент времени без нарушения корректной работы симуляции. + ////////////////////////////////////////////////////////////////// + // Количество шагов, за которое пересекающиеся тела должны разделиться + public var penResolutionSteps:int = 10; + // Величина допустимой глубины пересечения + public var allowedPenetration:Number = 0.1; + // Максимальная скорость, добавляемая с целью разделения тел + public var maxPenResolutionSpeed:Number = 0.5; + // Количество итераций для обработки упругих контактов + public var collisionIterations:int = 5; + // Количество итераций для обработки неупругих контактов + public var contactIterations:int = 5; + // Флаг использования предсказания состояний + public var usePrediction:Boolean = false; + + // Переменные для процедуры разделения тел путём непосредственного их перемещенеия. + // !!!!!!!!!!!!!!!!!!!!!!!! + // !!! Экспериментально !!! + // !!!!!!!!!!!!!!!!!!!!!!!! + public var staticSeparationIterations:int = 10; + public var staticSeparationSteps:int = 10; + public var maxAngleMove:Number = 10; + public var useStaticSeparation:Boolean = false; + + // Вектор гравитации + altphysics var _gravity:Vector3 = new Vector3(0, 0, -9.8); + // Использующийся детектор столкновений + altphysics var collisionDetector:ICollisionDetector; + // Список тел, участвующих в симуляции + altphysics var bodies:Vector. = new Vector.(); + // Количество тел, участвующих в симуляции + altphysics var bodiesNum:int; + // Список контактов на текущем шаге симуляции + altphysics var contacts:Vector. = new Vector.(MAX_CONTACTS, true); + // Количество контактов на текущем шаге симуляции + altphysics var contactsNum:int; + // Список ограничений + altphysics var constraints:Vector. = new Vector.(); + // Количество ограничений + altphysics var constraintsNum:int; + // Временная метка. Число прошедших шагов с начала симуляции. + altphysics var timeStamp:uint; + + // Временные переменные для избежания создания экземпляров + private var _r:Vector3 = new Vector3(); + private var _t:Vector3 = new Vector3(); + private var _v:Vector3 = new Vector3(); + private var _v1:Vector3 = new Vector3(); + private var _v2:Vector3 = new Vector3(); + + /** + * + */ + public function RigidWorld() { + for (var i:int = 0; i < MAX_CONTACTS; i++) contacts[i] = new Contact(); + collisionDetector = new KdTreeCollisionDetector(); + } + + /** + * Вектор гравитации. + */ + public function get gravity():Vector3 { + return _gravity.vClone(); + } + + /** + * @private + */ + public function set gravity(value:Vector3):void { + _gravity.vCopy(value); + } + + /** + * Добавляет тело в симуляцию. + * @param body + * @return + */ + public function addBody(body:Body, isStatic:Boolean):Boolean { + var i:int = bodies.indexOf(body); + if (i > -1) return false; + bodies[bodiesNum++] = body; + body.world = this; + body.id = lastBodyId++; + if (body.collisionPrimitives != null) { + for (i = 0; i < body.collisionPrimitivesNum; i++) collisionDetector.addPrimitive(body.collisionPrimitives[i], isStatic); + } + return true; + } + + /** + * Удаляет тело из симуляции. + * @param body + * @return + */ + public function removeBody(body:Body, isStatic:Boolean):Boolean { + var i:int = bodies.indexOf(body); + if (i == -1) return false; + bodies.splice(i, 1); + body.world = null; + if (body.collisionPrimitives != null) { + for (i = 0; i < body.collisionPrimitivesNum; i++) collisionDetector.removePrimitive(body.collisionPrimitives[i], isStatic); + } + bodiesNum--; + return true; + } + + /** + * Добавляет ограничение. + * @param c + */ + public function addConstraint(c:Constraint):void { + constraints[constraintsNum++] = c; + c.world = this; + } + + /** + * Удаляет ограничение. + * @param c + */ + public function removeConstraint(c:Constraint):Boolean { + var idx:int = constraints.indexOf(c); + if (idx < 0) return false; + constraints.splice(idx, 1) + constraintsNum--; + c.world = null; + return true; + } + + /** + * Применяет к телам действующте на них силы. Аккумуляторы сил и моментов тел очищаются после завершения шага + * симуляции, поэтому на момент вызова метода могут уже содержать некоторые значения. + * + * @param dt промежуток времени, в течении которого действуют силы + */ + private function applyForces(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i]; + body.addExternalForces(dt); + body.calcAccelerations(); + // Ускорение свободного падения применяется только к подвижным телам во избежание некорректного изменения + // фиктивной скорости неподвижных тел. + if (body.movable) body.accel.vAdd(_gravity); + } + } + + /** + * Определяет все столкновения на текущем шаге симуляции и заполняет список получившихся контактов. + * + * @param dt длительность шага симуляции + */ + private function detectCollisions(dt:Number):void { + var i:int; + var j:int; + var body:Body; + + for (i = 0; i < bodiesNum; i++) { + body = bodies[i]; + body.contactsNum = 0; + body.saveState(); + // При включённом режиме предсказания состояние тел интегрируется на один шаг вперёд + if (usePrediction) { + body.integrateVelocity(dt); + body.integratePosition(dt); + } + body.calcDerivedData(); + } + + contactsNum = collisionDetector.getAllCollisions(contacts); + + // Расчёт относительных векторов точки контакта вынесен сюда из-за необходимости учитывать + // положение тел в предсказанном состоянии + for (i = 0; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + var b1:Body = contact.body1; + var b2:Body = contact.body2; + for (j = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.r1.x = cp.pos.x - b1.state.pos.x; + cp.r1.y = cp.pos.y - b1.state.pos.y; + cp.r1.z = cp.pos.z - b1.state.pos.z; + if (b2 != null) { + cp.r2.x = cp.pos.x - b2.state.pos.x; + cp.r2.y = cp.pos.y - b2.state.pos.y; + cp.r2.z = cp.pos.z - b2.state.pos.z; + } + } + } + + // Восстановление состояния тел + if (usePrediction) { + for (i = 0; i < bodiesNum; i++) { + body = bodies[i]; + body.restoreState(); + body.calcDerivedData(); + } + } + } + + /** + * Подготваливает полученные из детектора столкновений контакты, расчитывая значения, не меняющиеся + * в ходе шага симуляции. + */ + private function preProcessContacts(dt:Number):void { + var i:int; + for (i = 0; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + var b1:Body = contact.body1; + var b2:Body = contact.body2; + contact.restitution = b1.material.restitution; + if (b2 != null && b2.material.restitution < contact.restitution) contact.restitution = b2.material.restitution; + contact.friction = b1.material.friction; + if (b2 != null && b2.material.friction < contact.friction) contact.friction = b2.material.friction; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.accumImpulseN = 0; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + cp.velByUnitImpulseN = 0; + if (b1.movable) { + cp.angularInertia1 = _v.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1).vDot(contact.normal); + cp.velByUnitImpulseN += b1.invMass + cp.angularInertia1; + } + if (b2 != null && b2.movable) { + cp.angularInertia2 = _v.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2).vDot(contact.normal); + cp.velByUnitImpulseN += b2.invMass + cp.angularInertia2; + } + // Расчёт требуемой конечной скорости для упругого контакта + calcSepVelocity(b1, b2, cp, _v); + cp.normalVel = _v.vDot(contact.normal); + if (cp.normalVel < 0) cp.normalVel = - contact.restitution*cp.normalVel; + // Скорость разделения неупругого контакта + cp.minSepVel = cp.penetration > allowedPenetration ? (cp.penetration - allowedPenetration)/(penResolutionSteps*dt) : 0; + if (cp.minSepVel > maxPenResolutionSpeed) cp.minSepVel = maxPenResolutionSpeed; + } + } + for (i = 0; i < constraintsNum; i++) (constraints[i] as Constraint).preProcess(dt); + } + + /** + * + * @param dt + * @param forceInelastic + */ + private function processContacts(dt:Number, forceInelastic:Boolean):void { + var iterNum:int = forceInelastic ? contactIterations : collisionIterations; + var i:int; + var forwardLoop:Boolean = false; + for (var iter:int = 0; iter < iterNum; iter++) { + forwardLoop = !forwardLoop; + for (i = 0; i < contactsNum; i++) resolveContact(contacts[i], forceInelastic, forwardLoop); + // Ограничения + for (i = 0; i < constraintsNum; i++) (constraints[i] as Constraint).apply(dt); + } + + // Разделение контактов путём непосредственного изменения координат и ориентации. Экспериментально. + if (forceInelastic && useStaticSeparation) performStaticSeparation(); + } + + /** + * + */ + private function resolveContact(contactInfo:Contact, forceInelastic:Boolean, forwardLoop:Boolean):void { + var b1:Body = contactInfo.body1; + var b2:Body = contactInfo.body2; + var normal:Vector3 = contactInfo.normal; + var i:int; + if (forwardLoop) { + for (i = 0; i < contactInfo.pcount; i++) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } else { + for (i = contactInfo.pcount - 1; i >= 0; i--) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } + } + + /** + * + * @param idx + * @param b1 + * @param b2 + * @param colInfo + * @param normal + * @param forceInelastic + */ + private function resolveContactPoint(idx:int, b1:Body, b2:Body, contact:Contact, normal:Vector3, forceInelastic:Boolean):void { + var cp:ContactPoint = contact.points[idx]; + if (!forceInelastic) cp.satisfied = true; + + var newVel:Number = 0; + calcSepVelocity(b1, b2, cp, _v); + var sepVel:Number = _v.vDot(contact.normal); + if (forceInelastic) { + var minSpeVel:Number = useStaticSeparation ? 0 : cp.minSepVel; + if (sepVel < minSpeVel) cp.satisfied = false; + else if (cp.satisfied) return; + newVel = minSpeVel; + } else { + newVel = cp.normalVel; + } + var deltaVel:Number = newVel - sepVel; + var impulse:Number = deltaVel/cp.velByUnitImpulseN; + var accumImpulse:Number = cp.accumImpulseN + impulse; + if (accumImpulse < 0) accumImpulse = 0; + var deltaImpulse:Number = accumImpulse - cp.accumImpulseN; + cp.accumImpulseN = accumImpulse; + // Применяем импульс к телам + if (b1.movable) b1.applyRelPosWorldImpulse(cp.r1, normal, deltaImpulse); + if (b2 != null && b2.movable) b2.applyRelPosWorldImpulse(cp.r2, normal, -deltaImpulse); + + // Учёт силы трения + calcSepVelocity(b1, b2, cp, _v); + // Расчитываем изменение касательной скорости на единицу касательного импульса + var tanSpeedByUnitImpulse:Number = 0; + _v.vAddScaled(-_v.vDot(contact.normal), contact.normal); + var tanSpeed:Number = _v.vLength(); + if (tanSpeed < 0.001) return; + _t.vCopy(_v).vNormalize().vReverse(); + // dV = b.invMass + ((invI * (r % t)) % r) * t + if (b1.movable) { + _v.vCross2(cp.r1, _t).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1); + tanSpeedByUnitImpulse += b1.invMass + _v.vDot(_t); + } + if (b2 != null && b2.movable) { + _v.vCross2(cp.r2, _t).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2); + tanSpeedByUnitImpulse += b2.invMass + _v.vDot(_t); + } + + var tanImpulse:Number = tanSpeed/tanSpeedByUnitImpulse; + var max:Number = contact.friction*cp.accumImpulseN; + if (max < 0) { + if (tanImpulse < max) tanImpulse = max; + } else { + if (tanImpulse > max) tanImpulse = max; + } + + // Применяем импульс к телам + if (b1.movable) b1.applyRelPosWorldImpulse(cp.r1, _t, tanImpulse); + if (b2 != null && b2.movable) b2.applyRelPosWorldImpulse(cp.r2, _t, -tanImpulse); + } + + /** + * + * @param cp + * @param normal + * @return + */ + private function calcSepVelocity(body1:Body, body2:Body, cp:ContactPoint, result:Vector3):void { + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 + result.vCopy(body1.state.velocity).vAdd(_v1.vCross2(body1.state.rotation, cp.r1)); + // V2 = V2_c + w2%r2 + if (body2 != null) result.vSubtract(body2.state.velocity).vSubtract(_v2.vCross2(body2.state.rotation, cp.r2)); + } + + /** + * + * @param dt + */ + private function intergateVelocities(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + (bodies[i] as Body).integrateVelocity(dt); + } + } + + /** + * + * @param dt + */ + private function integratePositions(dt:Number):void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i] as Body; + if (body.movable) body.integratePosition(dt); + } + } + + /** + * + */ + private function performStaticSeparation():void { + var iterNum:int = staticSeparationIterations; +// iterNum = 100; + + // 1. В начале каждой итерации для всех контактов сбрасывается флаг satisfied с одновременным поиском наихудшего контакта, т.е. имеющего наибольшую величину пересечения тел. + // 2. Если найденный контакт имеет величину пересечения меньше предельно допустимой процедура прерывается, т.к. разделение контактов не требуется. + // 3. Если процедура продолжается, то выполняется разделение найденного контакта и установка его флага satisfied в true, после чего запускается внутренний цикл, состоящий + // из contactsNum - 1 итераций. На каждой итерации ищется наихудший контакт среди оставшихся (satisfied == false). Если такой контакт найден, то выполняется его разделение + // и переход к следующей итерации внутреннего цикла, иначе внутренний цикл прерывается и выполняется переход к следующей итерации внешнего цикла (шаг 1.). + for (var iter:int = 0; iter < iterNum; iter++) { + // Ищем контакт с максимальной величиной пересечения тел, одновременно сбрасывая флаги satisfied в false + var worstContact:Contact = contacts[0]; + var i:int; + for (i = 1; i < contactsNum; i++) { + var contact:Contact = contacts[i]; + contact.satisfied = false; + if (contact.maxPenetration > worstContact.maxPenetration) worstContact = contact; + } + if (worstContact.maxPenetration <= allowedPenetration) return; + + resolveInterpenetration(worstContact); + // Внутренний цикл по оставшимся контактам + for (i = 1; i < contactsNum; i++) { + worstContact = getWorstContact(); + if (worstContact == null) break; + resolveInterpenetration(worstContact); + } + } + } + + /** + * + */ + private function getWorstContact():Contact { + var maxPen:Number = 0; + var worst:Contact = null; + for (var i:int = 0; i < contactsNum; i++) { + var c:Contact = contacts[i]; + if (!c.satisfied && c.maxPenetration > maxPen) { + worst = c; + maxPen = c.maxPenetration; + } + } + return maxPen > allowedPenetration ? worst : null; + } + + /** + * Разделяет указанный контакт, выполняя staticSeparationSteps итераций по списку точек контакта. + * 1. В начале каждой итерации ищется точка с наибольшим пересечением, одновременно сбрасываются флаги satisfied у точек. + * 2. Для найденной точки выполняется процедура разделения. + * 3. После выполняется pcount - 1 итерация по списку точек, каждый раз ищется наихудшая среди имеющих satisfied == false, для которой выполняется процедура разделения. + * Если наихудшая точка не найдена, то выполнется переход к следующей итерации внешнего цикла (шаг 1). + * + * В конце процедуры разделения обновляются значения пересечений для остальных точек контакта, а также для всех прочих контактов, относящихся к телам текущего. + * + * @param contact контакт для разделения + */ + private function resolveInterpenetration(contact:Contact):void { + contact.satisfied = true; + + for (var step:int = 0; step < staticSeparationSteps; step++) { + var worstCp:ContactPoint = contact.points[0]; + var cp:ContactPoint; + var i:int; + for (i = 1; i < contact.pcount; i++) { + cp = contact.points[i]; + cp.satisfied = false; + if (cp.penetration > worstCp.penetration) worstCp = cp; + } + if (worstCp.penetration <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + // Разделяем оставшиеся точки + var maxPen:Number = 0; + for (i = 1; i < contact.pcount; i++) { + // Поиск наихудшей точки + for (var j:int = 0; j < contact.pcount; j++) { + cp = contact.points[j]; + if (cp.satisfied) continue; + if (cp.penetration > maxPen) { + maxPen = cp.penetration; + worstCp = cp; + } + } + if (maxPen <= allowedPenetration) break; + separateContactPoint(worstCp, contact); + } + } + } + + /** + * + * @param cp + * @param contact + */ + private function separateContactPoint(cp:ContactPoint, contact:Contact):void { + cp.satisfied = true; + + var b1:Body = contact.body1; + var b2:Body = contact.body2; + var totalMove:Number = cp.penetration - allowedPenetration; + var moveCoeff:Number = totalMove/cp.velByUnitImpulseN; + var linMove1:Number; + var angleMove1:Number; + if (b1.movable) { + linMove1 = b1.invMass*moveCoeff; + angleMove1 = cp.angularInertia1*moveCoeff; + if (angleMove1 > maxAngleMove) { + linMove1 += angleMove1 - maxAngleMove; + angleMove1 -= maxAngleMove; + } + b1.state.pos.vAddScaled(linMove1, contact.normal); + _v1.vCross2(cp.r1, contact.normal).vTransformBy3(b1.invInertiaWorld).vScale(angleMove1); + b1.state.orientation.addScaledVector(_v1, 1); + } + var linMove2:Number; + var angleMove2:Number; + if (b2 != null && b2.movable) { + linMove2 = b2.invMass*moveCoeff; + angleMove2 = cp.angularInertia2*moveCoeff; + if (angleMove2 > maxAngleMove) { + linMove2 += angleMove2 - maxAngleMove; + angleMove2 -= maxAngleMove; + } + b2.state.pos.vAddScaled(-linMove2, contact.normal); + _v2.vCross2(cp.r2, contact.normal).vTransformBy3(b2.invInertiaWorld).vScale(angleMove2); + _v2.vReverse(); + b2.state.orientation.addScaledVector(_v2, 1); + } + cp.penetration = allowedPenetration; + // Обновляем пересечения в других точках + var i:int; + for (i = 0; i < contact.pcount; i++) { + var cp1:ContactPoint = contact.points[i]; + if (cp1 == cp) continue; + var angularMove:Number; + if (b1.movable) { + angularMove = _v.vCross2(_v1, cp1.r1).vDot(contact.normal); + cp1.penetration -= linMove1 + angularMove; + } + if (b2 != null && b2.movable) { + angularMove = _v.vCross2(_v2, cp1.r2).vDot(contact.normal); + cp1.penetration -= linMove2 - angularMove; + } + // Обновление максимального значения для контакта + if (cp1.penetration > contact.maxPenetration) contact.maxPenetration = cp1.penetration; + } + // Обновляем пересечения для других контактов + var c:Contact; + var j:int; + if (b1.movable) { + for (i = 0; i < b1.contactsNum; i++) { + c = b1.contacts[i]; + if (c == contact) continue; + for (j = 0; j < c.pcount; j++) { + cp1 = c.points[j]; + if (b1 == c.body1) cp1.penetration -= linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r1).vDot(c.normal); + else cp1.penetration += linMove1*contact.normal.vDot(c.normal) + _v.vCross2(_v1, cp1.r2).vDot(c.normal); + if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; + } + } + } + if (b2 != null && b2.movable) { + for (i = 0; i < b2.contactsNum; i++) { + c = b2.contacts[i]; + if (c == contact) continue; + for (j = 0; j < c.pcount; j++) { + cp1 = c.points[j]; + if (b2 == c.body1) cp1.penetration -= linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r1).vDot(c.normal); + else cp1.penetration += linMove2*contact.normal.vDot(c.normal) + _v.vCross2(_v2, cp1.r2).vDot(c.normal); + if (c.maxPenetration < cp1.penetration) c.maxPenetration = cp1.penetration; + } + } + } + } + + /** + * + */ + private function postPhysics():void { + for (var i:int = 0; i < bodiesNum; i++) { + var body:Body = bodies[i]; + body.clearAccumulators(); + body.calcDerivedData(); + } + } + + /** + * + * @param dt + */ + public function runPhysics(dt:Number):void { + timeStamp++; + applyForces(dt); + detectCollisions(dt); + preProcessContacts(dt); + processContacts(dt, false); + intergateVelocities(dt); + processContacts(dt, true); + integratePositions(dt); + postPhysics(); + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/rigid/constraints/.svn/all-wcprops b/0.1.Vector3D/src/alternativa/physics/rigid/constraints/.svn/all-wcprops new file mode 100644 index 0000000..9e6cea2 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/rigid/constraints/.svn/all-wcprops @@ -0,0 +1,17 @@ +K 25 +svn:wc:ra_dav:version-url +V 130 +/!svn/ver/13893/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/rigid/constraints +END +Constraint.as +K 25 +svn:wc:ra_dav:version-url +V 144 +/!svn/ver/13892/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/rigid/constraints/Constraint.as +END +MaxDistanceConstraint.as +K 25 +svn:wc:ra_dav:version-url +V 155 +/!svn/ver/13893/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/rigid/constraints/MaxDistanceConstraint.as +END diff --git a/0.1.Vector3D/src/alternativa/physics/rigid/constraints/.svn/entries b/0.1.Vector3D/src/alternativa/physics/rigid/constraints/.svn/entries new file mode 100644 index 0000000..42de369 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/rigid/constraints/.svn/entries @@ -0,0 +1,52 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/rigid/constraints +http://svndev.alternativaplatform.com + + + +2009-06-03T09:16:50.564913Z +13893 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Constraint.as +file + + + + +2010-10-28T04:32:39.000000Z +757824f9eeb3c89243d104910e0d0529 +2009-04-23T13:43:38.903278Z +11779 +mike + +MaxDistanceConstraint.as +file + + + + +2010-10-28T04:32:39.000000Z +3fdf03f90fcf4b9e1bd8046b789014b4 +2009-06-03T09:16:50.564913Z +13893 +mike + diff --git a/0.1.Vector3D/src/alternativa/physics/rigid/constraints/.svn/format b/0.1.Vector3D/src/alternativa/physics/rigid/constraints/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/rigid/constraints/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.1.Vector3D/src/alternativa/physics/rigid/constraints/.svn/text-base/Constraint.as.svn-base b/0.1.Vector3D/src/alternativa/physics/rigid/constraints/.svn/text-base/Constraint.as.svn-base new file mode 100644 index 0000000..c2cd912 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/rigid/constraints/.svn/text-base/Constraint.as.svn-base @@ -0,0 +1,35 @@ +package alternativa.physics.rigid.constraints { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.RigidWorld; + + use namespace altphysics; + + /** + * + */ + public class Constraint { + + altphysics var satisfied:Boolean; + altphysics var world:RigidWorld; + + /** + * + */ + public function Constraint() { + } + + /** + * @param dt + */ + public function preProcess(dt:Number):void { + } + + /** + * @param dt + */ + public function apply(dt:Number):void { + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/rigid/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base b/0.1.Vector3D/src/alternativa/physics/rigid/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base new file mode 100644 index 0000000..0856660 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/rigid/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base @@ -0,0 +1,118 @@ +package alternativa.physics.rigid.constraints { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.BodyState; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class MaxDistanceConstraint extends Constraint { + // Первое тело ограничения + altphysics var body1:Body; + // Второе тело ограничения. Может быть null. + altphysics var body2:Body; + // Первая точка крепления в системе координат первого тела + altphysics var r1:Vector3; + // Вторая точка привязки в системе координат второго тела. В случае отсутствия второго тела, координаты задаются в мировой системе. + altphysics var r2:Vector3; + // Максимально допустимое расстояние между точками крепления + altphysics var maxDistance:Number; + // Радиус-вектор первой точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr1:Vector3 = new Vector3(); + // Радиус-вектор второй точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr2:Vector3 = new Vector3(); + // Минимальная скорость сближения для удовлетворения ограничения + private var minClosingVel:Number; + // Изменение относительной скорости на единицу импульса + private var velByUnitImpulseN:Number; + // Направление корректирующего импульса + private var impulseDirection:Vector3 = new Vector3(); + + // Вспомогательные переменные + private static var _v:Vector3 = new Vector3(); + private static var _v1:Vector3 = new Vector3(); + private static var _v2:Vector3 = new Vector3(); + + /** + * @param body1 + * @param body2 + * @param r1 + * @param r2 + * @param maxDistance + */ + public function MaxDistanceConstraint(body1:Body, body2:Body, r1:Vector3, r2:Vector3, maxDistance:Number) { + super(); + this.body1 = body1; + this.body2 = body2; + this.r1 = r1.vClone(); + this.r2 = r2.vClone(); + this.maxDistance = maxDistance; + } + + /** + * @param dt + */ + override public function preProcess(dt:Number):void { + // Вычислим расстояние между точками + body1.baseMatrix.transformVector(r1, wr1); + if (body2 != null) body2.baseMatrix.transformVector(r2, wr2); + else wr2.vCopy(r2); + var p1:Vector3 = body1.state.pos; + impulseDirection.x = wr2.x - wr1.x - p1.x; + impulseDirection.y = wr2.y - wr1.y - p1.y; + impulseDirection.z = wr2.z - wr1.z - p1.z; + if (body2 != null) { + var p2:Vector3 = body2.state.pos; + impulseDirection.x += p2.x; + impulseDirection.y += p2.y; + impulseDirection.z += p2.z; + } + + var delta:Number = impulseDirection.vLength() - maxDistance; + if (delta > 0) { + satisfied = false; + impulseDirection.vNormalize(); + minClosingVel = delta/(world.penResolutionSteps*dt); + if (minClosingVel > world.maxPenResolutionSpeed) minClosingVel = world.maxPenResolutionSpeed; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + velByUnitImpulseN = 0; + if (body1.movable) velByUnitImpulseN += body1.invMass + _v.vCross2(wr1, impulseDirection).vTransformBy3(body1.invInertiaWorld).vCross(wr1).vDot(impulseDirection); + if (body2 != null && body2.movable) velByUnitImpulseN += body2.invMass + _v.vCross2(wr2, impulseDirection).vTransformBy3(body2.invInertiaWorld).vCross(wr2).vDot(impulseDirection); + } else satisfied = true; + } + + /** + * @param dt + */ + override public function apply(dt:Number):void { + if (satisfied) return; + // Расчитываем проекцию скорости относительной скорости + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 + var state:BodyState = body1.state; + _v1.vCross2(state.rotation, wr1); + _v.x = state.velocity.x + _v1.x; + _v.y = state.velocity.y + _v1.y; + _v.z = state.velocity.z + _v1.z; + // V2 = V2_c + w2%r2 + if (body2 != null) { + state = body2.state; + _v2.vCross2(state.rotation, wr2); + _v.x -= state.velocity.x - _v2.x; + _v.y -= state.velocity.y - _v2.y; + _v.z -= state.velocity.z - _v2.z; + } + var closingVel:Number = _v.x*impulseDirection.x + _v.y*impulseDirection.y + _v.z*impulseDirection.z; + if (closingVel > minClosingVel) return; + var impulse:Number = (minClosingVel - closingVel)/velByUnitImpulseN; + if (body1.movable) body1.applyRelPosWorldImpulse(wr1, impulseDirection, impulse); + if (body2 != null && body2.movable) body2.applyRelPosWorldImpulse(wr2, impulseDirection, -impulse); + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/rigid/constraints/Constraint.as b/0.1.Vector3D/src/alternativa/physics/rigid/constraints/Constraint.as new file mode 100644 index 0000000..c2cd912 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/rigid/constraints/Constraint.as @@ -0,0 +1,35 @@ +package alternativa.physics.rigid.constraints { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.RigidWorld; + + use namespace altphysics; + + /** + * + */ + public class Constraint { + + altphysics var satisfied:Boolean; + altphysics var world:RigidWorld; + + /** + * + */ + public function Constraint() { + } + + /** + * @param dt + */ + public function preProcess(dt:Number):void { + } + + /** + * @param dt + */ + public function apply(dt:Number):void { + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/rigid/constraints/MaxDistanceConstraint.as b/0.1.Vector3D/src/alternativa/physics/rigid/constraints/MaxDistanceConstraint.as new file mode 100644 index 0000000..0856660 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/rigid/constraints/MaxDistanceConstraint.as @@ -0,0 +1,118 @@ +package alternativa.physics.rigid.constraints { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.rigid.BodyState; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class MaxDistanceConstraint extends Constraint { + // Первое тело ограничения + altphysics var body1:Body; + // Второе тело ограничения. Может быть null. + altphysics var body2:Body; + // Первая точка крепления в системе координат первого тела + altphysics var r1:Vector3; + // Вторая точка привязки в системе координат второго тела. В случае отсутствия второго тела, координаты задаются в мировой системе. + altphysics var r2:Vector3; + // Максимально допустимое расстояние между точками крепления + altphysics var maxDistance:Number; + // Радиус-вектор первой точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr1:Vector3 = new Vector3(); + // Радиус-вектор второй точки крепления, трансформированный без смещения в мировую систему координат + altphysics var wr2:Vector3 = new Vector3(); + // Минимальная скорость сближения для удовлетворения ограничения + private var minClosingVel:Number; + // Изменение относительной скорости на единицу импульса + private var velByUnitImpulseN:Number; + // Направление корректирующего импульса + private var impulseDirection:Vector3 = new Vector3(); + + // Вспомогательные переменные + private static var _v:Vector3 = new Vector3(); + private static var _v1:Vector3 = new Vector3(); + private static var _v2:Vector3 = new Vector3(); + + /** + * @param body1 + * @param body2 + * @param r1 + * @param r2 + * @param maxDistance + */ + public function MaxDistanceConstraint(body1:Body, body2:Body, r1:Vector3, r2:Vector3, maxDistance:Number) { + super(); + this.body1 = body1; + this.body2 = body2; + this.r1 = r1.vClone(); + this.r2 = r2.vClone(); + this.maxDistance = maxDistance; + } + + /** + * @param dt + */ + override public function preProcess(dt:Number):void { + // Вычислим расстояние между точками + body1.baseMatrix.transformVector(r1, wr1); + if (body2 != null) body2.baseMatrix.transformVector(r2, wr2); + else wr2.vCopy(r2); + var p1:Vector3 = body1.state.pos; + impulseDirection.x = wr2.x - wr1.x - p1.x; + impulseDirection.y = wr2.y - wr1.y - p1.y; + impulseDirection.z = wr2.z - wr1.z - p1.z; + if (body2 != null) { + var p2:Vector3 = body2.state.pos; + impulseDirection.x += p2.x; + impulseDirection.y += p2.y; + impulseDirection.z += p2.z; + } + + var delta:Number = impulseDirection.vLength() - maxDistance; + if (delta > 0) { + satisfied = false; + impulseDirection.vNormalize(); + minClosingVel = delta/(world.penResolutionSteps*dt); + if (minClosingVel > world.maxPenResolutionSpeed) minClosingVel = world.maxPenResolutionSpeed; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + velByUnitImpulseN = 0; + if (body1.movable) velByUnitImpulseN += body1.invMass + _v.vCross2(wr1, impulseDirection).vTransformBy3(body1.invInertiaWorld).vCross(wr1).vDot(impulseDirection); + if (body2 != null && body2.movable) velByUnitImpulseN += body2.invMass + _v.vCross2(wr2, impulseDirection).vTransformBy3(body2.invInertiaWorld).vCross(wr2).vDot(impulseDirection); + } else satisfied = true; + } + + /** + * @param dt + */ + override public function apply(dt:Number):void { + if (satisfied) return; + // Расчитываем проекцию скорости относительной скорости + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 + var state:BodyState = body1.state; + _v1.vCross2(state.rotation, wr1); + _v.x = state.velocity.x + _v1.x; + _v.y = state.velocity.y + _v1.y; + _v.z = state.velocity.z + _v1.z; + // V2 = V2_c + w2%r2 + if (body2 != null) { + state = body2.state; + _v2.vCross2(state.rotation, wr2); + _v.x -= state.velocity.x - _v2.x; + _v.y -= state.velocity.y - _v2.y; + _v.z -= state.velocity.z - _v2.z; + } + var closingVel:Number = _v.x*impulseDirection.x + _v.y*impulseDirection.y + _v.z*impulseDirection.z; + if (closingVel > minClosingVel) return; + var impulse:Number = (minClosingVel - closingVel)/velByUnitImpulseN; + if (body1.movable) body1.applyRelPosWorldImpulse(wr1, impulseDirection, impulse); + if (body2 != null && body2.movable) body2.applyRelPosWorldImpulse(wr2, impulseDirection, -impulse); + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/rigid/primitives/.svn/all-wcprops b/0.1.Vector3D/src/alternativa/physics/rigid/primitives/.svn/all-wcprops new file mode 100644 index 0000000..c33519e --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/rigid/primitives/.svn/all-wcprops @@ -0,0 +1,35 @@ +K 25 +svn:wc:ra_dav:version-url +V 129 +/!svn/ver/13893/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/rigid/primitives +END +RigidPlane.as +K 25 +svn:wc:ra_dav:version-url +V 143 +/!svn/ver/13893/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/rigid/primitives/RigidPlane.as +END +RigidSphere.as +K 25 +svn:wc:ra_dav:version-url +V 144 +/!svn/ver/13892/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/rigid/primitives/RigidSphere.as +END +RigidBox.as +K 25 +svn:wc:ra_dav:version-url +V 141 +/!svn/ver/13893/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/rigid/primitives/RigidBox.as +END +RigidCylinder.as +K 25 +svn:wc:ra_dav:version-url +V 146 +/!svn/ver/13892/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/rigid/primitives/RigidCylinder.as +END +RigidRect.as +K 25 +svn:wc:ra_dav:version-url +V 142 +/!svn/ver/13892/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/rigid/primitives/RigidRect.as +END diff --git a/0.1.Vector3D/src/alternativa/physics/rigid/primitives/.svn/entries b/0.1.Vector3D/src/alternativa/physics/rigid/primitives/.svn/entries new file mode 100644 index 0000000..7b7f6b3 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/rigid/primitives/.svn/entries @@ -0,0 +1,88 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/rigid/primitives +http://svndev.alternativaplatform.com + + + +2009-06-03T09:16:50.564913Z +13893 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +RigidPlane.as +file + + + + +2010-10-28T04:32:39.000000Z +c4713421e357ee1967dca9ab3ffa7290 +2009-06-03T09:16:50.564913Z +13893 +mike + +RigidSphere.as +file + + + + +2010-10-28T04:32:39.000000Z +79ef38eab05657ad9627bdac4ade7271 +2009-04-23T13:43:38.903278Z +11779 +mike + +RigidBox.as +file + + + + +2010-10-28T04:32:39.000000Z +da834a5d208ff30f3a74321f75301cd7 +2009-06-03T09:16:50.564913Z +13893 +mike + +RigidCylinder.as +file + + + + +2010-10-28T04:32:39.000000Z +61512d31f9e211dcb0864164aa6f5210 +2009-04-23T13:43:38.903278Z +11779 +mike + +RigidRect.as +file + + + + +2010-10-28T04:32:39.000000Z +4cff4e313a1f41d4ccfa6480ea78173e +2009-04-23T13:43:38.903278Z +11779 +mike + diff --git a/0.1.Vector3D/src/alternativa/physics/rigid/primitives/.svn/format b/0.1.Vector3D/src/alternativa/physics/rigid/primitives/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/rigid/primitives/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.1.Vector3D/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidBox.as.svn-base b/0.1.Vector3D/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidBox.as.svn-base new file mode 100644 index 0000000..c01ff61 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidBox.as.svn-base @@ -0,0 +1,56 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class RigidBox extends Body { + + /** + * + * @param halfSize + * @param mass + */ + public function RigidBox(halfSize:Vector3, mass:Number) { + super(0, Matrix3.ZERO); + addCollisionPrimitive(new CollisionBox(halfSize, 1)); + setParams(halfSize, mass); + } + + /** + * @param halfSize + * @param mass + */ + public function setParams(halfSize:Vector3, mass:Number):void { + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + invInertia.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + invInertia.a = 3*invMass/(yy + zz); + invInertia.f = 3*invMass/(zz + xx); + invInertia.k = 3*invMass/(xx + yy); + } + var prim:CollisionBox = collisionPrimitives[0] as CollisionBox; + prim.hs.vCopy(halfSize); + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidCylinder.as.svn-base b/0.1.Vector3D/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidCylinder.as.svn-base new file mode 100644 index 0000000..0f9f2b3 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidCylinder.as.svn-base @@ -0,0 +1,32 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + + use namespace altphysics; + + public class RigidCylinder extends Body { + + altphysics var r:Number; + altphysics var h:Number; + + public function RigidCylinder(mass:Number, radius:Number, height:Number) { + super(0, Matrix3.ZERO); + setParams(mass, radius, height); + } + + public function setParams(mass:Number, radius:Number, height:Number):void { + r = radius; + h = height; + invInertiaWorld.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + invInertia.a = invInertia.f = mass*(h*h/12 + r*r/4); + invInertia.k = 0.5*mass*r*r; + invInertia.invert(); + } + } + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidPlane.as.svn-base b/0.1.Vector3D/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidPlane.as.svn-base new file mode 100644 index 0000000..26f74b2 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidPlane.as.svn-base @@ -0,0 +1,23 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + public class RigidPlane extends Body { + + public var normal:Vector3 = new Vector3(); + public var offset:Number; + + public function RigidPlane() { + super(0, Matrix3.ZERO); + this.normal.vCopy(Vector3.Z_AXIS); + this.offset = 0; + movable = false; + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidRect.as.svn-base b/0.1.Vector3D/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidRect.as.svn-base new file mode 100644 index 0000000..3c3913d --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidRect.as.svn-base @@ -0,0 +1,20 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + public class RigidRect extends Body { + + public function RigidRect(halfWidth:Number, halfLength:Number) { + super(0, Matrix3.ZERO); + movable = false; + addCollisionPrimitive(new CollisionRect(new Vector3(halfWidth, halfLength, 0), 1)); + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidSphere.as.svn-base b/0.1.Vector3D/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidSphere.as.svn-base new file mode 100644 index 0000000..9668716 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/rigid/primitives/.svn/text-base/RigidSphere.as.svn-base @@ -0,0 +1,29 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + + use namespace altphysics; + + public class RigidSphere extends Body { + + public function RigidSphere(radius:Number, mass:Number) { + var m:Matrix3; + if (mass == Infinity) { + mass = 0; + m = Matrix3.ZERO; + } else { + mass = 1/mass; + var r2:Number = radius*radius; + m = new Matrix3(); + m.a = m.f = m.k = 2.5*mass/r2; + } + super(mass, m); + if (mass == 0) movable = false; + addCollisionPrimitive(new CollisionSphere(radius, 1)); + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/rigid/primitives/RigidBox.as b/0.1.Vector3D/src/alternativa/physics/rigid/primitives/RigidBox.as new file mode 100644 index 0000000..c01ff61 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/rigid/primitives/RigidBox.as @@ -0,0 +1,56 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + /** + * + */ + public class RigidBox extends Body { + + /** + * + * @param halfSize + * @param mass + */ + public function RigidBox(halfSize:Vector3, mass:Number) { + super(0, Matrix3.ZERO); + addCollisionPrimitive(new CollisionBox(halfSize, 1)); + setParams(halfSize, mass); + } + + /** + * @param halfSize + * @param mass + */ + public function setParams(halfSize:Vector3, mass:Number):void { + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + invInertia.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + invInertia.a = 3*invMass/(yy + zz); + invInertia.f = 3*invMass/(zz + xx); + invInertia.k = 3*invMass/(xx + yy); + } + var prim:CollisionBox = collisionPrimitives[0] as CollisionBox; + prim.hs.vCopy(halfSize); + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/rigid/primitives/RigidCylinder.as b/0.1.Vector3D/src/alternativa/physics/rigid/primitives/RigidCylinder.as new file mode 100644 index 0000000..0f9f2b3 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/rigid/primitives/RigidCylinder.as @@ -0,0 +1,32 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + + use namespace altphysics; + + public class RigidCylinder extends Body { + + altphysics var r:Number; + altphysics var h:Number; + + public function RigidCylinder(mass:Number, radius:Number, height:Number) { + super(0, Matrix3.ZERO); + setParams(mass, radius, height); + } + + public function setParams(mass:Number, radius:Number, height:Number):void { + r = radius; + h = height; + invInertiaWorld.copy(Matrix3.ZERO); + if (mass == Infinity) invMass = 0; + else { + invMass = 1/mass; + invInertia.a = invInertia.f = mass*(h*h/12 + r*r/4); + invInertia.k = 0.5*mass*r*r; + invInertia.invert(); + } + } + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/rigid/primitives/RigidPlane.as b/0.1.Vector3D/src/alternativa/physics/rigid/primitives/RigidPlane.as new file mode 100644 index 0000000..26f74b2 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/rigid/primitives/RigidPlane.as @@ -0,0 +1,23 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + public class RigidPlane extends Body { + + public var normal:Vector3 = new Vector3(); + public var offset:Number; + + public function RigidPlane() { + super(0, Matrix3.ZERO); + this.normal.vCopy(Vector3.Z_AXIS); + this.offset = 0; + movable = false; + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/rigid/primitives/RigidRect.as b/0.1.Vector3D/src/alternativa/physics/rigid/primitives/RigidRect.as new file mode 100644 index 0000000..3c3913d --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/rigid/primitives/RigidRect.as @@ -0,0 +1,20 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + import alternativa.physics.types.Vector3; + + use namespace altphysics; + + public class RigidRect extends Body { + + public function RigidRect(halfWidth:Number, halfLength:Number) { + super(0, Matrix3.ZERO); + movable = false; + addCollisionPrimitive(new CollisionRect(new Vector3(halfWidth, halfLength, 0), 1)); + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/rigid/primitives/RigidSphere.as b/0.1.Vector3D/src/alternativa/physics/rigid/primitives/RigidSphere.as new file mode 100644 index 0000000..9668716 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/rigid/primitives/RigidSphere.as @@ -0,0 +1,29 @@ +package alternativa.physics.rigid.primitives { + + import alternativa.physics.altphysics; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.physics.rigid.Body; + import alternativa.physics.types.Matrix3; + + use namespace altphysics; + + public class RigidSphere extends Body { + + public function RigidSphere(radius:Number, mass:Number) { + var m:Matrix3; + if (mass == Infinity) { + mass = 0; + m = Matrix3.ZERO; + } else { + mass = 1/mass; + var r2:Number = radius*radius; + m = new Matrix3(); + m.a = m.f = m.k = 2.5*mass/r2; + } + super(mass, m); + if (mass == 0) movable = false; + addCollisionPrimitive(new CollisionSphere(radius, 1)); + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/types/.svn/all-wcprops b/0.1.Vector3D/src/alternativa/physics/types/.svn/all-wcprops new file mode 100644 index 0000000..bf22fb8 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/types/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 118 +/!svn/ver/13893/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/types +END +Quaternion.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/13892/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/types/Quaternion.as +END +Vector3.as +K 25 +svn:wc:ra_dav:version-url +V 129 +/!svn/ver/13893/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/types/Vector3.as +END +Matrix3.as +K 25 +svn:wc:ra_dav:version-url +V 129 +/!svn/ver/13892/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/types/Matrix3.as +END +Matrix4.as +K 25 +svn:wc:ra_dav:version-url +V 129 +/!svn/ver/13892/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/types/Matrix4.as +END diff --git a/0.1.Vector3D/src/alternativa/physics/types/.svn/entries b/0.1.Vector3D/src/alternativa/physics/types/.svn/entries new file mode 100644 index 0000000..6a9a713 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/types/.svn/entries @@ -0,0 +1,76 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D/src/alternativa/physics/types +http://svndev.alternativaplatform.com + + + +2009-06-03T09:16:50.564913Z +13893 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Quaternion.as +file + + + + +2010-10-28T04:32:38.000000Z +595f36fdfea8e82e752bb989e97e3a9a +2009-05-04T04:55:56.466789Z +12306 +mike + +Vector3.as +file + + + + +2010-10-28T04:32:38.000000Z +c324eee86307bbf468a629c56a46bce4 +2009-06-03T09:16:50.564913Z +13893 +mike + +Matrix3.as +file + + + + +2010-10-28T04:32:38.000000Z +2e95a69258123653d92296f40390c112 +2009-05-26T08:55:28.854881Z +13390 +mike + +Matrix4.as +file + + + + +2010-10-28T04:32:38.000000Z +b2ed6277a2052c2568a1b4f19c469223 +2009-05-04T04:55:56.466789Z +12306 +mike + diff --git a/0.1.Vector3D/src/alternativa/physics/types/.svn/format b/0.1.Vector3D/src/alternativa/physics/types/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/types/.svn/format @@ -0,0 +1 @@ +8 diff --git a/0.1.Vector3D/src/alternativa/physics/types/.svn/text-base/Matrix3.as.svn-base b/0.1.Vector3D/src/alternativa/physics/types/.svn/text-base/Matrix3.as.svn-base new file mode 100644 index 0000000..86885d7 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/types/.svn/text-base/Matrix3.as.svn-base @@ -0,0 +1,367 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + /** + * + */ + public class Matrix3 { + + public static const ZERO:Matrix3 = new Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0); + public static const IDENTITY:Matrix3 = new Matrix3(); + + public var a:Number; + public var b:Number; + public var c:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix3(a:Number = 1, b:Number = 0, c:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1) { + this.a = a; + this.b = b; + this.c = c; + + this.e = e; + this.f = f; + this.g = g; + + this.i = i; + this.j = j; + this.k = k; + } + + /** + * + * @return + */ + public function toIdentity():Matrix3 { + a = f = k = 1; + b = c = e = g = i = j = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + var det:Number = 1/(-cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk); + + a = (ff*kk - gg*jj)*det; + b = (cc*jj - bb*kk)*det; + c = (bb*gg - cc*ff)*det; + e = (gg*ii - ee*kk)*det; + f = (aa*kk - cc*ii)*det; + g = (cc*ee - aa*gg)*det; + i = (ee*jj - ff*ii)*det; + j = (bb*ii - aa*jj)*det; + k = (aa*ff - bb*ee)*det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix3):Matrix3 { + a += m.a; + b += m.b; + c += m.c; + e += m.e; + f += m.f; + g += m.g; + i += m.i; + j += m.j; + k += m.k; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix3):Matrix3 { + a -= m.a; + b -= m.b; + c -= m.c; + e -= m.e; + f -= m.f; + g -= m.g; + i -= m.i; + j -= m.j; + k -= m.k; + + return this; + } + + /** + * + * @return + */ + public function transpose():Matrix3 { + var tmp:Number = b; + b = e; + e = tmp; + tmp = c; + c = i; + i = tmp; + tmp = g; + g = j; + j = tmp; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector3To3D(vin:Vector3, vout:Vector3D):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * + * @param matrix + */ + public function createSkewSymmetric(v:Vector3):Matrix3 { + a = f = k = 0; + b = -v.z; + c = v.y; + e = v.z; + g = -v.x; + i = -v.y; + j = v.x; + return this; + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(i:int, axis:Vector3):void { + switch (i) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + } + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix3):Matrix3 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @return + */ + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):Matrix3 { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + + return this; + } + + /** + * Метод формирует матрицу поворота на заданный угол относительно заданной оси. + * + * @param axis нормализованный вектор, задающий ось, относительно которой выполняется поворот + * @param angle угол поворота в радианах + */ + public function fromAxisAngle(axis:Vector3, angle:Number):void { + var c1:Number = Math.cos(angle); + var s:Number = Math.sin(angle); + var t:Number = 1 - c1; + var x:Number = axis.x; + var y:Number = axis.y; + var z:Number = axis.z; + + a = t*x*x + c1; + b = t*x*y - z*s; + c = t*x*z + y*s; + + e = t*x*y + z*s; + f = t*y*y + c1; + g = t*y*z - x*s; + + i = t*x*z - y*s; + j = t*y*z + x*s; + k = t*z*z + c1; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix3 { + return new Matrix3(a, b, c, e, f, g, i, j, k); + } + + public function toString():String { + return "[Matrix3 (" + a + ", " + b + ", " + c + "), (" + e + ", " + f + ", " + g + "), (" + i + ", " + j + ", " + k + ")]"; + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/types/.svn/text-base/Matrix4.as.svn-base b/0.1.Vector3D/src/alternativa/physics/types/.svn/text-base/Matrix4.as.svn-base new file mode 100644 index 0000000..8cc23b2 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/types/.svn/text-base/Matrix4.as.svn-base @@ -0,0 +1,402 @@ +package alternativa.physics.types { + import __AS3__.vec.Vector; + + /** + * + */ + public class Matrix4 { + + public var a:Number; + public var b:Number; + public var c:Number; + public var d:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + public var h:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + public var l:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix4(a:Number = 1, b:Number = 0, c:Number = 0, d:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, h:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1, l:Number = 0) { + this.a = a; + this.b = b; + this.c = c; + this.d = h; + + this.e = e; + this.f = f; + this.g = g; + this.h = h; + + this.i = i; + this.j = j; + this.k = k; + this.l = l; + } + + /** + * + * @return + */ + public function toIdentity():Matrix4 { + a = f = k = 1; + b = c = e = g = i = j = d = h = l = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + var det:Number = -cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk; + + a = (-gg*jj + ff*kk)/det; + b = (cc*jj - bb*kk)/det; + c = (-cc*ff + bb*gg)/det; + d = (dd*gg*jj - cc*hh*jj - dd*ff*kk + bb*hh*kk + cc*ff*ll - bb*gg*ll)/det; + e = (gg*ii - ee*kk)/det; + f = (-cc*ii + aa*kk)/det; + g = (cc*ee - aa*gg)/det; + h = (cc*hh*ii - dd*gg*ii + dd*ee*kk - aa*hh*kk - cc*ee*ll + aa*gg*ll)/det; + i = (-ff*ii + ee*jj)/det; + j = (bb*ii - aa*jj)/det; + k = (-bb*ee + aa*ff)/det; + l = (dd*ff*ii - bb*hh*ii - dd*ee*jj + aa*hh*jj + bb*ee*ll - aa*ff*ll)/det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + d = m.a*dd + m.b*hh + m.c*ll + m.d; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + h = m.e*dd + m.f*hh + m.g*ll + m.h; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + l = m.i*dd + m.j*hh + m.k*ll + m.l; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + d = aa*m.d + bb*m.h + cc*m.l + dd; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + h = ee*m.d + ff*m.h + gg*m.l + hh; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + l = ii*m.d + jj*m.h + kk*m.l + ll; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix4):Matrix4 { + a += m.a; + b += m.b; + c += m.c; + d += m.d; + e += m.e; + f += m.f; + g += m.g; + h += m.h; + i += m.i; + j += m.j; + k += m.k; + l += m.l; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix4):Matrix4 { + a -= m.a; + b -= m.b; + c -= m.c; + d -= m.d; + e -= m.e; + f -= m.f; + g -= m.g; + h -= m.h; + i -= m.i; + j -= m.j; + k -= m.k; + l -= m.l; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectors(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverse(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(idx:int, axis:Vector3):void { + switch (idx) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + case 3: + axis.x = d; + axis.y = h; + axis.z = l; + return; + } + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix4):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = m.d; + e = m.e; + f = m.f; + g = m.g; + h = m.h; + i = m.i; + j = m.j; + k = m.k; + l = m.l; + + return this; + } + + public function setFromMatrix3(m:Matrix3, offset:Vector3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = offset.x; + e = m.e; + f = m.f; + g = m.g; + h = offset.y; + i = m.i; + j = m.j; + k = m.k; + l = offset.z; + + return this; + } + + /** + * @param rotations + */ + public function getEulerAngles(rotations:Vector3):void { + if (-1 < i && i < 1) { + rotations.x = Math.atan2(j, k); + rotations.y = -Math.asin(i); + rotations.z = Math.atan2(e, a); + } else { + rotations.x = 0; + rotations.y = 0.5*((i <= -1) ? Math.PI : -Math.PI); + rotations.z = Math.atan2(-b, f); + } + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix4 { + return new Matrix4(a, b, c, d, e, f, g, h, i, j, k, l); + } + + public function toString():String { + return "[Matrix4 [" + a.toFixed(3) + " " + b.toFixed(3) + " " + c.toFixed(3) + " " + d.toFixed(3) + "] [" + e.toFixed(3) + " " + f.toFixed(3) + " " + g.toFixed(3) + " " + h.toFixed(3) + "] [" + i.toFixed(3) + " " + j.toFixed(3) + " " + k.toFixed(3) + " " + l.toFixed(3) + "]]"; + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/types/.svn/text-base/Quaternion.as.svn-base b/0.1.Vector3D/src/alternativa/physics/types/.svn/text-base/Quaternion.as.svn-base new file mode 100644 index 0000000..7ad2af0 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/types/.svn/text-base/Quaternion.as.svn-base @@ -0,0 +1,396 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + public class Quaternion { + + public var w:Number; + public var x:Number; + public var y:Number; + public var z:Number; + + public static function multiply(q1:Quaternion, q2:Quaternion, result:Quaternion):void { + result.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z; + result.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y; + result.y = q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z; + result.z = q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x; + } + + public static function createFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngle(axis, angle); + return q; + } + + public static function createFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngleComponents(x, y, z, angle); + return q; + } + + public function Quaternion(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0) { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + } + + public function reset(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0):Quaternion { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + return this; + } + + public function normalize():Quaternion { + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * Умножает на указанный кватернион слева: this * q + * + * @param q множитель + */ + public function prepend(q:Quaternion):Quaternion { + var ww:Number = w*q.w - x*q.x - y*q.y - z*q.z; + var xx:Number = w*q.x + x*q.w + y*q.z - z*q.y; + var yy:Number = w*q.y + y*q.w + z*q.x - x*q.z; + var zz:Number = w*q.z + z*q.w + x*q.y - y*q.x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Умножает на указанный кватернион справа: q * this + * + * @param q множитель + */ + public function append(q:Quaternion):Quaternion { + var ww:Number = q.w*w - q.x*x - q.y*y - q.z*z; + var xx:Number = q.w*x + q.x*w + q.y*z - q.z*y; + var yy:Number = q.w*y + q.y*w + q.z*x - q.x*z; + var zz:Number = q.w*z + q.z*w + q.x*y - q.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * + * @param vector + */ + public function rotateByVector(v:Vector3):Quaternion { + var ww:Number = -v.x*x - v.y*y - v.z*z; + var xx:Number = v.x*w + v.y*z - v.z*y; + var yy:Number = v.y*w + v.z*x - v.x*z; + var zz:Number = v.z*w + v.x*y - v.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Добавляет вращение, приданное вектором угловой скорости за указанное время. + * + * @param v + * @param scale + */ + public function addScaledVector(v:Vector3, scale:Number):Quaternion { + var vx:Number = v.x*scale; + var vy:Number = v.y*scale; + var vz:Number = v.z*scale; + var ww:Number = -x*vx - y*vy - z*vz; + var xx:Number = vx*w + vy*z - vz*y; + var yy:Number = vy*w + vz*x - vx*z; + var zz:Number = vz*w + vx*y - vy*x; + w += 0.5*ww; + x += 0.5*xx; + y += 0.5*yy; + z += 0.5*zz; + // inlined normalize + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param m + * @return + */ + public function toMatrix3(m:Matrix3):Quaternion { + var xx2:Number = 2*x*x; + var yy2:Number = 2*y*y; + var zz2:Number = 2*z*z; + var xy2:Number = 2*x*y; + var yz2:Number = 2*y*z; + var zx2:Number = 2*z*x; + var wx2:Number = 2*w*x; + var wy2:Number = 2*w*y; + var wz2:Number = 2*w*z; + + m.a = 1 - yy2 - zz2; + m.b = xy2 - wz2; + m.c = zx2 + wy2; + + m.e = xy2 + wz2; + m.f = 1 - xx2 - zz2; + m.g = yz2 - wx2; + + m.i = zx2 - wy2; + m.j = yz2 + wx2; + m.k = 1 - xx2 - yy2; + return this; + } + + /** + * + * @return + */ + public function length():Number { + return Math.sqrt(w*w + x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function lengthSqr():Number { + return w*w + x*x + y*y + z*z; + } + + /** + * + * @param axis + * @param angle + * @return + */ + public function setFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z); + x = axis.x*k; + y = axis.y*k; + z = axis.z*k; + return this; + } + + /** + * + * @param x + * @param y + * @param z + * @param angle + */ + public function setFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(x*x + y*y + z*z); + this.x = x*k; + this.y = y*k; + this.z = z*k; + return this; + } + + /** + * + * @param vector + */ + public function toAxisVector(v:Vector3 = null):Vector3 { + if (w < -1 || w > 1) { + normalize(); + } + if (v == null) { + v = new Vector3(); + } + if (w > -1 && w < 1) { + if (w == 0) { + v.x = x; + v.y = y; + v.z = z; + } else { + var angle:Number = 2*Math.acos(w); + var coeff:Number = 1/Math.sqrt(1 - w*w); + v.x = x*coeff*angle; + v.y = y*coeff*angle; + v.z = z*coeff*angle; + } + } else { + v.x = 0; + v.y = 0; + v.z = 0; + } + return v; + } + + /** + * + * @param rotations + */ + public function getEulerAngles(angles:Vector3):Vector3 { + var qi2:Number = 2*x*x; + var qj2:Number = 2*y*y; + var qk2:Number = 2*z*z; + var qij:Number = 2*x*y; + var qjk:Number = 2*y*z; + var qki:Number = 2*z*x; + var qri:Number = 2*w*x; + var qrj:Number = 2*w*y; + var qrk:Number = 2*w*z; + + var aa:Number = 1 - qj2 - qk2; + var bb:Number = qij - qrk; + var ee:Number = qij + qrk; + var ff:Number = 1 - qi2 - qk2; + var ii:Number = qki - qrj; + var jj:Number = qjk + qri; + var kk:Number = 1 - qi2 - qj2; + + if (-1 < ii && ii < 1) { + if (angles == null) { + angles = new Vector3(Math.atan2(jj, kk), -Math.asin(ii), Math.atan2(ee, aa)); + } else { + angles.x = Math.atan2(jj, kk); + angles.y = -Math.asin(ii); + angles.z = Math.atan2(ee, aa); + } + } else { + if (angles == null) { + angles = new Vector3(0, 0.5*((ii <= -1) ? Math.PI : -Math.PI), Math.atan2(-bb, ff)); + } else { + angles.x = 0; + angles.y = 0.5*((ii <= -1) ? Math.PI : -Math.PI); + angles.z = Math.atan2(-bb, ff); + } + } + return angles; + } + + /** + * + */ + public function conjugate():void { + x = -x; + y = -y; + z = -z; + } + + /** + * Выполняет линейную интерполяцию. + * + * @param q1 начало отрезка + * @param q2 конец отрезка + * @param t время, обычно задаётся в интервале [0, 1] + */ + public function nlerp(q1:Quaternion, q2:Quaternion, t:Number):Quaternion { + var d:Number = 1 - t; + w = q1.w*d + q2.w*t; + x = q1.x*d + q2.x*t; + y = q1.y*d + q2.y*t; + z = q1.z*d + q2.z*t; + // inlined normalize + d = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param q + * @return + */ + public function subtract(q:Quaternion):Quaternion { + w -= q.w; + x -= q.x; + y -= q.y; + z -= q.z; + return this; + } + + /** + * + * @param q1 + * @param q2 + * @return + */ + public function diff(q1:Quaternion, q2:Quaternion):Quaternion { + w = q2.w - q1.w; + x = q2.x - q1.x; + y = q2.y - q1.y; + z = q2.z - q1.z; + return this; + } + + /** + * + * @param q + * @return + */ + public function copy(q:Quaternion):Quaternion { + w = q.w; + x = q.x; + y = q.y; + z = q.z; + return this; + } + + /** + * @param reslult + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + result.w = w; + return result; + } + + /** + * + * @return + */ + public function clone():Quaternion { + return new Quaternion(w, x, y, z); + } + + /** + * + * @return + */ + public function toString():String { + return "[" + w + ", " + x + ", " + y + ", " + z + "]"; + } + } +} diff --git a/0.1.Vector3D/src/alternativa/physics/types/.svn/text-base/Vector3.as.svn-base b/0.1.Vector3D/src/alternativa/physics/types/.svn/text-base/Vector3.as.svn-base new file mode 100644 index 0000000..298ddad --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/types/.svn/text-base/Vector3.as.svn-base @@ -0,0 +1,331 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + /** + * + */ + public class Vector3 extends Vector3D { + + public static const ZERO:Vector3 = new Vector3(0, 0, 0); + public static const X_AXIS:Vector3 = new Vector3(1, 0, 0); + public static const Y_AXIS:Vector3 = new Vector3(0, 1, 0); + public static const Z_AXIS:Vector3 = new Vector3(0, 0, 1); + + /** + * + * @param x + * @param y + * @param z + */ + public function Vector3(x:Number = 0, y:Number = 0, z:Number = 0) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * + * @return + */ + public function vLength():Number { + return Math.sqrt(x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function vLengthSqr():Number { + return x*x + y*y + z*z; + } + + /** + * + * @param length + * @return + */ + public function vSetLength(length:Number):Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = length; + } else { + var k:Number = length/Math.sqrt(x*x + y*y + z*z); + x *= k; + y *= k; + z *= k; + } + return this; + } + + /** + * + * @return + */ + public function vNormalize():Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = 1; + } else { + var k:Number = Math.sqrt(d); + x /= k; + y /= k; + z /= k; + } + return this; + } + + /** + * Сложение координат. + * + * @param v точка, координаты которой прибавляются к собственным + */ + public function vAdd(v:Vector3):Vector3 { + x += v.x; + y += v.y; + z += v.z; + return this; + } + + /** + * + * @param k + * @param v + * @return + */ + public function vAddScaled(k:Number, v:Vector3):Vector3 { + x += k*v.x; + y += k*v.y; + z += k*v.z; + return this; + } + + /** + * Вычитание координат. + * + * @param v точка, координаты которой вычитаются из собственных + */ + public function vSubtract(v:Vector3):Vector3 { + x -= v.x; + y -= v.y; + z -= v.z; + return this; + } + + /** + * Вычисляет сумму векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + * @return this + */ + public function vSum(a:Vector3, b:Vector3):Vector3 { + x = a.x + b.x; + y = a.y + b.y; + z = a.z + b.z; + return this; + } + + /** + * Вычисление разности векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + */ + public function vDiff(a:Vector3, b:Vector3):Vector3 { + x = a.x - b.x; + y = a.y - b.y; + z = a.z - b.z; + return this; + } + + /** + * Умножение на скаляр. + * + * @param k число, на которое умножаются координаты + */ + public function vScale(k:Number):Vector3 { + x *= k; + y *= k; + z *= k; + return this; + } + + /** + * Инвертирование вектора. + */ + public function vReverse():Vector3 { + x = -x; + y = -y; + z = -z; + return this; + } + + /** + * + * @param v + * @return + */ + public function vDot(v:Vector3):Number { + return x*v.x + y*v.y + z*v.z; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross(v:Vector3):Vector3 { + var xx:Number = y*v.z - z*v.y; + var yy:Number = z*v.x - x*v.z; + var zz:Number = x*v.y - y*v.x; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross2(a:Vector3, b:Vector3):Vector3 { + x = a.y*b.z - a.z*b.y; + y = a.z*b.x - a.x*b.z; + z = a.x*b.y - a.y*b.x; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy3(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * + * @param matrix + */ + public function vTransformBy3Tr(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz + m.d; + y = m.e*xx + m.f*yy + m.g*zz + m.h; + z = m.i*xx + m.j*yy + m.k*zz + m.l; + return this; + } + + /** + * + * @param m + * @return + */ + public function vTransformInverseBy4(m:Matrix4):Vector3 { + var xx:Number = x - m.d; + var yy:Number = y - m.h; + var zz:Number = z - m.l; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * + * @param m + */ + public function vDeltaTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * Установка координат. + */ + public function vReset(x:Number = 0, y:Number = 0, z:Number = 0):Vector3 { + this.x = x; + this.y = y; + this.z = z; + return this; + } + + /** + * Копирование координат точки. + * + * @param v точка, координаты которой копируются + */ + public function vCopy(v:Vector3):Vector3 { + x = v.x; + y = v.y; + z = v.z; + return this; + } + + /** + * Клонирование точки. + * + * @return клонированная точка + */ + public function vClone():Vector3 { + return new Vector3(x, y, z); + } + + /** + * Копирует координаты вектора в экземпляр класса Vector3D. + * + * @param result экземпляр, в который копируются координаты + * @return переданный параметр + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + return result; + } + + /** + * @param source + * @return + */ + public function copyFromVector3D(source:Vector3D):Vector3 { + x = source.x; + y = source.y; + z = source.z; + return this; + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/types/Matrix3.as b/0.1.Vector3D/src/alternativa/physics/types/Matrix3.as new file mode 100644 index 0000000..86885d7 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/types/Matrix3.as @@ -0,0 +1,367 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + /** + * + */ + public class Matrix3 { + + public static const ZERO:Matrix3 = new Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0); + public static const IDENTITY:Matrix3 = new Matrix3(); + + public var a:Number; + public var b:Number; + public var c:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix3(a:Number = 1, b:Number = 0, c:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1) { + this.a = a; + this.b = b; + this.c = c; + + this.e = e; + this.f = f; + this.g = g; + + this.i = i; + this.j = j; + this.k = k; + } + + /** + * + * @return + */ + public function toIdentity():Matrix3 { + a = f = k = 1; + b = c = e = g = i = j = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + var det:Number = 1/(-cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk); + + a = (ff*kk - gg*jj)*det; + b = (cc*jj - bb*kk)*det; + c = (bb*gg - cc*ff)*det; + e = (gg*ii - ee*kk)*det; + f = (aa*kk - cc*ii)*det; + g = (cc*ee - aa*gg)*det; + i = (ee*jj - ff*ii)*det; + j = (bb*ii - aa*jj)*det; + k = (aa*ff - bb*ee)*det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix3):Matrix3 { + a += m.a; + b += m.b; + c += m.c; + e += m.e; + f += m.f; + g += m.g; + i += m.i; + j += m.j; + k += m.k; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix3):Matrix3 { + a -= m.a; + b -= m.b; + c -= m.c; + e -= m.e; + f -= m.f; + g -= m.g; + i -= m.i; + j -= m.j; + k -= m.k; + + return this; + } + + /** + * + * @return + */ + public function transpose():Matrix3 { + var tmp:Number = b; + b = e; + e = tmp; + tmp = c; + c = i; + i = tmp; + tmp = g; + g = j; + j = tmp; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector3To3D(vin:Vector3, vout:Vector3D):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * + * @param matrix + */ + public function createSkewSymmetric(v:Vector3):Matrix3 { + a = f = k = 0; + b = -v.z; + c = v.y; + e = v.z; + g = -v.x; + i = -v.y; + j = v.x; + return this; + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(i:int, axis:Vector3):void { + switch (i) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + } + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix3):Matrix3 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * @return + */ + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):Matrix3 { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + + return this; + } + + /** + * Метод формирует матрицу поворота на заданный угол относительно заданной оси. + * + * @param axis нормализованный вектор, задающий ось, относительно которой выполняется поворот + * @param angle угол поворота в радианах + */ + public function fromAxisAngle(axis:Vector3, angle:Number):void { + var c1:Number = Math.cos(angle); + var s:Number = Math.sin(angle); + var t:Number = 1 - c1; + var x:Number = axis.x; + var y:Number = axis.y; + var z:Number = axis.z; + + a = t*x*x + c1; + b = t*x*y - z*s; + c = t*x*z + y*s; + + e = t*x*y + z*s; + f = t*y*y + c1; + g = t*y*z - x*s; + + i = t*x*z - y*s; + j = t*y*z + x*s; + k = t*z*z + c1; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix3 { + return new Matrix3(a, b, c, e, f, g, i, j, k); + } + + public function toString():String { + return "[Matrix3 (" + a + ", " + b + ", " + c + "), (" + e + ", " + f + ", " + g + "), (" + i + ", " + j + ", " + k + ")]"; + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/types/Matrix4.as b/0.1.Vector3D/src/alternativa/physics/types/Matrix4.as new file mode 100644 index 0000000..8cc23b2 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/types/Matrix4.as @@ -0,0 +1,402 @@ +package alternativa.physics.types { + import __AS3__.vec.Vector; + + /** + * + */ + public class Matrix4 { + + public var a:Number; + public var b:Number; + public var c:Number; + public var d:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + public var h:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + public var l:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix4(a:Number = 1, b:Number = 0, c:Number = 0, d:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, h:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1, l:Number = 0) { + this.a = a; + this.b = b; + this.c = c; + this.d = h; + + this.e = e; + this.f = f; + this.g = g; + this.h = h; + + this.i = i; + this.j = j; + this.k = k; + this.l = l; + } + + /** + * + * @return + */ + public function toIdentity():Matrix4 { + a = f = k = 1; + b = c = e = g = i = j = d = h = l = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + var det:Number = -cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk; + + a = (-gg*jj + ff*kk)/det; + b = (cc*jj - bb*kk)/det; + c = (-cc*ff + bb*gg)/det; + d = (dd*gg*jj - cc*hh*jj - dd*ff*kk + bb*hh*kk + cc*ff*ll - bb*gg*ll)/det; + e = (gg*ii - ee*kk)/det; + f = (-cc*ii + aa*kk)/det; + g = (cc*ee - aa*gg)/det; + h = (cc*hh*ii - dd*gg*ii + dd*ee*kk - aa*hh*kk - cc*ee*ll + aa*gg*ll)/det; + i = (-ff*ii + ee*jj)/det; + j = (bb*ii - aa*jj)/det; + k = (-bb*ee + aa*ff)/det; + l = (dd*ff*ii - bb*hh*ii - dd*ee*jj + aa*hh*jj + bb*ee*ll - aa*ff*ll)/det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + d = m.a*dd + m.b*hh + m.c*ll + m.d; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + h = m.e*dd + m.f*hh + m.g*ll + m.h; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + l = m.i*dd + m.j*hh + m.k*ll + m.l; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param matrix правый операнд умножения + */ + public function prepend(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + d = aa*m.d + bb*m.h + cc*m.l + dd; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + h = ee*m.d + ff*m.h + gg*m.l + hh; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + l = ii*m.d + jj*m.h + kk*m.l + ll; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix4):Matrix4 { + a += m.a; + b += m.b; + c += m.c; + d += m.d; + e += m.e; + f += m.f; + g += m.g; + h += m.h; + i += m.i; + j += m.j; + k += m.k; + l += m.l; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix4):Matrix4 { + a -= m.a; + b -= m.b; + c -= m.c; + d -= m.d; + e -= m.e; + f -= m.f; + g -= m.g; + h -= m.h; + i -= m.i; + j -= m.j; + k -= m.k; + l -= m.l; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectors(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param Vector3 + * @param arrout + * @param Vector3 + */ + public function transformVectorsInverse(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param i + * @param axis + */ + public function getAxis(idx:int, axis:Vector3):void { + switch (idx) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + case 3: + axis.x = d; + axis.y = h; + axis.z = l; + return; + } + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param pin входной вектор + * @param pout вектор, в который записывается результат трансформации + */ + public function deltaTransformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Копирование значений указанной матрицы. + * + * @param matrix матрица, значения которой копируются + */ + public function copy(m:Matrix4):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = m.d; + e = m.e; + f = m.f; + g = m.g; + h = m.h; + i = m.i; + j = m.j; + k = m.k; + l = m.l; + + return this; + } + + public function setFromMatrix3(m:Matrix3, offset:Vector3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = offset.x; + e = m.e; + f = m.f; + g = m.g; + h = offset.y; + i = m.i; + j = m.j; + k = m.k; + l = offset.z; + + return this; + } + + /** + * @param rotations + */ + public function getEulerAngles(rotations:Vector3):void { + if (-1 < i && i < 1) { + rotations.x = Math.atan2(j, k); + rotations.y = -Math.asin(i); + rotations.z = Math.atan2(e, a); + } else { + rotations.x = 0; + rotations.y = 0.5*((i <= -1) ? Math.PI : -Math.PI); + rotations.z = Math.atan2(-b, f); + } + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix4 { + return new Matrix4(a, b, c, d, e, f, g, h, i, j, k, l); + } + + public function toString():String { + return "[Matrix4 [" + a.toFixed(3) + " " + b.toFixed(3) + " " + c.toFixed(3) + " " + d.toFixed(3) + "] [" + e.toFixed(3) + " " + f.toFixed(3) + " " + g.toFixed(3) + " " + h.toFixed(3) + "] [" + i.toFixed(3) + " " + j.toFixed(3) + " " + k.toFixed(3) + " " + l.toFixed(3) + "]]"; + } + + } +} \ No newline at end of file diff --git a/0.1.Vector3D/src/alternativa/physics/types/Quaternion.as b/0.1.Vector3D/src/alternativa/physics/types/Quaternion.as new file mode 100644 index 0000000..7ad2af0 --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/types/Quaternion.as @@ -0,0 +1,396 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + public class Quaternion { + + public var w:Number; + public var x:Number; + public var y:Number; + public var z:Number; + + public static function multiply(q1:Quaternion, q2:Quaternion, result:Quaternion):void { + result.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z; + result.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y; + result.y = q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z; + result.z = q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x; + } + + public static function createFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngle(axis, angle); + return q; + } + + public static function createFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngleComponents(x, y, z, angle); + return q; + } + + public function Quaternion(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0) { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + } + + public function reset(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0):Quaternion { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + return this; + } + + public function normalize():Quaternion { + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * Умножает на указанный кватернион слева: this * q + * + * @param q множитель + */ + public function prepend(q:Quaternion):Quaternion { + var ww:Number = w*q.w - x*q.x - y*q.y - z*q.z; + var xx:Number = w*q.x + x*q.w + y*q.z - z*q.y; + var yy:Number = w*q.y + y*q.w + z*q.x - x*q.z; + var zz:Number = w*q.z + z*q.w + x*q.y - y*q.x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Умножает на указанный кватернион справа: q * this + * + * @param q множитель + */ + public function append(q:Quaternion):Quaternion { + var ww:Number = q.w*w - q.x*x - q.y*y - q.z*z; + var xx:Number = q.w*x + q.x*w + q.y*z - q.z*y; + var yy:Number = q.w*y + q.y*w + q.z*x - q.x*z; + var zz:Number = q.w*z + q.z*w + q.x*y - q.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * + * @param vector + */ + public function rotateByVector(v:Vector3):Quaternion { + var ww:Number = -v.x*x - v.y*y - v.z*z; + var xx:Number = v.x*w + v.y*z - v.z*y; + var yy:Number = v.y*w + v.z*x - v.x*z; + var zz:Number = v.z*w + v.x*y - v.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Добавляет вращение, приданное вектором угловой скорости за указанное время. + * + * @param v + * @param scale + */ + public function addScaledVector(v:Vector3, scale:Number):Quaternion { + var vx:Number = v.x*scale; + var vy:Number = v.y*scale; + var vz:Number = v.z*scale; + var ww:Number = -x*vx - y*vy - z*vz; + var xx:Number = vx*w + vy*z - vz*y; + var yy:Number = vy*w + vz*x - vx*z; + var zz:Number = vz*w + vx*y - vy*x; + w += 0.5*ww; + x += 0.5*xx; + y += 0.5*yy; + z += 0.5*zz; + // inlined normalize + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param m + * @return + */ + public function toMatrix3(m:Matrix3):Quaternion { + var xx2:Number = 2*x*x; + var yy2:Number = 2*y*y; + var zz2:Number = 2*z*z; + var xy2:Number = 2*x*y; + var yz2:Number = 2*y*z; + var zx2:Number = 2*z*x; + var wx2:Number = 2*w*x; + var wy2:Number = 2*w*y; + var wz2:Number = 2*w*z; + + m.a = 1 - yy2 - zz2; + m.b = xy2 - wz2; + m.c = zx2 + wy2; + + m.e = xy2 + wz2; + m.f = 1 - xx2 - zz2; + m.g = yz2 - wx2; + + m.i = zx2 - wy2; + m.j = yz2 + wx2; + m.k = 1 - xx2 - yy2; + return this; + } + + /** + * + * @return + */ + public function length():Number { + return Math.sqrt(w*w + x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function lengthSqr():Number { + return w*w + x*x + y*y + z*z; + } + + /** + * + * @param axis + * @param angle + * @return + */ + public function setFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z); + x = axis.x*k; + y = axis.y*k; + z = axis.z*k; + return this; + } + + /** + * + * @param x + * @param y + * @param z + * @param angle + */ + public function setFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(x*x + y*y + z*z); + this.x = x*k; + this.y = y*k; + this.z = z*k; + return this; + } + + /** + * + * @param vector + */ + public function toAxisVector(v:Vector3 = null):Vector3 { + if (w < -1 || w > 1) { + normalize(); + } + if (v == null) { + v = new Vector3(); + } + if (w > -1 && w < 1) { + if (w == 0) { + v.x = x; + v.y = y; + v.z = z; + } else { + var angle:Number = 2*Math.acos(w); + var coeff:Number = 1/Math.sqrt(1 - w*w); + v.x = x*coeff*angle; + v.y = y*coeff*angle; + v.z = z*coeff*angle; + } + } else { + v.x = 0; + v.y = 0; + v.z = 0; + } + return v; + } + + /** + * + * @param rotations + */ + public function getEulerAngles(angles:Vector3):Vector3 { + var qi2:Number = 2*x*x; + var qj2:Number = 2*y*y; + var qk2:Number = 2*z*z; + var qij:Number = 2*x*y; + var qjk:Number = 2*y*z; + var qki:Number = 2*z*x; + var qri:Number = 2*w*x; + var qrj:Number = 2*w*y; + var qrk:Number = 2*w*z; + + var aa:Number = 1 - qj2 - qk2; + var bb:Number = qij - qrk; + var ee:Number = qij + qrk; + var ff:Number = 1 - qi2 - qk2; + var ii:Number = qki - qrj; + var jj:Number = qjk + qri; + var kk:Number = 1 - qi2 - qj2; + + if (-1 < ii && ii < 1) { + if (angles == null) { + angles = new Vector3(Math.atan2(jj, kk), -Math.asin(ii), Math.atan2(ee, aa)); + } else { + angles.x = Math.atan2(jj, kk); + angles.y = -Math.asin(ii); + angles.z = Math.atan2(ee, aa); + } + } else { + if (angles == null) { + angles = new Vector3(0, 0.5*((ii <= -1) ? Math.PI : -Math.PI), Math.atan2(-bb, ff)); + } else { + angles.x = 0; + angles.y = 0.5*((ii <= -1) ? Math.PI : -Math.PI); + angles.z = Math.atan2(-bb, ff); + } + } + return angles; + } + + /** + * + */ + public function conjugate():void { + x = -x; + y = -y; + z = -z; + } + + /** + * Выполняет линейную интерполяцию. + * + * @param q1 начало отрезка + * @param q2 конец отрезка + * @param t время, обычно задаётся в интервале [0, 1] + */ + public function nlerp(q1:Quaternion, q2:Quaternion, t:Number):Quaternion { + var d:Number = 1 - t; + w = q1.w*d + q2.w*t; + x = q1.x*d + q2.x*t; + y = q1.y*d + q2.y*t; + z = q1.z*d + q2.z*t; + // inlined normalize + d = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param q + * @return + */ + public function subtract(q:Quaternion):Quaternion { + w -= q.w; + x -= q.x; + y -= q.y; + z -= q.z; + return this; + } + + /** + * + * @param q1 + * @param q2 + * @return + */ + public function diff(q1:Quaternion, q2:Quaternion):Quaternion { + w = q2.w - q1.w; + x = q2.x - q1.x; + y = q2.y - q1.y; + z = q2.z - q1.z; + return this; + } + + /** + * + * @param q + * @return + */ + public function copy(q:Quaternion):Quaternion { + w = q.w; + x = q.x; + y = q.y; + z = q.z; + return this; + } + + /** + * @param reslult + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + result.w = w; + return result; + } + + /** + * + * @return + */ + public function clone():Quaternion { + return new Quaternion(w, x, y, z); + } + + /** + * + * @return + */ + public function toString():String { + return "[" + w + ", " + x + ", " + y + ", " + z + "]"; + } + } +} diff --git a/0.1.Vector3D/src/alternativa/physics/types/Vector3.as b/0.1.Vector3D/src/alternativa/physics/types/Vector3.as new file mode 100644 index 0000000..298ddad --- /dev/null +++ b/0.1.Vector3D/src/alternativa/physics/types/Vector3.as @@ -0,0 +1,331 @@ +package alternativa.physics.types { + import flash.geom.Vector3D; + + + /** + * + */ + public class Vector3 extends Vector3D { + + public static const ZERO:Vector3 = new Vector3(0, 0, 0); + public static const X_AXIS:Vector3 = new Vector3(1, 0, 0); + public static const Y_AXIS:Vector3 = new Vector3(0, 1, 0); + public static const Z_AXIS:Vector3 = new Vector3(0, 0, 1); + + /** + * + * @param x + * @param y + * @param z + */ + public function Vector3(x:Number = 0, y:Number = 0, z:Number = 0) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * + * @return + */ + public function vLength():Number { + return Math.sqrt(x*x + y*y + z*z); + } + + /** + * + * @return + */ + public function vLengthSqr():Number { + return x*x + y*y + z*z; + } + + /** + * + * @param length + * @return + */ + public function vSetLength(length:Number):Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = length; + } else { + var k:Number = length/Math.sqrt(x*x + y*y + z*z); + x *= k; + y *= k; + z *= k; + } + return this; + } + + /** + * + * @return + */ + public function vNormalize():Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = 1; + } else { + var k:Number = Math.sqrt(d); + x /= k; + y /= k; + z /= k; + } + return this; + } + + /** + * Сложение координат. + * + * @param v точка, координаты которой прибавляются к собственным + */ + public function vAdd(v:Vector3):Vector3 { + x += v.x; + y += v.y; + z += v.z; + return this; + } + + /** + * + * @param k + * @param v + * @return + */ + public function vAddScaled(k:Number, v:Vector3):Vector3 { + x += k*v.x; + y += k*v.y; + z += k*v.z; + return this; + } + + /** + * Вычитание координат. + * + * @param v точка, координаты которой вычитаются из собственных + */ + public function vSubtract(v:Vector3):Vector3 { + x -= v.x; + y -= v.y; + z -= v.z; + return this; + } + + /** + * Вычисляет сумму векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + * @return this + */ + public function vSum(a:Vector3, b:Vector3):Vector3 { + x = a.x + b.x; + y = a.y + b.y; + z = a.z + b.z; + return this; + } + + /** + * Вычисление разности векторов. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + */ + public function vDiff(a:Vector3, b:Vector3):Vector3 { + x = a.x - b.x; + y = a.y - b.y; + z = a.z - b.z; + return this; + } + + /** + * Умножение на скаляр. + * + * @param k число, на которое умножаются координаты + */ + public function vScale(k:Number):Vector3 { + x *= k; + y *= k; + z *= k; + return this; + } + + /** + * Инвертирование вектора. + */ + public function vReverse():Vector3 { + x = -x; + y = -y; + z = -z; + return this; + } + + /** + * + * @param v + * @return + */ + public function vDot(v:Vector3):Number { + return x*v.x + y*v.y + z*v.z; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross(v:Vector3):Vector3 { + var xx:Number = y*v.z - z*v.y; + var yy:Number = z*v.x - x*v.z; + var zz:Number = x*v.y - y*v.x; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Вычисляет векторное произведение с заданным вектором и записывает результат в текущий вектор. + * + * @param v + */ + public function vCross2(a:Vector3, b:Vector3):Vector3 { + x = a.y*b.z - a.z*b.y; + y = a.z*b.x - a.x*b.z; + z = a.x*b.y - a.y*b.x; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy3(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * + * @param matrix + */ + public function vTransformBy3Tr(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * Трансформация точки (вектора). Новым значением координат становится результат умножения матрицы на вектор вида + * M × r. + * + * @param m матрица трансформации + */ + public function vTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz + m.d; + y = m.e*xx + m.f*yy + m.g*zz + m.h; + z = m.i*xx + m.j*yy + m.k*zz + m.l; + return this; + } + + /** + * + * @param m + * @return + */ + public function vTransformInverseBy4(m:Matrix4):Vector3 { + var xx:Number = x - m.d; + var yy:Number = y - m.h; + var zz:Number = z - m.l; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * + * @param m + */ + public function vDeltaTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * Установка координат. + */ + public function vReset(x:Number = 0, y:Number = 0, z:Number = 0):Vector3 { + this.x = x; + this.y = y; + this.z = z; + return this; + } + + /** + * Копирование координат точки. + * + * @param v точка, координаты которой копируются + */ + public function vCopy(v:Vector3):Vector3 { + x = v.x; + y = v.y; + z = v.z; + return this; + } + + /** + * Клонирование точки. + * + * @return клонированная точка + */ + public function vClone():Vector3 { + return new Vector3(x, y, z); + } + + /** + * Копирует координаты вектора в экземпляр класса Vector3D. + * + * @param result экземпляр, в который копируются координаты + * @return переданный параметр + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + return result; + } + + /** + * @param source + * @return + */ + public function copyFromVector3D(source:Vector3D):Vector3 { + x = source.x; + y = source.y; + z = source.z; + return this; + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/.actionScriptProperties b/2.0.0.0/.actionScriptProperties new file mode 100644 index 0000000..f73adc8 --- /dev/null +++ b/2.0.0.0/.actionScriptProperties @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/2.0.0.0/.flexLibProperties b/2.0.0.0/.flexLibProperties new file mode 100644 index 0000000..1d3e45a --- /dev/null +++ b/2.0.0.0/.flexLibProperties @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/2.0.0.0/.project b/2.0.0.0/.project new file mode 100644 index 0000000..1f1425f --- /dev/null +++ b/2.0.0.0/.project @@ -0,0 +1,19 @@ + + + AlternativaPhysics + + + + + + com.adobe.flexbuilder.project.flexbuilder + + + + + + org.maven.ide.eclipse.maven2Nature + com.adobe.flexbuilder.project.flexlibnature + com.adobe.flexbuilder.project.actionscriptnature + + diff --git a/2.0.0.0/.settings/.svn/all-wcprops b/2.0.0.0/.settings/.svn/all-wcprops new file mode 100644 index 0000000..fc939f9 --- /dev/null +++ b/2.0.0.0/.settings/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 89 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/.settings +END +org.eclipse.core.resources.prefs +K 25 +svn:wc:ra_dav:version-url +V 122 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/.settings/org.eclipse.core.resources.prefs +END diff --git a/2.0.0.0/.settings/.svn/entries b/2.0.0.0/.settings/.svn/entries new file mode 100644 index 0000000..197ff79 --- /dev/null +++ b/2.0.0.0/.settings/.svn/entries @@ -0,0 +1,40 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/.settings +http://svndev.alternativaplatform.com + + + +2009-04-01T12:25:29.638016Z +10301 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +org.eclipse.core.resources.prefs +file + + + + +2010-10-28T04:32:40.000000Z +4f70b476f3e5075e399505021df2f89b +2009-04-01T12:25:29.638016Z +10301 +mike + diff --git a/2.0.0.0/.settings/.svn/format b/2.0.0.0/.settings/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/2.0.0.0/.settings/.svn/format @@ -0,0 +1 @@ +8 diff --git a/2.0.0.0/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base b/2.0.0.0/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base new file mode 100644 index 0000000..b3d0d49 --- /dev/null +++ b/2.0.0.0/.settings/.svn/text-base/org.eclipse.core.resources.prefs.svn-base @@ -0,0 +1,3 @@ +#Fri Mar 20 00:19:33 YEKT 2009 +eclipse.preferences.version=1 +encoding/=UTF-8 diff --git a/2.0.0.0/.settings/org.eclipse.core.resources.prefs b/2.0.0.0/.settings/org.eclipse.core.resources.prefs new file mode 100644 index 0000000..b3d0d49 --- /dev/null +++ b/2.0.0.0/.settings/org.eclipse.core.resources.prefs @@ -0,0 +1,3 @@ +#Fri Mar 20 00:19:33 YEKT 2009 +eclipse.preferences.version=1 +encoding/=UTF-8 diff --git a/2.0.0.0/.svn/all-wcprops b/2.0.0.0/.svn/all-wcprops new file mode 100644 index 0000000..61bb5df --- /dev/null +++ b/2.0.0.0/.svn/all-wcprops @@ -0,0 +1,35 @@ +K 25 +svn:wc:ra_dav:version-url +V 79 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0 +END +.flexLibProperties +K 25 +svn:wc:ra_dav:version-url +V 98 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/.flexLibProperties +END +.project +K 25 +svn:wc:ra_dav:version-url +V 88 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/.project +END +pom.xml +K 25 +svn:wc:ra_dav:version-url +V 87 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/pom.xml +END +.actionScriptProperties +K 25 +svn:wc:ra_dav:version-url +V 103 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/.actionScriptProperties +END +AlternativaPhysics.iml +K 25 +svn:wc:ra_dav:version-url +V 102 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/AlternativaPhysics.iml +END diff --git a/2.0.0.0/.svn/dir-prop-base b/2.0.0.0/.svn/dir-prop-base new file mode 100644 index 0000000..e9d95d1 --- /dev/null +++ b/2.0.0.0/.svn/dir-prop-base @@ -0,0 +1,8 @@ +K 13 +svn:mergeinfo +V 357 +/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.5.Optimized:19806-19860 +/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.7.0.LinkedList:20360-22217,22226-22252 +/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.0.9:29404-29439 +/platform/clients/fp10/libraries/AlternativaPhysics/branches/0.1.Vector3D:13892-14098 +END diff --git a/2.0.0.0/.svn/entries b/2.0.0.0/.svn/entries new file mode 100644 index 0000000..95b4adc --- /dev/null +++ b/2.0.0.0/.svn/entries @@ -0,0 +1,97 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0 +http://svndev.alternativaplatform.com + + + +2010-05-12T11:32:14.711790Z +34250 +mike +has-props + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +META-INF +dir + +.flexLibProperties +file + + + + +2010-10-28T04:32:40.000000Z +a194fb1b136f5bb6db733ac9097bddd8 +2010-04-01T08:37:11.976264Z +30874 +mike + +.project +file + + + + +2010-10-28T04:32:40.000000Z +1e90cc68b52b93173b2b5de88eb5c3a3 +2009-04-20T17:15:55.260110Z +11541 +mike + +src +dir + +pom.xml +file + + + + +2010-10-28T04:32:40.000000Z +b0627a880d256def5aa9551237b12d2b +2010-05-12T11:32:14.711790Z +34250 +mike + +.actionScriptProperties +file + + + + +2010-10-28T04:32:40.000000Z +b8656f882fff2baedd57a92db86fcf8c +2010-04-01T08:37:11.976264Z +30874 +mike + +AlternativaPhysics.iml +file + + + + +2010-10-28T04:32:40.000000Z +19d99215e37e0dfe7c20f56cd7eca6cd +2010-05-12T11:32:14.711790Z +34250 +mike + +.settings +dir + diff --git a/2.0.0.0/.svn/format b/2.0.0.0/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/2.0.0.0/.svn/format @@ -0,0 +1 @@ +8 diff --git a/2.0.0.0/.svn/text-base/.actionScriptProperties.svn-base b/2.0.0.0/.svn/text-base/.actionScriptProperties.svn-base new file mode 100644 index 0000000..f73adc8 --- /dev/null +++ b/2.0.0.0/.svn/text-base/.actionScriptProperties.svn-base @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/2.0.0.0/.svn/text-base/.flexLibProperties.svn-base b/2.0.0.0/.svn/text-base/.flexLibProperties.svn-base new file mode 100644 index 0000000..1d3e45a --- /dev/null +++ b/2.0.0.0/.svn/text-base/.flexLibProperties.svn-base @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/2.0.0.0/.svn/text-base/.project.svn-base b/2.0.0.0/.svn/text-base/.project.svn-base new file mode 100644 index 0000000..1f1425f --- /dev/null +++ b/2.0.0.0/.svn/text-base/.project.svn-base @@ -0,0 +1,19 @@ + + + AlternativaPhysics + + + + + + com.adobe.flexbuilder.project.flexbuilder + + + + + + org.maven.ide.eclipse.maven2Nature + com.adobe.flexbuilder.project.flexlibnature + com.adobe.flexbuilder.project.actionscriptnature + + diff --git a/2.0.0.0/.svn/text-base/AlternativaPhysics.iml.svn-base b/2.0.0.0/.svn/text-base/AlternativaPhysics.iml.svn-base new file mode 100644 index 0000000..087d6cd --- /dev/null +++ b/2.0.0.0/.svn/text-base/AlternativaPhysics.iml.svn-base @@ -0,0 +1,103 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/2.0.0.0/.svn/text-base/pom.xml.svn-base b/2.0.0.0/.svn/text-base/pom.xml.svn-base new file mode 100644 index 0000000..e4537f1 --- /dev/null +++ b/2.0.0.0/.svn/text-base/pom.xml.svn-base @@ -0,0 +1,33 @@ + + 4.0.0 + platform.clients.fp10.libraries + AlternativaPhysics + swc + 2.0.0.0 + + platform.clients.fp10.tools.maven + BasePom + 2.0.6.0 + + + scm:svn:http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0 + + + + platform.clients.fp10.libraries + AlternativaOSGi + swc + external + + + + + + platform.clients.fp10.libraries + AlternativaOSGi + swc + 2.0.3.0 + + + + \ No newline at end of file diff --git a/2.0.0.0/AlternativaPhysics.iml b/2.0.0.0/AlternativaPhysics.iml new file mode 100644 index 0000000..087d6cd --- /dev/null +++ b/2.0.0.0/AlternativaPhysics.iml @@ -0,0 +1,103 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/2.0.0.0/META-INF/.svn/all-wcprops b/2.0.0.0/META-INF/.svn/all-wcprops new file mode 100644 index 0000000..83352e8 --- /dev/null +++ b/2.0.0.0/META-INF/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 88 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/META-INF +END +MANIFEST.MF +K 25 +svn:wc:ra_dav:version-url +V 100 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/META-INF/MANIFEST.MF +END diff --git a/2.0.0.0/META-INF/.svn/entries b/2.0.0.0/META-INF/.svn/entries new file mode 100644 index 0000000..0f4d83c --- /dev/null +++ b/2.0.0.0/META-INF/.svn/entries @@ -0,0 +1,40 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/META-INF +http://svndev.alternativaplatform.com + + + +2010-03-09T04:55:13.477965Z +29440 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +MANIFEST.MF +file + + + + +2010-10-28T04:32:40.000000Z +7b64dceb50a6905850ff00a0bfbe77eb +2009-04-20T17:15:55.260110Z +11541 +mike + diff --git a/2.0.0.0/META-INF/.svn/format b/2.0.0.0/META-INF/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/2.0.0.0/META-INF/.svn/format @@ -0,0 +1 @@ +8 diff --git a/2.0.0.0/META-INF/.svn/text-base/MANIFEST.MF.svn-base b/2.0.0.0/META-INF/.svn/text-base/MANIFEST.MF.svn-base new file mode 100644 index 0000000..5066a55 --- /dev/null +++ b/2.0.0.0/META-INF/.svn/text-base/MANIFEST.MF.svn-base @@ -0,0 +1 @@ +Bundle-Name: platform.clients.fp10.libraries.AlternativaPhysics diff --git a/2.0.0.0/META-INF/MANIFEST.MF b/2.0.0.0/META-INF/MANIFEST.MF new file mode 100644 index 0000000..5066a55 --- /dev/null +++ b/2.0.0.0/META-INF/MANIFEST.MF @@ -0,0 +1 @@ +Bundle-Name: platform.clients.fp10.libraries.AlternativaPhysics diff --git a/2.0.0.0/pom.xml b/2.0.0.0/pom.xml new file mode 100644 index 0000000..e4537f1 --- /dev/null +++ b/2.0.0.0/pom.xml @@ -0,0 +1,33 @@ + + 4.0.0 + platform.clients.fp10.libraries + AlternativaPhysics + swc + 2.0.0.0 + + platform.clients.fp10.tools.maven + BasePom + 2.0.6.0 + + + scm:svn:http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0 + + + + platform.clients.fp10.libraries + AlternativaOSGi + swc + external + + + + + + platform.clients.fp10.libraries + AlternativaOSGi + swc + 2.0.3.0 + + + + \ No newline at end of file diff --git a/2.0.0.0/src/.svn/all-wcprops b/2.0.0.0/src/.svn/all-wcprops new file mode 100644 index 0000000..3bd8395 --- /dev/null +++ b/2.0.0.0/src/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 83 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src +END diff --git a/2.0.0.0/src/.svn/entries b/2.0.0.0/src/.svn/entries new file mode 100644 index 0000000..350a1a6 --- /dev/null +++ b/2.0.0.0/src/.svn/entries @@ -0,0 +1,31 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src +http://svndev.alternativaplatform.com + + + +2010-05-12T11:32:14.711790Z +34250 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +alternativa +dir + diff --git a/2.0.0.0/src/.svn/format b/2.0.0.0/src/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/2.0.0.0/src/.svn/format @@ -0,0 +1 @@ +8 diff --git a/2.0.0.0/src/alternativa/.svn/all-wcprops b/2.0.0.0/src/alternativa/.svn/all-wcprops new file mode 100644 index 0000000..aef4314 --- /dev/null +++ b/2.0.0.0/src/alternativa/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 95 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa +END diff --git a/2.0.0.0/src/alternativa/.svn/entries b/2.0.0.0/src/alternativa/.svn/entries new file mode 100644 index 0000000..c49cb5c --- /dev/null +++ b/2.0.0.0/src/alternativa/.svn/entries @@ -0,0 +1,34 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa +http://svndev.alternativaplatform.com + + + +2010-05-12T11:32:14.711790Z +34250 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +physics +dir + +math +dir + diff --git a/2.0.0.0/src/alternativa/.svn/format b/2.0.0.0/src/alternativa/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/2.0.0.0/src/alternativa/.svn/format @@ -0,0 +1 @@ +8 diff --git a/2.0.0.0/src/alternativa/math/.svn/all-wcprops b/2.0.0.0/src/alternativa/math/.svn/all-wcprops new file mode 100644 index 0000000..ce1d46e --- /dev/null +++ b/2.0.0.0/src/alternativa/math/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 100 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/math +END +Quaternion.as +K 25 +svn:wc:ra_dav:version-url +V 114 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/math/Quaternion.as +END +Vector3.as +K 25 +svn:wc:ra_dav:version-url +V 111 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/math/Vector3.as +END +Matrix3.as +K 25 +svn:wc:ra_dav:version-url +V 111 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/math/Matrix3.as +END +Matrix4.as +K 25 +svn:wc:ra_dav:version-url +V 111 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/math/Matrix4.as +END diff --git a/2.0.0.0/src/alternativa/math/.svn/entries b/2.0.0.0/src/alternativa/math/.svn/entries new file mode 100644 index 0000000..1ef7597 --- /dev/null +++ b/2.0.0.0/src/alternativa/math/.svn/entries @@ -0,0 +1,76 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/math +http://svndev.alternativaplatform.com + + + +2010-05-12T11:32:14.711790Z +34250 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Quaternion.as +file + + + + +2010-10-28T04:32:40.000000Z +e2ea4456ad77220ef1f54c8a3fbb7f8b +2010-05-12T11:32:14.711790Z +34250 +mike + +Vector3.as +file + + + + +2010-10-28T04:32:40.000000Z +96d3576c3b90192be3b968d17fee2923 +2010-05-12T11:32:14.711790Z +34250 +mike + +Matrix3.as +file + + + + +2010-10-28T04:32:40.000000Z +c5485fd4a8f25e0d4bea6d7fb61e5729 +2010-05-12T11:32:14.711790Z +34250 +mike + +Matrix4.as +file + + + + +2010-10-28T04:32:40.000000Z +044e5b447976b20a4dfcd150830fb4cd +2010-05-12T11:32:14.711790Z +34250 +mike + diff --git a/2.0.0.0/src/alternativa/math/.svn/format b/2.0.0.0/src/alternativa/math/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/2.0.0.0/src/alternativa/math/.svn/format @@ -0,0 +1 @@ +8 diff --git a/2.0.0.0/src/alternativa/math/.svn/text-base/Matrix3.as.svn-base b/2.0.0.0/src/alternativa/math/.svn/text-base/Matrix3.as.svn-base new file mode 100644 index 0000000..cab0fe2 --- /dev/null +++ b/2.0.0.0/src/alternativa/math/.svn/text-base/Matrix3.as.svn-base @@ -0,0 +1,393 @@ +package alternativa.math { + import flash.geom.Vector3D; + + /** + * Матрица 3x3, используемая для описания ориентации. + */ + public class Matrix3 { + + public static const ZERO:Matrix3 = new Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0); + public static const IDENTITY:Matrix3 = new Matrix3(); + + public var a:Number; + public var b:Number; + public var c:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix3(a:Number = 1, b:Number = 0, c:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1) { + this.a = a; + this.b = b; + this.c = c; + + this.e = e; + this.f = f; + this.g = g; + + this.i = i; + this.j = j; + this.k = k; + } + + /** + * + * @return this + */ + public function toIdentity():Matrix3 { + a = f = k = 1; + b = c = e = g = i = j = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + var det:Number = 1/(-cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk); + + a = (ff*kk - gg*jj)*det; + b = (cc*jj - bb*kk)*det; + c = (bb*gg - cc*ff)*det; + e = (gg*ii - ee*kk)*det; + f = (aa*kk - cc*ii)*det; + g = (cc*ee - aa*gg)*det; + i = (ee*jj - ff*ii)*det; + j = (bb*ii - aa*jj)*det; + k = (aa*ff - bb*ee)*det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param m правый операнд умножения + */ + public function prepend(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param m правый операнд умножения + */ + public function prependTransposed(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.b + cc*m.c; + b = aa*m.e + bb*m.f + cc*m.g; + c = aa*m.i + bb*m.j + cc*m.k; + e = ee*m.a + ff*m.b + gg*m.c; + f = ee*m.e + ff*m.f + gg*m.g; + g = ee*m.i + ff*m.j + gg*m.k; + i = ii*m.a + jj*m.b + kk*m.c; + j = ii*m.e + jj*m.f + kk*m.g; + k = ii*m.i + jj*m.j + kk*m.k; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix3):Matrix3 { + a += m.a; + b += m.b; + c += m.c; + e += m.e; + f += m.f; + g += m.g; + i += m.i; + j += m.j; + k += m.k; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix3):Matrix3 { + a -= m.a; + b -= m.b; + c -= m.c; + e -= m.e; + f -= m.f; + g -= m.g; + i -= m.i; + j -= m.j; + k -= m.k; + + return this; + } + + /** + * + * @return this + */ + public function transpose():Matrix3 { + var tmp:Number = b; + b = e; + e = tmp; + tmp = c; + c = i; + i = tmp; + tmp = g; + g = j; + j = tmp; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector3To3D(vin:Vector3, vout:Vector3D):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * + * @param v + */ + public function createSkewSymmetric(v:Vector3):Matrix3 { + a = f = k = 0; + b = -v.z; + c = v.y; + e = v.z; + g = -v.x; + i = -v.y; + j = v.x; + return this; + } + + /** + * Копирование значений указанной матрицы. + * + * @param m матрица, значения которой копируются + */ + public function copy(m:Matrix3):Matrix3 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * Устанавливает вращательную часть матрицы на основе указанных эйлеровых углов. + * + * @param rx угол поворота по оси X в радианах + * @param ry угол поворота по оси Y в радианах + * @param rz угол поворота по оси Z в радианах + */ + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):void { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + } + + /** + * Метод формирует матрицу поворота на заданный угол относительно заданной оси. + * + * @param axis нормализованный вектор, задающий ось, относительно которой выполняется поворот + * @param angle угол поворота в радианах + */ + public function fromAxisAngle(axis:Vector3, angle:Number):void { + var c1:Number = Math.cos(angle); + var s:Number = Math.sin(angle); + var t:Number = 1 - c1; + var x:Number = axis.x; + var y:Number = axis.y; + var z:Number = axis.z; + + a = t*x*x + c1; + b = t*x*y - z*s; + c = t*x*z + y*s; + + e = t*x*y + z*s; + f = t*y*y + c1; + g = t*y*z - x*s; + + i = t*x*z - y*s; + j = t*y*z + x*s; + k = t*z*z + c1; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix3 { + return new Matrix3(a, b, c, e, f, g, i, j, k); + } + + /** + * Формирует строковое представление объекта. + * + * @return строковое представление объекта + */ + public function toString():String { + return "[Matrix3 (" + a + ", " + b + ", " + c + "), (" + e + ", " + f + ", " + g + "), (" + i + ", " + j + ", " + k + ")]"; + } + + /** + * @param angles + */ + public function getEulerAngles(angles:Vector3):void { + if (-1 < i && i < 1) { + angles.x = Math.atan2(j, k); + angles.y = -Math.asin(i); + angles.z = Math.atan2(e, a); + } else { + angles.x = 0; + angles.y = (i <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-b, f); + } + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/math/.svn/text-base/Matrix4.as.svn-base b/2.0.0.0/src/alternativa/math/.svn/text-base/Matrix4.as.svn-base new file mode 100644 index 0000000..ee38ee4 --- /dev/null +++ b/2.0.0.0/src/alternativa/math/.svn/text-base/Matrix4.as.svn-base @@ -0,0 +1,572 @@ +package alternativa.math { + + /** + * + */ + public class Matrix4 { + + public static const IDENTITY:Matrix4 = new Matrix4(); + + public var a:Number; + public var b:Number; + public var c:Number; + public var d:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + public var h:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + public var l:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix4(a:Number = 1, b:Number = 0, c:Number = 0, d:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, h:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1, l:Number = 0) { + this.a = a; + this.b = b; + this.c = c; + this.d = d; + + this.e = e; + this.f = f; + this.g = g; + this.h = h; + + this.i = i; + this.j = j; + this.k = k; + this.l = l; + } + + /** + * + * @return this + */ + public function toIdentity():Matrix4 { + a = f = k = 1; + b = c = e = g = i = j = d = h = l = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + var det:Number = -cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk; + + a = (-gg*jj + ff*kk)/det; + b = (cc*jj - bb*kk)/det; + c = (-cc*ff + bb*gg)/det; + d = (dd*gg*jj - cc*hh*jj - dd*ff*kk + bb*hh*kk + cc*ff*ll - bb*gg*ll)/det; + e = (gg*ii - ee*kk)/det; + f = (-cc*ii + aa*kk)/det; + g = (cc*ee - aa*gg)/det; + h = (cc*hh*ii - dd*gg*ii + dd*ee*kk - aa*hh*kk - cc*ee*ll + aa*gg*ll)/det; + i = (-ff*ii + ee*jj)/det; + j = (bb*ii - aa*jj)/det; + k = (-bb*ee + aa*ff)/det; + l = (dd*ff*ii - bb*hh*ii - dd*ee*jj + aa*hh*jj + bb*ee*ll - aa*ff*ll)/det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + d = m.a*dd + m.b*hh + m.c*ll + m.d; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + h = m.e*dd + m.f*hh + m.g*ll + m.h; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + l = m.i*dd + m.j*hh + m.k*ll + m.l; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param m правый операнд умножения + */ + public function prepend(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + d = aa*m.d + bb*m.h + cc*m.l + dd; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + h = ee*m.d + ff*m.h + gg*m.l + hh; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + l = ii*m.d + jj*m.h + kk*m.l + ll; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix4):Matrix4 { + a += m.a; + b += m.b; + c += m.c; + d += m.d; + e += m.e; + f += m.f; + g += m.g; + h += m.h; + i += m.i; + j += m.j; + k += m.k; + l += m.l; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix4):Matrix4 { + a -= m.a; + b -= m.b; + c -= m.c; + d -= m.d; + e -= m.e; + f -= m.f; + g -= m.g; + h -= m.h; + i -= m.i; + j -= m.j; + k -= m.k; + l -= m.l; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + + /** + * + * @param arrin + * @param arrout + */ + public function transformVectors(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param arrout + * @param len + */ + public function transformVectorsN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param arrout + */ + public function transformVectorsInverse(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param arrin + * @param arrout + */ + public function transformVectorsInverseN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param idx + * @param axis + */ + public function getAxis(idx:int, axis:Vector3):void { + switch (idx) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + case 3: + axis.x = d; + axis.y = h; + axis.z = l; + return; + } + } + + /** + * + * @param xAxis + * @param yAxis + * @param zAxis + * @param pos + */ + public function setAxes(xAxis:Vector3, yAxis:Vector3, zAxis:Vector3, pos:Vector3):void { + a = xAxis.x; + e = xAxis.y; + i = xAxis.z; + + b = yAxis.x; + f = yAxis.y; + j = yAxis.z; + + c = zAxis.x; + g = zAxis.y; + k = zAxis.z; + + d = pos.x; + h = pos.y; + l = pos.z; + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function deltaTransformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function deltaTransformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Копирование значений указанной матрицы. + * + * @param m матрица, значения которой копируются + */ + public function copy(m:Matrix4):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = m.d; + e = m.e; + f = m.f; + g = m.g; + h = m.h; + i = m.i; + j = m.j; + k = m.k; + l = m.l; + + return this; + } + + /** + * + * @param m + * @param offset + * @return this + */ + public function setFromMatrix3(m:Matrix3, offset:Vector3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = offset.x; + e = m.e; + f = m.f; + g = m.g; + h = offset.y; + i = m.i; + j = m.j; + k = m.k; + l = offset.z; + + return this; + } + + /** + * + * @param m + * @return this + */ + public function setOrientationFromMatrix3(m:Matrix3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * + * @param angles + */ + public function getEulerAngles(angles:Vector3):void { + if (-1 < i && i < 1) { + angles.x = Math.atan2(j, k); + angles.y = -Math.asin(i); + angles.z = Math.atan2(e, a); + } else { + angles.x = 0; + angles.y = (i <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-b, f); + } + } + + /** + * Устанавливает координаты матрицы. + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + d = pos.x; + h = pos.y; + l = pos.z; + } + + /** + * Устанавливает координаты матрицы. + * + * @param x + * @param y + * @param z + */ + public function setPositionXYZ(x:Number, y:Number, z:Number):void { + d = x; + h = y; + l = z; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix4 { + return new Matrix4(a, b, c, d, e, f, g, h, i, j, k, l); + } + + /** + * Формирует строковое представление объекта. + * + * @return строковое представление объекта + */ + public function toString():String { + return "[Matrix4 [" + a.toFixed(3) + " " + b.toFixed(3) + " " + c.toFixed(3) + " " + d.toFixed(3) + "] [" + e.toFixed(3) + " " + f.toFixed(3) + " " + g.toFixed(3) + " " + h.toFixed(3) + "] [" + i.toFixed(3) + " " + j.toFixed(3) + " " + k.toFixed(3) + " " + l.toFixed(3) + "]]"; + } + + /** + * + */ + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):void { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + } + + /** + * Метод формирует матрицу поворота на заданный угол относительно заданной оси. + * + * @param axis нормализованный вектор, задающий ось, относительно которой выполняется поворот + * @param angle угол поворота в радианах + */ + public function fromAxisAngle(axis:Vector3, angle:Number):void { + var c1:Number = Math.cos(angle); + var s:Number = Math.sin(angle); + var t:Number = 1 - c1; + var x:Number = axis.x; + var y:Number = axis.y; + var z:Number = axis.z; + + a = t*x*x + c1; + b = t*x*y - z*s; + c = t*x*z + y*s; + + e = t*x*y + z*s; + f = t*y*y + c1; + g = t*y*z - x*s; + + i = t*x*z - y*s; + j = t*y*z + x*s; + k = t*z*z + c1; + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/math/.svn/text-base/Quaternion.as.svn-base b/2.0.0.0/src/alternativa/math/.svn/text-base/Quaternion.as.svn-base new file mode 100644 index 0000000..2c19cbb --- /dev/null +++ b/2.0.0.0/src/alternativa/math/.svn/text-base/Quaternion.as.svn-base @@ -0,0 +1,515 @@ +package alternativa.math { + import flash.geom.Vector3D; + + /** + * Кватернион. + */ + public class Quaternion { + + public static const IDENTITY:Quaternion = new Quaternion(); + + public var w:Number; + public var x:Number; + public var y:Number; + public var z:Number; + + public static function multiply(q1:Quaternion, q2:Quaternion, result:Quaternion):void { + result.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z; + result.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y; + result.y = q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z; + result.z = q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x; + } + + public static function createFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngle(axis, angle); + return q; + } + + public static function createFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngleComponents(x, y, z, angle); + return q; + } + + /** + * + * @param x + * @param y + * @param z + */ + public static function createFromEulerAnglesXYZ(x:Number, y:Number, z:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngleComponents(1, 0, 0, x); + + _q.setFromAxisAngleComponents(0, 1, 0, y); + q.append(_q); + q.normalize(); + + _q.setFromAxisAngleComponents(0, 0, 1, z); + q.append(_q); + q.normalize(); + return q; + } + + + private static var _q:Quaternion = new Quaternion(); + + public function Quaternion(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0) { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + } + + public function reset(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0):Quaternion { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + return this; + } + + public function normalize():Quaternion { + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * Умножает на указанный кватернион слева: this * q + * + * @param q множитель + */ + public function prepend(q:Quaternion):Quaternion { + var ww:Number = w*q.w - x*q.x - y*q.y - z*q.z; + var xx:Number = w*q.x + x*q.w + y*q.z - z*q.y; + var yy:Number = w*q.y + y*q.w + z*q.x - x*q.z; + var zz:Number = w*q.z + z*q.w + x*q.y - y*q.x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Умножает на указанный кватернион справа: q * this + * + * @param q множитель + */ + public function append(q:Quaternion):Quaternion { + var ww:Number = q.w*w - q.x*x - q.y*y - q.z*z; + var xx:Number = q.w*x + q.x*w + q.y*z - q.z*y; + var yy:Number = q.w*y + q.y*w + q.z*x - q.x*z; + var zz:Number = q.w*z + q.z*w + q.x*y - q.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * + * @param v + */ + public function rotateByVector(v:Vector3):Quaternion { + var ww:Number = -v.x*x - v.y*y - v.z*z; + var xx:Number = v.x*w + v.y*z - v.z*y; + var yy:Number = v.y*w + v.z*x - v.x*z; + var zz:Number = v.z*w + v.x*y - v.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Добавляет вращение, приданное вектором угловой скорости за указанное время. + * + * @param v + * @param scale + */ + public function addScaledVector(v:Vector3, scale:Number):Quaternion { + var vx:Number = v.x*scale; + var vy:Number = v.y*scale; + var vz:Number = v.z*scale; + var ww:Number = -x*vx - y*vy - z*vz; + var xx:Number = vx*w + vy*z - vz*y; + var yy:Number = vy*w + vz*x - vx*z; + var zz:Number = vz*w + vx*y - vy*x; + w += 0.5*ww; + x += 0.5*xx; + y += 0.5*yy; + z += 0.5*zz; + // inlined normalize + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param m + * @return this + */ + public function toMatrix3(m:Matrix3):Quaternion { + var xx2:Number = 2.0*x*x; + var yy2:Number = 2.0*y*y; + var zz2:Number = 2.0*z*z; + var xy2:Number = 2.0*x*y; + var yz2:Number = 2.0*y*z; + var zx2:Number = 2.0*z*x; + var wx2:Number = 2.0*w*x; + var wy2:Number = 2.0*w*y; + var wz2:Number = 2.0*w*z; + + m.a = 1.0 - yy2 - zz2; + m.b = xy2 - wz2; + m.c = zx2 + wy2; + + m.e = xy2 + wz2; + m.f = 1.0 - xx2 - zz2; + m.g = yz2 - wx2; + + m.i = zx2 - wy2; + m.j = yz2 + wx2; + m.k = 1.0 - xx2 - yy2; + return this; + } + + /** + * + * @param m + * @return this + */ + public function toMatrix4(m:Matrix4):Quaternion { + var xx2:Number = 2.0*x*x; + var yy2:Number = 2.0*y*y; + var zz2:Number = 2.0*z*z; + var xy2:Number = 2.0*x*y; + var yz2:Number = 2.0*y*z; + var zx2:Number = 2.0*z*x; + var wx2:Number = 2.0*w*x; + var wy2:Number = 2.0*w*y; + var wz2:Number = 2.0*w*z; + + m.a = 1.0 - yy2 - zz2; + m.b = xy2 - wz2; + m.c = zx2 + wy2; + + m.e = xy2 + wz2; + m.f = 1.0 - xx2 - zz2; + m.g = yz2 - wx2; + + m.i = zx2 - wy2; + m.j = yz2 + wx2; + m.k = 1.0 - xx2 - yy2; + return this; + } + + /** + * + * @return длина + */ + public function length():Number { + return Math.sqrt(w*w + x*x + y*y + z*z); + } + + /** + * + * @return квадрат длины + */ + public function lengthSqr():Number { + return w*w + x*x + y*y + z*z; + } + + /** + * + * @param axis + * @param angle + * @return this + */ + public function setFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z); + x = axis.x*k; + y = axis.y*k; + z = axis.z*k; + return this; + } + + /** + * + * @param x + * @param y + * @param z + * @param angle + */ + public function setFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(x*x + y*y + z*z); + this.x = x*k; + this.y = y*k; + this.z = z*k; + return this; + } + + /** + * + * @param v + */ + public function toAxisVector(v:Vector3 = null):Vector3 { + if (w < -1 || w > 1) { + normalize(); + } + if (v == null) { + v = new Vector3(); + } + if (w > -1 && w < 1) { + if (w == 0) { + v.x = x; + v.y = y; + v.z = z; + } else { + var angle:Number = 2*Math.acos(w); + var coeff:Number = 1/Math.sqrt(1 - w*w); + v.x = x*coeff*angle; + v.y = y*coeff*angle; + v.z = z*coeff*angle; + } + } else { + v.x = 0; + v.y = 0; + v.z = 0; + } + return v; + } + + /** + * + * @param angles + */ + public function getEulerAngles(angles:Vector3):Vector3 { + var qi2:Number = 2*x*x; + var qj2:Number = 2*y*y; + var qk2:Number = 2*z*z; + var qij:Number = 2*x*y; + var qjk:Number = 2*y*z; + var qki:Number = 2*z*x; + var qri:Number = 2*w*x; + var qrj:Number = 2*w*y; + var qrk:Number = 2*w*z; + + var aa:Number = 1 - qj2 - qk2; + var bb:Number = qij - qrk; + var ee:Number = qij + qrk; + var ff:Number = 1 - qi2 - qk2; + var ii:Number = qki - qrj; + var jj:Number = qjk + qri; + var kk:Number = 1 - qi2 - qj2; + + if (-1 < ii && ii < 1) { + if (angles == null) { + angles = new Vector3(Math.atan2(jj, kk), -Math.asin(ii), Math.atan2(ee, aa)); + } else { + angles.x = Math.atan2(jj, kk); + angles.y = -Math.asin(ii); + angles.z = Math.atan2(ee, aa); + } + } else { + if (angles == null) { + angles = new Vector3(0, 0.5*((ii <= -1) ? Math.PI : -Math.PI), Math.atan2(-bb, ff)); + } else { + angles.x = 0; + angles.y = (ii <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-bb, ff); + } + } + return angles; + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setFromEulerAnglesXYZ(x:Number, y:Number, z:Number):void { + setFromAxisAngleComponents(1, 0, 0, x); + + _q.setFromAxisAngleComponents(0, 1, 0, y); + append(_q); + normalize(); + + _q.setFromAxisAngleComponents(0, 0, 1, z); + append(_q); + normalize(); + } + + /** + * + */ + public function conjugate():void { + x = -x; + y = -y; + z = -z; + } + + /** + * Выполняет линейную интерполяцию. + * + * @param q1 начало отрезка + * @param q2 конец отрезка + * @param t время, обычно задаётся в интервале [0, 1] + */ + public function nlerp(q1:Quaternion, q2:Quaternion, t:Number):Quaternion { + var d:Number = 1 - t; + w = q1.w*d + q2.w*t; + x = q1.x*d + q2.x*t; + y = q1.y*d + q2.y*t; + z = q1.z*d + q2.z*t; + // inlined normalize + d = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param q + * @return this + */ + public function subtract(q:Quaternion):Quaternion { + w -= q.w; + x -= q.x; + y -= q.y; + z -= q.z; + return this; + } + + /** + * + * @param q1 + * @param q2 + * @return this + */ + public function diff(q1:Quaternion, q2:Quaternion):Quaternion { + w = q2.w - q1.w; + x = q2.x - q1.x; + y = q2.y - q1.y; + z = q2.z - q1.z; + return this; + } + + /** + * + * @param q + * @return this + */ + public function copy(q:Quaternion):Quaternion { + w = q.w; + x = q.x; + y = q.y; + z = q.z; + return this; + } + + /** + * @param result + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + result.w = w; + return result; + } + + /** + * + * @return клон + */ + public function clone():Quaternion { + return new Quaternion(w, x, y, z); + } + + /** + * Формирует строковое представление объекта. + * + * @return строковое представление объекта + */ + public function toString():String { + return "[" + w + ", " + x + ", " + y + ", " + z + "]"; + } + + /** + * Выполняет сферическую интерполяцию между двумя заданными кватернионами по наименьшему расстоянию. + * + * @param a первый кватерион + * @param b второй кватернион + * @param t параметр интерполяции, обычно принадлежит отрезку [0, 1] + * @return this + */ + public function slerp(a:Quaternion, b:Quaternion, t:Number):Quaternion { + var flip:Number = 1; + // Так как одна и та же ориентация представляется двумя значениями q и -q, нужно сменить знак одного из кватернионов + // если скалярное произведение отрицательно. Иначе будет получено интерполированное значение по наибольшему расстоянию. + var cosine:Number = a.w*b.w + a.x*b.x + a.y*b.y + a.z*b.z; + if (cosine < 0) { + cosine = -cosine; + flip = -1; + } + + if ((1 - cosine) < 0.001) { + // Вблизи нуля используется линейная интерполяция + var k1:Number = 1 - t; + var k2:Number = t*flip; + w = a.w*k1 + b.w*k2; + x = a.x*k1 + b.x*k2; + y = a.y*k1 + b.y*k2; + z = a.z*k1 + b.z*k2; + normalize(); + } else { + var theta:Number = Math.acos(cosine); + var sine:Number = Math.sin(theta); + var beta:Number = Math.sin((1 - t)*theta)/sine; + var alpha:Number = Math.sin(t*theta)/sine*flip; + w = a.w*beta + b.w*alpha; + x = a.x*beta + b.x*alpha; + y = a.y*beta + b.y*alpha; + z = a.z*beta + b.z*alpha; + } + + return this; + } + + } +} diff --git a/2.0.0.0/src/alternativa/math/.svn/text-base/Vector3.as.svn-base b/2.0.0.0/src/alternativa/math/.svn/text-base/Vector3.as.svn-base new file mode 100644 index 0000000..127be33 --- /dev/null +++ b/2.0.0.0/src/alternativa/math/.svn/text-base/Vector3.as.svn-base @@ -0,0 +1,391 @@ +package alternativa.math { + import flash.geom.Vector3D; + + /** + * + */ + public class Vector3 { + + public static const ZERO:Vector3 = new Vector3(0, 0, 0); + public static const X_AXIS:Vector3 = new Vector3(1, 0, 0); + public static const Y_AXIS:Vector3 = new Vector3(0, 1, 0); + public static const Z_AXIS:Vector3 = new Vector3(0, 0, 1); + + public static const RIGHT:Vector3 = new Vector3(1, 0, 0); + public static const LEFT:Vector3 = new Vector3(-1, 0, 0); + + public static const FORWARD:Vector3 = new Vector3(0, 1, 0); + public static const BACK:Vector3 = new Vector3(0, -1, 0); + + public static const UP:Vector3 = new Vector3(0, 0, 1); + public static const DOWN:Vector3 = new Vector3(0, 0, -1); + + public var x:Number; + public var y:Number; + public var z:Number; + + /** + * Создаёт новый экземпляр. + * + * @param x + * @param y + * @param z + */ + public function Vector3(x:Number = 0, y:Number = 0, z:Number = 0) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * Вычисляет длину вектора. + * + * @return длина вектора + */ + public function length():Number { + return Math.sqrt(x*x + y*y + z*z); + } + + /** + * Вычисляет квадрат длины вектора. + * + * @return квадрат длины вектора + */ + public function lengthSqr():Number { + return x*x + y*y + z*z; + } + + /** + * Устанавливает длину вектора. + * + * @param length новая длина + * @return this + */ + public function setLength(length:Number):Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = length; + } else { + var k:Number = length/Math.sqrt(x*x + y*y + z*z); + x *= k; + y *= k; + z *= k; + } + return this; + } + + /** + * Нормализует вектор. + * + * @return this + */ + public function normalize():Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = 1; + } else { + d = Math.sqrt(d); + x /= d; + y /= d; + z /= d; + } + return this; + } + + /** + * Прибавляет вектор. + * + * @param v прибавляемый вектор + * @return this + */ + public function add(v:Vector3):Vector3 { + x += v.x; + y += v.y; + z += v.z; + return this; + } + + /** + * Прибавляет вектор с домножением на скаляр. + * + * @param k множитель + * @param v вектор + * @return this + */ + public function addScaled(k:Number, v:Vector3):Vector3 { + x += k*v.x; + y += k*v.y; + z += k*v.z; + return this; + } + + /** + * Вычитает вектор. + * + * @param v вычитаемый вектор + * @return this + */ + public function subtract(v:Vector3):Vector3 { + x -= v.x; + y -= v.y; + z -= v.z; + return this; + } + + /** + * Вычисляет сумму векторов и сохраняет её в текущем векторе. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + * @return this + */ + public function sum(a:Vector3, b:Vector3):Vector3 { + x = a.x + b.x; + y = a.y + b.y; + z = a.z + b.z; + return this; + } + + /** + * Вычисляет разность векторов и сохраняет её в текущем векторе. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + * @return this + */ + public function diff(a:Vector3, b:Vector3):Vector3 { + x = a.x - b.x; + y = a.y - b.y; + z = a.z - b.z; + return this; + } + + /** + * Умножает вектор на скаляр. + * + * @param k множитель + * @return this + */ + public function scale(k:Number):Vector3 { + x *= k; + y *= k; + z *= k; + return this; + } + + /** + * Инвертирует вектор. + * + * @return this + */ + public function reverse():Vector3 { + x = -x; + y = -y; + z = -z; + return this; + } + + /** + * Вычисляет скалярное произведение с вектором. + * + * @param v вектор + * @return скалярное произведение с вектором + */ + public function dot(v:Vector3):Number { + return x*v.x + y*v.y + z*v.z; + } + + /** + * Вычисляет векторное произведение с вектором и записывает результат в текущий. + * + * @param v вектор, на который умножается текущий + * @return this + */ + public function cross(v:Vector3):Vector3 { + var xx:Number = y*v.z - z*v.y; + var yy:Number = z*v.x - x*v.z; + var zz:Number = x*v.y - y*v.x; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Вычисляет векторное произведение и записывает результат в текущий вектор. + * + * @param a первый вектор произведения + * @param b второй вектор произведения + * @return this + */ + public function cross2(a:Vector3, b:Vector3):Vector3 { + x = a.y*b.z - a.z*b.y; + y = a.z*b.x - a.x*b.z; + z = a.x*b.y - a.y*b.x; + return this; + } + + /** + * Трансформирует вектор заданной матрицей. + * + * @param m матрица трансформации + * @return this + */ + public function transformBy3(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * Трансформирует вектор транспонированной заданной матрицей. + * + * @param m матрица трансформации + * @return this + */ + public function transformBy3Tr(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * Трансформирует вектор заданной матрицей. + * + * @param m матрица трансформации + * @return this + */ + public function transformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz + m.d; + y = m.e*xx + m.f*yy + m.g*zz + m.h; + z = m.i*xx + m.j*yy + m.k*zz + m.l; + return this; + } + + /** + * Выполняет обратную трансформацию заданной матрицей. Матрица должна быть ортогональной. + * + * @param m матрица прямой трансформации + * @return this + */ + public function transformInverseBy4(m:Matrix4):Vector3 { + var xx:Number = x - m.d; + var yy:Number = y - m.h; + var zz:Number = z - m.l; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * Выполняет трансформацию вектора матрицей без учёта смещения. + * + * @param m матрица трансформации + * @return this + */ + public function deltaTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * Установливает компоненты вектора. + * + * @return this + */ + public function reset(x:Number = 0, y:Number = 0, z:Number = 0):Vector3 { + this.x = x; + this.y = y; + this.z = z; + return this; + } + + /** + * Копирует компоненты вектора. + * + * @param v копируемый вектор + * @return this + */ + public function copy(v:Vector3):Vector3 { + x = v.x; + y = v.y; + z = v.z; + return this; + } + + /** + * Клонирует вектор. + * + * @return клонированный вектор + */ + public function clone():Vector3 { + return new Vector3(x, y, z); + } + + /** + * Копирует компоненты вектора в экземпляр класса Vector3D. + * + * @param result экземпляр, в который копируются компоненты + * @return переданный параметр + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + return result; + } + + /** + * Копирует компоненты вектора. + * + * @param source копируемый вектор + * @return this + */ + public function copyFromVector3D(source:Vector3D):Vector3 { + x = source.x; + y = source.y; + z = source.z; + return this; + } + + /** + * Вычисляет расстояние до указанной точки. + * + * @param v точка + * @return расстояние до точки + */ + public function distanceTo(v:Vector3):Number { + var dx:Number = x - v.x; + var dy:Number = y - v.y; + var dz:Number = z - v.z; + return Math.sqrt(dx*dx + dy*dy + dz*dz); + } + + /** + * Формирует строковое представление вектора. + * + * @return строковое представление вектора + */ + public function toString():String { + return "Vector3(" + x + ", " + y + ", " + z + ")"; + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/math/Matrix3.as b/2.0.0.0/src/alternativa/math/Matrix3.as new file mode 100644 index 0000000..cab0fe2 --- /dev/null +++ b/2.0.0.0/src/alternativa/math/Matrix3.as @@ -0,0 +1,393 @@ +package alternativa.math { + import flash.geom.Vector3D; + + /** + * Матрица 3x3, используемая для описания ориентации. + */ + public class Matrix3 { + + public static const ZERO:Matrix3 = new Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0); + public static const IDENTITY:Matrix3 = new Matrix3(); + + public var a:Number; + public var b:Number; + public var c:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix3(a:Number = 1, b:Number = 0, c:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1) { + this.a = a; + this.b = b; + this.c = c; + + this.e = e; + this.f = f; + this.g = g; + + this.i = i; + this.j = j; + this.k = k; + } + + /** + * + * @return this + */ + public function toIdentity():Matrix3 { + a = f = k = 1; + b = c = e = g = i = j = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + var det:Number = 1/(-cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk); + + a = (ff*kk - gg*jj)*det; + b = (cc*jj - bb*kk)*det; + c = (bb*gg - cc*ff)*det; + e = (gg*ii - ee*kk)*det; + f = (aa*kk - cc*ii)*det; + g = (cc*ee - aa*gg)*det; + i = (ee*jj - ff*ii)*det; + j = (bb*ii - aa*jj)*det; + k = (aa*ff - bb*ee)*det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param m правый операнд умножения + */ + public function prepend(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param m правый операнд умножения + */ + public function prependTransposed(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.b + cc*m.c; + b = aa*m.e + bb*m.f + cc*m.g; + c = aa*m.i + bb*m.j + cc*m.k; + e = ee*m.a + ff*m.b + gg*m.c; + f = ee*m.e + ff*m.f + gg*m.g; + g = ee*m.i + ff*m.j + gg*m.k; + i = ii*m.a + jj*m.b + kk*m.c; + j = ii*m.e + jj*m.f + kk*m.g; + k = ii*m.i + jj*m.j + kk*m.k; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix3):Matrix3 { + a += m.a; + b += m.b; + c += m.c; + e += m.e; + f += m.f; + g += m.g; + i += m.i; + j += m.j; + k += m.k; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix3):Matrix3 { + a -= m.a; + b -= m.b; + c -= m.c; + e -= m.e; + f -= m.f; + g -= m.g; + i -= m.i; + j -= m.j; + k -= m.k; + + return this; + } + + /** + * + * @return this + */ + public function transpose():Matrix3 { + var tmp:Number = b; + b = e; + e = tmp; + tmp = c; + c = i; + i = tmp; + tmp = g; + g = j; + j = tmp; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector3To3D(vin:Vector3, vout:Vector3D):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * + * @param v + */ + public function createSkewSymmetric(v:Vector3):Matrix3 { + a = f = k = 0; + b = -v.z; + c = v.y; + e = v.z; + g = -v.x; + i = -v.y; + j = v.x; + return this; + } + + /** + * Копирование значений указанной матрицы. + * + * @param m матрица, значения которой копируются + */ + public function copy(m:Matrix3):Matrix3 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * Устанавливает вращательную часть матрицы на основе указанных эйлеровых углов. + * + * @param rx угол поворота по оси X в радианах + * @param ry угол поворота по оси Y в радианах + * @param rz угол поворота по оси Z в радианах + */ + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):void { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + } + + /** + * Метод формирует матрицу поворота на заданный угол относительно заданной оси. + * + * @param axis нормализованный вектор, задающий ось, относительно которой выполняется поворот + * @param angle угол поворота в радианах + */ + public function fromAxisAngle(axis:Vector3, angle:Number):void { + var c1:Number = Math.cos(angle); + var s:Number = Math.sin(angle); + var t:Number = 1 - c1; + var x:Number = axis.x; + var y:Number = axis.y; + var z:Number = axis.z; + + a = t*x*x + c1; + b = t*x*y - z*s; + c = t*x*z + y*s; + + e = t*x*y + z*s; + f = t*y*y + c1; + g = t*y*z - x*s; + + i = t*x*z - y*s; + j = t*y*z + x*s; + k = t*z*z + c1; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix3 { + return new Matrix3(a, b, c, e, f, g, i, j, k); + } + + /** + * Формирует строковое представление объекта. + * + * @return строковое представление объекта + */ + public function toString():String { + return "[Matrix3 (" + a + ", " + b + ", " + c + "), (" + e + ", " + f + ", " + g + "), (" + i + ", " + j + ", " + k + ")]"; + } + + /** + * @param angles + */ + public function getEulerAngles(angles:Vector3):void { + if (-1 < i && i < 1) { + angles.x = Math.atan2(j, k); + angles.y = -Math.asin(i); + angles.z = Math.atan2(e, a); + } else { + angles.x = 0; + angles.y = (i <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-b, f); + } + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/math/Matrix4.as b/2.0.0.0/src/alternativa/math/Matrix4.as new file mode 100644 index 0000000..ee38ee4 --- /dev/null +++ b/2.0.0.0/src/alternativa/math/Matrix4.as @@ -0,0 +1,572 @@ +package alternativa.math { + + /** + * + */ + public class Matrix4 { + + public static const IDENTITY:Matrix4 = new Matrix4(); + + public var a:Number; + public var b:Number; + public var c:Number; + public var d:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + public var h:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + public var l:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix4(a:Number = 1, b:Number = 0, c:Number = 0, d:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, h:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1, l:Number = 0) { + this.a = a; + this.b = b; + this.c = c; + this.d = d; + + this.e = e; + this.f = f; + this.g = g; + this.h = h; + + this.i = i; + this.j = j; + this.k = k; + this.l = l; + } + + /** + * + * @return this + */ + public function toIdentity():Matrix4 { + a = f = k = 1; + b = c = e = g = i = j = d = h = l = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + var det:Number = -cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk; + + a = (-gg*jj + ff*kk)/det; + b = (cc*jj - bb*kk)/det; + c = (-cc*ff + bb*gg)/det; + d = (dd*gg*jj - cc*hh*jj - dd*ff*kk + bb*hh*kk + cc*ff*ll - bb*gg*ll)/det; + e = (gg*ii - ee*kk)/det; + f = (-cc*ii + aa*kk)/det; + g = (cc*ee - aa*gg)/det; + h = (cc*hh*ii - dd*gg*ii + dd*ee*kk - aa*hh*kk - cc*ee*ll + aa*gg*ll)/det; + i = (-ff*ii + ee*jj)/det; + j = (bb*ii - aa*jj)/det; + k = (-bb*ee + aa*ff)/det; + l = (dd*ff*ii - bb*hh*ii - dd*ee*jj + aa*hh*jj + bb*ee*ll - aa*ff*ll)/det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + d = m.a*dd + m.b*hh + m.c*ll + m.d; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + h = m.e*dd + m.f*hh + m.g*ll + m.h; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + l = m.i*dd + m.j*hh + m.k*ll + m.l; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param m правый операнд умножения + */ + public function prepend(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + d = aa*m.d + bb*m.h + cc*m.l + dd; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + h = ee*m.d + ff*m.h + gg*m.l + hh; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + l = ii*m.d + jj*m.h + kk*m.l + ll; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix4):Matrix4 { + a += m.a; + b += m.b; + c += m.c; + d += m.d; + e += m.e; + f += m.f; + g += m.g; + h += m.h; + i += m.i; + j += m.j; + k += m.k; + l += m.l; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix4):Matrix4 { + a -= m.a; + b -= m.b; + c -= m.c; + d -= m.d; + e -= m.e; + f -= m.f; + g -= m.g; + h -= m.h; + i -= m.i; + j -= m.j; + k -= m.k; + l -= m.l; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorInverse(vin:Vector3, vout:Vector3):void { + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + + /** + * + * @param arrin + * @param arrout + */ + public function transformVectors(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param arrout + * @param len + */ + public function transformVectorsN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param arrout + */ + public function transformVectorsInverse(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param arrin + * @param arrout + */ + public function transformVectorsInverseN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param idx + * @param axis + */ + public function getAxis(idx:int, axis:Vector3):void { + switch (idx) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + case 3: + axis.x = d; + axis.y = h; + axis.z = l; + return; + } + } + + /** + * + * @param xAxis + * @param yAxis + * @param zAxis + * @param pos + */ + public function setAxes(xAxis:Vector3, yAxis:Vector3, zAxis:Vector3, pos:Vector3):void { + a = xAxis.x; + e = xAxis.y; + i = xAxis.z; + + b = yAxis.x; + f = yAxis.y; + j = yAxis.z; + + c = zAxis.x; + g = zAxis.y; + k = zAxis.z; + + d = pos.x; + h = pos.y; + l = pos.z; + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function deltaTransformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function deltaTransformVectorInverse(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Копирование значений указанной матрицы. + * + * @param m матрица, значения которой копируются + */ + public function copy(m:Matrix4):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = m.d; + e = m.e; + f = m.f; + g = m.g; + h = m.h; + i = m.i; + j = m.j; + k = m.k; + l = m.l; + + return this; + } + + /** + * + * @param m + * @param offset + * @return this + */ + public function setFromMatrix3(m:Matrix3, offset:Vector3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = offset.x; + e = m.e; + f = m.f; + g = m.g; + h = offset.y; + i = m.i; + j = m.j; + k = m.k; + l = offset.z; + + return this; + } + + /** + * + * @param m + * @return this + */ + public function setOrientationFromMatrix3(m:Matrix3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * + * @param angles + */ + public function getEulerAngles(angles:Vector3):void { + if (-1 < i && i < 1) { + angles.x = Math.atan2(j, k); + angles.y = -Math.asin(i); + angles.z = Math.atan2(e, a); + } else { + angles.x = 0; + angles.y = (i <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-b, f); + } + } + + /** + * Устанавливает координаты матрицы. + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + d = pos.x; + h = pos.y; + l = pos.z; + } + + /** + * Устанавливает координаты матрицы. + * + * @param x + * @param y + * @param z + */ + public function setPositionXYZ(x:Number, y:Number, z:Number):void { + d = x; + h = y; + l = z; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix4 { + return new Matrix4(a, b, c, d, e, f, g, h, i, j, k, l); + } + + /** + * Формирует строковое представление объекта. + * + * @return строковое представление объекта + */ + public function toString():String { + return "[Matrix4 [" + a.toFixed(3) + " " + b.toFixed(3) + " " + c.toFixed(3) + " " + d.toFixed(3) + "] [" + e.toFixed(3) + " " + f.toFixed(3) + " " + g.toFixed(3) + " " + h.toFixed(3) + "] [" + i.toFixed(3) + " " + j.toFixed(3) + " " + k.toFixed(3) + " " + l.toFixed(3) + "]]"; + } + + /** + * + */ + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):void { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + } + + /** + * Метод формирует матрицу поворота на заданный угол относительно заданной оси. + * + * @param axis нормализованный вектор, задающий ось, относительно которой выполняется поворот + * @param angle угол поворота в радианах + */ + public function fromAxisAngle(axis:Vector3, angle:Number):void { + var c1:Number = Math.cos(angle); + var s:Number = Math.sin(angle); + var t:Number = 1 - c1; + var x:Number = axis.x; + var y:Number = axis.y; + var z:Number = axis.z; + + a = t*x*x + c1; + b = t*x*y - z*s; + c = t*x*z + y*s; + + e = t*x*y + z*s; + f = t*y*y + c1; + g = t*y*z - x*s; + + i = t*x*z - y*s; + j = t*y*z + x*s; + k = t*z*z + c1; + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/math/Quaternion.as b/2.0.0.0/src/alternativa/math/Quaternion.as new file mode 100644 index 0000000..2c19cbb --- /dev/null +++ b/2.0.0.0/src/alternativa/math/Quaternion.as @@ -0,0 +1,515 @@ +package alternativa.math { + import flash.geom.Vector3D; + + /** + * Кватернион. + */ + public class Quaternion { + + public static const IDENTITY:Quaternion = new Quaternion(); + + public var w:Number; + public var x:Number; + public var y:Number; + public var z:Number; + + public static function multiply(q1:Quaternion, q2:Quaternion, result:Quaternion):void { + result.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z; + result.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y; + result.y = q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z; + result.z = q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x; + } + + public static function createFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngle(axis, angle); + return q; + } + + public static function createFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngleComponents(x, y, z, angle); + return q; + } + + /** + * + * @param x + * @param y + * @param z + */ + public static function createFromEulerAnglesXYZ(x:Number, y:Number, z:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngleComponents(1, 0, 0, x); + + _q.setFromAxisAngleComponents(0, 1, 0, y); + q.append(_q); + q.normalize(); + + _q.setFromAxisAngleComponents(0, 0, 1, z); + q.append(_q); + q.normalize(); + return q; + } + + + private static var _q:Quaternion = new Quaternion(); + + public function Quaternion(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0) { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + } + + public function reset(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0):Quaternion { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + return this; + } + + public function normalize():Quaternion { + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * Умножает на указанный кватернион слева: this * q + * + * @param q множитель + */ + public function prepend(q:Quaternion):Quaternion { + var ww:Number = w*q.w - x*q.x - y*q.y - z*q.z; + var xx:Number = w*q.x + x*q.w + y*q.z - z*q.y; + var yy:Number = w*q.y + y*q.w + z*q.x - x*q.z; + var zz:Number = w*q.z + z*q.w + x*q.y - y*q.x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Умножает на указанный кватернион справа: q * this + * + * @param q множитель + */ + public function append(q:Quaternion):Quaternion { + var ww:Number = q.w*w - q.x*x - q.y*y - q.z*z; + var xx:Number = q.w*x + q.x*w + q.y*z - q.z*y; + var yy:Number = q.w*y + q.y*w + q.z*x - q.x*z; + var zz:Number = q.w*z + q.z*w + q.x*y - q.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * + * @param v + */ + public function rotateByVector(v:Vector3):Quaternion { + var ww:Number = -v.x*x - v.y*y - v.z*z; + var xx:Number = v.x*w + v.y*z - v.z*y; + var yy:Number = v.y*w + v.z*x - v.x*z; + var zz:Number = v.z*w + v.x*y - v.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Добавляет вращение, приданное вектором угловой скорости за указанное время. + * + * @param v + * @param scale + */ + public function addScaledVector(v:Vector3, scale:Number):Quaternion { + var vx:Number = v.x*scale; + var vy:Number = v.y*scale; + var vz:Number = v.z*scale; + var ww:Number = -x*vx - y*vy - z*vz; + var xx:Number = vx*w + vy*z - vz*y; + var yy:Number = vy*w + vz*x - vx*z; + var zz:Number = vz*w + vx*y - vy*x; + w += 0.5*ww; + x += 0.5*xx; + y += 0.5*yy; + z += 0.5*zz; + // inlined normalize + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param m + * @return this + */ + public function toMatrix3(m:Matrix3):Quaternion { + var xx2:Number = 2.0*x*x; + var yy2:Number = 2.0*y*y; + var zz2:Number = 2.0*z*z; + var xy2:Number = 2.0*x*y; + var yz2:Number = 2.0*y*z; + var zx2:Number = 2.0*z*x; + var wx2:Number = 2.0*w*x; + var wy2:Number = 2.0*w*y; + var wz2:Number = 2.0*w*z; + + m.a = 1.0 - yy2 - zz2; + m.b = xy2 - wz2; + m.c = zx2 + wy2; + + m.e = xy2 + wz2; + m.f = 1.0 - xx2 - zz2; + m.g = yz2 - wx2; + + m.i = zx2 - wy2; + m.j = yz2 + wx2; + m.k = 1.0 - xx2 - yy2; + return this; + } + + /** + * + * @param m + * @return this + */ + public function toMatrix4(m:Matrix4):Quaternion { + var xx2:Number = 2.0*x*x; + var yy2:Number = 2.0*y*y; + var zz2:Number = 2.0*z*z; + var xy2:Number = 2.0*x*y; + var yz2:Number = 2.0*y*z; + var zx2:Number = 2.0*z*x; + var wx2:Number = 2.0*w*x; + var wy2:Number = 2.0*w*y; + var wz2:Number = 2.0*w*z; + + m.a = 1.0 - yy2 - zz2; + m.b = xy2 - wz2; + m.c = zx2 + wy2; + + m.e = xy2 + wz2; + m.f = 1.0 - xx2 - zz2; + m.g = yz2 - wx2; + + m.i = zx2 - wy2; + m.j = yz2 + wx2; + m.k = 1.0 - xx2 - yy2; + return this; + } + + /** + * + * @return длина + */ + public function length():Number { + return Math.sqrt(w*w + x*x + y*y + z*z); + } + + /** + * + * @return квадрат длины + */ + public function lengthSqr():Number { + return w*w + x*x + y*y + z*z; + } + + /** + * + * @param axis + * @param angle + * @return this + */ + public function setFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z); + x = axis.x*k; + y = axis.y*k; + z = axis.z*k; + return this; + } + + /** + * + * @param x + * @param y + * @param z + * @param angle + */ + public function setFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(x*x + y*y + z*z); + this.x = x*k; + this.y = y*k; + this.z = z*k; + return this; + } + + /** + * + * @param v + */ + public function toAxisVector(v:Vector3 = null):Vector3 { + if (w < -1 || w > 1) { + normalize(); + } + if (v == null) { + v = new Vector3(); + } + if (w > -1 && w < 1) { + if (w == 0) { + v.x = x; + v.y = y; + v.z = z; + } else { + var angle:Number = 2*Math.acos(w); + var coeff:Number = 1/Math.sqrt(1 - w*w); + v.x = x*coeff*angle; + v.y = y*coeff*angle; + v.z = z*coeff*angle; + } + } else { + v.x = 0; + v.y = 0; + v.z = 0; + } + return v; + } + + /** + * + * @param angles + */ + public function getEulerAngles(angles:Vector3):Vector3 { + var qi2:Number = 2*x*x; + var qj2:Number = 2*y*y; + var qk2:Number = 2*z*z; + var qij:Number = 2*x*y; + var qjk:Number = 2*y*z; + var qki:Number = 2*z*x; + var qri:Number = 2*w*x; + var qrj:Number = 2*w*y; + var qrk:Number = 2*w*z; + + var aa:Number = 1 - qj2 - qk2; + var bb:Number = qij - qrk; + var ee:Number = qij + qrk; + var ff:Number = 1 - qi2 - qk2; + var ii:Number = qki - qrj; + var jj:Number = qjk + qri; + var kk:Number = 1 - qi2 - qj2; + + if (-1 < ii && ii < 1) { + if (angles == null) { + angles = new Vector3(Math.atan2(jj, kk), -Math.asin(ii), Math.atan2(ee, aa)); + } else { + angles.x = Math.atan2(jj, kk); + angles.y = -Math.asin(ii); + angles.z = Math.atan2(ee, aa); + } + } else { + if (angles == null) { + angles = new Vector3(0, 0.5*((ii <= -1) ? Math.PI : -Math.PI), Math.atan2(-bb, ff)); + } else { + angles.x = 0; + angles.y = (ii <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-bb, ff); + } + } + return angles; + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setFromEulerAnglesXYZ(x:Number, y:Number, z:Number):void { + setFromAxisAngleComponents(1, 0, 0, x); + + _q.setFromAxisAngleComponents(0, 1, 0, y); + append(_q); + normalize(); + + _q.setFromAxisAngleComponents(0, 0, 1, z); + append(_q); + normalize(); + } + + /** + * + */ + public function conjugate():void { + x = -x; + y = -y; + z = -z; + } + + /** + * Выполняет линейную интерполяцию. + * + * @param q1 начало отрезка + * @param q2 конец отрезка + * @param t время, обычно задаётся в интервале [0, 1] + */ + public function nlerp(q1:Quaternion, q2:Quaternion, t:Number):Quaternion { + var d:Number = 1 - t; + w = q1.w*d + q2.w*t; + x = q1.x*d + q2.x*t; + y = q1.y*d + q2.y*t; + z = q1.z*d + q2.z*t; + // inlined normalize + d = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param q + * @return this + */ + public function subtract(q:Quaternion):Quaternion { + w -= q.w; + x -= q.x; + y -= q.y; + z -= q.z; + return this; + } + + /** + * + * @param q1 + * @param q2 + * @return this + */ + public function diff(q1:Quaternion, q2:Quaternion):Quaternion { + w = q2.w - q1.w; + x = q2.x - q1.x; + y = q2.y - q1.y; + z = q2.z - q1.z; + return this; + } + + /** + * + * @param q + * @return this + */ + public function copy(q:Quaternion):Quaternion { + w = q.w; + x = q.x; + y = q.y; + z = q.z; + return this; + } + + /** + * @param result + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + result.w = w; + return result; + } + + /** + * + * @return клон + */ + public function clone():Quaternion { + return new Quaternion(w, x, y, z); + } + + /** + * Формирует строковое представление объекта. + * + * @return строковое представление объекта + */ + public function toString():String { + return "[" + w + ", " + x + ", " + y + ", " + z + "]"; + } + + /** + * Выполняет сферическую интерполяцию между двумя заданными кватернионами по наименьшему расстоянию. + * + * @param a первый кватерион + * @param b второй кватернион + * @param t параметр интерполяции, обычно принадлежит отрезку [0, 1] + * @return this + */ + public function slerp(a:Quaternion, b:Quaternion, t:Number):Quaternion { + var flip:Number = 1; + // Так как одна и та же ориентация представляется двумя значениями q и -q, нужно сменить знак одного из кватернионов + // если скалярное произведение отрицательно. Иначе будет получено интерполированное значение по наибольшему расстоянию. + var cosine:Number = a.w*b.w + a.x*b.x + a.y*b.y + a.z*b.z; + if (cosine < 0) { + cosine = -cosine; + flip = -1; + } + + if ((1 - cosine) < 0.001) { + // Вблизи нуля используется линейная интерполяция + var k1:Number = 1 - t; + var k2:Number = t*flip; + w = a.w*k1 + b.w*k2; + x = a.x*k1 + b.x*k2; + y = a.y*k1 + b.y*k2; + z = a.z*k1 + b.z*k2; + normalize(); + } else { + var theta:Number = Math.acos(cosine); + var sine:Number = Math.sin(theta); + var beta:Number = Math.sin((1 - t)*theta)/sine; + var alpha:Number = Math.sin(t*theta)/sine*flip; + w = a.w*beta + b.w*alpha; + x = a.x*beta + b.x*alpha; + y = a.y*beta + b.y*alpha; + z = a.z*beta + b.z*alpha; + } + + return this; + } + + } +} diff --git a/2.0.0.0/src/alternativa/math/Vector3.as b/2.0.0.0/src/alternativa/math/Vector3.as new file mode 100644 index 0000000..127be33 --- /dev/null +++ b/2.0.0.0/src/alternativa/math/Vector3.as @@ -0,0 +1,391 @@ +package alternativa.math { + import flash.geom.Vector3D; + + /** + * + */ + public class Vector3 { + + public static const ZERO:Vector3 = new Vector3(0, 0, 0); + public static const X_AXIS:Vector3 = new Vector3(1, 0, 0); + public static const Y_AXIS:Vector3 = new Vector3(0, 1, 0); + public static const Z_AXIS:Vector3 = new Vector3(0, 0, 1); + + public static const RIGHT:Vector3 = new Vector3(1, 0, 0); + public static const LEFT:Vector3 = new Vector3(-1, 0, 0); + + public static const FORWARD:Vector3 = new Vector3(0, 1, 0); + public static const BACK:Vector3 = new Vector3(0, -1, 0); + + public static const UP:Vector3 = new Vector3(0, 0, 1); + public static const DOWN:Vector3 = new Vector3(0, 0, -1); + + public var x:Number; + public var y:Number; + public var z:Number; + + /** + * Создаёт новый экземпляр. + * + * @param x + * @param y + * @param z + */ + public function Vector3(x:Number = 0, y:Number = 0, z:Number = 0) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * Вычисляет длину вектора. + * + * @return длина вектора + */ + public function length():Number { + return Math.sqrt(x*x + y*y + z*z); + } + + /** + * Вычисляет квадрат длины вектора. + * + * @return квадрат длины вектора + */ + public function lengthSqr():Number { + return x*x + y*y + z*z; + } + + /** + * Устанавливает длину вектора. + * + * @param length новая длина + * @return this + */ + public function setLength(length:Number):Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = length; + } else { + var k:Number = length/Math.sqrt(x*x + y*y + z*z); + x *= k; + y *= k; + z *= k; + } + return this; + } + + /** + * Нормализует вектор. + * + * @return this + */ + public function normalize():Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = 1; + } else { + d = Math.sqrt(d); + x /= d; + y /= d; + z /= d; + } + return this; + } + + /** + * Прибавляет вектор. + * + * @param v прибавляемый вектор + * @return this + */ + public function add(v:Vector3):Vector3 { + x += v.x; + y += v.y; + z += v.z; + return this; + } + + /** + * Прибавляет вектор с домножением на скаляр. + * + * @param k множитель + * @param v вектор + * @return this + */ + public function addScaled(k:Number, v:Vector3):Vector3 { + x += k*v.x; + y += k*v.y; + z += k*v.z; + return this; + } + + /** + * Вычитает вектор. + * + * @param v вычитаемый вектор + * @return this + */ + public function subtract(v:Vector3):Vector3 { + x -= v.x; + y -= v.y; + z -= v.z; + return this; + } + + /** + * Вычисляет сумму векторов и сохраняет её в текущем векторе. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + * @return this + */ + public function sum(a:Vector3, b:Vector3):Vector3 { + x = a.x + b.x; + y = a.y + b.y; + z = a.z + b.z; + return this; + } + + /** + * Вычисляет разность векторов и сохраняет её в текущем векторе. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + * @return this + */ + public function diff(a:Vector3, b:Vector3):Vector3 { + x = a.x - b.x; + y = a.y - b.y; + z = a.z - b.z; + return this; + } + + /** + * Умножает вектор на скаляр. + * + * @param k множитель + * @return this + */ + public function scale(k:Number):Vector3 { + x *= k; + y *= k; + z *= k; + return this; + } + + /** + * Инвертирует вектор. + * + * @return this + */ + public function reverse():Vector3 { + x = -x; + y = -y; + z = -z; + return this; + } + + /** + * Вычисляет скалярное произведение с вектором. + * + * @param v вектор + * @return скалярное произведение с вектором + */ + public function dot(v:Vector3):Number { + return x*v.x + y*v.y + z*v.z; + } + + /** + * Вычисляет векторное произведение с вектором и записывает результат в текущий. + * + * @param v вектор, на который умножается текущий + * @return this + */ + public function cross(v:Vector3):Vector3 { + var xx:Number = y*v.z - z*v.y; + var yy:Number = z*v.x - x*v.z; + var zz:Number = x*v.y - y*v.x; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Вычисляет векторное произведение и записывает результат в текущий вектор. + * + * @param a первый вектор произведения + * @param b второй вектор произведения + * @return this + */ + public function cross2(a:Vector3, b:Vector3):Vector3 { + x = a.y*b.z - a.z*b.y; + y = a.z*b.x - a.x*b.z; + z = a.x*b.y - a.y*b.x; + return this; + } + + /** + * Трансформирует вектор заданной матрицей. + * + * @param m матрица трансформации + * @return this + */ + public function transformBy3(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * Трансформирует вектор транспонированной заданной матрицей. + * + * @param m матрица трансформации + * @return this + */ + public function transformBy3Tr(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * Трансформирует вектор заданной матрицей. + * + * @param m матрица трансформации + * @return this + */ + public function transformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz + m.d; + y = m.e*xx + m.f*yy + m.g*zz + m.h; + z = m.i*xx + m.j*yy + m.k*zz + m.l; + return this; + } + + /** + * Выполняет обратную трансформацию заданной матрицей. Матрица должна быть ортогональной. + * + * @param m матрица прямой трансформации + * @return this + */ + public function transformInverseBy4(m:Matrix4):Vector3 { + var xx:Number = x - m.d; + var yy:Number = y - m.h; + var zz:Number = z - m.l; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * Выполняет трансформацию вектора матрицей без учёта смещения. + * + * @param m матрица трансформации + * @return this + */ + public function deltaTransformBy4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * Установливает компоненты вектора. + * + * @return this + */ + public function reset(x:Number = 0, y:Number = 0, z:Number = 0):Vector3 { + this.x = x; + this.y = y; + this.z = z; + return this; + } + + /** + * Копирует компоненты вектора. + * + * @param v копируемый вектор + * @return this + */ + public function copy(v:Vector3):Vector3 { + x = v.x; + y = v.y; + z = v.z; + return this; + } + + /** + * Клонирует вектор. + * + * @return клонированный вектор + */ + public function clone():Vector3 { + return new Vector3(x, y, z); + } + + /** + * Копирует компоненты вектора в экземпляр класса Vector3D. + * + * @param result экземпляр, в который копируются компоненты + * @return переданный параметр + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + return result; + } + + /** + * Копирует компоненты вектора. + * + * @param source копируемый вектор + * @return this + */ + public function copyFromVector3D(source:Vector3D):Vector3 { + x = source.x; + y = source.y; + z = source.z; + return this; + } + + /** + * Вычисляет расстояние до указанной точки. + * + * @param v точка + * @return расстояние до точки + */ + public function distanceTo(v:Vector3):Number { + var dx:Number = x - v.x; + var dy:Number = y - v.y; + var dz:Number = z - v.z; + return Math.sqrt(dx*dx + dy*dy + dz*dz); + } + + /** + * Формирует строковое представление вектора. + * + * @return строковое представление вектора + */ + public function toString():String { + return "Vector3(" + x + ", " + y + ", " + z + ")"; + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/.svn/all-wcprops b/2.0.0.0/src/alternativa/physics/.svn/all-wcprops new file mode 100644 index 0000000..a156990 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/.svn/all-wcprops @@ -0,0 +1,71 @@ +K 25 +svn:wc:ra_dav:version-url +V 103 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics +END +Body.as +K 25 +svn:wc:ra_dav:version-url +V 111 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/Body.as +END +BodyState.as +K 25 +svn:wc:ra_dav:version-url +V 116 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/BodyState.as +END +CollisionPrimitiveListItem.as +K 25 +svn:wc:ra_dav:version-url +V 133 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/CollisionPrimitiveListItem.as +END +CollisionPrimitiveList.as +K 25 +svn:wc:ra_dav:version-url +V 129 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/CollisionPrimitiveList.as +END +PhysicsScene.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/PhysicsScene.as +END +PhysicsUtils.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/PhysicsUtils.as +END +ContactPoint.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/ContactPoint.as +END +BodyListItem.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/BodyListItem.as +END +BodyList.as +K 25 +svn:wc:ra_dav:version-url +V 115 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/BodyList.as +END +Contact.as +K 25 +svn:wc:ra_dav:version-url +V 114 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/Contact.as +END +BodyMaterial.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/BodyMaterial.as +END diff --git a/2.0.0.0/src/alternativa/physics/.svn/entries b/2.0.0.0/src/alternativa/physics/.svn/entries new file mode 100644 index 0000000..6ea576a --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/.svn/entries @@ -0,0 +1,166 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics +http://svndev.alternativaplatform.com + + + +2010-05-12T11:32:14.711790Z +34250 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Body.as +file + + + + +2010-10-28T04:32:40.000000Z +c325edce8bb1addbaf2167322948f2b3 +2010-05-12T11:32:14.711790Z +34250 +mike + +BodyState.as +file + + + + +2010-10-28T04:32:40.000000Z +b3105d13f93c5d6d8d2bf3ce02c7e8d9 +2010-03-12T16:45:48.613787Z +29719 +mike + +CollisionPrimitiveListItem.as +file + + + + +2010-10-28T04:32:40.000000Z +cec5dbfefef812d87481478455f367b1 +2009-10-19T07:17:33.113306Z +22253 +mike + +CollisionPrimitiveList.as +file + + + + +2010-10-28T04:32:40.000000Z +5752b1f4909c5146720b58fbfffed468 +2009-11-01T12:35:52.270571Z +22955 +mike + +PhysicsScene.as +file + + + + +2010-10-28T04:32:40.000000Z +ca333717d0b8f2bb5f347fe33bc7fd38 +2010-05-12T11:32:14.711790Z +34250 +mike + +PhysicsUtils.as +file + + + + +2010-10-28T04:32:40.000000Z +dce0a94f8a861291a2a8942ed0ba54a5 +2009-11-24T08:44:15.941480Z +23758 +mike + +constraints +dir + +ContactPoint.as +file + + + + +2010-10-28T04:32:40.000000Z +9eedaf1a541ae335b614fcc4170012f2 +2010-03-12T16:45:48.613787Z +29719 +mike + +BodyListItem.as +file + + + + +2010-10-28T04:32:40.000000Z +2fc2b598310b24aef3f8b90e14a8ba4f +2009-10-19T07:17:33.113306Z +22253 +mike + +collision +dir + +BodyList.as +file + + + + +2010-10-28T04:32:40.000000Z +08a90332660bd0273728006d1402dd25 +2009-11-01T12:35:52.270571Z +22955 +mike + +Contact.as +file + + + + +2010-10-28T04:32:40.000000Z +eb89f4f37bcf2d21eadc733f244fcf64 +2010-02-12T12:34:25.192477Z +28424 +mike + +BodyMaterial.as +file + + + + +2010-10-28T04:32:40.000000Z +9b700692ebb8017882f3e8cec095e1a9 +2009-10-19T07:17:33.113306Z +22253 +mike + diff --git a/2.0.0.0/src/alternativa/physics/.svn/format b/2.0.0.0/src/alternativa/physics/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/.svn/format @@ -0,0 +1 @@ +8 diff --git a/2.0.0.0/src/alternativa/physics/.svn/text-base/Body.as.svn-base b/2.0.0.0/src/alternativa/physics/.svn/text-base/Body.as.svn-base new file mode 100644 index 0000000..28c9b1a --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/.svn/text-base/Body.as.svn-base @@ -0,0 +1,461 @@ +package alternativa.physics { + + import alternativa.math.Matrix3; + import alternativa.math.Matrix4; + import alternativa.math.Quaternion; + import alternativa.math.Vector3; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.IBodyCollisionPredicate; + import alternativa.physics.collision.types.BoundBox; + + /** + * + */ + public class Body { + + public static var linDamping:Number = 0.997; + public static var rotDamping:Number = 0.997; + + // Идентификатор тела, уникальный в пределах мира + public var id:int; + // Имя тела + public var name:String; + // Мир, в котором находится тело + public var world:PhysicsScene; + // Флаг подвижности тела + public var movable:Boolean = true; + // Флаг указывает, может ли тело быть заморожено + public var canFreeze:Boolean = false; + + public var freezeCounter:int; + public var frozen:Boolean = false; + + public var useGravity:Boolean = true; + + // Ограничивающий бокс тела + public var aabb:BoundBox = new BoundBox(); + // Предикат, через который тело получает сообщения о столкновениях + public var postCollisionPredicate:IBodyCollisionPredicate; + + // Текущее состояние тела + public var state:BodyState = new BodyState(); + // Предыдущее состояние тела + public var prevState:BodyState = new BodyState(); + // Линейное ускорение тела на текущем шаге симуляции + public var accel:Vector3 = new Vector3(); + // Угловое ускорение тела на текущем шаге симуляции + public var angleAccel:Vector3 = new Vector3(); + // Физический материал тела + public var material:BodyMaterial = new BodyMaterial(); + // Обратная масса тела + public var invMass:Number = 1; + // Обратная матрица тензора инерции в локальных координатах + public var invInertia:Matrix3 = new Matrix3(); + // Обратная матрица тензора инерции в мировых координатах + public var invInertiaWorld:Matrix3 = new Matrix3(); + // Базисная матрица тела в мировых координатах + public var baseMatrix:Matrix3 = new Matrix3(); + + public const MAX_CONTACTS:int = 20; + public var contacts:Vector. = new Vector.(MAX_CONTACTS); + public var contactsNum:int; + + public var collisionPrimitives:CollisionPrimitiveList; + + // Аккумулятор сил + public var forceAccum:Vector3 = new Vector3(); + // Аккумулятор моментов + public var torqueAccum:Vector3 = new Vector3(); + + // Внутренние переменные для избежания создания экземпляров + private static var _r:Vector3 = new Vector3(); + private static var _f:Vector3 = new Vector3(); + private static var _q:Quaternion = new Quaternion(); + + /** + * + * @param invMass + * @param invInertia + */ + public function Body(invMass:Number, invInertia:Matrix3) { + this.invMass = invMass; + this.invInertia.copy(invInertia); + } + + /** + * @param primitive + * @param localTransform + */ + public function addCollisionPrimitive(primitive:CollisionPrimitive, localTransform:Matrix4 = null):void { + if (primitive == null) { + throw new ArgumentError("Primitive cannot be null"); + } + if (collisionPrimitives == null) { + collisionPrimitives = new CollisionPrimitiveList(); + } + collisionPrimitives.append(primitive); + primitive.setBody(this, localTransform); + } + + /** + * + * @param primitive + */ + public function removeCollisionPrimitive(primitive:CollisionPrimitive):void { + if (collisionPrimitives == null) return; + primitive.setBody(null); + collisionPrimitives.remove(primitive); + if (collisionPrimitives.size == 0) { + collisionPrimitives = null; + } + } + + /** + * + * @param t + * @param pos + * @param orientation + */ + public function interpolate(t:Number, pos:Vector3, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.w = prevState.orientation.w*t1 + state.orientation.w*t; + orientation.x = prevState.orientation.x*t1 + state.orientation.x*t; + orientation.y = prevState.orientation.y*t1 + state.orientation.y*t; + orientation.z = prevState.orientation.z*t1 + state.orientation.z*t; + } + + /** + * + * @param t + * @param pos + * @param orientation + */ + public function interpolateSLERP(t:Number, pos:Vector3, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.slerp(prevState.orientation, state.orientation, t); + } + + /** + * + * @param t + * @param m + */ + public function interpolateToMatrix(t:Number, m:Matrix4):void { + var t1:Number = 1 - t; + m.d = prevState.pos.x*t1 + state.pos.x*t; + m.h = prevState.pos.y*t1 + state.pos.y*t; + m.l = prevState.pos.z*t1 + state.pos.z*t; + _q.w = prevState.orientation.w*t1 + state.orientation.w*t; + _q.x = prevState.orientation.x*t1 + state.orientation.x*t; + _q.y = prevState.orientation.y*t1 + state.orientation.y*t; + _q.z = prevState.orientation.z*t1 + state.orientation.z*t; + _q.normalize(); + _q.toMatrix4(m); + } + + /** + * + * @param t + * @param m + */ + public function interpolateToMatrixSLERP(t:Number, m:Matrix4):void { + var t1:Number = 1 - t; + m.d = prevState.pos.x*t1 + state.pos.x*t; + m.h = prevState.pos.y*t1 + state.pos.y*t; + m.l = prevState.pos.z*t1 + state.pos.z*t; + _q.slerp(prevState.orientation, state.orientation, t); + _q.normalize(); + _q.toMatrix4(m); + } + + /** + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + state.pos.copy(pos); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setPositionXYZ(x:Number, y:Number, z:Number):void { + state.pos.reset(x, y, z); + } + + /** + * + * @param vel + */ + public function setVelocity(vel:Vector3):void { + state.velocity.copy(vel); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setVelocityXYZ(x:Number, y:Number, z:Number):void { + state.velocity.reset(x, y, z); + } + + /** + * + * @param rot + */ + public function setRotation(rot:Vector3):void { + state.rotation.copy(rot); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setRotationXYZ(x:Number, y:Number, z:Number):void { + state.rotation.reset(x, y, z); + } + + /** + * + * @param q + */ + public function setOrientation(q:Quaternion):void { + state.orientation.copy(q); + } + + /** + * @param r + * @param dir + * @param magnitude + */ + public function applyRelPosWorldImpulse(r:Vector3, dir:Vector3, magnitude:Number):void { + var d:Number = magnitude*invMass; + // Линейная часть + state.velocity.x += d*dir.x; + state.velocity.y += d*dir.y; + state.velocity.z += d*dir.z; + + // Вращательная часть + var x:Number = (r.y*dir.z - r.z*dir.y)*magnitude; + var y:Number = (r.z*dir.x - r.x*dir.z)*magnitude; + var z:Number = (r.x*dir.y - r.y*dir.x)*magnitude; + + state.rotation.x += invInertiaWorld.a*x + invInertiaWorld.b*y + invInertiaWorld.c*z; + state.rotation.y += invInertiaWorld.e*x + invInertiaWorld.f*y + invInertiaWorld.g*z; + state.rotation.z += invInertiaWorld.i*x + invInertiaWorld.j*y + invInertiaWorld.k*z; + } + + /** + * @param f + */ + public function addForce(f:Vector3):void { + forceAccum.add(f); + } + + /** + * + * @param fx + * @param fy + * @param fz + */ + public function addForceXYZ(fx:Number, fy:Number, fz:Number):void { + forceAccum.x += fx; + forceAccum.y += fy; + forceAccum.z += fz; + } + + /** + * + * @param px + * @param py + * @param pz + * @param fx + * @param fy + * @param fz + */ + public function addWorldForceXYZ(px:Number, py:Number, pz:Number, fx:Number, fy:Number, fz:Number):void { + forceAccum.x += fx; + forceAccum.y += fy; + forceAccum.z += fz; + + var pos:Vector3 = state.pos; + var rx:Number = px - pos.x; + var ry:Number = py - pos.y; + var rz:Number = pz - pos.z; + +// var x:Number = ry*fz - rz*fy; +// var y:Number = rz*fx - rx*fz; +// var z:Number = rx*fy - ry*fx; + + torqueAccum.x += ry*fz - rz*fy; + torqueAccum.y += rz*fx - rx*fz; + torqueAccum.z += rx*fy - ry*fx; + } + + /** + * @param pos + * @param f + */ + public function addWorldForce(pos:Vector3, force:Vector3):void { + forceAccum.add(force); + torqueAccum.add(_r.diff(pos, state.pos).cross(force)); + } + + /** + * @param pos + * @param f + */ + public function addWorldForceScaled(pos:Vector3, force:Vector3, scale:Number):void { + _f.x = scale*force.x; + _f.y = scale*force.y; + _f.z = scale*force.z; + forceAccum.add(_f); + torqueAccum.add(_r.diff(pos, state.pos).cross(_f)); + } + + /** + * @param pos + * @param f + */ + public function addLocalForce(pos:Vector3, force:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(pos, _r); + // Трансформируем вектор силы в мировую систему + baseMatrix.transformVector(force, _f); + // Добавляем силу и момент + forceAccum.add(_f); + torqueAccum.add(_r.cross(_f)); + } + + /** + * + * @param localPos + * @param worldForce + */ + public function addWorldForceAtLocalPoint(localPos:Vector3, worldForce:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(localPos, _r); + // Добавляем силу и момент + forceAccum.add(worldForce); + torqueAccum.add(_r.cross(worldForce)); + } + + /** + * @param dt + */ + public function beforePhysicsStep(dt:Number):void { + } + + /** + * @param t + */ + public function addTorque(t:Vector3):void { + torqueAccum.add(t); + } + + /** + * + */ + internal function clearAccumulators():void { + forceAccum.x = forceAccum.y = forceAccum.z = 0; + torqueAccum.x = torqueAccum.y = torqueAccum.z = 0; + } + + /** + * + */ + internal function calcAccelerations():void { + accel.x = forceAccum.x*invMass; + accel.y = forceAccum.y*invMass; + accel.z = forceAccum.z*invMass; + angleAccel.x = invInertiaWorld.a*torqueAccum.x + invInertiaWorld.b*torqueAccum.y + invInertiaWorld.c*torqueAccum.z; + angleAccel.y = invInertiaWorld.e*torqueAccum.x + invInertiaWorld.f*torqueAccum.y + invInertiaWorld.g*torqueAccum.z; + angleAccel.z = invInertiaWorld.i*torqueAccum.x + invInertiaWorld.j*torqueAccum.y + invInertiaWorld.k*torqueAccum.z; + } + + /** + * Вычисляет производные данные. + */ + public function calcDerivedData():void { + // Вычисление базисной матрицы и обратного тензора инерции в мировых координатах + state.orientation.toMatrix3(baseMatrix); + invInertiaWorld.copy(invInertia).append(baseMatrix).prependTransposed(baseMatrix); + if (collisionPrimitives != null) { + aabb.infinity(); + var item:CollisionPrimitiveListItem = collisionPrimitives.head; + while (item != null) { + var primitive:CollisionPrimitive = item.primitive; + primitive.transform.setFromMatrix3(baseMatrix, state.pos); + if (primitive.localTransform != null) { + primitive.transform.prepend(primitive.localTransform); + } + primitive.calculateAABB(); + aabb.addBoundBox(primitive.aabb); + item = item.next; + } + } + } + + /** + * + */ + public function saveState():void { + prevState.copy(state); + } + + /** + * + */ + internal function restoreState():void { + state.copy(prevState); + } + + /** + * @param dt + */ + internal function integrateVelocity(dt:Number):void { + // v = v + a*t + state.velocity.x += accel.x*dt; + state.velocity.y += accel.y*dt; + state.velocity.z += accel.z*dt; + // rot = rot + eps*t + state.rotation.x += angleAccel.x*dt; + state.rotation.y += angleAccel.y*dt; + state.rotation.z += angleAccel.z*dt; + + state.velocity.x *= linDamping; + state.velocity.y *= linDamping; + state.velocity.z *= linDamping; + + state.rotation.x *= rotDamping; + state.rotation.y *= rotDamping; + state.rotation.z *= rotDamping; + } + + /** + * + */ + internal function integratePosition(dt:Number):void { + // pos = pos + v*t + state.pos.x += state.velocity.x*dt; + state.pos.y += state.velocity.y*dt; + state.pos.z += state.velocity.z*dt; + // q = q + 0.5*rot*q + state.orientation.addScaledVector(state.rotation, dt); + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/.svn/text-base/BodyList.as.svn-base b/2.0.0.0/src/alternativa/physics/.svn/text-base/BodyList.as.svn-base new file mode 100644 index 0000000..aa8f79f --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/.svn/text-base/BodyList.as.svn-base @@ -0,0 +1,76 @@ +package alternativa.physics { + + /** + * + */ + public class BodyList { + + public var head:BodyListItem; + public var tail:BodyListItem; + public var size:int; + + /** + * + */ + public function BodyList() { + } + + /** + * + * @param body + */ + public function append(body:Body):void { + var item:BodyListItem = BodyListItem.create(body); + if (head == null) { + head = tail = item; + } else { + tail.next = item; + item.prev = tail; + tail = item; + } + size++; + } + + /** + * + * @param body + */ + public function remove(body:Body):Boolean { + var item:BodyListItem = findItem(body); + if (item == null) return false; + if (item == head) { + if (size == 1) { + head = tail = null; + } else { + head = item.next; + head.prev = null; + } + } else { + if (item == tail) { + tail = item.prev; + tail.next = null; + } else { + item.prev.next = item.next; + item.next.prev = item.prev; + } + } + item.dispose(); + size--; + return true; + } + + /** + * + * @param body + * @return + */ + public function findItem(body:Body):BodyListItem { + var item:BodyListItem = head; + while (item != null && item.body != body) { + item = item.next; + } + return item; + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/.svn/text-base/BodyListItem.as.svn-base b/2.0.0.0/src/alternativa/physics/.svn/text-base/BodyListItem.as.svn-base new file mode 100644 index 0000000..382e11c --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/.svn/text-base/BodyListItem.as.svn-base @@ -0,0 +1,74 @@ +package alternativa.physics { + + /** + * + */ + public class BodyListItem { + + // Верхний элемент хранилища + private static var poolTop:BodyListItem; + + /** + * Создаёт новый элемент списка. + * + * @param primitive примитив, содержащийся в элементе + * @return новый элемент списка + */ + public static function create(body:Body):BodyListItem { + var item:BodyListItem; + if (poolTop == null) { + item = new BodyListItem(body); + } else { + item = poolTop; + poolTop = item.next; + item.next = null; + item.body = body; + } + return item; + } + + /** + * Очищает хранилище. + */ + public static function clearPool():void { + var item:BodyListItem = poolTop; + while (item != null) { + poolTop = item.next; + item.next = null; + item = poolTop; + } + } + + /** + * + */ + public var body:Body; + /** + * + */ + public var next:BodyListItem; + /** + * + */ + public var prev:BodyListItem; + + /** + * + * @param body + */ + public function BodyListItem(body:Body) { + this.body = body; + } + + /** + * + */ + public function dispose():void { + body = null; + prev = null; + next = poolTop; + poolTop = this; + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/.svn/text-base/BodyMaterial.as.svn-base b/2.0.0.0/src/alternativa/physics/.svn/text-base/BodyMaterial.as.svn-base new file mode 100644 index 0000000..7a7c47d --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/.svn/text-base/BodyMaterial.as.svn-base @@ -0,0 +1,11 @@ +package alternativa.physics { + + public class BodyMaterial { + + public var restitution:Number = 0; + public var friction:Number = 0.3; +// public var dynamicFriction:Number = 0.2; +// public var dynamicFriction:Number = 0.2; + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/.svn/text-base/BodyState.as.svn-base b/2.0.0.0/src/alternativa/physics/.svn/text-base/BodyState.as.svn-base new file mode 100644 index 0000000..f397afe --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/.svn/text-base/BodyState.as.svn-base @@ -0,0 +1,39 @@ +package alternativa.physics { + import alternativa.math.Quaternion; + import alternativa.math.Vector3; + + /** + * Класс описывает состояние твёрдого тела. + */ + public class BodyState { + /** + * Положение тела. + */ + public var pos:Vector3 = new Vector3(); + /** + * Ориентация тела. + */ + public var orientation:Quaternion = new Quaternion(); + /** + * Скорость тела. + */ + public var velocity:Vector3 = new Vector3(); + /** + * Угловая скорость тела. + */ + public var rotation:Vector3 = new Vector3(); + + /** + * Копирует значение указанного объекта. + * + * @param state + */ + public function copy(state:BodyState):void { + pos.copy(state.pos); + orientation.copy(state.orientation); + velocity.copy(state.velocity); + rotation.copy(state.rotation); + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/.svn/text-base/CollisionPrimitiveList.as.svn-base b/2.0.0.0/src/alternativa/physics/.svn/text-base/CollisionPrimitiveList.as.svn-base new file mode 100644 index 0000000..5d5304f --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/.svn/text-base/CollisionPrimitiveList.as.svn-base @@ -0,0 +1,89 @@ +package alternativa.physics { + import alternativa.physics.collision.CollisionPrimitive; + + /** + * + */ + public class CollisionPrimitiveList { + + public var head:CollisionPrimitiveListItem; + public var tail:CollisionPrimitiveListItem; + public var size:int; + + /** + * + */ + public function CollisionPrimitiveList() { + } + + /** + * + * @param primitive + */ + public function append(primitive:CollisionPrimitive):void { + var item:CollisionPrimitiveListItem = CollisionPrimitiveListItem.create(primitive); + if (head == null) { + head = tail = item; + } else { + tail.next = item; + item.prev = tail; + tail = item; + } + size++; + } + + /** + * + * @param primitve + */ + public function remove(primitve:CollisionPrimitive):void { + var item:CollisionPrimitiveListItem = findItem(primitve); + if (item == null) return; + if (item == head) { + if (size == 1) { + head = tail = null; + } else { + head = item.next; + head.prev = null; + } + } else { + if (item == tail) { + tail = tail.prev; + tail.next = null; + } else { + item.prev.next = item.next; + item.next.prev = item.prev; + } + } + item.dispose(); + size--; + } + + /** + * + * @param primitive + * @return + */ + public function findItem(primitive:CollisionPrimitive):CollisionPrimitiveListItem { + var item:CollisionPrimitiveListItem = head; + while (item != null && item.primitive != primitive) { + item = item.next; + } + return item; + } + + /** + * + */ + public function clear():void { + while (head != null) { + var item:CollisionPrimitiveListItem = head; + head = head.next; + item.dispose(); + } + tail = null; + size = 0; + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/.svn/text-base/CollisionPrimitiveListItem.as.svn-base b/2.0.0.0/src/alternativa/physics/.svn/text-base/CollisionPrimitiveListItem.as.svn-base new file mode 100644 index 0000000..d204532 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/.svn/text-base/CollisionPrimitiveListItem.as.svn-base @@ -0,0 +1,76 @@ +package alternativa.physics { + import alternativa.physics.collision.CollisionPrimitive; + + /** + * Элемент списка примитивов. + */ + public class CollisionPrimitiveListItem { + + // Верхний элемент хранилища + private static var poolTop:CollisionPrimitiveListItem; + + /** + * Создаёт новый элемент списка. + * + * @param primitive примитив, содержащийся в элементе + * @return новый элемент списка + */ + public static function create(primitive:CollisionPrimitive):CollisionPrimitiveListItem { + var item:CollisionPrimitiveListItem; + if (poolTop == null) { + item = new CollisionPrimitiveListItem(primitive); + } else { + item = poolTop; + item.primitive = primitive; + poolTop = item.next; + item.next = null; + } + return item; + } + + /** + * Очищает хранилище. + */ + public static function clearPool():void { + var curr:CollisionPrimitiveListItem = poolTop; + while (curr != null) { + poolTop = curr.next; + curr.next = null; + curr = poolTop; + } + } + + /** + * Примитив, хранящийся в элементе списка. + */ + public var primitive:CollisionPrimitive; + /** + * Ссылка не следующий элемент списка. + */ + public var next:CollisionPrimitiveListItem; + /** + * Ссылка не предыдущий элемент списка. + */ + public var prev:CollisionPrimitiveListItem; + + /** + * Создаёт новый экземпляр. + * + * @param primitive примитив, хранящийся в элементе списка + */ + public function CollisionPrimitiveListItem(primitive:CollisionPrimitive) { + this.primitive = primitive; + } + + /** + * Очищает внутренние ссылки и помещает элемент в хранилище для дальнейшего использования. + */ + public function dispose():void { + primitive = null; + prev = null; + next = poolTop; + poolTop = this; + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/.svn/text-base/Contact.as.svn-base b/2.0.0.0/src/alternativa/physics/.svn/text-base/Contact.as.svn-base new file mode 100644 index 0000000..3f010ec --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/.svn/text-base/Contact.as.svn-base @@ -0,0 +1,69 @@ +package alternativa.physics { + + + import alternativa.math.Vector3; + + /** + * Описывает контакт одного или двух тел. + */ + public class Contact { + /** + * Первое тело контакта, не может быть равно null. + */ + public var body1:Body; + /** + * Второе тело контакта, может быть равно null. + */ + public var body2:Body; + /** + * Предрассчитанный взаимный коэффициент отскока. + */ + public var restitution:Number; + /** + * Предрассчитанный взаимный коэффициент трения. + */ + public var friction:Number; + /** + * Нормаль контакта. Направлена к первому телу. + */ + public var normal:Vector3 = new Vector3(); + /** + * Список точек контакта. + */ + public var points:Vector. = new Vector.(MAX_POINTS, true); + /** + * Количество точек контакта. + */ + public var pcount:int; + /** + * Максимальная глубина пересечения. + */ + public var maxPenetration:Number = 0; + /** + * Флаг показывает, разрешён контакт или нет. + */ + public var satisfied:Boolean; + /** + * Следующий контакт в списке. + */ + public var next:Contact; + /** + * Индекс контакта. Первый контакт в списке имеет индекс 0. + */ + public var index:int; + + // Максимальное количество точек контакта + private const MAX_POINTS:int = 8; + + /** + * + */ + public function Contact(index:int) { + this.index = index; + for (var i:int = 0; i < MAX_POINTS; i++) { + points[i] = new ContactPoint(); + } + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/.svn/text-base/ContactPoint.as.svn-base b/2.0.0.0/src/alternativa/physics/.svn/text-base/ContactPoint.as.svn-base new file mode 100644 index 0000000..d1d8eb6 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/.svn/text-base/ContactPoint.as.svn-base @@ -0,0 +1,55 @@ +package alternativa.physics { + import alternativa.math.Vector3; + + /** + * + */ + public class ContactPoint { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + + // Величины, расчитываемые перед началом фазы решения контактов + + // Требуемая проекция конечной скорости на нормаль для упругого контакта + public var normalVel:Number; + // Минимальная скорость разделения неупругого контакта + public var minSepVel:Number; + // Изменение проекции скорости на единицу нормального импульса + public var velByUnitImpulseN:Number; + + public var angularInertia1:Number; + public var angularInertia2:Number; + + // Радиус-вектор точки контакта относительно центра первого тела + public var r1:Vector3 = new Vector3(); + // Радиус-вектор точки контакта относительно центра второго тела + public var r2:Vector3 = new Vector3(); + + // Величины, накапливаемые во время фазы решения контактов + + // Накопленный импульс, применяемый для получения требуемой относительной скорости в точке контакта + public var accumImpulseN:Number; + // Накопленный импульс, применяемый для разделения тел в точке контакта. Не создаёт момента. +// public var accumSepImpulse:Number; + + public var satisfied:Boolean; + + /** + * + * @param cp + */ + public function copyFrom(cp:ContactPoint):void { + pos.copy(cp.pos); + penetration = cp.penetration; + feature1 = cp.feature1; + feature2 = cp.feature2; + r1.copy(cp.r1); + r2.copy(cp.r2); + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/.svn/text-base/PhysicsScene.as.svn-base b/2.0.0.0/src/alternativa/physics/.svn/text-base/PhysicsScene.as.svn-base new file mode 100644 index 0000000..d9f5db1 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/.svn/text-base/PhysicsScene.as.svn-base @@ -0,0 +1,549 @@ +package alternativa.physics { + + import alternativa.physics.collision.ICollisionDetector; + import alternativa.physics.collision.KdTreeCollisionDetector; + import alternativa.physics.constraints.Constraint; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + /** + * Класс реализует физическую симуляцию поведения твёрдых тел. + */ + public class PhysicsScene { + + private static var lastBodyId:int; + + // Максимальное количество контактов + public const MAX_CONTACTS:int = 1000; + + ////////////////////////////////////////////////////////////////// + // Настроечные параметры симуляции. Могут быть изменены в любой + // момент времени без нарушения корректной работы симуляции. + ////////////////////////////////////////////////////////////////// + // Количество шагов, за которое пересекающиеся тела должны разделиться + public var penResolutionSteps:int = 10; + // Величина допустимой глубины пересечения + public var allowedPenetration:Number = 0.1; + // Максимальная скорость, добавляемая с целью разделения тел + public var maxPenResolutionSpeed:Number = 0.5; + // Количество итераций для обработки упругих контактов + public var collisionIterations:int = 5; + // Количество итераций для обработки неупругих контактов + public var contactIterations:int = 5; + // Флаг использования предсказания состояний + public var usePrediction:Boolean = false; + + public var freezeSteps:int = 10; + public var linSpeedFreezeLimit:Number = 1; + public var angSpeedFreezeLimit:Number = 0.01; + + // Вектор гравитации + public var _gravity:Vector3 = new Vector3(0, 0, -9.8); + // Модуль вектора гравитации + public var _gravityMagnitude:Number = 9.8; + + // Использующийся детектор столкновений + public var collisionDetector:ICollisionDetector; + + // Список тел, участвующих в симуляции + public var bodies:BodyList = new BodyList(); + + // Список контактов на текущем шаге симуляции + public var contacts:Contact; + // Список ограничений + public var constraints:Vector. = new Vector.(); + // Количество ограничений + public var constraintsNum:int; + // Временная метка. Число прошедших шагов с начала симуляции. + public var frame:int; + // Время с начала симуляции, мс + public var time:int; + // Первый неиспользованный контакт на текущем шаге симуляции + private var borderContact:Contact; + + // Временные переменные для избежания создания экземпляров + private var _t:Vector3 = new Vector3(); + private var _v:Vector3 = new Vector3(); + + /** + * + */ + public function PhysicsScene() { + contacts = new Contact(0); + var contact:Contact = contacts; + for (var i:int = 1; i < MAX_CONTACTS; i++) { + contact.next = new Contact(i); + contact = contact.next; + } + collisionDetector = new KdTreeCollisionDetector(); + } + + /** + * Вектор гравитации. + */ + public function get gravity():Vector3 { + return _gravity.clone(); + } + + /** + * @private + */ + public function set gravity(value:Vector3):void { + _gravity.copy(value); + _gravityMagnitude = _gravity.length(); + } + + /** + * Добавляет тело в симуляцию. + * + * @param body + */ + public function addBody(body:Body):void { + body.id = lastBodyId++; + body.world = this; + bodies.append(body); + } + + /** + * Удаляет тело из симуляции. + * @param body + */ + public function removeBody(body:Body):void { + if (bodies.remove(body)) { + body.world = null; + } + } + + /** + * Добавляет ограничение. + * @param c + */ + public function addConstraint(c:Constraint):void { + constraints[constraintsNum++] = c; + c.world = this; + } + + /** + * Удаляет ограничение. + * @param c + */ + public function removeConstraint(c:Constraint):Boolean { + var idx:int = constraints.indexOf(c); + if (idx < 0) return false; + constraints.splice(idx, 1); + constraintsNum--; + c.world = null; + return true; + } + + /** + * Применяет к телам действующте на них силы. Аккумуляторы сил и моментов тел очищаются после завершения шага + * симуляции, поэтому на момент вызова метода могут уже содержать некоторые значения. + * + * @param dt промежуток времени, в течении которого действуют силы + */ + private function applyForces(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + body.beforePhysicsStep(dt); + body.calcAccelerations(); + // Ускорение свободного падения применяется только к подвижным телам во избежание некорректного изменения + // фиктивной скорости неподвижных тел. + if (body.useGravity && body.movable && !body.frozen) { + body.accel.x += _gravity.x; + body.accel.y += _gravity.y; + body.accel.z += _gravity.z; + } + item = item.next; + } + } + + /** + * Определяет все столкновения на текущем шаге симуляции и заполняет список получившихся контактов. + * + * @param dt длительность шага симуляции + */ + private function detectCollisions(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + if (!body.frozen) { + body.contactsNum = 0; + body.saveState(); + // При включённом режиме предсказания состояние тел интегрируется на один шаг вперёд + if (usePrediction) { + body.integrateVelocity(dt); + body.integratePosition(dt); + } + body.calcDerivedData(); + } + item = item.next; + } + + borderContact = collisionDetector.getAllContacts(contacts); + + // Расчёт относительных векторов точки контакта вынесен сюда из-за необходимости учитывать + // положение тел в предсказанном состоянии + var contact:Contact = contacts; + while (contact != borderContact) { + var b1:Body = contact.body1; + var b2:Body = contact.body2; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + var bPos:Vector3 = b1.state.pos; + cp.r1.x = cp.pos.x - bPos.x; + cp.r1.y = cp.pos.y - bPos.y; + cp.r1.z = cp.pos.z - bPos.z; + if (b2 != null) { + bPos = b2.state.pos; + cp.r2.x = cp.pos.x - bPos.x; + cp.r2.y = cp.pos.y - bPos.y; + cp.r2.z = cp.pos.z - bPos.z; + } + } + contact = contact.next; + } + + // Восстановление состояния тел + if (usePrediction) { + item = bodies.head; + while (item != null) { + body = item.body; + if (!body.frozen) { + body.restoreState(); + body.calcDerivedData(); + } + item = item.next; + } + } + } + + /** + * Подготваливает полученные из детектора столкновений контакты, расчитывая значения, не меняющиеся + * в ходе шага симуляции. + */ + private function preProcessContacts(dt:Number):void { + var contact:Contact = contacts; + while (contact != borderContact) { + var b1:Body = contact.body1; + var b2:Body = contact.body2; + // Столкнувшиеся тела размораживаются + if (b1.frozen) { + b1.frozen = false; + b1.freezeCounter = 0; + } + if (b2 != null && b2.frozen) { + b2.frozen = false; + b2.freezeCounter = 0; + } + contact.restitution = b1.material.restitution; + if (b2 != null && b2.material.restitution < contact.restitution) contact.restitution = b2.material.restitution; + contact.friction = b1.material.friction; + if (b2 != null && b2.material.friction < contact.friction) contact.friction = b2.material.friction; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.accumImpulseN = 0; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + cp.velByUnitImpulseN = 0; + if (b1.movable) { + cp.angularInertia1 = _v.cross2(cp.r1, contact.normal).transformBy3(b1.invInertiaWorld).cross(cp.r1).dot(contact.normal); + cp.velByUnitImpulseN += b1.invMass + cp.angularInertia1; + } + if (b2 != null && b2.movable) { + cp.angularInertia2 = _v.cross2(cp.r2, contact.normal).transformBy3(b2.invInertiaWorld).cross(cp.r2).dot(contact.normal); + cp.velByUnitImpulseN += b2.invMass + cp.angularInertia2; + } + // Расчёт требуемой конечной скорости для упругого контакта + calcSepVelocity(b1, b2, cp, _v); + cp.normalVel = _v.dot(contact.normal); + if (cp.normalVel < 0) cp.normalVel = - contact.restitution*cp.normalVel; + // Скорость разделения неупругого контакта + cp.minSepVel = cp.penetration > allowedPenetration ? (cp.penetration - allowedPenetration)/(penResolutionSteps*dt) : 0; + if (cp.minSepVel > maxPenResolutionSpeed) cp.minSepVel = maxPenResolutionSpeed; + } + contact = contact.next; + } + for (var i:int = 0; i < constraintsNum; i++) { + var constraint:Constraint = constraints[i]; + constraint.preProcess(dt); + } + } + + /** + * + * @param dt + * @param forceInelastic + */ + private function processContacts(dt:Number, forceInelastic:Boolean):void { + var iterNum:int = forceInelastic ? contactIterations : collisionIterations; + var i:int; + var forwardLoop:Boolean = false; + for (var iter:int = 0; iter < iterNum; iter++) { + forwardLoop = !forwardLoop; + var contact:Contact = contacts; + while (contact != borderContact) { + resolveContact(contact, forceInelastic, forwardLoop); + contact = contact.next; + } + // Ограничения + for (i = 0; i < constraintsNum; i++) { + var constraint:Constraint = constraints[i]; + constraint.apply(dt); + } + } + } + + /** + * + * @param contactInfo + * @param forceInelastic + * @param forwardLoop + */ + private function resolveContact(contactInfo:Contact, forceInelastic:Boolean, forwardLoop:Boolean):void { + var b1:Body = contactInfo.body1; + var b2:Body = contactInfo.body2; + var normal:Vector3 = contactInfo.normal; + var i:int; + if (forwardLoop) { + for (i = 0; i < contactInfo.pcount; i++) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } else { + for (i = contactInfo.pcount - 1; i >= 0; i--) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } + } + + /** + * + * @param idx + * @param b1 + * @param b2 + * @param contact + * @param normal + * @param forceInelastic + */ + private function resolveContactPoint(idx:int, b1:Body, b2:Body, contact:Contact, normal:Vector3, forceInelastic:Boolean):void { + var cp:ContactPoint = contact.points[idx]; + if (!forceInelastic) { + cp.satisfied = true; + } + + var newVel:Number = 0; + calcSepVelocity(b1, b2, cp, _v); + var cnormal:Vector3 = contact.normal; + var sepVel:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + if (forceInelastic) { + var minSpeVel:Number = cp.minSepVel; + if (sepVel < minSpeVel) { + cp.satisfied = false; + } else if (cp.satisfied) return; + newVel = minSpeVel; + } else { + newVel = cp.normalVel; + } + var deltaVel:Number = newVel - sepVel; + var impulse:Number = deltaVel/cp.velByUnitImpulseN; + var accumImpulse:Number = cp.accumImpulseN + impulse; + if (accumImpulse < 0) { + accumImpulse = 0; + } + var deltaImpulse:Number = accumImpulse - cp.accumImpulseN; + cp.accumImpulseN = accumImpulse; + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, normal, deltaImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, normal, -deltaImpulse); + } + + // Учёт силы трения + calcSepVelocity(b1, b2, cp, _v); + // Расчитываем изменение касательной скорости на единицу касательного импульса + var tanSpeedByUnitImpulse:Number = 0; + +// _v.vAddScaled(-_v.vDot(contact.normal), contact.normal); + var dot:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + _v.x -= dot*cnormal.x; + _v.y -= dot*cnormal.y; + _v.z -= dot*cnormal.z; + + var tanSpeed:Number = _v.length(); + if (tanSpeed < 0.001) return; + +// _t.vCopy(_v).vNormalize().vReverse(); + _t.x = -_v.x; + _t.y = -_v.y; + _t.z = -_v.z; + _t.normalize(); + + var r:Vector3; + var m:Matrix3; + var xx:Number; + var yy:Number; + var zz:Number; + // dV = b.invMass + ((invI * (r % t)) % r) * t + if (b1.movable) { +// _v.vCross2(cp.r1, _t).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1); + r = cp.r1; + m = b1.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b1.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + if (b2 != null && b2.movable) { +// _v.vCross2(cp.r2, _t).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2); + + r = cp.r2; + m = b2.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b2.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + + var tanImpulse:Number = tanSpeed/tanSpeedByUnitImpulse; + var max:Number = contact.friction*cp.accumImpulseN; + if (max < 0) { + if (tanImpulse < max) tanImpulse = max; + } else { + if (tanImpulse > max) tanImpulse = max; + } + + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, _t, tanImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, _t, -tanImpulse); + } + } + + /** + * + * @param body1 + * @param body2 + * @param cp + * @param result + */ + private function calcSepVelocity(body1:Body, body2:Body, cp:ContactPoint, result:Vector3):void { + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 +// result.vCopy(body1.state.velocity).vAdd(_v1.vCross2(body1.state.rotation, cp.r1)); + var rot:Vector3 = body1.state.rotation; + var v:Vector3 = cp.r1; + var x:Number = rot.y*v.z - rot.z*v.y; + var y:Number = rot.z*v.x - rot.x*v.z; + var z:Number = rot.x*v.y - rot.y*v.x; + v = body1.state.velocity; + result.x = v.x + x; + result.y = v.y + y; + result.z = v.z + z; + // V2 = V2_c + w2%r2 + if (body2 != null) { +// result.vSubtract(body2.state.velocity).vSubtract(_v2.vCross2(body2.state.rotation, cp.r2)); + rot = body2.state.rotation; + v = cp.r2; + x = rot.y*v.z - rot.z*v.y; + y = rot.z*v.x - rot.x*v.z; + z = rot.x*v.y - rot.y*v.x; + v = body2.state.velocity; + result.x -= v.x + x; + result.y -= v.y + y; + result.z -= v.z + z; + } + } + + /** + * + * @param dt + */ + private function intergateVelocities(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + item.body.integrateVelocity(dt); + item = item.next; + } + } + + /** + * + * @param dt + */ + private function integratePositions(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + if (body.movable && !body.frozen) { + body.integratePosition(dt); + } + item = item.next; + } + } + + /** + * + */ + private function postPhysics():void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + body.clearAccumulators(); + body.calcDerivedData(); + if (body.canFreeze) { + if (body.state.velocity.length() < linSpeedFreezeLimit && body.state.rotation.length() < angSpeedFreezeLimit) { + if (!body.frozen) { + body.freezeCounter++; + if (body.freezeCounter >= freezeSteps) + body.frozen = true; + } + } else { + body.freezeCounter = 0; + body.frozen = false; + } + } + item = item.next; + } + } + + /** + * + * @param delta + */ + public function update(delta:int):void { + frame++; + time += delta; + var dt:Number = 0.001*delta; + applyForces(dt); + detectCollisions(dt); + preProcessContacts(dt); + processContacts(dt, false); + intergateVelocities(dt); + processContacts(dt, true); + integratePositions(dt); + postPhysics(); + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/.svn/text-base/PhysicsUtils.as.svn-base b/2.0.0.0/src/alternativa/physics/.svn/text-base/PhysicsUtils.as.svn-base new file mode 100644 index 0000000..5a2e46f --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/.svn/text-base/PhysicsUtils.as.svn-base @@ -0,0 +1,49 @@ +package alternativa.physics { + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + public class PhysicsUtils { + + /** + * @param mass + * @param halfSize + * @param result + */ + public static function getBoxInvInertia(mass:Number, halfSize:Vector3, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + result.a = 3/(mass*(yy + zz)); + result.f = 3/(mass*(zz + xx)); + result.k = 3/(mass*(xx + yy)); + } + + /** + * @param mass + * @param r + * @param h + * @param result + */ + public static function getCylinderInvInertia(mass:Number, r:Number, h:Number, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + + result.a = result.f = 1/(mass*(h*h/12 + r*r/4)); + result.k = 2/(mass*r*r); + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/Body.as b/2.0.0.0/src/alternativa/physics/Body.as new file mode 100644 index 0000000..28c9b1a --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/Body.as @@ -0,0 +1,461 @@ +package alternativa.physics { + + import alternativa.math.Matrix3; + import alternativa.math.Matrix4; + import alternativa.math.Quaternion; + import alternativa.math.Vector3; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.IBodyCollisionPredicate; + import alternativa.physics.collision.types.BoundBox; + + /** + * + */ + public class Body { + + public static var linDamping:Number = 0.997; + public static var rotDamping:Number = 0.997; + + // Идентификатор тела, уникальный в пределах мира + public var id:int; + // Имя тела + public var name:String; + // Мир, в котором находится тело + public var world:PhysicsScene; + // Флаг подвижности тела + public var movable:Boolean = true; + // Флаг указывает, может ли тело быть заморожено + public var canFreeze:Boolean = false; + + public var freezeCounter:int; + public var frozen:Boolean = false; + + public var useGravity:Boolean = true; + + // Ограничивающий бокс тела + public var aabb:BoundBox = new BoundBox(); + // Предикат, через который тело получает сообщения о столкновениях + public var postCollisionPredicate:IBodyCollisionPredicate; + + // Текущее состояние тела + public var state:BodyState = new BodyState(); + // Предыдущее состояние тела + public var prevState:BodyState = new BodyState(); + // Линейное ускорение тела на текущем шаге симуляции + public var accel:Vector3 = new Vector3(); + // Угловое ускорение тела на текущем шаге симуляции + public var angleAccel:Vector3 = new Vector3(); + // Физический материал тела + public var material:BodyMaterial = new BodyMaterial(); + // Обратная масса тела + public var invMass:Number = 1; + // Обратная матрица тензора инерции в локальных координатах + public var invInertia:Matrix3 = new Matrix3(); + // Обратная матрица тензора инерции в мировых координатах + public var invInertiaWorld:Matrix3 = new Matrix3(); + // Базисная матрица тела в мировых координатах + public var baseMatrix:Matrix3 = new Matrix3(); + + public const MAX_CONTACTS:int = 20; + public var contacts:Vector. = new Vector.(MAX_CONTACTS); + public var contactsNum:int; + + public var collisionPrimitives:CollisionPrimitiveList; + + // Аккумулятор сил + public var forceAccum:Vector3 = new Vector3(); + // Аккумулятор моментов + public var torqueAccum:Vector3 = new Vector3(); + + // Внутренние переменные для избежания создания экземпляров + private static var _r:Vector3 = new Vector3(); + private static var _f:Vector3 = new Vector3(); + private static var _q:Quaternion = new Quaternion(); + + /** + * + * @param invMass + * @param invInertia + */ + public function Body(invMass:Number, invInertia:Matrix3) { + this.invMass = invMass; + this.invInertia.copy(invInertia); + } + + /** + * @param primitive + * @param localTransform + */ + public function addCollisionPrimitive(primitive:CollisionPrimitive, localTransform:Matrix4 = null):void { + if (primitive == null) { + throw new ArgumentError("Primitive cannot be null"); + } + if (collisionPrimitives == null) { + collisionPrimitives = new CollisionPrimitiveList(); + } + collisionPrimitives.append(primitive); + primitive.setBody(this, localTransform); + } + + /** + * + * @param primitive + */ + public function removeCollisionPrimitive(primitive:CollisionPrimitive):void { + if (collisionPrimitives == null) return; + primitive.setBody(null); + collisionPrimitives.remove(primitive); + if (collisionPrimitives.size == 0) { + collisionPrimitives = null; + } + } + + /** + * + * @param t + * @param pos + * @param orientation + */ + public function interpolate(t:Number, pos:Vector3, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.w = prevState.orientation.w*t1 + state.orientation.w*t; + orientation.x = prevState.orientation.x*t1 + state.orientation.x*t; + orientation.y = prevState.orientation.y*t1 + state.orientation.y*t; + orientation.z = prevState.orientation.z*t1 + state.orientation.z*t; + } + + /** + * + * @param t + * @param pos + * @param orientation + */ + public function interpolateSLERP(t:Number, pos:Vector3, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.slerp(prevState.orientation, state.orientation, t); + } + + /** + * + * @param t + * @param m + */ + public function interpolateToMatrix(t:Number, m:Matrix4):void { + var t1:Number = 1 - t; + m.d = prevState.pos.x*t1 + state.pos.x*t; + m.h = prevState.pos.y*t1 + state.pos.y*t; + m.l = prevState.pos.z*t1 + state.pos.z*t; + _q.w = prevState.orientation.w*t1 + state.orientation.w*t; + _q.x = prevState.orientation.x*t1 + state.orientation.x*t; + _q.y = prevState.orientation.y*t1 + state.orientation.y*t; + _q.z = prevState.orientation.z*t1 + state.orientation.z*t; + _q.normalize(); + _q.toMatrix4(m); + } + + /** + * + * @param t + * @param m + */ + public function interpolateToMatrixSLERP(t:Number, m:Matrix4):void { + var t1:Number = 1 - t; + m.d = prevState.pos.x*t1 + state.pos.x*t; + m.h = prevState.pos.y*t1 + state.pos.y*t; + m.l = prevState.pos.z*t1 + state.pos.z*t; + _q.slerp(prevState.orientation, state.orientation, t); + _q.normalize(); + _q.toMatrix4(m); + } + + /** + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + state.pos.copy(pos); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setPositionXYZ(x:Number, y:Number, z:Number):void { + state.pos.reset(x, y, z); + } + + /** + * + * @param vel + */ + public function setVelocity(vel:Vector3):void { + state.velocity.copy(vel); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setVelocityXYZ(x:Number, y:Number, z:Number):void { + state.velocity.reset(x, y, z); + } + + /** + * + * @param rot + */ + public function setRotation(rot:Vector3):void { + state.rotation.copy(rot); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setRotationXYZ(x:Number, y:Number, z:Number):void { + state.rotation.reset(x, y, z); + } + + /** + * + * @param q + */ + public function setOrientation(q:Quaternion):void { + state.orientation.copy(q); + } + + /** + * @param r + * @param dir + * @param magnitude + */ + public function applyRelPosWorldImpulse(r:Vector3, dir:Vector3, magnitude:Number):void { + var d:Number = magnitude*invMass; + // Линейная часть + state.velocity.x += d*dir.x; + state.velocity.y += d*dir.y; + state.velocity.z += d*dir.z; + + // Вращательная часть + var x:Number = (r.y*dir.z - r.z*dir.y)*magnitude; + var y:Number = (r.z*dir.x - r.x*dir.z)*magnitude; + var z:Number = (r.x*dir.y - r.y*dir.x)*magnitude; + + state.rotation.x += invInertiaWorld.a*x + invInertiaWorld.b*y + invInertiaWorld.c*z; + state.rotation.y += invInertiaWorld.e*x + invInertiaWorld.f*y + invInertiaWorld.g*z; + state.rotation.z += invInertiaWorld.i*x + invInertiaWorld.j*y + invInertiaWorld.k*z; + } + + /** + * @param f + */ + public function addForce(f:Vector3):void { + forceAccum.add(f); + } + + /** + * + * @param fx + * @param fy + * @param fz + */ + public function addForceXYZ(fx:Number, fy:Number, fz:Number):void { + forceAccum.x += fx; + forceAccum.y += fy; + forceAccum.z += fz; + } + + /** + * + * @param px + * @param py + * @param pz + * @param fx + * @param fy + * @param fz + */ + public function addWorldForceXYZ(px:Number, py:Number, pz:Number, fx:Number, fy:Number, fz:Number):void { + forceAccum.x += fx; + forceAccum.y += fy; + forceAccum.z += fz; + + var pos:Vector3 = state.pos; + var rx:Number = px - pos.x; + var ry:Number = py - pos.y; + var rz:Number = pz - pos.z; + +// var x:Number = ry*fz - rz*fy; +// var y:Number = rz*fx - rx*fz; +// var z:Number = rx*fy - ry*fx; + + torqueAccum.x += ry*fz - rz*fy; + torqueAccum.y += rz*fx - rx*fz; + torqueAccum.z += rx*fy - ry*fx; + } + + /** + * @param pos + * @param f + */ + public function addWorldForce(pos:Vector3, force:Vector3):void { + forceAccum.add(force); + torqueAccum.add(_r.diff(pos, state.pos).cross(force)); + } + + /** + * @param pos + * @param f + */ + public function addWorldForceScaled(pos:Vector3, force:Vector3, scale:Number):void { + _f.x = scale*force.x; + _f.y = scale*force.y; + _f.z = scale*force.z; + forceAccum.add(_f); + torqueAccum.add(_r.diff(pos, state.pos).cross(_f)); + } + + /** + * @param pos + * @param f + */ + public function addLocalForce(pos:Vector3, force:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(pos, _r); + // Трансформируем вектор силы в мировую систему + baseMatrix.transformVector(force, _f); + // Добавляем силу и момент + forceAccum.add(_f); + torqueAccum.add(_r.cross(_f)); + } + + /** + * + * @param localPos + * @param worldForce + */ + public function addWorldForceAtLocalPoint(localPos:Vector3, worldForce:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(localPos, _r); + // Добавляем силу и момент + forceAccum.add(worldForce); + torqueAccum.add(_r.cross(worldForce)); + } + + /** + * @param dt + */ + public function beforePhysicsStep(dt:Number):void { + } + + /** + * @param t + */ + public function addTorque(t:Vector3):void { + torqueAccum.add(t); + } + + /** + * + */ + internal function clearAccumulators():void { + forceAccum.x = forceAccum.y = forceAccum.z = 0; + torqueAccum.x = torqueAccum.y = torqueAccum.z = 0; + } + + /** + * + */ + internal function calcAccelerations():void { + accel.x = forceAccum.x*invMass; + accel.y = forceAccum.y*invMass; + accel.z = forceAccum.z*invMass; + angleAccel.x = invInertiaWorld.a*torqueAccum.x + invInertiaWorld.b*torqueAccum.y + invInertiaWorld.c*torqueAccum.z; + angleAccel.y = invInertiaWorld.e*torqueAccum.x + invInertiaWorld.f*torqueAccum.y + invInertiaWorld.g*torqueAccum.z; + angleAccel.z = invInertiaWorld.i*torqueAccum.x + invInertiaWorld.j*torqueAccum.y + invInertiaWorld.k*torqueAccum.z; + } + + /** + * Вычисляет производные данные. + */ + public function calcDerivedData():void { + // Вычисление базисной матрицы и обратного тензора инерции в мировых координатах + state.orientation.toMatrix3(baseMatrix); + invInertiaWorld.copy(invInertia).append(baseMatrix).prependTransposed(baseMatrix); + if (collisionPrimitives != null) { + aabb.infinity(); + var item:CollisionPrimitiveListItem = collisionPrimitives.head; + while (item != null) { + var primitive:CollisionPrimitive = item.primitive; + primitive.transform.setFromMatrix3(baseMatrix, state.pos); + if (primitive.localTransform != null) { + primitive.transform.prepend(primitive.localTransform); + } + primitive.calculateAABB(); + aabb.addBoundBox(primitive.aabb); + item = item.next; + } + } + } + + /** + * + */ + public function saveState():void { + prevState.copy(state); + } + + /** + * + */ + internal function restoreState():void { + state.copy(prevState); + } + + /** + * @param dt + */ + internal function integrateVelocity(dt:Number):void { + // v = v + a*t + state.velocity.x += accel.x*dt; + state.velocity.y += accel.y*dt; + state.velocity.z += accel.z*dt; + // rot = rot + eps*t + state.rotation.x += angleAccel.x*dt; + state.rotation.y += angleAccel.y*dt; + state.rotation.z += angleAccel.z*dt; + + state.velocity.x *= linDamping; + state.velocity.y *= linDamping; + state.velocity.z *= linDamping; + + state.rotation.x *= rotDamping; + state.rotation.y *= rotDamping; + state.rotation.z *= rotDamping; + } + + /** + * + */ + internal function integratePosition(dt:Number):void { + // pos = pos + v*t + state.pos.x += state.velocity.x*dt; + state.pos.y += state.velocity.y*dt; + state.pos.z += state.velocity.z*dt; + // q = q + 0.5*rot*q + state.orientation.addScaledVector(state.rotation, dt); + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/BodyList.as b/2.0.0.0/src/alternativa/physics/BodyList.as new file mode 100644 index 0000000..aa8f79f --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/BodyList.as @@ -0,0 +1,76 @@ +package alternativa.physics { + + /** + * + */ + public class BodyList { + + public var head:BodyListItem; + public var tail:BodyListItem; + public var size:int; + + /** + * + */ + public function BodyList() { + } + + /** + * + * @param body + */ + public function append(body:Body):void { + var item:BodyListItem = BodyListItem.create(body); + if (head == null) { + head = tail = item; + } else { + tail.next = item; + item.prev = tail; + tail = item; + } + size++; + } + + /** + * + * @param body + */ + public function remove(body:Body):Boolean { + var item:BodyListItem = findItem(body); + if (item == null) return false; + if (item == head) { + if (size == 1) { + head = tail = null; + } else { + head = item.next; + head.prev = null; + } + } else { + if (item == tail) { + tail = item.prev; + tail.next = null; + } else { + item.prev.next = item.next; + item.next.prev = item.prev; + } + } + item.dispose(); + size--; + return true; + } + + /** + * + * @param body + * @return + */ + public function findItem(body:Body):BodyListItem { + var item:BodyListItem = head; + while (item != null && item.body != body) { + item = item.next; + } + return item; + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/BodyListItem.as b/2.0.0.0/src/alternativa/physics/BodyListItem.as new file mode 100644 index 0000000..382e11c --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/BodyListItem.as @@ -0,0 +1,74 @@ +package alternativa.physics { + + /** + * + */ + public class BodyListItem { + + // Верхний элемент хранилища + private static var poolTop:BodyListItem; + + /** + * Создаёт новый элемент списка. + * + * @param primitive примитив, содержащийся в элементе + * @return новый элемент списка + */ + public static function create(body:Body):BodyListItem { + var item:BodyListItem; + if (poolTop == null) { + item = new BodyListItem(body); + } else { + item = poolTop; + poolTop = item.next; + item.next = null; + item.body = body; + } + return item; + } + + /** + * Очищает хранилище. + */ + public static function clearPool():void { + var item:BodyListItem = poolTop; + while (item != null) { + poolTop = item.next; + item.next = null; + item = poolTop; + } + } + + /** + * + */ + public var body:Body; + /** + * + */ + public var next:BodyListItem; + /** + * + */ + public var prev:BodyListItem; + + /** + * + * @param body + */ + public function BodyListItem(body:Body) { + this.body = body; + } + + /** + * + */ + public function dispose():void { + body = null; + prev = null; + next = poolTop; + poolTop = this; + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/BodyMaterial.as b/2.0.0.0/src/alternativa/physics/BodyMaterial.as new file mode 100644 index 0000000..7a7c47d --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/BodyMaterial.as @@ -0,0 +1,11 @@ +package alternativa.physics { + + public class BodyMaterial { + + public var restitution:Number = 0; + public var friction:Number = 0.3; +// public var dynamicFriction:Number = 0.2; +// public var dynamicFriction:Number = 0.2; + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/BodyState.as b/2.0.0.0/src/alternativa/physics/BodyState.as new file mode 100644 index 0000000..f397afe --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/BodyState.as @@ -0,0 +1,39 @@ +package alternativa.physics { + import alternativa.math.Quaternion; + import alternativa.math.Vector3; + + /** + * Класс описывает состояние твёрдого тела. + */ + public class BodyState { + /** + * Положение тела. + */ + public var pos:Vector3 = new Vector3(); + /** + * Ориентация тела. + */ + public var orientation:Quaternion = new Quaternion(); + /** + * Скорость тела. + */ + public var velocity:Vector3 = new Vector3(); + /** + * Угловая скорость тела. + */ + public var rotation:Vector3 = new Vector3(); + + /** + * Копирует значение указанного объекта. + * + * @param state + */ + public function copy(state:BodyState):void { + pos.copy(state.pos); + orientation.copy(state.orientation); + velocity.copy(state.velocity); + rotation.copy(state.rotation); + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/CollisionPrimitiveList.as b/2.0.0.0/src/alternativa/physics/CollisionPrimitiveList.as new file mode 100644 index 0000000..5d5304f --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/CollisionPrimitiveList.as @@ -0,0 +1,89 @@ +package alternativa.physics { + import alternativa.physics.collision.CollisionPrimitive; + + /** + * + */ + public class CollisionPrimitiveList { + + public var head:CollisionPrimitiveListItem; + public var tail:CollisionPrimitiveListItem; + public var size:int; + + /** + * + */ + public function CollisionPrimitiveList() { + } + + /** + * + * @param primitive + */ + public function append(primitive:CollisionPrimitive):void { + var item:CollisionPrimitiveListItem = CollisionPrimitiveListItem.create(primitive); + if (head == null) { + head = tail = item; + } else { + tail.next = item; + item.prev = tail; + tail = item; + } + size++; + } + + /** + * + * @param primitve + */ + public function remove(primitve:CollisionPrimitive):void { + var item:CollisionPrimitiveListItem = findItem(primitve); + if (item == null) return; + if (item == head) { + if (size == 1) { + head = tail = null; + } else { + head = item.next; + head.prev = null; + } + } else { + if (item == tail) { + tail = tail.prev; + tail.next = null; + } else { + item.prev.next = item.next; + item.next.prev = item.prev; + } + } + item.dispose(); + size--; + } + + /** + * + * @param primitive + * @return + */ + public function findItem(primitive:CollisionPrimitive):CollisionPrimitiveListItem { + var item:CollisionPrimitiveListItem = head; + while (item != null && item.primitive != primitive) { + item = item.next; + } + return item; + } + + /** + * + */ + public function clear():void { + while (head != null) { + var item:CollisionPrimitiveListItem = head; + head = head.next; + item.dispose(); + } + tail = null; + size = 0; + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/CollisionPrimitiveListItem.as b/2.0.0.0/src/alternativa/physics/CollisionPrimitiveListItem.as new file mode 100644 index 0000000..d204532 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/CollisionPrimitiveListItem.as @@ -0,0 +1,76 @@ +package alternativa.physics { + import alternativa.physics.collision.CollisionPrimitive; + + /** + * Элемент списка примитивов. + */ + public class CollisionPrimitiveListItem { + + // Верхний элемент хранилища + private static var poolTop:CollisionPrimitiveListItem; + + /** + * Создаёт новый элемент списка. + * + * @param primitive примитив, содержащийся в элементе + * @return новый элемент списка + */ + public static function create(primitive:CollisionPrimitive):CollisionPrimitiveListItem { + var item:CollisionPrimitiveListItem; + if (poolTop == null) { + item = new CollisionPrimitiveListItem(primitive); + } else { + item = poolTop; + item.primitive = primitive; + poolTop = item.next; + item.next = null; + } + return item; + } + + /** + * Очищает хранилище. + */ + public static function clearPool():void { + var curr:CollisionPrimitiveListItem = poolTop; + while (curr != null) { + poolTop = curr.next; + curr.next = null; + curr = poolTop; + } + } + + /** + * Примитив, хранящийся в элементе списка. + */ + public var primitive:CollisionPrimitive; + /** + * Ссылка не следующий элемент списка. + */ + public var next:CollisionPrimitiveListItem; + /** + * Ссылка не предыдущий элемент списка. + */ + public var prev:CollisionPrimitiveListItem; + + /** + * Создаёт новый экземпляр. + * + * @param primitive примитив, хранящийся в элементе списка + */ + public function CollisionPrimitiveListItem(primitive:CollisionPrimitive) { + this.primitive = primitive; + } + + /** + * Очищает внутренние ссылки и помещает элемент в хранилище для дальнейшего использования. + */ + public function dispose():void { + primitive = null; + prev = null; + next = poolTop; + poolTop = this; + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/Contact.as b/2.0.0.0/src/alternativa/physics/Contact.as new file mode 100644 index 0000000..3f010ec --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/Contact.as @@ -0,0 +1,69 @@ +package alternativa.physics { + + + import alternativa.math.Vector3; + + /** + * Описывает контакт одного или двух тел. + */ + public class Contact { + /** + * Первое тело контакта, не может быть равно null. + */ + public var body1:Body; + /** + * Второе тело контакта, может быть равно null. + */ + public var body2:Body; + /** + * Предрассчитанный взаимный коэффициент отскока. + */ + public var restitution:Number; + /** + * Предрассчитанный взаимный коэффициент трения. + */ + public var friction:Number; + /** + * Нормаль контакта. Направлена к первому телу. + */ + public var normal:Vector3 = new Vector3(); + /** + * Список точек контакта. + */ + public var points:Vector. = new Vector.(MAX_POINTS, true); + /** + * Количество точек контакта. + */ + public var pcount:int; + /** + * Максимальная глубина пересечения. + */ + public var maxPenetration:Number = 0; + /** + * Флаг показывает, разрешён контакт или нет. + */ + public var satisfied:Boolean; + /** + * Следующий контакт в списке. + */ + public var next:Contact; + /** + * Индекс контакта. Первый контакт в списке имеет индекс 0. + */ + public var index:int; + + // Максимальное количество точек контакта + private const MAX_POINTS:int = 8; + + /** + * + */ + public function Contact(index:int) { + this.index = index; + for (var i:int = 0; i < MAX_POINTS; i++) { + points[i] = new ContactPoint(); + } + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/ContactPoint.as b/2.0.0.0/src/alternativa/physics/ContactPoint.as new file mode 100644 index 0000000..d1d8eb6 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/ContactPoint.as @@ -0,0 +1,55 @@ +package alternativa.physics { + import alternativa.math.Vector3; + + /** + * + */ + public class ContactPoint { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + + // Величины, расчитываемые перед началом фазы решения контактов + + // Требуемая проекция конечной скорости на нормаль для упругого контакта + public var normalVel:Number; + // Минимальная скорость разделения неупругого контакта + public var minSepVel:Number; + // Изменение проекции скорости на единицу нормального импульса + public var velByUnitImpulseN:Number; + + public var angularInertia1:Number; + public var angularInertia2:Number; + + // Радиус-вектор точки контакта относительно центра первого тела + public var r1:Vector3 = new Vector3(); + // Радиус-вектор точки контакта относительно центра второго тела + public var r2:Vector3 = new Vector3(); + + // Величины, накапливаемые во время фазы решения контактов + + // Накопленный импульс, применяемый для получения требуемой относительной скорости в точке контакта + public var accumImpulseN:Number; + // Накопленный импульс, применяемый для разделения тел в точке контакта. Не создаёт момента. +// public var accumSepImpulse:Number; + + public var satisfied:Boolean; + + /** + * + * @param cp + */ + public function copyFrom(cp:ContactPoint):void { + pos.copy(cp.pos); + penetration = cp.penetration; + feature1 = cp.feature1; + feature2 = cp.feature2; + r1.copy(cp.r1); + r2.copy(cp.r2); + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/PhysicsScene.as b/2.0.0.0/src/alternativa/physics/PhysicsScene.as new file mode 100644 index 0000000..d9f5db1 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/PhysicsScene.as @@ -0,0 +1,549 @@ +package alternativa.physics { + + import alternativa.physics.collision.ICollisionDetector; + import alternativa.physics.collision.KdTreeCollisionDetector; + import alternativa.physics.constraints.Constraint; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + /** + * Класс реализует физическую симуляцию поведения твёрдых тел. + */ + public class PhysicsScene { + + private static var lastBodyId:int; + + // Максимальное количество контактов + public const MAX_CONTACTS:int = 1000; + + ////////////////////////////////////////////////////////////////// + // Настроечные параметры симуляции. Могут быть изменены в любой + // момент времени без нарушения корректной работы симуляции. + ////////////////////////////////////////////////////////////////// + // Количество шагов, за которое пересекающиеся тела должны разделиться + public var penResolutionSteps:int = 10; + // Величина допустимой глубины пересечения + public var allowedPenetration:Number = 0.1; + // Максимальная скорость, добавляемая с целью разделения тел + public var maxPenResolutionSpeed:Number = 0.5; + // Количество итераций для обработки упругих контактов + public var collisionIterations:int = 5; + // Количество итераций для обработки неупругих контактов + public var contactIterations:int = 5; + // Флаг использования предсказания состояний + public var usePrediction:Boolean = false; + + public var freezeSteps:int = 10; + public var linSpeedFreezeLimit:Number = 1; + public var angSpeedFreezeLimit:Number = 0.01; + + // Вектор гравитации + public var _gravity:Vector3 = new Vector3(0, 0, -9.8); + // Модуль вектора гравитации + public var _gravityMagnitude:Number = 9.8; + + // Использующийся детектор столкновений + public var collisionDetector:ICollisionDetector; + + // Список тел, участвующих в симуляции + public var bodies:BodyList = new BodyList(); + + // Список контактов на текущем шаге симуляции + public var contacts:Contact; + // Список ограничений + public var constraints:Vector. = new Vector.(); + // Количество ограничений + public var constraintsNum:int; + // Временная метка. Число прошедших шагов с начала симуляции. + public var frame:int; + // Время с начала симуляции, мс + public var time:int; + // Первый неиспользованный контакт на текущем шаге симуляции + private var borderContact:Contact; + + // Временные переменные для избежания создания экземпляров + private var _t:Vector3 = new Vector3(); + private var _v:Vector3 = new Vector3(); + + /** + * + */ + public function PhysicsScene() { + contacts = new Contact(0); + var contact:Contact = contacts; + for (var i:int = 1; i < MAX_CONTACTS; i++) { + contact.next = new Contact(i); + contact = contact.next; + } + collisionDetector = new KdTreeCollisionDetector(); + } + + /** + * Вектор гравитации. + */ + public function get gravity():Vector3 { + return _gravity.clone(); + } + + /** + * @private + */ + public function set gravity(value:Vector3):void { + _gravity.copy(value); + _gravityMagnitude = _gravity.length(); + } + + /** + * Добавляет тело в симуляцию. + * + * @param body + */ + public function addBody(body:Body):void { + body.id = lastBodyId++; + body.world = this; + bodies.append(body); + } + + /** + * Удаляет тело из симуляции. + * @param body + */ + public function removeBody(body:Body):void { + if (bodies.remove(body)) { + body.world = null; + } + } + + /** + * Добавляет ограничение. + * @param c + */ + public function addConstraint(c:Constraint):void { + constraints[constraintsNum++] = c; + c.world = this; + } + + /** + * Удаляет ограничение. + * @param c + */ + public function removeConstraint(c:Constraint):Boolean { + var idx:int = constraints.indexOf(c); + if (idx < 0) return false; + constraints.splice(idx, 1); + constraintsNum--; + c.world = null; + return true; + } + + /** + * Применяет к телам действующте на них силы. Аккумуляторы сил и моментов тел очищаются после завершения шага + * симуляции, поэтому на момент вызова метода могут уже содержать некоторые значения. + * + * @param dt промежуток времени, в течении которого действуют силы + */ + private function applyForces(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + body.beforePhysicsStep(dt); + body.calcAccelerations(); + // Ускорение свободного падения применяется только к подвижным телам во избежание некорректного изменения + // фиктивной скорости неподвижных тел. + if (body.useGravity && body.movable && !body.frozen) { + body.accel.x += _gravity.x; + body.accel.y += _gravity.y; + body.accel.z += _gravity.z; + } + item = item.next; + } + } + + /** + * Определяет все столкновения на текущем шаге симуляции и заполняет список получившихся контактов. + * + * @param dt длительность шага симуляции + */ + private function detectCollisions(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + if (!body.frozen) { + body.contactsNum = 0; + body.saveState(); + // При включённом режиме предсказания состояние тел интегрируется на один шаг вперёд + if (usePrediction) { + body.integrateVelocity(dt); + body.integratePosition(dt); + } + body.calcDerivedData(); + } + item = item.next; + } + + borderContact = collisionDetector.getAllContacts(contacts); + + // Расчёт относительных векторов точки контакта вынесен сюда из-за необходимости учитывать + // положение тел в предсказанном состоянии + var contact:Contact = contacts; + while (contact != borderContact) { + var b1:Body = contact.body1; + var b2:Body = contact.body2; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + var bPos:Vector3 = b1.state.pos; + cp.r1.x = cp.pos.x - bPos.x; + cp.r1.y = cp.pos.y - bPos.y; + cp.r1.z = cp.pos.z - bPos.z; + if (b2 != null) { + bPos = b2.state.pos; + cp.r2.x = cp.pos.x - bPos.x; + cp.r2.y = cp.pos.y - bPos.y; + cp.r2.z = cp.pos.z - bPos.z; + } + } + contact = contact.next; + } + + // Восстановление состояния тел + if (usePrediction) { + item = bodies.head; + while (item != null) { + body = item.body; + if (!body.frozen) { + body.restoreState(); + body.calcDerivedData(); + } + item = item.next; + } + } + } + + /** + * Подготваливает полученные из детектора столкновений контакты, расчитывая значения, не меняющиеся + * в ходе шага симуляции. + */ + private function preProcessContacts(dt:Number):void { + var contact:Contact = contacts; + while (contact != borderContact) { + var b1:Body = contact.body1; + var b2:Body = contact.body2; + // Столкнувшиеся тела размораживаются + if (b1.frozen) { + b1.frozen = false; + b1.freezeCounter = 0; + } + if (b2 != null && b2.frozen) { + b2.frozen = false; + b2.freezeCounter = 0; + } + contact.restitution = b1.material.restitution; + if (b2 != null && b2.material.restitution < contact.restitution) contact.restitution = b2.material.restitution; + contact.friction = b1.material.friction; + if (b2 != null && b2.material.friction < contact.friction) contact.friction = b2.material.friction; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.accumImpulseN = 0; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + cp.velByUnitImpulseN = 0; + if (b1.movable) { + cp.angularInertia1 = _v.cross2(cp.r1, contact.normal).transformBy3(b1.invInertiaWorld).cross(cp.r1).dot(contact.normal); + cp.velByUnitImpulseN += b1.invMass + cp.angularInertia1; + } + if (b2 != null && b2.movable) { + cp.angularInertia2 = _v.cross2(cp.r2, contact.normal).transformBy3(b2.invInertiaWorld).cross(cp.r2).dot(contact.normal); + cp.velByUnitImpulseN += b2.invMass + cp.angularInertia2; + } + // Расчёт требуемой конечной скорости для упругого контакта + calcSepVelocity(b1, b2, cp, _v); + cp.normalVel = _v.dot(contact.normal); + if (cp.normalVel < 0) cp.normalVel = - contact.restitution*cp.normalVel; + // Скорость разделения неупругого контакта + cp.minSepVel = cp.penetration > allowedPenetration ? (cp.penetration - allowedPenetration)/(penResolutionSteps*dt) : 0; + if (cp.minSepVel > maxPenResolutionSpeed) cp.minSepVel = maxPenResolutionSpeed; + } + contact = contact.next; + } + for (var i:int = 0; i < constraintsNum; i++) { + var constraint:Constraint = constraints[i]; + constraint.preProcess(dt); + } + } + + /** + * + * @param dt + * @param forceInelastic + */ + private function processContacts(dt:Number, forceInelastic:Boolean):void { + var iterNum:int = forceInelastic ? contactIterations : collisionIterations; + var i:int; + var forwardLoop:Boolean = false; + for (var iter:int = 0; iter < iterNum; iter++) { + forwardLoop = !forwardLoop; + var contact:Contact = contacts; + while (contact != borderContact) { + resolveContact(contact, forceInelastic, forwardLoop); + contact = contact.next; + } + // Ограничения + for (i = 0; i < constraintsNum; i++) { + var constraint:Constraint = constraints[i]; + constraint.apply(dt); + } + } + } + + /** + * + * @param contactInfo + * @param forceInelastic + * @param forwardLoop + */ + private function resolveContact(contactInfo:Contact, forceInelastic:Boolean, forwardLoop:Boolean):void { + var b1:Body = contactInfo.body1; + var b2:Body = contactInfo.body2; + var normal:Vector3 = contactInfo.normal; + var i:int; + if (forwardLoop) { + for (i = 0; i < contactInfo.pcount; i++) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } else { + for (i = contactInfo.pcount - 1; i >= 0; i--) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } + } + + /** + * + * @param idx + * @param b1 + * @param b2 + * @param contact + * @param normal + * @param forceInelastic + */ + private function resolveContactPoint(idx:int, b1:Body, b2:Body, contact:Contact, normal:Vector3, forceInelastic:Boolean):void { + var cp:ContactPoint = contact.points[idx]; + if (!forceInelastic) { + cp.satisfied = true; + } + + var newVel:Number = 0; + calcSepVelocity(b1, b2, cp, _v); + var cnormal:Vector3 = contact.normal; + var sepVel:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + if (forceInelastic) { + var minSpeVel:Number = cp.minSepVel; + if (sepVel < minSpeVel) { + cp.satisfied = false; + } else if (cp.satisfied) return; + newVel = minSpeVel; + } else { + newVel = cp.normalVel; + } + var deltaVel:Number = newVel - sepVel; + var impulse:Number = deltaVel/cp.velByUnitImpulseN; + var accumImpulse:Number = cp.accumImpulseN + impulse; + if (accumImpulse < 0) { + accumImpulse = 0; + } + var deltaImpulse:Number = accumImpulse - cp.accumImpulseN; + cp.accumImpulseN = accumImpulse; + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, normal, deltaImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, normal, -deltaImpulse); + } + + // Учёт силы трения + calcSepVelocity(b1, b2, cp, _v); + // Расчитываем изменение касательной скорости на единицу касательного импульса + var tanSpeedByUnitImpulse:Number = 0; + +// _v.vAddScaled(-_v.vDot(contact.normal), contact.normal); + var dot:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + _v.x -= dot*cnormal.x; + _v.y -= dot*cnormal.y; + _v.z -= dot*cnormal.z; + + var tanSpeed:Number = _v.length(); + if (tanSpeed < 0.001) return; + +// _t.vCopy(_v).vNormalize().vReverse(); + _t.x = -_v.x; + _t.y = -_v.y; + _t.z = -_v.z; + _t.normalize(); + + var r:Vector3; + var m:Matrix3; + var xx:Number; + var yy:Number; + var zz:Number; + // dV = b.invMass + ((invI * (r % t)) % r) * t + if (b1.movable) { +// _v.vCross2(cp.r1, _t).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1); + r = cp.r1; + m = b1.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b1.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + if (b2 != null && b2.movable) { +// _v.vCross2(cp.r2, _t).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2); + + r = cp.r2; + m = b2.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b2.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + + var tanImpulse:Number = tanSpeed/tanSpeedByUnitImpulse; + var max:Number = contact.friction*cp.accumImpulseN; + if (max < 0) { + if (tanImpulse < max) tanImpulse = max; + } else { + if (tanImpulse > max) tanImpulse = max; + } + + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, _t, tanImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, _t, -tanImpulse); + } + } + + /** + * + * @param body1 + * @param body2 + * @param cp + * @param result + */ + private function calcSepVelocity(body1:Body, body2:Body, cp:ContactPoint, result:Vector3):void { + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 +// result.vCopy(body1.state.velocity).vAdd(_v1.vCross2(body1.state.rotation, cp.r1)); + var rot:Vector3 = body1.state.rotation; + var v:Vector3 = cp.r1; + var x:Number = rot.y*v.z - rot.z*v.y; + var y:Number = rot.z*v.x - rot.x*v.z; + var z:Number = rot.x*v.y - rot.y*v.x; + v = body1.state.velocity; + result.x = v.x + x; + result.y = v.y + y; + result.z = v.z + z; + // V2 = V2_c + w2%r2 + if (body2 != null) { +// result.vSubtract(body2.state.velocity).vSubtract(_v2.vCross2(body2.state.rotation, cp.r2)); + rot = body2.state.rotation; + v = cp.r2; + x = rot.y*v.z - rot.z*v.y; + y = rot.z*v.x - rot.x*v.z; + z = rot.x*v.y - rot.y*v.x; + v = body2.state.velocity; + result.x -= v.x + x; + result.y -= v.y + y; + result.z -= v.z + z; + } + } + + /** + * + * @param dt + */ + private function intergateVelocities(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + item.body.integrateVelocity(dt); + item = item.next; + } + } + + /** + * + * @param dt + */ + private function integratePositions(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + if (body.movable && !body.frozen) { + body.integratePosition(dt); + } + item = item.next; + } + } + + /** + * + */ + private function postPhysics():void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + body.clearAccumulators(); + body.calcDerivedData(); + if (body.canFreeze) { + if (body.state.velocity.length() < linSpeedFreezeLimit && body.state.rotation.length() < angSpeedFreezeLimit) { + if (!body.frozen) { + body.freezeCounter++; + if (body.freezeCounter >= freezeSteps) + body.frozen = true; + } + } else { + body.freezeCounter = 0; + body.frozen = false; + } + } + item = item.next; + } + } + + /** + * + * @param delta + */ + public function update(delta:int):void { + frame++; + time += delta; + var dt:Number = 0.001*delta; + applyForces(dt); + detectCollisions(dt); + preProcessContacts(dt); + processContacts(dt, false); + intergateVelocities(dt); + processContacts(dt, true); + integratePositions(dt); + postPhysics(); + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/PhysicsUtils.as b/2.0.0.0/src/alternativa/physics/PhysicsUtils.as new file mode 100644 index 0000000..5a2e46f --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/PhysicsUtils.as @@ -0,0 +1,49 @@ +package alternativa.physics { + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + public class PhysicsUtils { + + /** + * @param mass + * @param halfSize + * @param result + */ + public static function getBoxInvInertia(mass:Number, halfSize:Vector3, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + result.a = 3/(mass*(yy + zz)); + result.f = 3/(mass*(zz + xx)); + result.k = 3/(mass*(xx + yy)); + } + + /** + * @param mass + * @param r + * @param h + * @param result + */ + public static function getCylinderInvInertia(mass:Number, r:Number, h:Number, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + + result.a = result.f = 1/(mass*(h*h/12 + r*r/4)); + result.k = 2/(mass*r*r); + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/collision/.svn/all-wcprops b/2.0.0.0/src/alternativa/physics/collision/.svn/all-wcprops new file mode 100644 index 0000000..c670595 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/.svn/all-wcprops @@ -0,0 +1,65 @@ +K 25 +svn:wc:ra_dav:version-url +V 113 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/collision +END +ICollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 135 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/collision/ICollisionDetector.as +END +CollisionKdNode.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/collision/CollisionKdNode.as +END +CollisionKdTree2D.as +K 25 +svn:wc:ra_dav:version-url +V 134 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/collision/CollisionKdTree2D.as +END +IRayCollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 139 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/collision/IRayCollisionPredicate.as +END +IBodyCollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/collision/IBodyCollisionPredicate.as +END +CollisionPrimitive.as +K 25 +svn:wc:ra_dav:version-url +V 135 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/collision/CollisionPrimitive.as +END +KdTreeCollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/collision/KdTreeCollisionDetector.as +END +ICollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 136 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/collision/ICollisionPredicate.as +END +ICollider.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/collision/ICollider.as +END +CollisionKdTree.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/collision/CollisionKdTree.as +END diff --git a/2.0.0.0/src/alternativa/physics/collision/.svn/entries b/2.0.0.0/src/alternativa/physics/collision/.svn/entries new file mode 100644 index 0000000..f245a54 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/.svn/entries @@ -0,0 +1,157 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/collision +http://svndev.alternativaplatform.com + + + +2010-05-12T11:32:14.711790Z +34250 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +colliders +dir + +ICollisionDetector.as +file + + + + +2010-10-28T04:32:40.000000Z +60fce7ab667a5c17f27ea92335b56c3d +2010-04-01T08:37:11.976264Z +30874 +mike + +CollisionKdNode.as +file + + + + +2010-10-28T04:32:40.000000Z +a6fab491cd9bd45a5472bf4ffc321e08 +2010-02-12T12:34:25.192477Z +28424 +mike + +CollisionKdTree2D.as +file + + + + +2010-10-28T04:32:40.000000Z +82bb636d8b49cdfbc22694dd8b01e71d +2010-02-12T12:34:25.192477Z +28424 +mike + +IRayCollisionPredicate.as +file + + + + +2010-10-28T04:32:40.000000Z +32f9b1eaeacfcb40e7ae1927f52b8ab8 +2009-10-19T07:17:33.113306Z +22253 +mike + +types +dir + +IBodyCollisionPredicate.as +file + + + + +2010-10-28T04:32:40.000000Z +618f907a588979a8d06d4f4bcf6618a0 +2009-10-19T07:17:33.113306Z +22253 +mike + +CollisionPrimitive.as +file + + + + +2010-10-28T04:32:40.000000Z +cbd09b4defca3baf37421bd77f6bd223 +2010-04-01T08:37:11.976264Z +30874 +mike + +KdTreeCollisionDetector.as +file + + + + +2010-10-28T04:32:40.000000Z +9830dea81d6f25e16ccd8780d92f3c07 +2010-04-01T08:37:11.976264Z +30874 +mike + +ICollisionPredicate.as +file + + + + +2010-10-28T04:32:40.000000Z +65f3250d36556fffc457c740fb7d38b3 +2009-10-19T07:17:33.113306Z +22253 +mike + +ICollider.as +file + + + + +2010-10-28T04:32:40.000000Z +128a7d7479e13caeccecb45161b27b22 +2009-10-19T07:17:33.113306Z +22253 +mike + +primitives +dir + +CollisionKdTree.as +file + + + + +2010-10-28T04:32:40.000000Z +77d47ba10fe3bd9d1a003da13f295a36 +2010-04-01T08:37:11.976264Z +30874 +mike + diff --git a/2.0.0.0/src/alternativa/physics/collision/.svn/format b/2.0.0.0/src/alternativa/physics/collision/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/.svn/format @@ -0,0 +1 @@ +8 diff --git a/2.0.0.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base b/2.0.0.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base new file mode 100644 index 0000000..f450f1e --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base @@ -0,0 +1,18 @@ +package alternativa.physics.collision { + + + import alternativa.physics.collision.types.BoundBox; + + public class CollisionKdNode { + public var indices:Vector.; + public var splitIndices:Vector.; + public var boundBox:BoundBox; + public var parent:CollisionKdNode; + public var splitTree:CollisionKdTree2D; + + public var axis:int = -1; // 0 - x, 1 - y, 2 - z + public var coord:Number; + public var positiveNode:CollisionKdNode; + public var negativeNode:CollisionKdNode; + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base b/2.0.0.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base new file mode 100644 index 0000000..672563a --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base @@ -0,0 +1,264 @@ +package alternativa.physics.collision { + + + + import alternativa.physics.collision.types.BoundBox; + + /** + * @author mike + */ + public class CollisionKdTree { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + public var rootNode:CollisionKdNode; + public var staticChildren:Vector.; + public var numStaticChildren:int; + public var staticBoundBoxes:Vector. = new Vector.(); + + private var splitAxis:int; + private var splitCoord:Number; + private var splitCost:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + + /** + * @param boundBox + */ + public function createTree(collisionPrimitives:Vector., boundBox:BoundBox = null):void { + staticChildren = collisionPrimitives.concat(); + numStaticChildren = staticChildren.length; + // Создаём корневую ноду + rootNode = new CollisionKdNode(); + rootNode.indices = new Vector.(); + // Расчитываем баунды объектов и рутовой ноды + var rootNodeBoundBox:BoundBox = rootNode.boundBox = (boundBox != null) ? boundBox : new BoundBox(); + for (var i:int = 0; i < numStaticChildren; ++i) { + var child:CollisionPrimitive = staticChildren[i]; + var childBoundBox:BoundBox = staticBoundBoxes[i] = child.calculateAABB(); + rootNodeBoundBox.addBoundBox(childBoundBox); + rootNode.indices[i] = i; + } + staticBoundBoxes.length = numStaticChildren; + // Разделяем рутовую ноду + splitNode(rootNode); + + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + var indices:Vector. = node.indices; + var numPrimitives:int = indices.length; + if (numPrimitives <= minPrimitivesPerNode) return; + + // Подготовка баунда с погрешностями + var nodeBoundBox:BoundBox = node.boundBox; + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + // Собираем опорные координаты + var i:int; + var j:int; + var numSplitCoordsX:int = 0 + var numSplitCoordsY:int = 0; + var numSplitCoordsZ:int = 0; + for (i = 0; i < numPrimitives; ++i) { + var boundBox:BoundBox = staticBoundBoxes[indices[i]]; + + if (boundBox.maxX - boundBox.minX <= doubleThreshold) { + if (boundBox.minX <= nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.minX; + else if (boundBox.maxX >= nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.maxX; + else splitCoordsX[numSplitCoordsX++] = (boundBox.minX + boundBox.maxX)*0.5; + } else { + if (boundBox.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = boundBox.minX; + if (boundBox.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = boundBox.maxX; + } + + if (boundBox.maxY - boundBox.minY <= doubleThreshold) { + if (boundBox.minY <= nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.minY; + else if (boundBox.maxY >= nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.maxY; + else splitCoordsY[numSplitCoordsY++] = (boundBox.minY + boundBox.maxY)*0.5; + } else { + if (boundBox.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = boundBox.minY; + if (boundBox.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = boundBox.maxY; + } + + if (boundBox.maxZ - boundBox.minZ <= doubleThreshold) { + if (boundBox.minZ <= nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.minZ; + else if (boundBox.maxZ >= nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.maxZ; + else splitCoordsZ[numSplitCoordsZ++] = (boundBox.minZ + boundBox.maxZ)*0.5; + } else { + if (boundBox.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.minZ; + if (boundBox.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numPrimitives; ++i) { + boundBox = staticBoundBoxes[indices[i]]; + var min:Number = axisX ? boundBox.minX : (axisY ? boundBox.minY : boundBox.minZ); + var max:Number = axisX ? boundBox.maxX : (axisY ? boundBox.maxY : boundBox.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(indices[i]); + indices[i] = -1; + } else { + if (node.splitIndices == null) node.splitIndices = new Vector.(); + node.splitIndices.push(indices[i]); + indices[i] = -1; + } + } else { + if (min >= coordMin) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(indices[i]); + indices[i] = -1; + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numPrimitives; ++i) { + if (indices[i] >= 0) indices[j++] = indices[i]; + } + if (j > 0) indices.length = j; + else node.indices = null; + + if (node.splitIndices != null) { + node.splitTree = new CollisionKdTree2D(this, node); + node.splitTree.createTree(); + } + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { +// trace(maxCoord - minCoord, _bb[axis + 3] - _bb[axis]); + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict) { + if (cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + /** + * + */ + public function traceTree():void { + traceNode("", rootNode); + } + + /** + * @param str + * @param node + */ + private function traceNode(str:String, node:CollisionKdNode):void { + if (node == null) return; + trace(str, node.axis == -1 ? "end" : ((node.axis == 0) ? "X" : ((node.axis == 1) ? "Y" : "Z")), "splitCoord=" + splitCoord, "bound", node.boundBox, "objs:", node.indices); + traceNode(str + "-", node.negativeNode); + traceNode(str + "+", node.positiveNode); + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base b/2.0.0.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base new file mode 100644 index 0000000..2621517 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base @@ -0,0 +1,226 @@ +package alternativa.physics.collision { + + + import alternativa.physics.collision.types.BoundBox; + + /** + * + */ + public class CollisionKdTree2D { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + + public var parentTree:CollisionKdTree; + public var parentNode:CollisionKdNode; + public var rootNode:CollisionKdNode; + + private var splitAxis:int; + private var splitCost:Number; + private var splitCoord:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + /** + * + * @param parentTree + * @param parentNode + */ + public function CollisionKdTree2D(parentTree:CollisionKdTree, parentNode:CollisionKdNode) { + this.parentTree = parentTree; + this.parentNode = parentNode; + } + + /** + * + */ + public function createTree():void { + rootNode = new CollisionKdNode(); + rootNode.boundBox = parentNode.boundBox.clone(); + rootNode.indices = new Vector.(); + var numObjects:int = parentNode.splitIndices.length; + for (var i:int = 0; i < numObjects; ++i) rootNode.indices[i] = parentNode.splitIndices[i]; + + splitNode(rootNode); + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + if (node.indices.length <= minPrimitivesPerNode) return; + + var objects:Vector. = node.indices; + var i:int; + var j:int; + + var nodeBoundBox:BoundBox = node.boundBox; + + // Подготовка баунда с погрешностями + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + // Собираем опорные координаты + var numSplitCoordsX:int; + var numSplitCoordsY:int; + var numSplitCoordsZ:int; + var numObjects:int = objects.length; + for (i = 0; i < numObjects; ++i) { + var bb:BoundBox = staticBoundBoxes[objects[i]]; + + if (parentNode.axis != 0) { + if (bb.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = bb.minX; + if (bb.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = bb.maxX; + } + + if (parentNode.axis != 1) { + if (bb.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = bb.minY; + if (bb.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = bb.maxY; + } + + if (parentNode.axis != 2) { + if (bb.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = bb.minZ; + if (bb.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = bb.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + if (parentNode.axis != 0) checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + if (parentNode.axis != 1) checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + if (parentNode.axis != 2) checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numObjects; ++i) { + bb = staticBoundBoxes[objects[i]]; + var min:Number = axisX ? bb.minX : (axisY ? bb.minY : bb.minZ); + var max:Number = axisX ? bb.maxX : (axisY ? bb.maxY : bb.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(objects[i]); + objects[i] = -1; + } + } else { + if (min >= coordMin) { + if (max > coordMax) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(objects[i]); + objects[i] = -1; + } + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numObjects; ++i) { + if (objects[i] >= 0) objects[j++] = objects[i]; + } + if (j > 0) objects.length = j; + else node.indices = null; + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/collision/.svn/text-base/CollisionPrimitive.as.svn-base b/2.0.0.0/src/alternativa/physics/collision/.svn/text-base/CollisionPrimitive.as.svn-base new file mode 100644 index 0000000..8516aa4 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/.svn/text-base/CollisionPrimitive.as.svn-base @@ -0,0 +1,140 @@ +package alternativa.physics.collision { + + import alternativa.physics.Body; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * Базовый класс для примитивов, использующихся детектором столкновений. + */ + public class CollisionPrimitive { + // Константы типов примитива + public static const BOX:int = 1; + public static const PLANE:int = 2; + public static const SPHERE:int = 3; + public static const RECT:int = 4; + public static const TRIANGLE:int = 5; + + // Тип примитива + public var type:int; + // Группы примитива. Каждая группа определяется установленным битом. Столкновения проверяются только для примитивов, + // имеющих хотя бы одну общую группу. + public var collisionGroup:int; + // Предикат примитива, вызывающийся детектором при нахождении столкновения. В зависимости от возвращённого + // предикатом результата столкновение либо регистрируется, либо игнорируется. + // Nullable + public var postCollisionPredicate:ICollisionPredicate; + // Тело, владеющее примитивом. Поле сделано открытым для быстрого доступа на чтение. Установка значения должна выполняться вызовом метода setBody(). + // Nullable + public var body:Body; + // Трансформация примитива в системе координат тела, если оно указано. Не допускается масштабирование матрицы. + public var localTransform:Matrix4; + // Полная трансформация примитива. Не допускается масштабирование матрицы. + public var transform:Matrix4 = new Matrix4(); + // AABB в мировой системе координат. Расчитывается системой вызовом функции calculateBoundBox(). + public var aabb:BoundBox = new BoundBox(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param type тип примитива + * @param collisionGroup группа примитива + */ + public function CollisionPrimitive(type:int, collisionGroup:int) { + this.type = type; + this.collisionGroup = collisionGroup; + } + + /** + * Устанавливает тело, владеющее примитивом. + * + * @param body тело, которое владеет примитивом + * @param localTransform трансформация примитива в системе координат тела. Указание значения null равносильно + * заданию единичной матрицы, однако в первом случае не будет дополнительного умножения матриц при вычислении полной трансформации примитива. + */ + public function setBody(body:Body, localTransform:Matrix4 = null):void { + if (this.body == body) return; + this.body = body; + if (body != null) { + if (localTransform != null) { + if (this.localTransform == null) { + this.localTransform = new Matrix4(); + } + this.localTransform.copy(localTransform); + } else { + this.localTransform = null; + } + } + } + + /** + * Рассчитывает AABB примитива. Наследники должны переопределять этот метод, реализуя в нём корректный рассчёт. + * + * @return ссылка на свой AABB + */ + public function calculateAABB():BoundBox { + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + public function raycast(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + return -1; + } + + /** + * Клонирует примитив. Переопределять не рекомендуется. Вместо этого переопределяются методы createPrimitive() и copyFrom(). + * + * @return клон примитива + */ + public function clone():CollisionPrimitive { + var p:CollisionPrimitive = createPrimitive(); + return p.copyFrom(this); + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + if (source == null) { + throw new ArgumentError("Parameter source cannot be null"); + } + type = source.type; + transform.copy(source.transform); + collisionGroup = source.collisionGroup; + setBody(source.body, source.localTransform); + aabb.copyFrom(source.aabb); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + public function toString():String { + return "[CollisionPrimitive type=" + type + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + protected function createPrimitive():CollisionPrimitive { + return new CollisionPrimitive(type, collisionGroup); + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base b/2.0.0.0/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base new file mode 100644 index 0000000..4ace681 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.Body; + + public interface IBodyCollisionPredicate { + function considerBodies(body1:Body, body2:Body):Boolean; + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base b/2.0.0.0/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base new file mode 100644 index 0000000..e6aac2a --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base @@ -0,0 +1,29 @@ +package alternativa.physics.collision { + + import alternativa.physics.Contact; + + /** + * Интерфейс определителя столкновений между двумя примитивами. + */ + public interface ICollider { + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base b/2.0.0.0/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base new file mode 100644 index 0000000..53e70e7 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base @@ -0,0 +1,64 @@ +package alternativa.physics.collision { + import alternativa.physics.Contact; + import alternativa.physics.collision.types.RayHit; + import alternativa.math.Vector3; + + /** + * Интерфейс детектора столкновений. + */ + public interface ICollisionDetector { + + /** + * Получает все столкновения в текущей конфигурации физической геометрии. + * + * @param contacts список контактов, в кторые будет записана информация о столкновении + * @return количество найденных столкновений + */ + function getAllContacts(contacts:Contact):Contact; + + /** + * Тестирует луч на пересечение с физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param direction направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function raycast(origin:Vector3, direction:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayHit):Boolean; + + /** + * + * @param origin + * @param direction + * @param collisionGroup + * @param maxTime + * @param predicate + * @param result + * @return + */ + function raycastStatic(origin:Vector3, direction:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayHit):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @return + */ + function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base b/2.0.0.0/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base new file mode 100644 index 0000000..33d30ca --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base @@ -0,0 +1,8 @@ +package alternativa.physics.collision { + + public interface ICollisionPredicate { + + function considerCollision(primitive:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base b/2.0.0.0/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base new file mode 100644 index 0000000..b319f62 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.Body; + + public interface IRayCollisionPredicate { + function considerBody(body:Body):Boolean; + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base b/2.0.0.0/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base new file mode 100644 index 0000000..1c35356 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base @@ -0,0 +1,482 @@ +package alternativa.physics.collision { + + import alternativa.physics.Body; + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + 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.RayHit; + import alternativa.math.Vector3; + + /** + * Детектор, хранящий статическую геометрию в kD-дереве и использующий дерево для ускорения тестов на пересечения. + */ + public class KdTreeCollisionDetector implements ICollisionDetector { + + public var tree:CollisionKdTree; + public var dynamicPrimitives:Vector.; + public var dynamicPrimitivesNum:int; + public 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:RayHit = new RayHit(); + + /** + * + */ + public function KdTreeCollisionDetector() { + tree = new CollisionKdTree(); + dynamicPrimitives = new Vector.(); + + 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.):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 contacts; + } + + /** + * @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 raycast(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayHit):Boolean { + var hasStaticIntersection:Boolean = raycastStatic(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 raycastStatic(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayHit):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. = tree.staticChildren; +// var indices:Vector. = 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:RayHit):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.raycast(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:RayHit):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., indices:Vector., predicate:IRayCollisionPredicate, intersection:RayHit):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.raycast(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; + } \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/collision/CollisionKdNode.as b/2.0.0.0/src/alternativa/physics/collision/CollisionKdNode.as new file mode 100644 index 0000000..f450f1e --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/CollisionKdNode.as @@ -0,0 +1,18 @@ +package alternativa.physics.collision { + + + import alternativa.physics.collision.types.BoundBox; + + public class CollisionKdNode { + public var indices:Vector.; + public var splitIndices:Vector.; + public var boundBox:BoundBox; + public var parent:CollisionKdNode; + public var splitTree:CollisionKdTree2D; + + public var axis:int = -1; // 0 - x, 1 - y, 2 - z + public var coord:Number; + public var positiveNode:CollisionKdNode; + public var negativeNode:CollisionKdNode; + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/collision/CollisionKdTree.as b/2.0.0.0/src/alternativa/physics/collision/CollisionKdTree.as new file mode 100644 index 0000000..672563a --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/CollisionKdTree.as @@ -0,0 +1,264 @@ +package alternativa.physics.collision { + + + + import alternativa.physics.collision.types.BoundBox; + + /** + * @author mike + */ + public class CollisionKdTree { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + public var rootNode:CollisionKdNode; + public var staticChildren:Vector.; + public var numStaticChildren:int; + public var staticBoundBoxes:Vector. = new Vector.(); + + private var splitAxis:int; + private var splitCoord:Number; + private var splitCost:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + + /** + * @param boundBox + */ + public function createTree(collisionPrimitives:Vector., boundBox:BoundBox = null):void { + staticChildren = collisionPrimitives.concat(); + numStaticChildren = staticChildren.length; + // Создаём корневую ноду + rootNode = new CollisionKdNode(); + rootNode.indices = new Vector.(); + // Расчитываем баунды объектов и рутовой ноды + var rootNodeBoundBox:BoundBox = rootNode.boundBox = (boundBox != null) ? boundBox : new BoundBox(); + for (var i:int = 0; i < numStaticChildren; ++i) { + var child:CollisionPrimitive = staticChildren[i]; + var childBoundBox:BoundBox = staticBoundBoxes[i] = child.calculateAABB(); + rootNodeBoundBox.addBoundBox(childBoundBox); + rootNode.indices[i] = i; + } + staticBoundBoxes.length = numStaticChildren; + // Разделяем рутовую ноду + splitNode(rootNode); + + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + var indices:Vector. = node.indices; + var numPrimitives:int = indices.length; + if (numPrimitives <= minPrimitivesPerNode) return; + + // Подготовка баунда с погрешностями + var nodeBoundBox:BoundBox = node.boundBox; + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + // Собираем опорные координаты + var i:int; + var j:int; + var numSplitCoordsX:int = 0 + var numSplitCoordsY:int = 0; + var numSplitCoordsZ:int = 0; + for (i = 0; i < numPrimitives; ++i) { + var boundBox:BoundBox = staticBoundBoxes[indices[i]]; + + if (boundBox.maxX - boundBox.minX <= doubleThreshold) { + if (boundBox.minX <= nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.minX; + else if (boundBox.maxX >= nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.maxX; + else splitCoordsX[numSplitCoordsX++] = (boundBox.minX + boundBox.maxX)*0.5; + } else { + if (boundBox.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = boundBox.minX; + if (boundBox.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = boundBox.maxX; + } + + if (boundBox.maxY - boundBox.minY <= doubleThreshold) { + if (boundBox.minY <= nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.minY; + else if (boundBox.maxY >= nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.maxY; + else splitCoordsY[numSplitCoordsY++] = (boundBox.minY + boundBox.maxY)*0.5; + } else { + if (boundBox.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = boundBox.minY; + if (boundBox.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = boundBox.maxY; + } + + if (boundBox.maxZ - boundBox.minZ <= doubleThreshold) { + if (boundBox.minZ <= nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.minZ; + else if (boundBox.maxZ >= nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.maxZ; + else splitCoordsZ[numSplitCoordsZ++] = (boundBox.minZ + boundBox.maxZ)*0.5; + } else { + if (boundBox.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.minZ; + if (boundBox.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numPrimitives; ++i) { + boundBox = staticBoundBoxes[indices[i]]; + var min:Number = axisX ? boundBox.minX : (axisY ? boundBox.minY : boundBox.minZ); + var max:Number = axisX ? boundBox.maxX : (axisY ? boundBox.maxY : boundBox.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(indices[i]); + indices[i] = -1; + } else { + if (node.splitIndices == null) node.splitIndices = new Vector.(); + node.splitIndices.push(indices[i]); + indices[i] = -1; + } + } else { + if (min >= coordMin) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(indices[i]); + indices[i] = -1; + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numPrimitives; ++i) { + if (indices[i] >= 0) indices[j++] = indices[i]; + } + if (j > 0) indices.length = j; + else node.indices = null; + + if (node.splitIndices != null) { + node.splitTree = new CollisionKdTree2D(this, node); + node.splitTree.createTree(); + } + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { +// trace(maxCoord - minCoord, _bb[axis + 3] - _bb[axis]); + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict) { + if (cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + /** + * + */ + public function traceTree():void { + traceNode("", rootNode); + } + + /** + * @param str + * @param node + */ + private function traceNode(str:String, node:CollisionKdNode):void { + if (node == null) return; + trace(str, node.axis == -1 ? "end" : ((node.axis == 0) ? "X" : ((node.axis == 1) ? "Y" : "Z")), "splitCoord=" + splitCoord, "bound", node.boundBox, "objs:", node.indices); + traceNode(str + "-", node.negativeNode); + traceNode(str + "+", node.positiveNode); + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/collision/CollisionKdTree2D.as b/2.0.0.0/src/alternativa/physics/collision/CollisionKdTree2D.as new file mode 100644 index 0000000..2621517 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/CollisionKdTree2D.as @@ -0,0 +1,226 @@ +package alternativa.physics.collision { + + + import alternativa.physics.collision.types.BoundBox; + + /** + * + */ + public class CollisionKdTree2D { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + + public var parentTree:CollisionKdTree; + public var parentNode:CollisionKdNode; + public var rootNode:CollisionKdNode; + + private var splitAxis:int; + private var splitCost:Number; + private var splitCoord:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + /** + * + * @param parentTree + * @param parentNode + */ + public function CollisionKdTree2D(parentTree:CollisionKdTree, parentNode:CollisionKdNode) { + this.parentTree = parentTree; + this.parentNode = parentNode; + } + + /** + * + */ + public function createTree():void { + rootNode = new CollisionKdNode(); + rootNode.boundBox = parentNode.boundBox.clone(); + rootNode.indices = new Vector.(); + var numObjects:int = parentNode.splitIndices.length; + for (var i:int = 0; i < numObjects; ++i) rootNode.indices[i] = parentNode.splitIndices[i]; + + splitNode(rootNode); + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + if (node.indices.length <= minPrimitivesPerNode) return; + + var objects:Vector. = node.indices; + var i:int; + var j:int; + + var nodeBoundBox:BoundBox = node.boundBox; + + // Подготовка баунда с погрешностями + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + // Собираем опорные координаты + var numSplitCoordsX:int; + var numSplitCoordsY:int; + var numSplitCoordsZ:int; + var numObjects:int = objects.length; + for (i = 0; i < numObjects; ++i) { + var bb:BoundBox = staticBoundBoxes[objects[i]]; + + if (parentNode.axis != 0) { + if (bb.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = bb.minX; + if (bb.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = bb.maxX; + } + + if (parentNode.axis != 1) { + if (bb.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = bb.minY; + if (bb.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = bb.maxY; + } + + if (parentNode.axis != 2) { + if (bb.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = bb.minZ; + if (bb.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = bb.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + if (parentNode.axis != 0) checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + if (parentNode.axis != 1) checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + if (parentNode.axis != 2) checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numObjects; ++i) { + bb = staticBoundBoxes[objects[i]]; + var min:Number = axisX ? bb.minX : (axisY ? bb.minY : bb.minZ); + var max:Number = axisX ? bb.maxX : (axisY ? bb.maxY : bb.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(objects[i]); + objects[i] = -1; + } + } else { + if (min >= coordMin) { + if (max > coordMax) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(objects[i]); + objects[i] = -1; + } + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numObjects; ++i) { + if (objects[i] >= 0) objects[j++] = objects[i]; + } + if (j > 0) objects.length = j; + else node.indices = null; + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/collision/CollisionPrimitive.as b/2.0.0.0/src/alternativa/physics/collision/CollisionPrimitive.as new file mode 100644 index 0000000..8516aa4 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/CollisionPrimitive.as @@ -0,0 +1,140 @@ +package alternativa.physics.collision { + + import alternativa.physics.Body; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * Базовый класс для примитивов, использующихся детектором столкновений. + */ + public class CollisionPrimitive { + // Константы типов примитива + public static const BOX:int = 1; + public static const PLANE:int = 2; + public static const SPHERE:int = 3; + public static const RECT:int = 4; + public static const TRIANGLE:int = 5; + + // Тип примитива + public var type:int; + // Группы примитива. Каждая группа определяется установленным битом. Столкновения проверяются только для примитивов, + // имеющих хотя бы одну общую группу. + public var collisionGroup:int; + // Предикат примитива, вызывающийся детектором при нахождении столкновения. В зависимости от возвращённого + // предикатом результата столкновение либо регистрируется, либо игнорируется. + // Nullable + public var postCollisionPredicate:ICollisionPredicate; + // Тело, владеющее примитивом. Поле сделано открытым для быстрого доступа на чтение. Установка значения должна выполняться вызовом метода setBody(). + // Nullable + public var body:Body; + // Трансформация примитива в системе координат тела, если оно указано. Не допускается масштабирование матрицы. + public var localTransform:Matrix4; + // Полная трансформация примитива. Не допускается масштабирование матрицы. + public var transform:Matrix4 = new Matrix4(); + // AABB в мировой системе координат. Расчитывается системой вызовом функции calculateBoundBox(). + public var aabb:BoundBox = new BoundBox(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param type тип примитива + * @param collisionGroup группа примитива + */ + public function CollisionPrimitive(type:int, collisionGroup:int) { + this.type = type; + this.collisionGroup = collisionGroup; + } + + /** + * Устанавливает тело, владеющее примитивом. + * + * @param body тело, которое владеет примитивом + * @param localTransform трансформация примитива в системе координат тела. Указание значения null равносильно + * заданию единичной матрицы, однако в первом случае не будет дополнительного умножения матриц при вычислении полной трансформации примитива. + */ + public function setBody(body:Body, localTransform:Matrix4 = null):void { + if (this.body == body) return; + this.body = body; + if (body != null) { + if (localTransform != null) { + if (this.localTransform == null) { + this.localTransform = new Matrix4(); + } + this.localTransform.copy(localTransform); + } else { + this.localTransform = null; + } + } + } + + /** + * Рассчитывает AABB примитива. Наследники должны переопределять этот метод, реализуя в нём корректный рассчёт. + * + * @return ссылка на свой AABB + */ + public function calculateAABB():BoundBox { + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + public function raycast(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + return -1; + } + + /** + * Клонирует примитив. Переопределять не рекомендуется. Вместо этого переопределяются методы createPrimitive() и copyFrom(). + * + * @return клон примитива + */ + public function clone():CollisionPrimitive { + var p:CollisionPrimitive = createPrimitive(); + return p.copyFrom(this); + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + if (source == null) { + throw new ArgumentError("Parameter source cannot be null"); + } + type = source.type; + transform.copy(source.transform); + collisionGroup = source.collisionGroup; + setBody(source.body, source.localTransform); + aabb.copyFrom(source.aabb); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + public function toString():String { + return "[CollisionPrimitive type=" + type + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + protected function createPrimitive():CollisionPrimitive { + return new CollisionPrimitive(type, collisionGroup); + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/collision/IBodyCollisionPredicate.as b/2.0.0.0/src/alternativa/physics/collision/IBodyCollisionPredicate.as new file mode 100644 index 0000000..4ace681 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/IBodyCollisionPredicate.as @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.Body; + + public interface IBodyCollisionPredicate { + function considerBodies(body1:Body, body2:Body):Boolean; + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/collision/ICollider.as b/2.0.0.0/src/alternativa/physics/collision/ICollider.as new file mode 100644 index 0000000..e6aac2a --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/ICollider.as @@ -0,0 +1,29 @@ +package alternativa.physics.collision { + + import alternativa.physics.Contact; + + /** + * Интерфейс определителя столкновений между двумя примитивами. + */ + public interface ICollider { + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/collision/ICollisionDetector.as b/2.0.0.0/src/alternativa/physics/collision/ICollisionDetector.as new file mode 100644 index 0000000..53e70e7 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/ICollisionDetector.as @@ -0,0 +1,64 @@ +package alternativa.physics.collision { + import alternativa.physics.Contact; + import alternativa.physics.collision.types.RayHit; + import alternativa.math.Vector3; + + /** + * Интерфейс детектора столкновений. + */ + public interface ICollisionDetector { + + /** + * Получает все столкновения в текущей конфигурации физической геометрии. + * + * @param contacts список контактов, в кторые будет записана информация о столкновении + * @return количество найденных столкновений + */ + function getAllContacts(contacts:Contact):Contact; + + /** + * Тестирует луч на пересечение с физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param direction направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function raycast(origin:Vector3, direction:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayHit):Boolean; + + /** + * + * @param origin + * @param direction + * @param collisionGroup + * @param maxTime + * @param predicate + * @param result + * @return + */ + function raycastStatic(origin:Vector3, direction:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayHit):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @return + */ + function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/collision/ICollisionPredicate.as b/2.0.0.0/src/alternativa/physics/collision/ICollisionPredicate.as new file mode 100644 index 0000000..33d30ca --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/ICollisionPredicate.as @@ -0,0 +1,8 @@ +package alternativa.physics.collision { + + public interface ICollisionPredicate { + + function considerCollision(primitive:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/collision/IRayCollisionPredicate.as b/2.0.0.0/src/alternativa/physics/collision/IRayCollisionPredicate.as new file mode 100644 index 0000000..b319f62 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/IRayCollisionPredicate.as @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.Body; + + public interface IRayCollisionPredicate { + function considerBody(body:Body):Boolean; + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/collision/KdTreeCollisionDetector.as b/2.0.0.0/src/alternativa/physics/collision/KdTreeCollisionDetector.as new file mode 100644 index 0000000..1c35356 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/KdTreeCollisionDetector.as @@ -0,0 +1,482 @@ +package alternativa.physics.collision { + + import alternativa.physics.Body; + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + 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.RayHit; + import alternativa.math.Vector3; + + /** + * Детектор, хранящий статическую геометрию в kD-дереве и использующий дерево для ускорения тестов на пересечения. + */ + public class KdTreeCollisionDetector implements ICollisionDetector { + + public var tree:CollisionKdTree; + public var dynamicPrimitives:Vector.; + public var dynamicPrimitivesNum:int; + public 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:RayHit = new RayHit(); + + /** + * + */ + public function KdTreeCollisionDetector() { + tree = new CollisionKdTree(); + dynamicPrimitives = new Vector.(); + + 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.):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 contacts; + } + + /** + * @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 raycast(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayHit):Boolean { + var hasStaticIntersection:Boolean = raycastStatic(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 raycastStatic(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayHit):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. = tree.staticChildren; +// var indices:Vector. = 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:RayHit):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.raycast(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:RayHit):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., indices:Vector., predicate:IRayCollisionPredicate, intersection:RayHit):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.raycast(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; + } \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/collision/colliders/.svn/all-wcprops b/2.0.0.0/src/alternativa/physics/collision/colliders/.svn/all-wcprops new file mode 100644 index 0000000..a8d6c08 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/colliders/.svn/all-wcprops @@ -0,0 +1,53 @@ +K 25 +svn:wc:ra_dav:version-url +V 123 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/collision/colliders +END +BoxSphereCollider.as +K 25 +svn:wc:ra_dav:version-url +V 144 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/collision/colliders/BoxSphereCollider.as +END +BoxBoxCollider.as +K 25 +svn:wc:ra_dav:version-url +V 141 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/collision/colliders/BoxBoxCollider.as +END +BoxRectCollider.as +K 25 +svn:wc:ra_dav:version-url +V 142 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/collision/colliders/BoxRectCollider.as +END +SpherePlaneCollider.as +K 25 +svn:wc:ra_dav:version-url +V 146 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/collision/colliders/SpherePlaneCollider.as +END +BoxCollider.as +K 25 +svn:wc:ra_dav:version-url +V 138 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/collision/colliders/BoxCollider.as +END +BoxPlaneCollider.as +K 25 +svn:wc:ra_dav:version-url +V 143 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/collision/colliders/BoxPlaneCollider.as +END +SphereSphereCollider.as +K 25 +svn:wc:ra_dav:version-url +V 147 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/collision/colliders/SphereSphereCollider.as +END +BoxTriangleCollider.as +K 25 +svn:wc:ra_dav:version-url +V 146 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/collision/colliders/BoxTriangleCollider.as +END diff --git a/2.0.0.0/src/alternativa/physics/collision/colliders/.svn/entries b/2.0.0.0/src/alternativa/physics/collision/colliders/.svn/entries new file mode 100644 index 0000000..5b0a203 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/colliders/.svn/entries @@ -0,0 +1,124 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/collision/colliders +http://svndev.alternativaplatform.com + + + +2010-04-01T08:37:11.976264Z +30874 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +BoxSphereCollider.as +file + + + + +2010-10-28T04:32:40.000000Z +e1094a2f905e1fa4b47e4f4880c8ea58 +2010-04-01T08:37:11.976264Z +30874 +mike + +BoxBoxCollider.as +file + + + + +2010-10-28T04:32:40.000000Z +f90064dec5c9dcb1f546a78bb1df479b +2010-04-01T08:37:11.976264Z +30874 +mike + +BoxRectCollider.as +file + + + + +2010-10-28T04:32:40.000000Z +bd2ea5aaf170e02ad31a1728db6b9fa9 +2010-04-01T08:37:11.976264Z +30874 +mike + +SpherePlaneCollider.as +file + + + + +2010-10-28T04:32:40.000000Z +67621ed47d7e90b72498acf2802c4e2d +2009-11-24T08:44:15.941480Z +23758 +mike + +BoxCollider.as +file + + + + +2010-10-28T04:32:40.000000Z +52e72ed30a24a3a43ce16a19930546b5 +2010-02-12T12:34:25.192477Z +28424 +mike + +BoxPlaneCollider.as +file + + + + +2010-10-28T04:32:40.000000Z +204c7b02a9a1913269c558658bad4262 +2010-02-12T12:34:25.192477Z +28424 +mike + +SphereSphereCollider.as +file + + + + +2010-10-28T04:32:40.000000Z +dba9cde04a5c73c74f8096ee3e86873b +2010-04-01T08:37:11.976264Z +30874 +mike + +BoxTriangleCollider.as +file + + + + +2010-10-28T04:32:40.000000Z +448502271cac4f09491ca128b4bb485a +2010-03-09T04:55:13.477965Z +29440 +mike + diff --git a/2.0.0.0/src/alternativa/physics/collision/colliders/.svn/format b/2.0.0.0/src/alternativa/physics/collision/colliders/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/colliders/.svn/format @@ -0,0 +1 @@ +8 diff --git a/2.0.0.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxBoxCollider.as.svn-base b/2.0.0.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxBoxCollider.as.svn-base new file mode 100644 index 0000000..789c21a --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxBoxCollider.as.svn-base @@ -0,0 +1,585 @@ +package alternativa.physics.collision.colliders { + + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * Расчитывает точки контакта двух боксов. Нормаль контакта направляется в сторону бокса с меньшим ID. + */ + public class BoxBoxCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + private var vectorToBox1:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var bestAxisIndex:int; + private var minOverlap:Number; + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + private var tmpPoints:Vector. = new Vector.(8, true); + private var pcount:int; + + /** + * + */ + public function BoxBoxCollider() { + for (var i:int = 0; i < 8; i++) { + tmpPoints[i] = new ContactPoint(); + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + + if (bestAxisIndex < 6) { + // Контакт грань-(грань|ребро|вершина) + if (!findFaceContactPoints(box1, box2, vectorToBox1, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 6; + findEdgesIntersection(box1, box2, vectorToBox1, int(bestAxisIndex/3), bestAxisIndex%3, contact); + } + contact.body1 = box1.body; + contact.body2 = box2.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + // Вектор из центра второго бокса в центр первого + vectorToBox1.x = transform1.d - transform2.d; + vectorToBox1.y = transform1.h - transform2.h; + vectorToBox1.z = transform1.l - transform2.l; + + // Проверка пересечения по основным осям + axis10.x = transform1.a; + axis10.y = transform1.e; + axis10.z = transform1.i; + if (!testMainAxis(box1, box2, axis10, 0, vectorToBox1)) return false; + axis11.x = transform1.b; + axis11.y = transform1.f; + axis11.z = transform1.j; + if (!testMainAxis(box1, box2, axis11, 1, vectorToBox1)) return false; + axis12.x = transform1.c; + axis12.y = transform1.g; + axis12.z = transform1.k; + if (!testMainAxis(box1, box2, axis12, 2, vectorToBox1)) return false; + + axis20.x = transform2.a; + axis20.y = transform2.e; + axis20.z = transform2.i; + if (!testMainAxis(box1, box2, axis20, 3, vectorToBox1)) return false; + axis21.x = transform2.b; + axis21.y = transform2.f; + axis21.z = transform2.j; + if (!testMainAxis(box1, box2, axis21, 4, vectorToBox1)) return false; + axis22.x = transform2.c; + axis22.y = transform2.g; + axis22.z = transform2.k; + if (!testMainAxis(box1, box2, axis22, 5, vectorToBox1)) return false; + + // Проверка производных осей + if (!testDerivedAxis(box1, box2, axis10, axis20, 6, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis21, 7, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis22, 8, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis11, axis20, 9, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis21, 10, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis22, 11, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis12, axis20, 12, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis21, 13, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis22, 14, vectorToBox1)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта грани одного бокса с гранью/ребром/вершиной другого. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param faceAxisIdx индекс оси первого бокса, перпендикулярной грани, с которой произошло столкновение + * @param contactInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var swapNormal:Boolean = false; + if (faceAxisIdx > 2) { + // Столкновение с гранью второго бокса. Для дальнейших расчётов боксы меняются местами, + // но нормаль контакта всё равно должна быть направлена в сторону первоначального box1 + var tmpBox:CollisionBox = box1; + box1 = box2; + box2 = tmpBox; + vectorToBox1.x = -vectorToBox1.x; + vectorToBox1.y = -vectorToBox1.y; + vectorToBox1.z = -vectorToBox1.z; + faceAxisIdx -= 3; + swapNormal = true; + } + + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + var colAxis:Vector3 = contact.normal; + + transform1.getAxis(faceAxisIdx, colAxis); + var negativeFace:Boolean = colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z > 0; + + var code:int = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box1.excludedFaces) != 0) return false; + + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + // Ищем ось второго бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + transform2.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisDot = dot; + incidentAxisIdx = axisIdx; + } + } + // Получаем список вершин грани второго бокса, переводим их в систему координат первого бокса и выполняем обрезку + // по грани первого бокса. Таким образом получается список потенциальных точек контакта. + transform2.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box2.hs, incidentAxisIdx, incidentAxisDot < 0, points1); + + // TODO: Вычислить результирующую матрицу, затем преобразовать векторы за один заход + transform2.transformVectorsN(points1, points2, 4); + transform1.transformVectorsInverseN(points2, points1, 4); + + var pnum:int = clip(box1.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points1[i]; + if ((pen = getPointBoxPenetration(box1.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + var cp:ContactPoint = tmpPoints[pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = transform1.a*v.x + transform1.b*v.y + transform1.c*v.z + transform1.d; + cpPos.y = transform1.e*v.x + transform1.f*v.y + transform1.g*v.z + transform1.h; + cpPos.z = transform1.i*v.x + transform1.j*v.y + transform1.k*v.z + transform1.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + + cp.penetration = pen; + } + } + if (swapNormal) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (pcount > 4) { + reducePoints(); + } + for (i = 0; i < pcount; i++) { + cp = contact.points[i]; + cp.copyFrom(tmpPoints[i]); + } + contact.pcount = pcount; + return true; + } + + /** + * + * @param contactInfo + */ + private function reducePoints():void { + var i:int; + var minIdx:int; + var cp1:ContactPoint; + var cp2:ContactPoint; + while (pcount > 4) { + var minLen:Number = 1e10; + var p1:Vector3 = (tmpPoints[int(pcount - 1)] as ContactPoint).pos; + var p2:Vector3; + for (i = 0; i < pcount; i++) { + p2 = (tmpPoints[i] as ContactPoint).pos; + var dx:Number = p2.x - p1.x; + var dy:Number = p2.y - p1.y; + var dz:Number = p2.z - p1.z; + var len:Number = dx*dx + dy*dy + dz*dz; + if (len < minLen) { + minLen = len; + minIdx = i; + } + p1 = p2; + } + var ii:int = minIdx == 0 ? (pcount - 1) : (minIdx - 1); + cp1 = tmpPoints[ii]; + cp2 = tmpPoints[minIdx]; + p1 = cp1.pos; + p2 = cp2.pos; + p2.x = 0.5*(p1.x + p2.x); + p2.y = 0.5*(p1.y + p2.y); + p2.z = 0.5*(p1.z + p2.z); + cp2.penetration = 0.5*(cp1.penetration + cp2.penetration); + if (minIdx > 0) { + for (i = minIdx; i < pcount; i++) { + tmpPoints[int(i - 1)] = tmpPoints[i]; + } + tmpPoints[int(pcount - 1)] = cp1; + } + pcount--; + } + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clip(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param contactInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):void { + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + transform1.getAxis(axisIdx1, axis10); + transform2.getAxis(axisIdx2, axis20); + + var colAxis:Vector3 = contact.normal; + + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону первого бокса + if (colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + + var tx:Number = box1.hs.x; + var ty:Number = box1.hs.y; + var tz:Number = box1.hs.z; + + var x2:Number = box2.hs.x; + var y2:Number = box2.hs.y; + var z2:Number = box2.hs.z; + // x + if (axisIdx1 == 0) { + tx = 0; + halfLen1 = box1.hs.x; + } else { + if (colAxis.x*transform1.a + colAxis.y*transform1.e + colAxis.z*transform1.i > 0) { + tx = -tx; + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = box2.hs.x; + } else { + if (colAxis.x*transform2.a + colAxis.y*transform2.e + colAxis.z*transform2.i < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + ty = 0; + halfLen1 = box1.hs.y; + } else { + if (colAxis.x*transform1.b + colAxis.y*transform1.f + colAxis.z*transform1.j > 0) { + ty = -ty; + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = box2.hs.y; + } else { + if (colAxis.x*transform2.b + colAxis.y*transform2.f + colAxis.z*transform2.j < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + tz = 0; + halfLen1 = box1.hs.z; + } else { + if (colAxis.x*transform1.c + colAxis.y*transform1.g + colAxis.z*transform1.k > 0) { + tz = -tz; + } + } + if (axisIdx2 == 2) { + z2 = 0; + halfLen2 = box2.hs.z; + } else { + if (colAxis.x*transform2.c + colAxis.y*transform2.g + colAxis.z*transform2.k < 0) { + z2 = -z2; + } + } + // Получаем глобальные координаты средних точек рёбер + var x1:Number = transform1.a*tx + transform1.b*ty + transform1.c*tz + transform1.d; + var y1:Number = transform1.e*tx + transform1.f*ty + transform1.g*tz + transform1.h; + var z1:Number = transform1.i*tx + transform1.j*ty + transform1.k*tz + transform1.l; + tx = x2; + ty = y2; + tz = z2; + x2 = transform2.a*tx + transform2.b*ty + transform2.c*tz + transform2.d; + y2 = transform2.e*tx + transform2.f*ty + transform2.g*tz + transform2.h; + z2 = transform2.i*tx + transform2.j*ty + transform2.k*tz + transform2.l; + + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + tx = x2 - x1; + ty = y2 - y1; + tz = z2 - z1; + var c1:Number = axis10.x*tx + axis10.y*ty + axis10.z*tz; + var c2:Number = axis20.x*tx + axis20.y*ty + axis20.z*tz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cp.pos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cp.pos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение боксов вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box1 + * @param box2 + * @param axis + * @param axisIndex + * @param toBox1 + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box1 + * @param box2 + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox1 + * @return + */ + private function testDerivedAxis(box1:CollisionBox, box2:CollisionBox, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия двух боксов вдоль заданной оси. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param axis ось + * @param vectorToBox1 вектор, соединяющий центр второго бокса с центром первого + * @return величина перекрытия боксов вдоль оси + */ + public function overlapOnAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, vectorToBox1:Vector3):Number { + var m:Matrix4 = box1.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box1.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box1.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box1.hs.z; + if (d < 0) d = -d; + projection += d; + + m = box2.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box2.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box2.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box2.hs.z; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox1.x*axis.x + vectorToBox1.y*axis.y + vectorToBox1.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} + import alternativa.math.Vector3; + + class CollisionPointTmp { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + } diff --git a/2.0.0.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxCollider.as.svn-base b/2.0.0.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxCollider.as.svn-base new file mode 100644 index 0000000..2429bb1 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxCollider.as.svn-base @@ -0,0 +1,427 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + /** + * + */ + public class BoxCollider implements ICollider { + + /** + * + */ + public function BoxCollider() { + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + return false; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + protected function getFaceVertsByAxis(hs:Vector3, axisIdx:int, negativeFace:Boolean, result:Vector.):void { + var v:Vector3; + switch (axisIdx) { + case 0: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + } + break; + case 1: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + } + break; + case 2: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + } else { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } + break; + } + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxPlaneCollider.as.svn-base b/2.0.0.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxPlaneCollider.as.svn-base new file mode 100644 index 0000000..3bebdc3 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxPlaneCollider.as.svn-base @@ -0,0 +1,88 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + public class BoxPlaneCollider implements ICollider { + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var normal:Vector3 = new Vector3(); + + public function BoxPlaneCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var box:RigidBox = body1 as RigidBox; +// var plane:RigidPlane; +// if (box == null) { +// box = body2 as RigidBox; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные координаты вершин бокса +// var sx:Number = box.halfSize.x; +// var sy:Number = box.halfSize.y; +// var sz:Number = box.halfSize.z; +// (verts1[0] as Vector3).reset(-sx, -sy, -sz); +// (verts1[1] as Vector3).reset(sx, -sy, -sz); +// (verts1[2] as Vector3).reset(sx, sy, -sz); +// (verts1[3] as Vector3).reset(-sx, sy, -sz); +// (verts1[4] as Vector3).reset(-sx, -sy, sz); +// (verts1[5] as Vector3).reset(sx, -sy, sz); +// (verts1[6] as Vector3).reset(sx, sy, sz); +// (verts1[7] as Vector3).reset(-sx, sy, sz); +// +// box.transform.transformVectors(verts1, verts2); +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// // Проверяем наличие столкновений с каждой вершиной +// collisionInfo.pcount = 0; +// for (var i:int = 0; i < 8; i++) { +// // Вершина добавляется в список точек столкновения, если лежит под плоскостью +// var dist:Number = (verts2[i] as Vector3).dot(normal); +// if (dist < offset) { +// var cp:ContactPoint; +// if (collisionInfo.pcount == collisionInfo.points.length) { +// cp = new ContactPoint(); +// collisionInfo.points[collisionInfo.pcount] = cp; +// } else { +// cp = collisionInfo.points[collisionInfo.pcount]; +// } +// cp.pos.copy(verts2[i]); +// cp.r1.diff(cp.pos, box.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); +// cp.penetration = offset - dist; +// collisionInfo.pcount++; +// } +// } +// if (collisionInfo.pcount > 0) { +// collisionInfo.body1 = box; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// return true; +// } + return false; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxRectCollider.as.svn-base b/2.0.0.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxRectCollider.as.svn-base new file mode 100644 index 0000000..d557911 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxRectCollider.as.svn-base @@ -0,0 +1,573 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * + */ + public class BoxRectCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + + private var vectorToBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxRectCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, rect, vectorToBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + if (!findEdgesIntersection(box, rect, vectorToBox, int(bestAxisIndex/2), bestAxisIndex%2, contact)) { + return false; + } + } + contact.body1 = box.body; + contact.body2 = rect.body; + + // Хак для танков, чтобы исключить утыкания танков в стыки статических примитивов + if (rect.transform.k > 0.99999) { + contact.normal.x = 0; + contact.normal.y = 0; + contact.normal.z = 1; + } + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + // Вектор из центра прямоугольника в центр бокса + vectorToBox.x = boxTransform.d - rectTransform.d; + vectorToBox.y = boxTransform.h - rectTransform.h; + vectorToBox.z = boxTransform.l - rectTransform.l; + + // Проверка пересечения по нормали прямоугольника + rectTransform.getAxis(2, axis22); + if (!testMainAxis(box, rect, axis22, 0, vectorToBox)) return false; + + // Проверка пересечения по основным осям бокса + boxTransform.getAxis(0, axis10); + if (!testMainAxis(box, rect, axis10, 1, vectorToBox)) return false; + boxTransform.getAxis(1, axis11); + if (!testMainAxis(box, rect, axis11, 2, vectorToBox)) return false; + boxTransform.getAxis(2, axis12); + if (!testMainAxis(box, rect, axis12, 3, vectorToBox)) return false; + + // Получаем направляющие рёбер прямоугольника + rectTransform.getAxis(0, axis20); + rectTransform.getAxis(1, axis21); + + // Проверка производных осей + if (!testDerivedAxis(box, rect, axis10, axis20, 4, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis10, axis21, 5, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis11, axis20, 6, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis11, axis21, 7, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis12, axis20, 8, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis12, axis21, 9, vectorToBox)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта бокса с прямоугольником. + * + * @param box бокс + * @param rect прямоугольник + * @param vectorToBox вектор, направленный из центра прямоугольника в центр бокса + * @param faceAxisIdx индекс оси, идентифицирующей полскость столкновения (грань бокса или полскость прямоугольника) + * @param colInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var pnum:int; + var i:int; + var v:Vector3; + var cp:ContactPoint; + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + var colAxis:Vector3 = contact.normal; + + var negativeFace:Boolean; + var code:int; + + if (faceAxisIdx == 0) { + // Столкновение с плоскостью прямоугольника + + // Проверим положение бокса относительно плоскости прямоугольника + colAxis.x = rectTransform.c; + colAxis.y = rectTransform.g; + colAxis.z = rectTransform.k; + +// var offset:Number = colAxis.x*rectTransform.d + colAxis.y*rectTransform.h + colAxis.z*rectTransform.l; +// if (bbPos.vDot(colAxis) < offset) return false; + + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisIdx = axisIdx; + incidentAxisDot = dot; + } + } + negativeFace = incidentAxisDot > 0; + + code = 1 << (incidentAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + + // Получаем список вершин грани бокса, переводим их в систему координат прямоугольника и выполняем обрезку + // по прямоугольнику. Таким образом получается список потенциальных точек контакта. + boxTransform.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box.hs, incidentAxisIdx, negativeFace, points1); + boxTransform.transformVectorsN(points1, points2, 4); + rectTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByRect(rect.hs); + // Проверяем каждую потенциальную точку на принадлежность нижней полуплоскости прямоугольника и добавляем такие точки в список контактов + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if (v.z < epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = -v.z; + var cpPos:Vector3 = cp.pos; + cpPos.x = rectTransform.a*v.x + rectTransform.b*v.y + rectTransform.c*v.z + rectTransform.d; + cpPos.y = rectTransform.e*v.x + rectTransform.f*v.y + rectTransform.g*v.z + rectTransform.h; + cpPos.z = rectTransform.i*v.x + rectTransform.j*v.y + rectTransform.k*v.z + rectTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } else { + // Столкновение с гранью бокса + faceAxisIdx--; + boxTransform.getAxis(faceAxisIdx, colAxis); + negativeFace = colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z > 0; + + code = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) { + return false; + } + + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (rectTransform.c*colAxis.x + rectTransform.g*colAxis.y + rectTransform.k*colAxis.z < 0) return false; + + getFaceVertsByAxis(rect.hs, 2, false, points1); + rectTransform.transformVectorsN(points1, points2, 4); + boxTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if ((pen = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = pen; + cpPos = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } + return true; + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * + * @param hs + * @return + */ + private function clipByRect(hs:Vector3):int { + var pnum:int = 4; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box первый бокс + * @param rect второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param colInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + boxTransform.getAxis(axisIdx1, axis10); + rectTransform.getAxis(axisIdx2, axis20); + var colAxis:Vector3 = contact.normal; + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону бокса + if (colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + var vx:Number = box.hs.x; + var vy:Number = box.hs.y; + var vz:Number = box.hs.z; + var x2:Number = rect.hs.x; + var y2:Number = rect.hs.y; + var z2:Number = rect.hs.z; + // x + if (axisIdx1 == 0) { + vx = 0; + halfLen1 = box.hs.x; + } else { + if (boxTransform.a*colAxis.x + boxTransform.e*colAxis.y + boxTransform.i*colAxis.z > 0) { + vx = -vx; + if ((box.excludedFaces & 2) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 1) != 0) { + return false; + } + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = rect.hs.x; + } else { + if (rectTransform.a*colAxis.x + rectTransform.e*colAxis.y + rectTransform.i*colAxis.z < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + vy = 0; + halfLen1 = box.hs.y; + } else { + if (boxTransform.b*colAxis.x + boxTransform.f*colAxis.y + boxTransform.j*colAxis.z > 0) { + vy = -vy; + if ((box.excludedFaces & 8) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 4) != 0) { + return false; + } + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = rect.hs.y; + } else { + if (rectTransform.b*colAxis.x + rectTransform.f*colAxis.y + rectTransform.j*colAxis.z < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + vz = 0; + halfLen1 = box.hs.z; + } else { + if (boxTransform.c*colAxis.x + boxTransform.g*colAxis.y + boxTransform.k*colAxis.z > 0) { + vz = -vz; + if ((box.excludedFaces & 32) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 16) != 0) { + return false; + } + } + } + // Получаем глобальные координаты средних точек рёбер + + var x1:Number = boxTransform.a*vx + boxTransform.b*vy + boxTransform.c*vz + boxTransform.d; + var y1:Number = boxTransform.e*vx + boxTransform.f*vy + boxTransform.g*vz + boxTransform.h; + var z1:Number = boxTransform.i*vx + boxTransform.j*vy + boxTransform.k*vz + boxTransform.l; + vx = x2; + vy = y2; + vz = z2; + x2 = rectTransform.a*vx + rectTransform.b*vy + rectTransform.c*vz + rectTransform.d; + y2 = rectTransform.e*vx + rectTransform.f*vy + rectTransform.g*vz + rectTransform.h; + z2 = rectTransform.i*vx + rectTransform.j*vy + rectTransform.k*vz + rectTransform.l; + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + vx = x2 - x1; + vy = y2 - y1; + vz = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cpPos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cpPos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cpPos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var v:Vector3 = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + return true; + } + + /** + * Проверяет пересечение вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box + * @param rect + * @param axis + * @param axisIndex + * @param vectorToBox + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param rect + * @param axis1 + * @param axis2 + * @param axisIndex + * @param vectorToBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, rect:CollisionRect, axis1:Vector3, axis2:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия вдоль заданной оси. + * + * @param box бокс + * @param rect прямоугольник + * @param axis ось + * @param vectorToBox вектор, соединяющий центр прямоугольника с центром бокса + * @return величина перекрытия вдоль оси + */ + public function overlapOnAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, vectorToBox:Vector3):Number { + var m:Matrix4 = box.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box.hs.z; + if (d < 0) d = -d; + projection += d; + + m = rect.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*rect.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*rect.hs.y; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox.x*axis.x + vectorToBox.y*axis.y + vectorToBox.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} diff --git a/2.0.0.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxSphereCollider.as.svn-base b/2.0.0.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxSphereCollider.as.svn-base new file mode 100644 index 0000000..aa5cd4b --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxSphereCollider.as.svn-base @@ -0,0 +1,161 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.math.Vector3; + + /** + * + */ + public class BoxSphereCollider implements ICollider { + + private var center:Vector3 = new Vector3(); + private var closestPt:Vector3 = new Vector3(); + + private var bPos:Vector3 = new Vector3(); + private var sPos:Vector3 = new Vector3(); + + /** + * + */ + public function BoxSphereCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.subtract(closestPt).lengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + // Зафиксированно столкновение + contact.body1 = sphere.body; + contact.body2 = box.body; + contact.normal.copy(closestPt).transformBy4(box.transform).subtract(sPos).normalize().reverse(); + contact.pcount = 1; + + var cp:ContactPoint = contact.points[0]; + cp.penetration = sphere.r - Math.sqrt(distSqr); + cp.pos.copy(contact.normal).scale(-sphere.r).add(sPos); + cp.r1.diff(cp.pos, sPos); + cp.r2.diff(cp.pos, bPos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + var distSqr:Number = center.subtract(closestPt).lengthSqr(); + return distSqr <= sphere.r*sphere.r; + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxTriangleCollider.as.svn-base b/2.0.0.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxTriangleCollider.as.svn-base new file mode 100644 index 0000000..935525d --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/colliders/.svn/text-base/BoxTriangleCollider.as.svn-base @@ -0,0 +1,725 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionTriangle; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * + */ + public class BoxTriangleCollider extends BoxCollider { + + public var epsilon:Number = 0.001; + + private var bestAxisIndex:int; + private var minOverlap:Number; + private var toBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var colNormal:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxTriangleCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, tri, toBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + if (!findEdgesIntersection(box, tri, toBox, bestAxisIndex%3, int(bestAxisIndex/3), contact)) return false; + } + + contact.body1 = box.body; + contact.body2 = tri.body; + + // Хак для танков, чтобы исключить утыкания танков в стыки статических примитивов + if (tri.transform.k > 0.99999) { + contact.normal.x = 0; + contact.normal.y = 0; + contact.normal.z = 1; + } + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + toBox.x = boxTransform.d - triTransform.d; + toBox.y = boxTransform.h - triTransform.h; + toBox.z = boxTransform.l - triTransform.l; + + minOverlap = 1e308; + + // Сначала проверяется нормаль треугольника + axis.x = triTransform.c; + axis.y = triTransform.g; + axis.z = triTransform.k; + if (!testMainAxis(box, tri, axis, 0, toBox)) return false; + + // Проверка основных осей бокса + axis10.x = boxTransform.a; + axis10.y = boxTransform.e; + axis10.z = boxTransform.i; + if (!testMainAxis(box, tri, axis10, 1, toBox)) return false; + axis11.x = boxTransform.b; + axis11.y = boxTransform.f; + axis11.z = boxTransform.j; + if (!testMainAxis(box, tri, axis11, 2, toBox)) return false; + axis12.x = boxTransform.c; + axis12.y = boxTransform.g; + axis12.z = boxTransform.k; + if (!testMainAxis(box, tri, axis12, 3, toBox)) return false; + + // Проверка производных осей + // TODO: заменить вычисления векторных произведений инлайнами + var v:Vector3 = tri.e0; + axis20.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis20.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis20.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis20, 4, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis20, 5, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis20, 6, toBox)) return false; + + v = tri.e1; + axis21.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis21.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis21.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis21, 7, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis21, 8, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis21, 9, toBox)) return false; + + v = tri.e2; + axis22.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis22.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis22.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis22, 10, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis22, 11, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis22, 12, toBox)) return false; + + return true; + } + + /** + * Тестирует пересечение примитивов вдоль заданной оси. + * + * @param box бокс + * @param tri треугольник + * @param axis ось + * @param axisIndex индекс оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return true, если примитивы имеют перекрытие проекций вдоль указанной оси, иначе false + */ + private function testMainAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, axisIndex:int, toBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param tri + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, tri:CollisionTriangle, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Рассчитывает величину перекрытия проекций бокса и треугольника на заданную ось. + * + * @param box бокс + * @param tri треугольник + * @param axis единичный направляющий вектор оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return величина перекрытия. Положительное значение указывает на наличие перекрытия, нулевое или отрицательное значение указывает на отсутствие перекрытия. + */ + private function overlapOnAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, toBox:Vector3):Number { + var t:Matrix4 = box.transform; + var projection:Number = (t.a*axis.x + t.e*axis.y + t.i*axis.z)*box.hs.x; + if (projection < 0) projection = -projection; + var d:Number = (t.b*axis.x + t.f*axis.y + t.j*axis.z)*box.hs.y; + projection += d < 0 ? -d : d; + d = (t.c*axis.x + t.g*axis.y + t.k*axis.z)*box.hs.z; + projection += d < 0 ? -d : d; + + var vectorProjection:Number = toBox.x*axis.x + toBox.y*axis.y + toBox.z*axis.z; + // Для оптимизации ось преобразуется в систему треугольника, а не его вершины в мировую систему + t = tri.transform; + var ax:Number = t.a*axis.x + t.e*axis.y + t.i*axis.z; + var ay:Number = t.b*axis.x + t.f*axis.y + t.j*axis.z; + var az:Number = t.c*axis.x + t.g*axis.y + t.k*axis.z; + var max:Number = 0; + if (vectorProjection < 0) { + vectorProjection = -vectorProjection; + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d < max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d < max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d < max) max = d; + max = -max; + } else { + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d > max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d > max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d > max) max = d; + } + + return projection + max - vectorProjection; + } + + /** + * Определяет точки контакта бокса и треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function findFaceContactPoints(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIndex:int, contact:Contact):Boolean { + if (faceAxisIndex == 0) { + // Столкновение с плоскостью треугольника + return getBoxToTriContact(box, tri, toBox, contact); + } else { + // Столкновение с гранью бокса + return getTriToBoxContact(box, tri, toBox, faceAxisIndex, contact); + } + } + + /** + * Определяет точки контакта бокса с плоскостью треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getBoxToTriContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + colNormal.x = triTransform.c; + colNormal.y = triTransform.g; + colNormal.z = triTransform.k; + + var over:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + if (!over) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var incAxisDot:Number = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colNormal.x + axis.y*colNormal.y + axis.z*colNormal.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incAxisDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Обрезка грани + var negativeFace:Boolean = incAxisDot > 0; + + var code:int = 1 << (incFaceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + getFaceVertsByAxis(box.hs, incFaceAxisIdx, negativeFace, points1); + boxTransform.transformVectorsN(points1, points2, 4); + triTransform.transformVectorsInverseN(points2, points1, 4); + var pnum:int = clipByTriangle(tri); + // Среди конечного списка точек определяются лежащие под плоскостью треугольника + var cp:ContactPoint; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points2[i]; + if ((over && v.z < 0) || (!over && v.z > 0)) { + cp = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z + triTransform.d; + cpPos.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z + triTransform.h; + cpPos.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z + triTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = over ? -v.z : v.z; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Оперделяет точки контакта треугольника с гранью бокса. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getTriToBoxContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + faceAxisIdx--; + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + boxTransform.getAxis(faceAxisIdx, colNormal); + var negativeFace:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + + var code:int = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + if (!negativeFace) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + var v:Vector3 = points1[0]; + v.x = tri.v0.x; + v.y = tri.v0.y; + v.z = tri.v0.z; + + v = points1[1]; + v.x = tri.v1.x; + v.y = tri.v1.y; + v.z = tri.v1.z; + + v = points1[2]; + v.x = tri.v2.x; + v.y = tri.v2.y; + v.z = tri.v2.z; + + triTransform.transformVectorsN(points1, points2, 3); + boxTransform.transformVectorsInverseN(points2, points1, 3); + + var pnum:int = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность боксу и добавляем такие точки в список контактов + var penetration:Number; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + v = points1[i]; + penetration = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace); + if (penetration > -epsilon) { + var cp:ContactPoint = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = penetration; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Вычисляет величину проникновения точки в бокс. + * + * @param hs вектор половинных размеров бокса + * @param p точка в системе координат бокса + * @param axisIndex индекс оси + * @param negativeFace если true, проверяется нижняя грань + * @return величина проникновения точки в бокс + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, negativeFace:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (negativeFace) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (negativeFace) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (negativeFace) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, вершины которой заданы в списке points1. Результат сохраняется в этом же списке. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 3; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Обрезает грань прямоугольника сторонами треугольника. Обрезка выполняется в системе координат и в проекции на + * плоскость треугольника. Входные вершины грани в количестве четырёх штук должны находиться в списке points1. + * Конечные вершины сохраняются в списке points2. + * + * @param tri треугольник + * @return количество врешин в конечном списке + */ + private function clipByTriangle(tri:CollisionTriangle):int { + var vnum:int = 4; + vnum = clipByLine(tri.v0, tri.e0, points1, vnum, points2); + if (vnum == 0) return 0; + vnum = clipByLine(tri.v1, tri.e1, points2, vnum, points1); + if (vnum == 0) return 0; + return clipByLine(tri.v2, tri.e2, points1, vnum, points2);; + } + + /** + * Обрезает полигон указанной прямой. + * + * @param linePoint точка на прямой + * @param lineDir единичный направляющий вектор прямой + * @param verticesIn список вершин исходного полигона + * @param vnum количество вершин исходного полигона + * @param verticesOut список, куда будут записаны вершины конечного полигона + * @return количество вершин конечного полигона + */ + private function clipByLine(linePoint:Vector3, lineDir:Vector3, verticesIn:Vector., vnum:int, verticesOut:Vector.):int { + var nx:Number = -lineDir.y; + var ny:Number = lineDir.x; + var offset:Number = linePoint.x*nx + linePoint.y*ny; + var v1:Vector3 = verticesIn[int(vnum - 1)]; + var offset1:Number = v1.x*nx + v1.y*ny; + var t:Number; + var v:Vector3; + var num:int = 0; + for (var i:int = 0; i < vnum; i++) { + var v2:Vector3 = verticesIn[i]; + var offset2:Number = v2.x*nx + v2.y*ny; + if (offset1 < offset) { + // Первая точка ребра во внешней полуплоскости + if (offset2 > offset) { + // Вторая точка ребра во внутренней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } else { + // Первая точка ребра во внутренней полуплоскости. Добавляем её в конечный список. + v = verticesOut[num]; + v.x = v1.x; + v.y = v1.y; + v.z = v1.z; + num++; + if (offset2 < offset) { + // Вторая точка ребра во внешней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } + v1 = v2; + offset1 = offset2; + } + return num; + } + + /** + * + * @param box + * @param tri + * @param toBox + * @param boxAxisIdx + * @param triAxisIdx + * @param contact + */ + private function findEdgesIntersection(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, boxAxisIdx:int, triAxisIdx:int, contact:Contact):Boolean { + // Определение точки и направляющего вектора ребра треугольника + var tmpx1:Number; + var tmpy1:Number; + var tmpz1:Number; + var tmpx2:Number; + var tmpy2:Number; + var tmpz2:Number; + switch (triAxisIdx) { + case 0: + tmpx1 = tri.e0.x; + tmpy1 = tri.e0.y; + tmpz1 = tri.e0.z; + tmpx2 = tri.v0.x; + tmpy2 = tri.v0.y; + tmpz2 = tri.v0.z; + break; + case 1: + tmpx1 = tri.e1.x; + tmpy1 = tri.e1.y; + tmpz1 = tri.e1.z; + tmpx2 = tri.v1.x; + tmpy2 = tri.v1.y; + tmpz2 = tri.v1.z; + break; + case 2: + tmpx1 = tri.e2.x; + tmpy1 = tri.e2.y; + tmpz1 = tri.e2.z; + tmpx2 = tri.v2.x; + tmpy2 = tri.v2.y; + tmpz2 = tri.v2.z; + break; + } + var triTransform:Matrix4 = tri.transform; + axis20.x = triTransform.a*tmpx1 + triTransform.b*tmpy1 + triTransform.c*tmpz1; + axis20.y = triTransform.e*tmpx1 + triTransform.f*tmpy1 + triTransform.g*tmpz1; + axis20.z = triTransform.i*tmpx1 + triTransform.j*tmpy1 + triTransform.k*tmpz1; + var x2:Number = triTransform.a*tmpx2 + triTransform.b*tmpy2 + triTransform.c*tmpz2 + triTransform.d; + var y2:Number = triTransform.e*tmpx2 + triTransform.f*tmpy2 + triTransform.g*tmpz2 + triTransform.h; + var z2:Number = triTransform.i*tmpx2 + triTransform.j*tmpy2 + triTransform.k*tmpz2 + triTransform.l; + + // Определение нормали контакта, точки и направляющего вектора ребра бокса + var boxTransform:Matrix4 = box.transform; + boxTransform.getAxis(boxAxisIdx, axis10); + + // Нормаль контакта + var v:Vector3 = contact.normal; + v.x = axis10.y*axis20.z - axis10.z*axis20.y; + v.y = axis10.z*axis20.x - axis10.x*axis20.z; + v.z = axis10.x*axis20.y - axis10.y*axis20.x; + k = 1/Math.sqrt(v.x*v.x + v.y*v.y + v.z*v.z); + v.x *= k; + v.y *= k; + v.z *= k; + // Разворот нормали в сторону бокса + if (v.x*toBox.x + v.y*toBox.y + v.z*toBox.z < 0) { + v.x = -v.x; + v.y = -v.y; + v.z = -v.z; + } + + var boxHalfLen:Number; + tmpx1 = box.hs.x; + tmpy1 = box.hs.y; + tmpz1 = box.hs.z; + // X + if (boxAxisIdx == 0) { + tmpx1 = 0; + boxHalfLen = box.hs.x; + } else { + if (boxTransform.a*v.x + boxTransform.e*v.y + boxTransform.i*v.z > 0) { + tmpx1 = -tmpx1; + if ((box.excludedFaces & 2) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 1) != 0) { + return false; + } + } + } + // Y + if (boxAxisIdx == 1) { + tmpy1 = 0; + boxHalfLen = box.hs.y; + } else { + if (boxTransform.b*v.x + boxTransform.f*v.y + boxTransform.j*v.z > 0) { + tmpy1 = -tmpy1; + if ((box.excludedFaces & 8) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 4) != 0) { + return false; + } + } + } + // Z + if (boxAxisIdx == 2) { + tmpz1 = 0; + boxHalfLen = box.hs.z; + } else { + if (boxTransform.c*v.x + boxTransform.g*v.y + boxTransform.k*v.z > 0) { + tmpz1 = -tmpz1; + if ((box.excludedFaces & 32) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 16) != 0) { + return false; + } + } + } + var x1:Number = boxTransform.a*tmpx1 + boxTransform.b*tmpy1 + boxTransform.c*tmpz1 + boxTransform.d; + var y1:Number = boxTransform.e*tmpx1 + boxTransform.f*tmpy1 + boxTransform.g*tmpz1 + boxTransform.h; + var z1:Number = boxTransform.i*tmpx1 + boxTransform.j*tmpy1 + boxTransform.k*tmpz1 + boxTransform.l; + + // Находим точку пересечения рёбер, решая систему уравнений + // axis10 - направляющий вектор ребра бокса + // x1, y1, z1 - средняя точка ребра бокса + // axis20 - направляющий вектор ребра треугольника + // x2, y2, z2 - начальная точка ребра треугольника + var k:Number = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + var vx:Number = x2 - x1; + var vy:Number = y2 - y1; + var vz:Number = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + + // Запись данных о контакте + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + v = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + v.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + v.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + v.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = v.x - boxTransform.d; + r.y = v.y - boxTransform.h; + r.z = v.z - boxTransform.l; + r = cp.r2; + r.x = v.x - triTransform.d; + r.y = v.y - triTransform.h; + r.z = v.z - triTransform.l; + return true; + } + + } +} diff --git a/2.0.0.0/src/alternativa/physics/collision/colliders/.svn/text-base/SpherePlaneCollider.as.svn-base b/2.0.0.0/src/alternativa/physics/collision/colliders/.svn/text-base/SpherePlaneCollider.as.svn-base new file mode 100644 index 0000000..30cd55f --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/colliders/.svn/text-base/SpherePlaneCollider.as.svn-base @@ -0,0 +1,68 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + /** + * + */ + public class SpherePlaneCollider implements ICollider { + + private var normal:Vector3 = new Vector3(); + + /** + * + */ + public function SpherePlaneCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var sphere:RigidSphere = body1 as RigidSphere; +// var plane:RigidPlane; +// if (sphere == null) { +// sphere = body2 as RigidSphere; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// +// var dist:Number = sphere.state.pos.dot(normal) - offset; +// if (dist > sphere.r) return false; +// +// collisionInfo.body1 = sphere; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// collisionInfo.pcount = 1; +// +// var cp:ContactPoint = collisionInfo.points[0]; +// cp.penetration = sphere.r - dist; +// cp.pos.copy(normal).reverse().scale(sphere.r).add(sphere.state.pos); +// cp.r1.diff(cp.pos, sphere.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/collision/colliders/.svn/text-base/SphereSphereCollider.as.svn-base b/2.0.0.0/src/alternativa/physics/collision/colliders/.svn/text-base/SphereSphereCollider.as.svn-base new file mode 100644 index 0000000..228d6c9 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/colliders/.svn/text-base/SphereSphereCollider.as.svn-base @@ -0,0 +1,68 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.math.Vector3; + + public class SphereSphereCollider implements ICollider { + + private var p1:Vector3 = new Vector3(); + private var p2:Vector3 = new Vector3(); + + public function SphereSphereCollider() { + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var s1:CollisionSphere; + var s2:CollisionSphere; + if (prim1.body != null) { + s1 = prim1 as CollisionSphere; + s2 = prim2 as CollisionSphere; + } else { + s1 = prim2 as CollisionSphere; + s2 = prim1 as CollisionSphere; + } + + s1.transform.getAxis(3, p1); + s2.transform.getAxis(3, p2); + var dx:Number = p1.x - p2.x; + var dy:Number = p1.y - p2.y; + var dz:Number = p1.z - p2.z; + var len:Number = dx*dx + dy*dy + dz*dz; + var sum:Number = s1.r + s2.r; + if (len > sum*sum) return false; + len = Math.sqrt(len); + dx /= len; + dy /= len; + dz /= len; + + contact.body1 = s1.body; + contact.body2 = s2.body; + contact.normal.x = dx; + contact.normal.y = dy; + contact.normal.z = dz; + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = sum - len; + cp.pos.x = p1.x - dx*s1.r; + cp.pos.y = p1.y - dy*s1.r; + cp.pos.z = p1.z - dz*s1.r; + cp.r1.diff(cp.pos, p1); + cp.r2.diff(cp.pos, p2); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/collision/colliders/BoxBoxCollider.as b/2.0.0.0/src/alternativa/physics/collision/colliders/BoxBoxCollider.as new file mode 100644 index 0000000..789c21a --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/colliders/BoxBoxCollider.as @@ -0,0 +1,585 @@ +package alternativa.physics.collision.colliders { + + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * Расчитывает точки контакта двух боксов. Нормаль контакта направляется в сторону бокса с меньшим ID. + */ + public class BoxBoxCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + private var vectorToBox1:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var bestAxisIndex:int; + private var minOverlap:Number; + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + private var tmpPoints:Vector. = new Vector.(8, true); + private var pcount:int; + + /** + * + */ + public function BoxBoxCollider() { + for (var i:int = 0; i < 8; i++) { + tmpPoints[i] = new ContactPoint(); + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + + if (bestAxisIndex < 6) { + // Контакт грань-(грань|ребро|вершина) + if (!findFaceContactPoints(box1, box2, vectorToBox1, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 6; + findEdgesIntersection(box1, box2, vectorToBox1, int(bestAxisIndex/3), bestAxisIndex%3, contact); + } + contact.body1 = box1.body; + contact.body2 = box2.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + // Вектор из центра второго бокса в центр первого + vectorToBox1.x = transform1.d - transform2.d; + vectorToBox1.y = transform1.h - transform2.h; + vectorToBox1.z = transform1.l - transform2.l; + + // Проверка пересечения по основным осям + axis10.x = transform1.a; + axis10.y = transform1.e; + axis10.z = transform1.i; + if (!testMainAxis(box1, box2, axis10, 0, vectorToBox1)) return false; + axis11.x = transform1.b; + axis11.y = transform1.f; + axis11.z = transform1.j; + if (!testMainAxis(box1, box2, axis11, 1, vectorToBox1)) return false; + axis12.x = transform1.c; + axis12.y = transform1.g; + axis12.z = transform1.k; + if (!testMainAxis(box1, box2, axis12, 2, vectorToBox1)) return false; + + axis20.x = transform2.a; + axis20.y = transform2.e; + axis20.z = transform2.i; + if (!testMainAxis(box1, box2, axis20, 3, vectorToBox1)) return false; + axis21.x = transform2.b; + axis21.y = transform2.f; + axis21.z = transform2.j; + if (!testMainAxis(box1, box2, axis21, 4, vectorToBox1)) return false; + axis22.x = transform2.c; + axis22.y = transform2.g; + axis22.z = transform2.k; + if (!testMainAxis(box1, box2, axis22, 5, vectorToBox1)) return false; + + // Проверка производных осей + if (!testDerivedAxis(box1, box2, axis10, axis20, 6, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis21, 7, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis22, 8, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis11, axis20, 9, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis21, 10, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis22, 11, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis12, axis20, 12, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis21, 13, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis22, 14, vectorToBox1)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта грани одного бокса с гранью/ребром/вершиной другого. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param faceAxisIdx индекс оси первого бокса, перпендикулярной грани, с которой произошло столкновение + * @param contactInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var swapNormal:Boolean = false; + if (faceAxisIdx > 2) { + // Столкновение с гранью второго бокса. Для дальнейших расчётов боксы меняются местами, + // но нормаль контакта всё равно должна быть направлена в сторону первоначального box1 + var tmpBox:CollisionBox = box1; + box1 = box2; + box2 = tmpBox; + vectorToBox1.x = -vectorToBox1.x; + vectorToBox1.y = -vectorToBox1.y; + vectorToBox1.z = -vectorToBox1.z; + faceAxisIdx -= 3; + swapNormal = true; + } + + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + var colAxis:Vector3 = contact.normal; + + transform1.getAxis(faceAxisIdx, colAxis); + var negativeFace:Boolean = colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z > 0; + + var code:int = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box1.excludedFaces) != 0) return false; + + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + // Ищем ось второго бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + transform2.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisDot = dot; + incidentAxisIdx = axisIdx; + } + } + // Получаем список вершин грани второго бокса, переводим их в систему координат первого бокса и выполняем обрезку + // по грани первого бокса. Таким образом получается список потенциальных точек контакта. + transform2.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box2.hs, incidentAxisIdx, incidentAxisDot < 0, points1); + + // TODO: Вычислить результирующую матрицу, затем преобразовать векторы за один заход + transform2.transformVectorsN(points1, points2, 4); + transform1.transformVectorsInverseN(points2, points1, 4); + + var pnum:int = clip(box1.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points1[i]; + if ((pen = getPointBoxPenetration(box1.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + var cp:ContactPoint = tmpPoints[pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = transform1.a*v.x + transform1.b*v.y + transform1.c*v.z + transform1.d; + cpPos.y = transform1.e*v.x + transform1.f*v.y + transform1.g*v.z + transform1.h; + cpPos.z = transform1.i*v.x + transform1.j*v.y + transform1.k*v.z + transform1.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + + cp.penetration = pen; + } + } + if (swapNormal) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (pcount > 4) { + reducePoints(); + } + for (i = 0; i < pcount; i++) { + cp = contact.points[i]; + cp.copyFrom(tmpPoints[i]); + } + contact.pcount = pcount; + return true; + } + + /** + * + * @param contactInfo + */ + private function reducePoints():void { + var i:int; + var minIdx:int; + var cp1:ContactPoint; + var cp2:ContactPoint; + while (pcount > 4) { + var minLen:Number = 1e10; + var p1:Vector3 = (tmpPoints[int(pcount - 1)] as ContactPoint).pos; + var p2:Vector3; + for (i = 0; i < pcount; i++) { + p2 = (tmpPoints[i] as ContactPoint).pos; + var dx:Number = p2.x - p1.x; + var dy:Number = p2.y - p1.y; + var dz:Number = p2.z - p1.z; + var len:Number = dx*dx + dy*dy + dz*dz; + if (len < minLen) { + minLen = len; + minIdx = i; + } + p1 = p2; + } + var ii:int = minIdx == 0 ? (pcount - 1) : (minIdx - 1); + cp1 = tmpPoints[ii]; + cp2 = tmpPoints[minIdx]; + p1 = cp1.pos; + p2 = cp2.pos; + p2.x = 0.5*(p1.x + p2.x); + p2.y = 0.5*(p1.y + p2.y); + p2.z = 0.5*(p1.z + p2.z); + cp2.penetration = 0.5*(cp1.penetration + cp2.penetration); + if (minIdx > 0) { + for (i = minIdx; i < pcount; i++) { + tmpPoints[int(i - 1)] = tmpPoints[i]; + } + tmpPoints[int(pcount - 1)] = cp1; + } + pcount--; + } + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clip(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param contactInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):void { + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + transform1.getAxis(axisIdx1, axis10); + transform2.getAxis(axisIdx2, axis20); + + var colAxis:Vector3 = contact.normal; + + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону первого бокса + if (colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + + var tx:Number = box1.hs.x; + var ty:Number = box1.hs.y; + var tz:Number = box1.hs.z; + + var x2:Number = box2.hs.x; + var y2:Number = box2.hs.y; + var z2:Number = box2.hs.z; + // x + if (axisIdx1 == 0) { + tx = 0; + halfLen1 = box1.hs.x; + } else { + if (colAxis.x*transform1.a + colAxis.y*transform1.e + colAxis.z*transform1.i > 0) { + tx = -tx; + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = box2.hs.x; + } else { + if (colAxis.x*transform2.a + colAxis.y*transform2.e + colAxis.z*transform2.i < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + ty = 0; + halfLen1 = box1.hs.y; + } else { + if (colAxis.x*transform1.b + colAxis.y*transform1.f + colAxis.z*transform1.j > 0) { + ty = -ty; + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = box2.hs.y; + } else { + if (colAxis.x*transform2.b + colAxis.y*transform2.f + colAxis.z*transform2.j < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + tz = 0; + halfLen1 = box1.hs.z; + } else { + if (colAxis.x*transform1.c + colAxis.y*transform1.g + colAxis.z*transform1.k > 0) { + tz = -tz; + } + } + if (axisIdx2 == 2) { + z2 = 0; + halfLen2 = box2.hs.z; + } else { + if (colAxis.x*transform2.c + colAxis.y*transform2.g + colAxis.z*transform2.k < 0) { + z2 = -z2; + } + } + // Получаем глобальные координаты средних точек рёбер + var x1:Number = transform1.a*tx + transform1.b*ty + transform1.c*tz + transform1.d; + var y1:Number = transform1.e*tx + transform1.f*ty + transform1.g*tz + transform1.h; + var z1:Number = transform1.i*tx + transform1.j*ty + transform1.k*tz + transform1.l; + tx = x2; + ty = y2; + tz = z2; + x2 = transform2.a*tx + transform2.b*ty + transform2.c*tz + transform2.d; + y2 = transform2.e*tx + transform2.f*ty + transform2.g*tz + transform2.h; + z2 = transform2.i*tx + transform2.j*ty + transform2.k*tz + transform2.l; + + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + tx = x2 - x1; + ty = y2 - y1; + tz = z2 - z1; + var c1:Number = axis10.x*tx + axis10.y*ty + axis10.z*tz; + var c2:Number = axis20.x*tx + axis20.y*ty + axis20.z*tz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cp.pos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cp.pos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение боксов вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box1 + * @param box2 + * @param axis + * @param axisIndex + * @param toBox1 + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box1 + * @param box2 + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox1 + * @return + */ + private function testDerivedAxis(box1:CollisionBox, box2:CollisionBox, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия двух боксов вдоль заданной оси. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param axis ось + * @param vectorToBox1 вектор, соединяющий центр второго бокса с центром первого + * @return величина перекрытия боксов вдоль оси + */ + public function overlapOnAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, vectorToBox1:Vector3):Number { + var m:Matrix4 = box1.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box1.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box1.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box1.hs.z; + if (d < 0) d = -d; + projection += d; + + m = box2.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box2.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box2.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box2.hs.z; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox1.x*axis.x + vectorToBox1.y*axis.y + vectorToBox1.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} + import alternativa.math.Vector3; + + class CollisionPointTmp { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + } diff --git a/2.0.0.0/src/alternativa/physics/collision/colliders/BoxCollider.as b/2.0.0.0/src/alternativa/physics/collision/colliders/BoxCollider.as new file mode 100644 index 0000000..2429bb1 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/colliders/BoxCollider.as @@ -0,0 +1,427 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + /** + * + */ + public class BoxCollider implements ICollider { + + /** + * + */ + public function BoxCollider() { + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + return false; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + protected function getFaceVertsByAxis(hs:Vector3, axisIdx:int, negativeFace:Boolean, result:Vector.):void { + var v:Vector3; + switch (axisIdx) { + case 0: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + } + break; + case 1: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + } + break; + case 2: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + } else { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } + break; + } + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/collision/colliders/BoxPlaneCollider.as b/2.0.0.0/src/alternativa/physics/collision/colliders/BoxPlaneCollider.as new file mode 100644 index 0000000..3bebdc3 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/colliders/BoxPlaneCollider.as @@ -0,0 +1,88 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + public class BoxPlaneCollider implements ICollider { + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var normal:Vector3 = new Vector3(); + + public function BoxPlaneCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var box:RigidBox = body1 as RigidBox; +// var plane:RigidPlane; +// if (box == null) { +// box = body2 as RigidBox; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные координаты вершин бокса +// var sx:Number = box.halfSize.x; +// var sy:Number = box.halfSize.y; +// var sz:Number = box.halfSize.z; +// (verts1[0] as Vector3).reset(-sx, -sy, -sz); +// (verts1[1] as Vector3).reset(sx, -sy, -sz); +// (verts1[2] as Vector3).reset(sx, sy, -sz); +// (verts1[3] as Vector3).reset(-sx, sy, -sz); +// (verts1[4] as Vector3).reset(-sx, -sy, sz); +// (verts1[5] as Vector3).reset(sx, -sy, sz); +// (verts1[6] as Vector3).reset(sx, sy, sz); +// (verts1[7] as Vector3).reset(-sx, sy, sz); +// +// box.transform.transformVectors(verts1, verts2); +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// // Проверяем наличие столкновений с каждой вершиной +// collisionInfo.pcount = 0; +// for (var i:int = 0; i < 8; i++) { +// // Вершина добавляется в список точек столкновения, если лежит под плоскостью +// var dist:Number = (verts2[i] as Vector3).dot(normal); +// if (dist < offset) { +// var cp:ContactPoint; +// if (collisionInfo.pcount == collisionInfo.points.length) { +// cp = new ContactPoint(); +// collisionInfo.points[collisionInfo.pcount] = cp; +// } else { +// cp = collisionInfo.points[collisionInfo.pcount]; +// } +// cp.pos.copy(verts2[i]); +// cp.r1.diff(cp.pos, box.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); +// cp.penetration = offset - dist; +// collisionInfo.pcount++; +// } +// } +// if (collisionInfo.pcount > 0) { +// collisionInfo.body1 = box; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// return true; +// } + return false; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/collision/colliders/BoxRectCollider.as b/2.0.0.0/src/alternativa/physics/collision/colliders/BoxRectCollider.as new file mode 100644 index 0000000..d557911 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/colliders/BoxRectCollider.as @@ -0,0 +1,573 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * + */ + public class BoxRectCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + + private var vectorToBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxRectCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, rect, vectorToBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + if (!findEdgesIntersection(box, rect, vectorToBox, int(bestAxisIndex/2), bestAxisIndex%2, contact)) { + return false; + } + } + contact.body1 = box.body; + contact.body2 = rect.body; + + // Хак для танков, чтобы исключить утыкания танков в стыки статических примитивов + if (rect.transform.k > 0.99999) { + contact.normal.x = 0; + contact.normal.y = 0; + contact.normal.z = 1; + } + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + // Вектор из центра прямоугольника в центр бокса + vectorToBox.x = boxTransform.d - rectTransform.d; + vectorToBox.y = boxTransform.h - rectTransform.h; + vectorToBox.z = boxTransform.l - rectTransform.l; + + // Проверка пересечения по нормали прямоугольника + rectTransform.getAxis(2, axis22); + if (!testMainAxis(box, rect, axis22, 0, vectorToBox)) return false; + + // Проверка пересечения по основным осям бокса + boxTransform.getAxis(0, axis10); + if (!testMainAxis(box, rect, axis10, 1, vectorToBox)) return false; + boxTransform.getAxis(1, axis11); + if (!testMainAxis(box, rect, axis11, 2, vectorToBox)) return false; + boxTransform.getAxis(2, axis12); + if (!testMainAxis(box, rect, axis12, 3, vectorToBox)) return false; + + // Получаем направляющие рёбер прямоугольника + rectTransform.getAxis(0, axis20); + rectTransform.getAxis(1, axis21); + + // Проверка производных осей + if (!testDerivedAxis(box, rect, axis10, axis20, 4, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis10, axis21, 5, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis11, axis20, 6, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis11, axis21, 7, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis12, axis20, 8, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis12, axis21, 9, vectorToBox)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта бокса с прямоугольником. + * + * @param box бокс + * @param rect прямоугольник + * @param vectorToBox вектор, направленный из центра прямоугольника в центр бокса + * @param faceAxisIdx индекс оси, идентифицирующей полскость столкновения (грань бокса или полскость прямоугольника) + * @param colInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var pnum:int; + var i:int; + var v:Vector3; + var cp:ContactPoint; + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + var colAxis:Vector3 = contact.normal; + + var negativeFace:Boolean; + var code:int; + + if (faceAxisIdx == 0) { + // Столкновение с плоскостью прямоугольника + + // Проверим положение бокса относительно плоскости прямоугольника + colAxis.x = rectTransform.c; + colAxis.y = rectTransform.g; + colAxis.z = rectTransform.k; + +// var offset:Number = colAxis.x*rectTransform.d + colAxis.y*rectTransform.h + colAxis.z*rectTransform.l; +// if (bbPos.vDot(colAxis) < offset) return false; + + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisIdx = axisIdx; + incidentAxisDot = dot; + } + } + negativeFace = incidentAxisDot > 0; + + code = 1 << (incidentAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + + // Получаем список вершин грани бокса, переводим их в систему координат прямоугольника и выполняем обрезку + // по прямоугольнику. Таким образом получается список потенциальных точек контакта. + boxTransform.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box.hs, incidentAxisIdx, negativeFace, points1); + boxTransform.transformVectorsN(points1, points2, 4); + rectTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByRect(rect.hs); + // Проверяем каждую потенциальную точку на принадлежность нижней полуплоскости прямоугольника и добавляем такие точки в список контактов + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if (v.z < epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = -v.z; + var cpPos:Vector3 = cp.pos; + cpPos.x = rectTransform.a*v.x + rectTransform.b*v.y + rectTransform.c*v.z + rectTransform.d; + cpPos.y = rectTransform.e*v.x + rectTransform.f*v.y + rectTransform.g*v.z + rectTransform.h; + cpPos.z = rectTransform.i*v.x + rectTransform.j*v.y + rectTransform.k*v.z + rectTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } else { + // Столкновение с гранью бокса + faceAxisIdx--; + boxTransform.getAxis(faceAxisIdx, colAxis); + negativeFace = colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z > 0; + + code = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) { + return false; + } + + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (rectTransform.c*colAxis.x + rectTransform.g*colAxis.y + rectTransform.k*colAxis.z < 0) return false; + + getFaceVertsByAxis(rect.hs, 2, false, points1); + rectTransform.transformVectorsN(points1, points2, 4); + boxTransform.transformVectorsInverseN(points2, points1, 4); + pnum = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if ((pen = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = pen; + cpPos = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } + return true; + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * + * @param hs + * @return + */ + private function clipByRect(hs:Vector3):int { + var pnum:int = 4; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box первый бокс + * @param rect второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param colInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + boxTransform.getAxis(axisIdx1, axis10); + rectTransform.getAxis(axisIdx2, axis20); + var colAxis:Vector3 = contact.normal; + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону бокса + if (colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + var vx:Number = box.hs.x; + var vy:Number = box.hs.y; + var vz:Number = box.hs.z; + var x2:Number = rect.hs.x; + var y2:Number = rect.hs.y; + var z2:Number = rect.hs.z; + // x + if (axisIdx1 == 0) { + vx = 0; + halfLen1 = box.hs.x; + } else { + if (boxTransform.a*colAxis.x + boxTransform.e*colAxis.y + boxTransform.i*colAxis.z > 0) { + vx = -vx; + if ((box.excludedFaces & 2) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 1) != 0) { + return false; + } + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = rect.hs.x; + } else { + if (rectTransform.a*colAxis.x + rectTransform.e*colAxis.y + rectTransform.i*colAxis.z < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + vy = 0; + halfLen1 = box.hs.y; + } else { + if (boxTransform.b*colAxis.x + boxTransform.f*colAxis.y + boxTransform.j*colAxis.z > 0) { + vy = -vy; + if ((box.excludedFaces & 8) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 4) != 0) { + return false; + } + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = rect.hs.y; + } else { + if (rectTransform.b*colAxis.x + rectTransform.f*colAxis.y + rectTransform.j*colAxis.z < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + vz = 0; + halfLen1 = box.hs.z; + } else { + if (boxTransform.c*colAxis.x + boxTransform.g*colAxis.y + boxTransform.k*colAxis.z > 0) { + vz = -vz; + if ((box.excludedFaces & 32) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 16) != 0) { + return false; + } + } + } + // Получаем глобальные координаты средних точек рёбер + + var x1:Number = boxTransform.a*vx + boxTransform.b*vy + boxTransform.c*vz + boxTransform.d; + var y1:Number = boxTransform.e*vx + boxTransform.f*vy + boxTransform.g*vz + boxTransform.h; + var z1:Number = boxTransform.i*vx + boxTransform.j*vy + boxTransform.k*vz + boxTransform.l; + vx = x2; + vy = y2; + vz = z2; + x2 = rectTransform.a*vx + rectTransform.b*vy + rectTransform.c*vz + rectTransform.d; + y2 = rectTransform.e*vx + rectTransform.f*vy + rectTransform.g*vz + rectTransform.h; + z2 = rectTransform.i*vx + rectTransform.j*vy + rectTransform.k*vz + rectTransform.l; + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + vx = x2 - x1; + vy = y2 - y1; + vz = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cpPos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cpPos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cpPos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var v:Vector3 = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + return true; + } + + /** + * Проверяет пересечение вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box + * @param rect + * @param axis + * @param axisIndex + * @param vectorToBox + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param rect + * @param axis1 + * @param axis2 + * @param axisIndex + * @param vectorToBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, rect:CollisionRect, axis1:Vector3, axis2:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия вдоль заданной оси. + * + * @param box бокс + * @param rect прямоугольник + * @param axis ось + * @param vectorToBox вектор, соединяющий центр прямоугольника с центром бокса + * @return величина перекрытия вдоль оси + */ + public function overlapOnAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, vectorToBox:Vector3):Number { + var m:Matrix4 = box.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box.hs.z; + if (d < 0) d = -d; + projection += d; + + m = rect.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*rect.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*rect.hs.y; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox.x*axis.x + vectorToBox.y*axis.y + vectorToBox.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} diff --git a/2.0.0.0/src/alternativa/physics/collision/colliders/BoxSphereCollider.as b/2.0.0.0/src/alternativa/physics/collision/colliders/BoxSphereCollider.as new file mode 100644 index 0000000..aa5cd4b --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/colliders/BoxSphereCollider.as @@ -0,0 +1,161 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.math.Vector3; + + /** + * + */ + public class BoxSphereCollider implements ICollider { + + private var center:Vector3 = new Vector3(); + private var closestPt:Vector3 = new Vector3(); + + private var bPos:Vector3 = new Vector3(); + private var sPos:Vector3 = new Vector3(); + + /** + * + */ + public function BoxSphereCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.subtract(closestPt).lengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + // Зафиксированно столкновение + contact.body1 = sphere.body; + contact.body2 = box.body; + contact.normal.copy(closestPt).transformBy4(box.transform).subtract(sPos).normalize().reverse(); + contact.pcount = 1; + + var cp:ContactPoint = contact.points[0]; + cp.penetration = sphere.r - Math.sqrt(distSqr); + cp.pos.copy(contact.normal).scale(-sphere.r).add(sPos); + cp.r1.diff(cp.pos, sPos); + cp.r2.diff(cp.pos, bPos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformVectorInverse(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + var distSqr:Number = center.subtract(closestPt).lengthSqr(); + return distSqr <= sphere.r*sphere.r; + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/collision/colliders/BoxTriangleCollider.as b/2.0.0.0/src/alternativa/physics/collision/colliders/BoxTriangleCollider.as new file mode 100644 index 0000000..935525d --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/colliders/BoxTriangleCollider.as @@ -0,0 +1,725 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionTriangle; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * + */ + public class BoxTriangleCollider extends BoxCollider { + + public var epsilon:Number = 0.001; + + private var bestAxisIndex:int; + private var minOverlap:Number; + private var toBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var colNormal:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxTriangleCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, tri, toBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + if (!findEdgesIntersection(box, tri, toBox, bestAxisIndex%3, int(bestAxisIndex/3), contact)) return false; + } + + contact.body1 = box.body; + contact.body2 = tri.body; + + // Хак для танков, чтобы исключить утыкания танков в стыки статических примитивов + if (tri.transform.k > 0.99999) { + contact.normal.x = 0; + contact.normal.y = 0; + contact.normal.z = 1; + } + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + toBox.x = boxTransform.d - triTransform.d; + toBox.y = boxTransform.h - triTransform.h; + toBox.z = boxTransform.l - triTransform.l; + + minOverlap = 1e308; + + // Сначала проверяется нормаль треугольника + axis.x = triTransform.c; + axis.y = triTransform.g; + axis.z = triTransform.k; + if (!testMainAxis(box, tri, axis, 0, toBox)) return false; + + // Проверка основных осей бокса + axis10.x = boxTransform.a; + axis10.y = boxTransform.e; + axis10.z = boxTransform.i; + if (!testMainAxis(box, tri, axis10, 1, toBox)) return false; + axis11.x = boxTransform.b; + axis11.y = boxTransform.f; + axis11.z = boxTransform.j; + if (!testMainAxis(box, tri, axis11, 2, toBox)) return false; + axis12.x = boxTransform.c; + axis12.y = boxTransform.g; + axis12.z = boxTransform.k; + if (!testMainAxis(box, tri, axis12, 3, toBox)) return false; + + // Проверка производных осей + // TODO: заменить вычисления векторных произведений инлайнами + var v:Vector3 = tri.e0; + axis20.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis20.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis20.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis20, 4, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis20, 5, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis20, 6, toBox)) return false; + + v = tri.e1; + axis21.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis21.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis21.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis21, 7, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis21, 8, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis21, 9, toBox)) return false; + + v = tri.e2; + axis22.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis22.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis22.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis22, 10, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis22, 11, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis22, 12, toBox)) return false; + + return true; + } + + /** + * Тестирует пересечение примитивов вдоль заданной оси. + * + * @param box бокс + * @param tri треугольник + * @param axis ось + * @param axisIndex индекс оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return true, если примитивы имеют перекрытие проекций вдоль указанной оси, иначе false + */ + private function testMainAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, axisIndex:int, toBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param tri + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, tri:CollisionTriangle, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Рассчитывает величину перекрытия проекций бокса и треугольника на заданную ось. + * + * @param box бокс + * @param tri треугольник + * @param axis единичный направляющий вектор оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return величина перекрытия. Положительное значение указывает на наличие перекрытия, нулевое или отрицательное значение указывает на отсутствие перекрытия. + */ + private function overlapOnAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, toBox:Vector3):Number { + var t:Matrix4 = box.transform; + var projection:Number = (t.a*axis.x + t.e*axis.y + t.i*axis.z)*box.hs.x; + if (projection < 0) projection = -projection; + var d:Number = (t.b*axis.x + t.f*axis.y + t.j*axis.z)*box.hs.y; + projection += d < 0 ? -d : d; + d = (t.c*axis.x + t.g*axis.y + t.k*axis.z)*box.hs.z; + projection += d < 0 ? -d : d; + + var vectorProjection:Number = toBox.x*axis.x + toBox.y*axis.y + toBox.z*axis.z; + // Для оптимизации ось преобразуется в систему треугольника, а не его вершины в мировую систему + t = tri.transform; + var ax:Number = t.a*axis.x + t.e*axis.y + t.i*axis.z; + var ay:Number = t.b*axis.x + t.f*axis.y + t.j*axis.z; + var az:Number = t.c*axis.x + t.g*axis.y + t.k*axis.z; + var max:Number = 0; + if (vectorProjection < 0) { + vectorProjection = -vectorProjection; + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d < max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d < max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d < max) max = d; + max = -max; + } else { + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d > max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d > max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d > max) max = d; + } + + return projection + max - vectorProjection; + } + + /** + * Определяет точки контакта бокса и треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function findFaceContactPoints(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIndex:int, contact:Contact):Boolean { + if (faceAxisIndex == 0) { + // Столкновение с плоскостью треугольника + return getBoxToTriContact(box, tri, toBox, contact); + } else { + // Столкновение с гранью бокса + return getTriToBoxContact(box, tri, toBox, faceAxisIndex, contact); + } + } + + /** + * Определяет точки контакта бокса с плоскостью треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getBoxToTriContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + colNormal.x = triTransform.c; + colNormal.y = triTransform.g; + colNormal.z = triTransform.k; + + var over:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + if (!over) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var incAxisDot:Number = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colNormal.x + axis.y*colNormal.y + axis.z*colNormal.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incAxisDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Обрезка грани + var negativeFace:Boolean = incAxisDot > 0; + + var code:int = 1 << (incFaceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + getFaceVertsByAxis(box.hs, incFaceAxisIdx, negativeFace, points1); + boxTransform.transformVectorsN(points1, points2, 4); + triTransform.transformVectorsInverseN(points2, points1, 4); + var pnum:int = clipByTriangle(tri); + // Среди конечного списка точек определяются лежащие под плоскостью треугольника + var cp:ContactPoint; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points2[i]; + if ((over && v.z < 0) || (!over && v.z > 0)) { + cp = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z + triTransform.d; + cpPos.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z + triTransform.h; + cpPos.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z + triTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = over ? -v.z : v.z; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Оперделяет точки контакта треугольника с гранью бокса. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getTriToBoxContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + faceAxisIdx--; + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + boxTransform.getAxis(faceAxisIdx, colNormal); + var negativeFace:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + + var code:int = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + if (!negativeFace) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + var v:Vector3 = points1[0]; + v.x = tri.v0.x; + v.y = tri.v0.y; + v.z = tri.v0.z; + + v = points1[1]; + v.x = tri.v1.x; + v.y = tri.v1.y; + v.z = tri.v1.z; + + v = points1[2]; + v.x = tri.v2.x; + v.y = tri.v2.y; + v.z = tri.v2.z; + + triTransform.transformVectorsN(points1, points2, 3); + boxTransform.transformVectorsInverseN(points2, points1, 3); + + var pnum:int = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность боксу и добавляем такие точки в список контактов + var penetration:Number; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + v = points1[i]; + penetration = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace); + if (penetration > -epsilon) { + var cp:ContactPoint = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = penetration; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Вычисляет величину проникновения точки в бокс. + * + * @param hs вектор половинных размеров бокса + * @param p точка в системе координат бокса + * @param axisIndex индекс оси + * @param negativeFace если true, проверяется нижняя грань + * @return величина проникновения точки в бокс + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, negativeFace:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (negativeFace) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (negativeFace) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (negativeFace) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, вершины которой заданы в списке points1. Результат сохраняется в этом же списке. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 3; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Обрезает грань прямоугольника сторонами треугольника. Обрезка выполняется в системе координат и в проекции на + * плоскость треугольника. Входные вершины грани в количестве четырёх штук должны находиться в списке points1. + * Конечные вершины сохраняются в списке points2. + * + * @param tri треугольник + * @return количество врешин в конечном списке + */ + private function clipByTriangle(tri:CollisionTriangle):int { + var vnum:int = 4; + vnum = clipByLine(tri.v0, tri.e0, points1, vnum, points2); + if (vnum == 0) return 0; + vnum = clipByLine(tri.v1, tri.e1, points2, vnum, points1); + if (vnum == 0) return 0; + return clipByLine(tri.v2, tri.e2, points1, vnum, points2);; + } + + /** + * Обрезает полигон указанной прямой. + * + * @param linePoint точка на прямой + * @param lineDir единичный направляющий вектор прямой + * @param verticesIn список вершин исходного полигона + * @param vnum количество вершин исходного полигона + * @param verticesOut список, куда будут записаны вершины конечного полигона + * @return количество вершин конечного полигона + */ + private function clipByLine(linePoint:Vector3, lineDir:Vector3, verticesIn:Vector., vnum:int, verticesOut:Vector.):int { + var nx:Number = -lineDir.y; + var ny:Number = lineDir.x; + var offset:Number = linePoint.x*nx + linePoint.y*ny; + var v1:Vector3 = verticesIn[int(vnum - 1)]; + var offset1:Number = v1.x*nx + v1.y*ny; + var t:Number; + var v:Vector3; + var num:int = 0; + for (var i:int = 0; i < vnum; i++) { + var v2:Vector3 = verticesIn[i]; + var offset2:Number = v2.x*nx + v2.y*ny; + if (offset1 < offset) { + // Первая точка ребра во внешней полуплоскости + if (offset2 > offset) { + // Вторая точка ребра во внутренней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } else { + // Первая точка ребра во внутренней полуплоскости. Добавляем её в конечный список. + v = verticesOut[num]; + v.x = v1.x; + v.y = v1.y; + v.z = v1.z; + num++; + if (offset2 < offset) { + // Вторая точка ребра во внешней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } + v1 = v2; + offset1 = offset2; + } + return num; + } + + /** + * + * @param box + * @param tri + * @param toBox + * @param boxAxisIdx + * @param triAxisIdx + * @param contact + */ + private function findEdgesIntersection(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, boxAxisIdx:int, triAxisIdx:int, contact:Contact):Boolean { + // Определение точки и направляющего вектора ребра треугольника + var tmpx1:Number; + var tmpy1:Number; + var tmpz1:Number; + var tmpx2:Number; + var tmpy2:Number; + var tmpz2:Number; + switch (triAxisIdx) { + case 0: + tmpx1 = tri.e0.x; + tmpy1 = tri.e0.y; + tmpz1 = tri.e0.z; + tmpx2 = tri.v0.x; + tmpy2 = tri.v0.y; + tmpz2 = tri.v0.z; + break; + case 1: + tmpx1 = tri.e1.x; + tmpy1 = tri.e1.y; + tmpz1 = tri.e1.z; + tmpx2 = tri.v1.x; + tmpy2 = tri.v1.y; + tmpz2 = tri.v1.z; + break; + case 2: + tmpx1 = tri.e2.x; + tmpy1 = tri.e2.y; + tmpz1 = tri.e2.z; + tmpx2 = tri.v2.x; + tmpy2 = tri.v2.y; + tmpz2 = tri.v2.z; + break; + } + var triTransform:Matrix4 = tri.transform; + axis20.x = triTransform.a*tmpx1 + triTransform.b*tmpy1 + triTransform.c*tmpz1; + axis20.y = triTransform.e*tmpx1 + triTransform.f*tmpy1 + triTransform.g*tmpz1; + axis20.z = triTransform.i*tmpx1 + triTransform.j*tmpy1 + triTransform.k*tmpz1; + var x2:Number = triTransform.a*tmpx2 + triTransform.b*tmpy2 + triTransform.c*tmpz2 + triTransform.d; + var y2:Number = triTransform.e*tmpx2 + triTransform.f*tmpy2 + triTransform.g*tmpz2 + triTransform.h; + var z2:Number = triTransform.i*tmpx2 + triTransform.j*tmpy2 + triTransform.k*tmpz2 + triTransform.l; + + // Определение нормали контакта, точки и направляющего вектора ребра бокса + var boxTransform:Matrix4 = box.transform; + boxTransform.getAxis(boxAxisIdx, axis10); + + // Нормаль контакта + var v:Vector3 = contact.normal; + v.x = axis10.y*axis20.z - axis10.z*axis20.y; + v.y = axis10.z*axis20.x - axis10.x*axis20.z; + v.z = axis10.x*axis20.y - axis10.y*axis20.x; + k = 1/Math.sqrt(v.x*v.x + v.y*v.y + v.z*v.z); + v.x *= k; + v.y *= k; + v.z *= k; + // Разворот нормали в сторону бокса + if (v.x*toBox.x + v.y*toBox.y + v.z*toBox.z < 0) { + v.x = -v.x; + v.y = -v.y; + v.z = -v.z; + } + + var boxHalfLen:Number; + tmpx1 = box.hs.x; + tmpy1 = box.hs.y; + tmpz1 = box.hs.z; + // X + if (boxAxisIdx == 0) { + tmpx1 = 0; + boxHalfLen = box.hs.x; + } else { + if (boxTransform.a*v.x + boxTransform.e*v.y + boxTransform.i*v.z > 0) { + tmpx1 = -tmpx1; + if ((box.excludedFaces & 2) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 1) != 0) { + return false; + } + } + } + // Y + if (boxAxisIdx == 1) { + tmpy1 = 0; + boxHalfLen = box.hs.y; + } else { + if (boxTransform.b*v.x + boxTransform.f*v.y + boxTransform.j*v.z > 0) { + tmpy1 = -tmpy1; + if ((box.excludedFaces & 8) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 4) != 0) { + return false; + } + } + } + // Z + if (boxAxisIdx == 2) { + tmpz1 = 0; + boxHalfLen = box.hs.z; + } else { + if (boxTransform.c*v.x + boxTransform.g*v.y + boxTransform.k*v.z > 0) { + tmpz1 = -tmpz1; + if ((box.excludedFaces & 32) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 16) != 0) { + return false; + } + } + } + var x1:Number = boxTransform.a*tmpx1 + boxTransform.b*tmpy1 + boxTransform.c*tmpz1 + boxTransform.d; + var y1:Number = boxTransform.e*tmpx1 + boxTransform.f*tmpy1 + boxTransform.g*tmpz1 + boxTransform.h; + var z1:Number = boxTransform.i*tmpx1 + boxTransform.j*tmpy1 + boxTransform.k*tmpz1 + boxTransform.l; + + // Находим точку пересечения рёбер, решая систему уравнений + // axis10 - направляющий вектор ребра бокса + // x1, y1, z1 - средняя точка ребра бокса + // axis20 - направляющий вектор ребра треугольника + // x2, y2, z2 - начальная точка ребра треугольника + var k:Number = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + var vx:Number = x2 - x1; + var vy:Number = y2 - y1; + var vz:Number = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + + // Запись данных о контакте + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + v = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + v.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + v.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + v.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = v.x - boxTransform.d; + r.y = v.y - boxTransform.h; + r.z = v.z - boxTransform.l; + r = cp.r2; + r.x = v.x - triTransform.d; + r.y = v.y - triTransform.h; + r.z = v.z - triTransform.l; + return true; + } + + } +} diff --git a/2.0.0.0/src/alternativa/physics/collision/colliders/SpherePlaneCollider.as b/2.0.0.0/src/alternativa/physics/collision/colliders/SpherePlaneCollider.as new file mode 100644 index 0000000..30cd55f --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/colliders/SpherePlaneCollider.as @@ -0,0 +1,68 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + /** + * + */ + public class SpherePlaneCollider implements ICollider { + + private var normal:Vector3 = new Vector3(); + + /** + * + */ + public function SpherePlaneCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var sphere:RigidSphere = body1 as RigidSphere; +// var plane:RigidPlane; +// if (sphere == null) { +// sphere = body2 as RigidSphere; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// +// var dist:Number = sphere.state.pos.dot(normal) - offset; +// if (dist > sphere.r) return false; +// +// collisionInfo.body1 = sphere; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// collisionInfo.pcount = 1; +// +// var cp:ContactPoint = collisionInfo.points[0]; +// cp.penetration = sphere.r - dist; +// cp.pos.copy(normal).reverse().scale(sphere.r).add(sphere.state.pos); +// cp.r1.diff(cp.pos, sphere.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/collision/colliders/SphereSphereCollider.as b/2.0.0.0/src/alternativa/physics/collision/colliders/SphereSphereCollider.as new file mode 100644 index 0000000..228d6c9 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/colliders/SphereSphereCollider.as @@ -0,0 +1,68 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.math.Vector3; + + public class SphereSphereCollider implements ICollider { + + private var p1:Vector3 = new Vector3(); + private var p2:Vector3 = new Vector3(); + + public function SphereSphereCollider() { + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var s1:CollisionSphere; + var s2:CollisionSphere; + if (prim1.body != null) { + s1 = prim1 as CollisionSphere; + s2 = prim2 as CollisionSphere; + } else { + s1 = prim2 as CollisionSphere; + s2 = prim1 as CollisionSphere; + } + + s1.transform.getAxis(3, p1); + s2.transform.getAxis(3, p2); + var dx:Number = p1.x - p2.x; + var dy:Number = p1.y - p2.y; + var dz:Number = p1.z - p2.z; + var len:Number = dx*dx + dy*dy + dz*dz; + var sum:Number = s1.r + s2.r; + if (len > sum*sum) return false; + len = Math.sqrt(len); + dx /= len; + dy /= len; + dz /= len; + + contact.body1 = s1.body; + contact.body2 = s2.body; + contact.normal.x = dx; + contact.normal.y = dy; + contact.normal.z = dz; + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = sum - len; + cp.pos.x = p1.x - dx*s1.r; + cp.pos.y = p1.y - dy*s1.r; + cp.pos.z = p1.z - dz*s1.r; + cp.r1.diff(cp.pos, p1); + cp.r2.diff(cp.pos, p2); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/collision/primitives/.svn/all-wcprops b/2.0.0.0/src/alternativa/physics/collision/primitives/.svn/all-wcprops new file mode 100644 index 0000000..6c13c52 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/primitives/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 124 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/collision/primitives +END +CollisionTriangle.as +K 25 +svn:wc:ra_dav:version-url +V 145 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/collision/primitives/CollisionTriangle.as +END +CollisionSphere.as +K 25 +svn:wc:ra_dav:version-url +V 143 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/collision/primitives/CollisionSphere.as +END +CollisionBox.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/collision/primitives/CollisionBox.as +END +CollisionRect.as +K 25 +svn:wc:ra_dav:version-url +V 141 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/collision/primitives/CollisionRect.as +END diff --git a/2.0.0.0/src/alternativa/physics/collision/primitives/.svn/entries b/2.0.0.0/src/alternativa/physics/collision/primitives/.svn/entries new file mode 100644 index 0000000..bd6b72d --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/primitives/.svn/entries @@ -0,0 +1,76 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/collision/primitives +http://svndev.alternativaplatform.com + + + +2010-04-01T08:37:11.976264Z +30874 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +CollisionTriangle.as +file + + + + +2010-10-28T04:32:40.000000Z +8a9c2eea3bc27e016889d59eeb2b5669 +2010-04-01T08:37:11.976264Z +30874 +mike + +CollisionSphere.as +file + + + + +2010-10-28T04:32:40.000000Z +9e28a33c4fc342dc016b7b53ac018e62 +2010-04-01T08:37:11.976264Z +30874 +mike + +CollisionBox.as +file + + + + +2010-10-28T04:32:40.000000Z +b0bec39d69669900e723a5ce9dacbabf +2010-04-01T08:37:11.976264Z +30874 +mike + +CollisionRect.as +file + + + + +2010-10-28T04:32:40.000000Z +ae1c608cdba8f05b23ecb9ab1cd66cee +2010-04-01T08:37:11.976264Z +30874 +mike + diff --git a/2.0.0.0/src/alternativa/physics/collision/primitives/.svn/format b/2.0.0.0/src/alternativa/physics/collision/primitives/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/primitives/.svn/format @@ -0,0 +1 @@ +8 diff --git a/2.0.0.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base b/2.0.0.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base new file mode 100644 index 0000000..ac62c8b --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base @@ -0,0 +1,202 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * Ориентированный бокс. + */ + public class CollisionBox extends CollisionPrimitive { + + // Половинные размеры вдоль каждой из осей + public var hs:Vector3 = new Vector3(); + + public var excludedFaces:int; + + /** + * @param hs + * @param collisionGroup + */ + public function CollisionBox(hs:Vector3, collisionGroup:int) { + super(BOX, collisionGroup); + this.hs.copy(hs); + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + var t:Matrix4 = transform; + + var xx:Number; + var yy:Number; + var zz:Number; + + xx = t.a < 0 ? -t.a : t.a; + yy = t.b < 0 ? -t.b : t.b; + zz = t.c < 0 ? -t.c : t.c; + aabb.maxX = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minX = -aabb.maxX; + + xx = t.e < 0 ? -t.e : t.e; + yy = t.f < 0 ? -t.f : t.f; + zz = t.g < 0 ? -t.g : t.g; + aabb.maxY = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minY = -aabb.maxY; + + xx = t.i < 0 ? -t.i : t.i; + yy = t.j < 0 ? -t.j : t.j; + zz = t.k < 0 ? -t.k : t.k; + aabb.maxZ = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var box:CollisionBox = source as CollisionBox; + if (box == null) return this; + super.copyFrom(box); + hs.copy(box.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionBox(hs, collisionGroup); + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function raycast(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + var tMin:Number = -1; + var tMax:Number = 1e308; + var t1:Number; + var t2:Number; + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + // X + if (vx < epsilon && vx > -epsilon) { + if (ox < -hs.x || ox > hs.x) return -1; + } else { + t1 = (-hs.x - ox)/vx; + t2 = (hs.x - ox)/vx; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.x = -1; + normal.y = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.x = 1; + normal.y = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Y + if (vy < epsilon && vy > -epsilon) { + if (oy < -hs.y || oy > hs.y) return -1; + } else { + t1 = (-hs.y - oy)/vy; + t2 = (hs.y - oy)/vy; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.y = -1; + normal.x = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.y = 1; + normal.x = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Z + if (vz < epsilon && vz > -epsilon) { + if (oz < -hs.z || oz > hs.z) return -1; + } else { + t1 = (-hs.z - oz)/vz; + t2 = (hs.z - oz)/vz; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.z = -1; + normal.x = normal.y = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.z = 1; + normal.x = normal.y = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Перевод нормали в мировую систему координат + // Inlined normal.vDeltaTransformBy4(transform); + vx = normal.x; + vy = normal.y; + vz = normal.z; + normal.x = transform.a*vx + transform.b*vy + transform.c*vz; + normal.y = transform.e*vx + transform.f*vy + transform.g*vz; + normal.z = transform.i*vx + transform.j*vy + transform.k*vz; + + return tMin; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionBox hs=" + hs + "]"; + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base b/2.0.0.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base new file mode 100644 index 0000000..f070566 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base @@ -0,0 +1,141 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * Ориентированный прямоугольник. Задаётся половинами размеров вдоль осей X (ширина) и Y (длина) локальной системы + * координат примитива. Таким образом, прямоугольник лежит в плоскости XY, его стороны параллельны осям этой + * плоскости, а нормаль направлена вдоль локальной оси Z. + * + * Прямоугольник может быть одно- или двусторонним. В случае одностороннего прямоугольника, + * столкновения с внутренней стороны не регистрируются. + */ + public class CollisionRect extends CollisionPrimitive { + // Половинные размеры прямоугольника вдоль осей X и Y. Нормаль направлена вдоль оси Z. + public var hs:Vector3 = new Vector3(); + // Флаг указывает, является примитив одно- или двусторонним + public var twoSided:Boolean = true; + + // Малое значение. Используется для указания фиктивной высоты примитива и в функции определения пересечения с сегментом. + private static const EPSILON:Number = 0.005; + + /** + * Создаёт новый экземпляр примитива. + * + * @param hs половинные размерв прямоугольника вдоль осей X и Y. Значение z игнорируется. + * @param collisionGroup группа примитива + */ + public function CollisionRect(hs:Vector3, collisionGroup:int) { + super(RECT, collisionGroup); + this.hs.copy(hs); + } + + /** + * Расчитывает ограничивающий бокс прямоугольника. Для избежания проблем высота примитива принимается равной + * не нулю, а малому значению. + * + * @return + */ + override public function calculateAABB():BoundBox { + // Баунд бокс прямоугольника имеет минимальную высоту, отличную от нуля во избежание проблем с построением kd-дерева + var t:Matrix4 = transform; + var xx:Number = t.a < 0 ? -t.a : t.a; + var yy:Number = t.b < 0 ? -t.b : t.b; + var zz:Number = t.c < 0 ? -t.c : t.c; + aabb.maxX = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minX = -aabb.maxX; + + xx = t.e < 0 ? -t.e : t.e; + yy = t.f < 0 ? -t.f : t.f; + zz = t.g < 0 ? -t.g : t.g; + aabb.maxY = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minY = -aabb.maxY; + + xx = t.i < 0 ? -t.i : t.i; + yy = t.j < 0 ? -t.j : t.j; + zz = t.k < 0 ? -t.k : t.k; + aabb.maxZ = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var rect:CollisionRect = source as CollisionRect; + if (rect == null) return this; + super.copyFrom(rect); + hs.copy(rect.hs); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionRect hs=" + hs + "]"; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionRect(hs, collisionGroup); + } + + /** + * @param origin + * @param vector + * @param collisionGroup + * @param threshold + * @param normal + * @return + */ + override public function raycast(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + + // Проверка параллельности сегмента и плоскости примитива + if (vz > -threshold && vz < threshold) return -1; + var t:Number = -oz/vz; + if (t < 0) return -1; + // Проверка вхождения точки пересечения в прямоугольник + ox += vx*t; + oy += vy*t; + oz = 0; + if (ox < (-hs.x - threshold) || ox > (hs.x + threshold) || oy < (-hs.y - threshold) || oy > (hs.y + threshold)) return -1; + + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + return t; + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base b/2.0.0.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base new file mode 100644 index 0000000..e6f4e00 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base @@ -0,0 +1,78 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Vector3; + + /** + * Сфера. + */ + public class CollisionSphere extends CollisionPrimitive { + + // Радиус сферы + public var r:Number = 0; + + /** + * + * @param r + * @param collisionGroup + */ + public function CollisionSphere(r:Number, collisionGroup:int) { + super(SPHERE, collisionGroup); + this.r = r; + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + aabb.maxX = transform.d + r; + aabb.minX = transform.d - r; + + aabb.maxY = transform.h + r; + aabb.minY = transform.h - r; + + aabb.maxZ = transform.l + r; + aabb.minZ = transform.l - r; + + return aabb; + } + + /** + * @param origin + * @param vector + * @param threshold + * @param normal + * @return + */ + override public function raycast(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var px:Number = origin.x - transform.d; + var py:Number = origin.y - transform.h; + var pz:Number = origin.z - transform.l; + var k:Number = vector.x*px + vector.y*py + vector.z*pz; + if (k > 0) return -1; + var a:Number = vector.x*vector.x + vector.y*vector.y + vector.z*vector.z; + var D:Number = k*k - a*(px*px + py*py + pz*pz - r*r); + if (D < 0) return -1; + return -(k + Math.sqrt(D))/a; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var sphere:CollisionSphere = source as CollisionSphere; + if (sphere == null) return this; + super.copyFrom(sphere); + r = sphere.r; + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionSphere(r, collisionGroup); + } + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base b/2.0.0.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base new file mode 100644 index 0000000..2611feb --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base @@ -0,0 +1,247 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Vector3; + + /** + * Примитив треугольник. + */ + public class CollisionTriangle extends CollisionPrimitive { + + public var v0:Vector3 = new Vector3(); + public var v1:Vector3 = new Vector3(); + public var v2:Vector3 = new Vector3(); + + public var e0:Vector3 = new Vector3(); + public var e1:Vector3 = new Vector3(); + public var e2:Vector3 = new Vector3(); + +// public var len0:Number; +// public var len1:Number; +// public var len2:Number; +// + /** + * + * @param v0 + * @param v1 + * @param v2 + * @param collisionGroup + */ + public function CollisionTriangle(v0:Vector3, v1:Vector3, v2:Vector3, collisionGroup:int) { + super(TRIANGLE, collisionGroup); + initVertices(v0, v1, v2); + } + + /** + * Рассчитывает AABB примитива. + * + * @return ссылка на свой AABB + */ + override public function calculateAABB():BoundBox { + var epsilon:Number = 0.005; + var a:Number; + var b:Number; + var eps_c:Number = epsilon*transform.c; + var eps_g:Number = epsilon*transform.g; + var eps_k:Number = epsilon*transform.k; + + // Вершина 0 + // Ось X + a = v0.x*transform.a + v0.y*transform.b; + aabb.minX = aabb.maxX = a + eps_c; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v0.x*transform.e + v0.y*transform.f; + aabb.minY = aabb.maxY = a + eps_g; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v0.x*transform.i + v0.y*transform.j; + aabb.minZ = aabb.maxZ = a + eps_k; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 1 + // Ось X + a = v1.x*transform.a + v1.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v1.x*transform.e + v1.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v1.x*transform.i + v1.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 2 + // Ось X + a = v2.x*transform.a + v2.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v2.x*transform.e + v2.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v2.x*transform.i + v2.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + aabb.minX += transform.d; + aabb.maxX += transform.d; + + aabb.minY += transform.h; + aabb.maxY += transform.h; + + aabb.minZ += transform.l; + aabb.maxZ += transform.l; + + if (isNaN(aabb.minX) || isNaN(aabb.minY) || isNaN(aabb.minZ) || isNaN(aabb.maxX) || isNaN(aabb.maxY) || isNaN(aabb.maxZ)) { + var aa:int = 1; + trace(aabb); + } + + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function raycast(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + // Луч трансформируется в систему координат примитива, затем проверяется пересечение + + var vz:Number = vector.x*transform.c + vector.y*transform.g + vector.z*transform.k; + // Если луч параллелен плоскости птимитива, то пересечения нет + if (vz < epsilon && vz > -epsilon) return -1; + + var tx:Number = origin.x - transform.d; + var ty:Number = origin.y - transform.h; + var tz:Number = origin.z - transform.l; + + var oz:Number = tx*transform.c + ty*transform.g + tz*transform.k; + var t:Number = -oz/vz; + if (t < 0) return -1; + + var ox:Number = tx*transform.a + ty*transform.e + tz*transform.i; + var oy:Number = tx*transform.b + ty*transform.f + tz*transform.j; + + tx = ox + t*(vector.x*transform.a + vector.y*transform.e + vector.z*transform.i); + ty = oy + t*(vector.x*transform.b + vector.y*transform.f + vector.z*transform.j); + tz = oz + t*vz; + + // Проверка вхождения точки в треугольник + if ((e0.x*(ty - v0.y) - e0.y*(tx - v0.x) < 0) || (e1.x*(ty - v1.y) - e1.y*(tx - v1.x) < 0) || (e2.x*(ty - v2.y) - e2.y*(tx - v2.x) < 0)) return -1; + + // Запись нормали + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + + return t; + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + super.copyFrom(source); + var tri:CollisionTriangle = source as CollisionTriangle; + if (tri != null) { + v0.copy(tri.v0); + v1.copy(tri.v1); + v2.copy(tri.v2); + + e0.copy(tri.e0); + e1.copy(tri.e1); + e2.copy(tri.e2); + +// len0 = tri.len0; +// len1 = tri.len1; +// len2 = tri.len2; + } + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionTriangle v0=" + v0 + ", v1=" + v1 + ", v2=" + v2 + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionTriangle(v0, v1, v2, collisionGroup); + } + + /** + * + * @param v0 + * @param v1 + * @param v2 + */ + private function initVertices(v0:Vector3, v1:Vector3, v2:Vector3):void { + this.v0.copy(v0); + this.v1.copy(v1); + this.v2.copy(v2); + + e0.diff(v1, v0); +// len0 = e0.vLength(); + e0.normalize(); + + e1.diff(v2, v1); +// len1 = e1.vLength(); + e1.normalize(); + + e2.diff(v0, v2); +// len2 = e2.vLength(); + e2.normalize(); + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/collision/primitives/CollisionBox.as b/2.0.0.0/src/alternativa/physics/collision/primitives/CollisionBox.as new file mode 100644 index 0000000..ac62c8b --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/primitives/CollisionBox.as @@ -0,0 +1,202 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * Ориентированный бокс. + */ + public class CollisionBox extends CollisionPrimitive { + + // Половинные размеры вдоль каждой из осей + public var hs:Vector3 = new Vector3(); + + public var excludedFaces:int; + + /** + * @param hs + * @param collisionGroup + */ + public function CollisionBox(hs:Vector3, collisionGroup:int) { + super(BOX, collisionGroup); + this.hs.copy(hs); + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + var t:Matrix4 = transform; + + var xx:Number; + var yy:Number; + var zz:Number; + + xx = t.a < 0 ? -t.a : t.a; + yy = t.b < 0 ? -t.b : t.b; + zz = t.c < 0 ? -t.c : t.c; + aabb.maxX = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minX = -aabb.maxX; + + xx = t.e < 0 ? -t.e : t.e; + yy = t.f < 0 ? -t.f : t.f; + zz = t.g < 0 ? -t.g : t.g; + aabb.maxY = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minY = -aabb.maxY; + + xx = t.i < 0 ? -t.i : t.i; + yy = t.j < 0 ? -t.j : t.j; + zz = t.k < 0 ? -t.k : t.k; + aabb.maxZ = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var box:CollisionBox = source as CollisionBox; + if (box == null) return this; + super.copyFrom(box); + hs.copy(box.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionBox(hs, collisionGroup); + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function raycast(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + var tMin:Number = -1; + var tMax:Number = 1e308; + var t1:Number; + var t2:Number; + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + // X + if (vx < epsilon && vx > -epsilon) { + if (ox < -hs.x || ox > hs.x) return -1; + } else { + t1 = (-hs.x - ox)/vx; + t2 = (hs.x - ox)/vx; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.x = -1; + normal.y = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.x = 1; + normal.y = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Y + if (vy < epsilon && vy > -epsilon) { + if (oy < -hs.y || oy > hs.y) return -1; + } else { + t1 = (-hs.y - oy)/vy; + t2 = (hs.y - oy)/vy; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.y = -1; + normal.x = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.y = 1; + normal.x = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Z + if (vz < epsilon && vz > -epsilon) { + if (oz < -hs.z || oz > hs.z) return -1; + } else { + t1 = (-hs.z - oz)/vz; + t2 = (hs.z - oz)/vz; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.z = -1; + normal.x = normal.y = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.z = 1; + normal.x = normal.y = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Перевод нормали в мировую систему координат + // Inlined normal.vDeltaTransformBy4(transform); + vx = normal.x; + vy = normal.y; + vz = normal.z; + normal.x = transform.a*vx + transform.b*vy + transform.c*vz; + normal.y = transform.e*vx + transform.f*vy + transform.g*vz; + normal.z = transform.i*vx + transform.j*vy + transform.k*vz; + + return tMin; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionBox hs=" + hs + "]"; + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/collision/primitives/CollisionRect.as b/2.0.0.0/src/alternativa/physics/collision/primitives/CollisionRect.as new file mode 100644 index 0000000..f070566 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/primitives/CollisionRect.as @@ -0,0 +1,141 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * Ориентированный прямоугольник. Задаётся половинами размеров вдоль осей X (ширина) и Y (длина) локальной системы + * координат примитива. Таким образом, прямоугольник лежит в плоскости XY, его стороны параллельны осям этой + * плоскости, а нормаль направлена вдоль локальной оси Z. + * + * Прямоугольник может быть одно- или двусторонним. В случае одностороннего прямоугольника, + * столкновения с внутренней стороны не регистрируются. + */ + public class CollisionRect extends CollisionPrimitive { + // Половинные размеры прямоугольника вдоль осей X и Y. Нормаль направлена вдоль оси Z. + public var hs:Vector3 = new Vector3(); + // Флаг указывает, является примитив одно- или двусторонним + public var twoSided:Boolean = true; + + // Малое значение. Используется для указания фиктивной высоты примитива и в функции определения пересечения с сегментом. + private static const EPSILON:Number = 0.005; + + /** + * Создаёт новый экземпляр примитива. + * + * @param hs половинные размерв прямоугольника вдоль осей X и Y. Значение z игнорируется. + * @param collisionGroup группа примитива + */ + public function CollisionRect(hs:Vector3, collisionGroup:int) { + super(RECT, collisionGroup); + this.hs.copy(hs); + } + + /** + * Расчитывает ограничивающий бокс прямоугольника. Для избежания проблем высота примитива принимается равной + * не нулю, а малому значению. + * + * @return + */ + override public function calculateAABB():BoundBox { + // Баунд бокс прямоугольника имеет минимальную высоту, отличную от нуля во избежание проблем с построением kd-дерева + var t:Matrix4 = transform; + var xx:Number = t.a < 0 ? -t.a : t.a; + var yy:Number = t.b < 0 ? -t.b : t.b; + var zz:Number = t.c < 0 ? -t.c : t.c; + aabb.maxX = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minX = -aabb.maxX; + + xx = t.e < 0 ? -t.e : t.e; + yy = t.f < 0 ? -t.f : t.f; + zz = t.g < 0 ? -t.g : t.g; + aabb.maxY = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minY = -aabb.maxY; + + xx = t.i < 0 ? -t.i : t.i; + yy = t.j < 0 ? -t.j : t.j; + zz = t.k < 0 ? -t.k : t.k; + aabb.maxZ = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var rect:CollisionRect = source as CollisionRect; + if (rect == null) return this; + super.copyFrom(rect); + hs.copy(rect.hs); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionRect hs=" + hs + "]"; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionRect(hs, collisionGroup); + } + + /** + * @param origin + * @param vector + * @param collisionGroup + * @param threshold + * @param normal + * @return + */ + override public function raycast(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + + // Проверка параллельности сегмента и плоскости примитива + if (vz > -threshold && vz < threshold) return -1; + var t:Number = -oz/vz; + if (t < 0) return -1; + // Проверка вхождения точки пересечения в прямоугольник + ox += vx*t; + oy += vy*t; + oz = 0; + if (ox < (-hs.x - threshold) || ox > (hs.x + threshold) || oy < (-hs.y - threshold) || oy > (hs.y + threshold)) return -1; + + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + return t; + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/collision/primitives/CollisionSphere.as b/2.0.0.0/src/alternativa/physics/collision/primitives/CollisionSphere.as new file mode 100644 index 0000000..e6f4e00 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/primitives/CollisionSphere.as @@ -0,0 +1,78 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Vector3; + + /** + * Сфера. + */ + public class CollisionSphere extends CollisionPrimitive { + + // Радиус сферы + public var r:Number = 0; + + /** + * + * @param r + * @param collisionGroup + */ + public function CollisionSphere(r:Number, collisionGroup:int) { + super(SPHERE, collisionGroup); + this.r = r; + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + aabb.maxX = transform.d + r; + aabb.minX = transform.d - r; + + aabb.maxY = transform.h + r; + aabb.minY = transform.h - r; + + aabb.maxZ = transform.l + r; + aabb.minZ = transform.l - r; + + return aabb; + } + + /** + * @param origin + * @param vector + * @param threshold + * @param normal + * @return + */ + override public function raycast(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var px:Number = origin.x - transform.d; + var py:Number = origin.y - transform.h; + var pz:Number = origin.z - transform.l; + var k:Number = vector.x*px + vector.y*py + vector.z*pz; + if (k > 0) return -1; + var a:Number = vector.x*vector.x + vector.y*vector.y + vector.z*vector.z; + var D:Number = k*k - a*(px*px + py*py + pz*pz - r*r); + if (D < 0) return -1; + return -(k + Math.sqrt(D))/a; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var sphere:CollisionSphere = source as CollisionSphere; + if (sphere == null) return this; + super.copyFrom(sphere); + r = sphere.r; + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionSphere(r, collisionGroup); + } + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/collision/primitives/CollisionTriangle.as b/2.0.0.0/src/alternativa/physics/collision/primitives/CollisionTriangle.as new file mode 100644 index 0000000..2611feb --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/primitives/CollisionTriangle.as @@ -0,0 +1,247 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Vector3; + + /** + * Примитив треугольник. + */ + public class CollisionTriangle extends CollisionPrimitive { + + public var v0:Vector3 = new Vector3(); + public var v1:Vector3 = new Vector3(); + public var v2:Vector3 = new Vector3(); + + public var e0:Vector3 = new Vector3(); + public var e1:Vector3 = new Vector3(); + public var e2:Vector3 = new Vector3(); + +// public var len0:Number; +// public var len1:Number; +// public var len2:Number; +// + /** + * + * @param v0 + * @param v1 + * @param v2 + * @param collisionGroup + */ + public function CollisionTriangle(v0:Vector3, v1:Vector3, v2:Vector3, collisionGroup:int) { + super(TRIANGLE, collisionGroup); + initVertices(v0, v1, v2); + } + + /** + * Рассчитывает AABB примитива. + * + * @return ссылка на свой AABB + */ + override public function calculateAABB():BoundBox { + var epsilon:Number = 0.005; + var a:Number; + var b:Number; + var eps_c:Number = epsilon*transform.c; + var eps_g:Number = epsilon*transform.g; + var eps_k:Number = epsilon*transform.k; + + // Вершина 0 + // Ось X + a = v0.x*transform.a + v0.y*transform.b; + aabb.minX = aabb.maxX = a + eps_c; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v0.x*transform.e + v0.y*transform.f; + aabb.minY = aabb.maxY = a + eps_g; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v0.x*transform.i + v0.y*transform.j; + aabb.minZ = aabb.maxZ = a + eps_k; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 1 + // Ось X + a = v1.x*transform.a + v1.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v1.x*transform.e + v1.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v1.x*transform.i + v1.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 2 + // Ось X + a = v2.x*transform.a + v2.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v2.x*transform.e + v2.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v2.x*transform.i + v2.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + aabb.minX += transform.d; + aabb.maxX += transform.d; + + aabb.minY += transform.h; + aabb.maxY += transform.h; + + aabb.minZ += transform.l; + aabb.maxZ += transform.l; + + if (isNaN(aabb.minX) || isNaN(aabb.minY) || isNaN(aabb.minZ) || isNaN(aabb.maxX) || isNaN(aabb.maxY) || isNaN(aabb.maxZ)) { + var aa:int = 1; + trace(aabb); + } + + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function raycast(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + // Луч трансформируется в систему координат примитива, затем проверяется пересечение + + var vz:Number = vector.x*transform.c + vector.y*transform.g + vector.z*transform.k; + // Если луч параллелен плоскости птимитива, то пересечения нет + if (vz < epsilon && vz > -epsilon) return -1; + + var tx:Number = origin.x - transform.d; + var ty:Number = origin.y - transform.h; + var tz:Number = origin.z - transform.l; + + var oz:Number = tx*transform.c + ty*transform.g + tz*transform.k; + var t:Number = -oz/vz; + if (t < 0) return -1; + + var ox:Number = tx*transform.a + ty*transform.e + tz*transform.i; + var oy:Number = tx*transform.b + ty*transform.f + tz*transform.j; + + tx = ox + t*(vector.x*transform.a + vector.y*transform.e + vector.z*transform.i); + ty = oy + t*(vector.x*transform.b + vector.y*transform.f + vector.z*transform.j); + tz = oz + t*vz; + + // Проверка вхождения точки в треугольник + if ((e0.x*(ty - v0.y) - e0.y*(tx - v0.x) < 0) || (e1.x*(ty - v1.y) - e1.y*(tx - v1.x) < 0) || (e2.x*(ty - v2.y) - e2.y*(tx - v2.x) < 0)) return -1; + + // Запись нормали + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + + return t; + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + super.copyFrom(source); + var tri:CollisionTriangle = source as CollisionTriangle; + if (tri != null) { + v0.copy(tri.v0); + v1.copy(tri.v1); + v2.copy(tri.v2); + + e0.copy(tri.e0); + e1.copy(tri.e1); + e2.copy(tri.e2); + +// len0 = tri.len0; +// len1 = tri.len1; +// len2 = tri.len2; + } + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionTriangle v0=" + v0 + ", v1=" + v1 + ", v2=" + v2 + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionTriangle(v0, v1, v2, collisionGroup); + } + + /** + * + * @param v0 + * @param v1 + * @param v2 + */ + private function initVertices(v0:Vector3, v1:Vector3, v2:Vector3):void { + this.v0.copy(v0); + this.v1.copy(v1); + this.v2.copy(v2); + + e0.diff(v1, v0); +// len0 = e0.vLength(); + e0.normalize(); + + e1.diff(v2, v1); +// len1 = e1.vLength(); + e1.normalize(); + + e2.diff(v0, v2); +// len2 = e2.vLength(); + e2.normalize(); + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/collision/types/.svn/all-wcprops b/2.0.0.0/src/alternativa/physics/collision/types/.svn/all-wcprops new file mode 100644 index 0000000..4d02432 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/types/.svn/all-wcprops @@ -0,0 +1,23 @@ +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/collision/types +END +BoundBox.as +K 25 +svn:wc:ra_dav:version-url +V 131 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/collision/types/BoundBox.as +END +RayHit.as +K 25 +svn:wc:ra_dav:version-url +V 129 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/collision/types/RayHit.as +END +Ray.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/collision/types/Ray.as +END diff --git a/2.0.0.0/src/alternativa/physics/collision/types/.svn/entries b/2.0.0.0/src/alternativa/physics/collision/types/.svn/entries new file mode 100644 index 0000000..6ac934e --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/types/.svn/entries @@ -0,0 +1,64 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/collision/types +http://svndev.alternativaplatform.com + + + +2010-05-12T11:32:14.711790Z +34250 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +BoundBox.as +file + + + + +2010-10-28T04:32:40.000000Z +d92d4ab3dde698feb674fb9488f59678 +2009-09-09T14:02:48.134744Z +19633 +mike + +RayHit.as +file + + + + +2010-10-28T04:32:40.000000Z +ac9441a2e2f03f116092fa83c4f91149 +2010-05-12T11:32:14.711790Z +34250 +mike + +Ray.as +file + + + + +2010-10-28T04:32:40.000000Z +852d765db9177ad511768635d0f5c564 +2010-04-01T08:37:11.976264Z +30874 +mike + diff --git a/2.0.0.0/src/alternativa/physics/collision/types/.svn/format b/2.0.0.0/src/alternativa/physics/collision/types/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/types/.svn/format @@ -0,0 +1 @@ +8 diff --git a/2.0.0.0/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base b/2.0.0.0/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base new file mode 100644 index 0000000..b4d5278 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base @@ -0,0 +1,75 @@ +package alternativa.physics.collision.types { + + /** + * Ограничивающий параллелепипед. + */ + public class BoundBox { + + public var minX:Number = 1e308; + public var minY:Number = 1e308; + public var minZ:Number = 1e308; + public var maxX:Number = -1e308; + public var maxY:Number = -1e308; + public var maxZ:Number = -1e308; + + public function setSize(minX:Number, minY:Number, minZ:Number, maxX:Number, maxY:Number, maxZ:Number):void { + this.minX = minX; + this.minY = minY; + this.minZ = minZ; + this.maxX = maxX; + this.maxY = maxY; + this.maxZ = maxZ; + } + + public function addBoundBox(boundBox:BoundBox):void { + minX = (boundBox.minX < minX) ? boundBox.minX : minX; + minY = (boundBox.minY < minY) ? boundBox.minY : minY; + minZ = (boundBox.minZ < minZ) ? boundBox.minZ : minZ; + maxX = (boundBox.maxX > maxX) ? boundBox.maxX : maxX; + maxY = (boundBox.maxY > maxY) ? boundBox.maxY : maxY; + maxZ = (boundBox.maxZ > maxZ) ? boundBox.maxZ : maxZ; + } + + public function addPoint(x:Number, y:Number, z:Number):void { + if (x < minX) minX = x; + if (x > maxX) maxX = x; + if (y < minY) minY = y; + if (y > maxY) maxY = y; + if (z < minZ) minZ = z; + if (z > maxZ) maxZ = z; + } + + public function infinity():void { + minX = 1e308; + minY = 1e308; + minZ = 1e308; + maxX = -1e308; + maxY = -1e308; + maxZ = -1e308; + } + + public function intersects(bb:BoundBox, epsilon:Number):Boolean { + return !(minX > bb.maxX + epsilon || maxX < bb.minX - epsilon || minY > bb.maxY + epsilon || maxY < bb.minY - epsilon || minZ > bb.maxZ + epsilon || maxZ < bb.minZ - epsilon); + } + + public function copyFrom(boundBox:BoundBox):void { + minX = boundBox.minX; + minY = boundBox.minY; + minZ = boundBox.minZ; + maxX = boundBox.maxX; + maxY = boundBox.maxY; + maxZ = boundBox.maxZ; + } + + public function clone():BoundBox { + var clone:BoundBox = new BoundBox(); + clone.copyFrom(this); + return clone; + } + + public function toString():String { + return "BoundBox [" + minX + ", " + minY + ", " + minZ + " : " + maxX + ", " + maxY + ", " + maxZ + "]"; + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base b/2.0.0.0/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base new file mode 100644 index 0000000..26938ad --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base @@ -0,0 +1,13 @@ +package alternativa.physics.collision.types { + import alternativa.math.Vector3; + + /** + * Структура, описывающая луч и сегмент прямой в пространстве. + */ + public class Ray { + // Начало луча. Начальная точка сегмента на прямой луча. + public var origin:Vector3 = new Vector3(); + // Направляющий вектор луча. Длина вектора задаёт длину сегмента на луче. Конечная точка сегмента вычисляется как origin + dir. + public var vector:Vector3 = new Vector3(); + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/collision/types/.svn/text-base/RayHit.as.svn-base b/2.0.0.0/src/alternativa/physics/collision/types/.svn/text-base/RayHit.as.svn-base new file mode 100644 index 0000000..f908bc4 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/types/.svn/text-base/RayHit.as.svn-base @@ -0,0 +1,30 @@ +package alternativa.physics.collision.types { + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.math.Vector3; + + /** + * Структура описывает точку пересечения луча с физической геометрией. + */ + public class RayHit { + // Физический примитив, с которым пересекается луч + public var primitive:CollisionPrimitive; + // Глобальные координаты точки пересечения + public var pos:Vector3 = new Vector3(); + // Глобальная нормаль поверхности физического примитива в точке пересечения + public var normal:Vector3 = new Vector3(); + // Время пересечения (|pos - ray.origin|/|ray.dir|) + public var t:Number = 0; + + /** + * Копирует данные объекта-источника. + * + * @param source источник данных + */ + public function copy(source:RayHit):void { + primitive = source.primitive; + pos.copy(source.pos); + normal.copy(source.normal); + t = source.t; + } + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/collision/types/BoundBox.as b/2.0.0.0/src/alternativa/physics/collision/types/BoundBox.as new file mode 100644 index 0000000..b4d5278 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/types/BoundBox.as @@ -0,0 +1,75 @@ +package alternativa.physics.collision.types { + + /** + * Ограничивающий параллелепипед. + */ + public class BoundBox { + + public var minX:Number = 1e308; + public var minY:Number = 1e308; + public var minZ:Number = 1e308; + public var maxX:Number = -1e308; + public var maxY:Number = -1e308; + public var maxZ:Number = -1e308; + + public function setSize(minX:Number, minY:Number, minZ:Number, maxX:Number, maxY:Number, maxZ:Number):void { + this.minX = minX; + this.minY = minY; + this.minZ = minZ; + this.maxX = maxX; + this.maxY = maxY; + this.maxZ = maxZ; + } + + public function addBoundBox(boundBox:BoundBox):void { + minX = (boundBox.minX < minX) ? boundBox.minX : minX; + minY = (boundBox.minY < minY) ? boundBox.minY : minY; + minZ = (boundBox.minZ < minZ) ? boundBox.minZ : minZ; + maxX = (boundBox.maxX > maxX) ? boundBox.maxX : maxX; + maxY = (boundBox.maxY > maxY) ? boundBox.maxY : maxY; + maxZ = (boundBox.maxZ > maxZ) ? boundBox.maxZ : maxZ; + } + + public function addPoint(x:Number, y:Number, z:Number):void { + if (x < minX) minX = x; + if (x > maxX) maxX = x; + if (y < minY) minY = y; + if (y > maxY) maxY = y; + if (z < minZ) minZ = z; + if (z > maxZ) maxZ = z; + } + + public function infinity():void { + minX = 1e308; + minY = 1e308; + minZ = 1e308; + maxX = -1e308; + maxY = -1e308; + maxZ = -1e308; + } + + public function intersects(bb:BoundBox, epsilon:Number):Boolean { + return !(minX > bb.maxX + epsilon || maxX < bb.minX - epsilon || minY > bb.maxY + epsilon || maxY < bb.minY - epsilon || minZ > bb.maxZ + epsilon || maxZ < bb.minZ - epsilon); + } + + public function copyFrom(boundBox:BoundBox):void { + minX = boundBox.minX; + minY = boundBox.minY; + minZ = boundBox.minZ; + maxX = boundBox.maxX; + maxY = boundBox.maxY; + maxZ = boundBox.maxZ; + } + + public function clone():BoundBox { + var clone:BoundBox = new BoundBox(); + clone.copyFrom(this); + return clone; + } + + public function toString():String { + return "BoundBox [" + minX + ", " + minY + ", " + minZ + " : " + maxX + ", " + maxY + ", " + maxZ + "]"; + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/collision/types/Ray.as b/2.0.0.0/src/alternativa/physics/collision/types/Ray.as new file mode 100644 index 0000000..26938ad --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/types/Ray.as @@ -0,0 +1,13 @@ +package alternativa.physics.collision.types { + import alternativa.math.Vector3; + + /** + * Структура, описывающая луч и сегмент прямой в пространстве. + */ + public class Ray { + // Начало луча. Начальная точка сегмента на прямой луча. + public var origin:Vector3 = new Vector3(); + // Направляющий вектор луча. Длина вектора задаёт длину сегмента на луче. Конечная точка сегмента вычисляется как origin + dir. + public var vector:Vector3 = new Vector3(); + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/collision/types/RayHit.as b/2.0.0.0/src/alternativa/physics/collision/types/RayHit.as new file mode 100644 index 0000000..f908bc4 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/collision/types/RayHit.as @@ -0,0 +1,30 @@ +package alternativa.physics.collision.types { + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.math.Vector3; + + /** + * Структура описывает точку пересечения луча с физической геометрией. + */ + public class RayHit { + // Физический примитив, с которым пересекается луч + public var primitive:CollisionPrimitive; + // Глобальные координаты точки пересечения + public var pos:Vector3 = new Vector3(); + // Глобальная нормаль поверхности физического примитива в точке пересечения + public var normal:Vector3 = new Vector3(); + // Время пересечения (|pos - ray.origin|/|ray.dir|) + public var t:Number = 0; + + /** + * Копирует данные объекта-источника. + * + * @param source источник данных + */ + public function copy(source:RayHit):void { + primitive = source.primitive; + pos.copy(source.pos); + normal.copy(source.normal); + t = source.t; + } + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/constraints/.svn/all-wcprops b/2.0.0.0/src/alternativa/physics/constraints/.svn/all-wcprops new file mode 100644 index 0000000..e7bf240 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/constraints/.svn/all-wcprops @@ -0,0 +1,17 @@ +K 25 +svn:wc:ra_dav:version-url +V 115 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/constraints +END +Constraint.as +K 25 +svn:wc:ra_dav:version-url +V 129 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/constraints/Constraint.as +END +MaxDistanceConstraint.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/34250/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/constraints/MaxDistanceConstraint.as +END diff --git a/2.0.0.0/src/alternativa/physics/constraints/.svn/entries b/2.0.0.0/src/alternativa/physics/constraints/.svn/entries new file mode 100644 index 0000000..ac5da6d --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/constraints/.svn/entries @@ -0,0 +1,52 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/tags/2.0.0.0/src/alternativa/physics/constraints +http://svndev.alternativaplatform.com + + + +2010-04-01T08:37:11.976264Z +30874 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Constraint.as +file + + + + +2010-10-28T04:32:40.000000Z +34b6cfa491d92dc4089c03b867461a2e +2010-04-01T08:37:11.976264Z +30874 +mike + +MaxDistanceConstraint.as +file + + + + +2010-10-28T04:32:40.000000Z +d75e28da7563213b2cd3a3f005ac8946 +2010-04-01T08:37:11.976264Z +30874 +mike + diff --git a/2.0.0.0/src/alternativa/physics/constraints/.svn/format b/2.0.0.0/src/alternativa/physics/constraints/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/constraints/.svn/format @@ -0,0 +1 @@ +8 diff --git a/2.0.0.0/src/alternativa/physics/constraints/.svn/text-base/Constraint.as.svn-base b/2.0.0.0/src/alternativa/physics/constraints/.svn/text-base/Constraint.as.svn-base new file mode 100644 index 0000000..51a7d71 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/constraints/.svn/text-base/Constraint.as.svn-base @@ -0,0 +1,32 @@ +package alternativa.physics.constraints { + + import alternativa.physics.PhysicsScene; + + /** + * + */ + public class Constraint { + + public var satisfied:Boolean; + public var world:PhysicsScene; + + /** + * + */ + public function Constraint() { + } + + /** + * @param dt + */ + public function preProcess(dt:Number):void { + } + + /** + * @param dt + */ + public function apply(dt:Number):void { + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base b/2.0.0.0/src/alternativa/physics/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base new file mode 100644 index 0000000..b106578 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base @@ -0,0 +1,180 @@ +package alternativa.physics.constraints { + + import alternativa.physics.Body; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + /** + * + */ + public class MaxDistanceConstraint extends Constraint { + // Первое тело ограничения + public var body1:Body; + // Второе тело ограничения. Может быть null. + public var body2:Body; + // Первая точка крепления в системе координат первого тела + public var r1:Vector3; + // Вторая точка привязки в системе координат второго тела. В случае отсутствия второго тела, координаты задаются в мировой системе. + public var r2:Vector3; + // Максимально допустимое расстояние между точками крепления + public var maxDistance:Number; + // Радиус-вектор первой точки крепления, трансформированный без смещения в мировую систему координат + public var wr1:Vector3 = new Vector3(); + // Радиус-вектор второй точки крепления, трансформированный без смещения в мировую систему координат + public var wr2:Vector3 = new Vector3(); + // Минимальная скорость сближения для удовлетворения ограничения + private var minClosingVel:Number; + // Изменение относительной скорости на единицу импульса + private var velByUnitImpulseN:Number; + // Направление корректирующего импульса + private var impulseDirection:Vector3 = new Vector3(); + + // Вспомогательные переменные +// private static var _v:Vector3 = new Vector3(); +// private static var _v1:Vector3 = new Vector3(); +// private static var _v2:Vector3 = new Vector3(); + + /** + * @param body1 + * @param body2 + * @param r1 + * @param r2 + * @param maxDistance + */ + public function MaxDistanceConstraint(body1:Body, body2:Body, r1:Vector3, r2:Vector3, maxDistance:Number) { + super(); + this.body1 = body1; + this.body2 = body2; + this.r1 = r1.clone(); + this.r2 = r2.clone(); + this.maxDistance = maxDistance; + } + + /** + * @param dt + */ + override public function preProcess(dt:Number):void { + // Вычислим расстояние между точками + var m:Matrix3 = body1.baseMatrix; + wr1.x = m.a*r1.x + m.b*r1.y + m.c*r1.z; + wr1.y = m.e*r1.x + m.f*r1.y + m.g*r1.z; + wr1.z = m.i*r1.x + m.j*r1.y + m.k*r1.z; + if (body2 != null) { + m = body2.baseMatrix; + wr2.x = m.a*r2.x + m.b*r2.y + m.c*r2.z; + wr2.y = m.e*r2.x + m.f*r2.y + m.g*r2.z; + wr2.z = m.i*r2.x + m.j*r2.y + m.k*r2.z; + } else { + wr2.x = r2.x; + wr2.y = r2.y; + wr2.z = r2.z; + } + var p1:Vector3 = body1.state.pos; + impulseDirection.x = wr2.x - wr1.x - p1.x; + impulseDirection.y = wr2.y - wr1.y - p1.y; + impulseDirection.z = wr2.z - wr1.z - p1.z; + if (body2 != null) { + var p2:Vector3 = body2.state.pos; + impulseDirection.x += p2.x; + impulseDirection.y += p2.y; + impulseDirection.z += p2.z; + } + + var len:Number = Math.sqrt(impulseDirection.x*impulseDirection.x + impulseDirection.y*impulseDirection.y + impulseDirection.z*impulseDirection.z); + var delta:Number = len - maxDistance; + if (delta > 0) { + satisfied = false; + if (len < 0.001) { + impulseDirection.x = 1; + } else { + len = 1/len; + impulseDirection.x *= len; + impulseDirection.y *= len; + impulseDirection.z *= len; + } + minClosingVel = delta/(world.penResolutionSteps*dt); + if (minClosingVel > world.maxPenResolutionSpeed) { + minClosingVel = world.maxPenResolutionSpeed; + } + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + var x:Number; + var y:Number; + var z:Number; + var vx:Number; + var vy:Number; + var vz:Number; + velByUnitImpulseN = 0; + if (body1.movable) { +// velByUnitImpulseN += body1.invMass + _v.vCross2(wr1, impulseDirection).vTransformBy3(body1.invInertiaWorld).vCross(wr1).vDot(impulseDirection); + vx = wr1.y*impulseDirection.z - wr1.z*impulseDirection.y; + vy = wr1.z*impulseDirection.x - wr1.x*impulseDirection.z; + vz = wr1.x*impulseDirection.y - wr1.y*impulseDirection.x; + + m = body1.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr1.z - z*wr1.y; + vy = z*wr1.x - x*wr1.z; + vz = x*wr1.y - y*wr1.x; + velByUnitImpulseN += body1.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + if (body2 != null && body2.movable) { +// velByUnitImpulseN += body2.invMass + _v.vCross2(wr2, impulseDirection).vTransformBy3(body2.invInertiaWorld).vCross(wr2).vDot(impulseDirection); + vx = wr2.y*impulseDirection.z - wr2.z*impulseDirection.y; + vy = wr2.z*impulseDirection.x - wr2.x*impulseDirection.z; + vz = wr2.x*impulseDirection.y - wr2.y*impulseDirection.x; + + m = body2.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr2.z - z*wr2.y; + vy = z*wr2.x - x*wr2.z; + vz = x*wr2.y - y*wr2.x; + velByUnitImpulseN += body2.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + } else { + satisfied = true; + } + } + + /** + * @param dt + */ + override public function apply(dt:Number):void { + if (satisfied) return; + // Расчитываем проекцию скорости относительной скорости + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 + var vel:Vector3 = body1.state.velocity; + var rot:Vector3 = body1.state.rotation; +// v1Cross2(state.rotation, wr1); + var vx:Number = vel.x + rot.y*wr1.z - rot.z*wr1.y; + var vy:Number = vel.y + rot.z*wr1.x - rot.x*wr1.z; + var vz:Number = vel.z + rot.x*wr1.y - rot.y*wr1.x; + // V2 = V2_c + w2%r2 + if (body2 != null) { + vel = body2.state.velocity; + rot = body2.state.rotation; +// _v2.vCross2(state.rotation, wr2); + vx -= vel.x + rot.y*wr2.z - rot.z*wr2.y; + vy -= vel.y + rot.z*wr2.x - rot.x*wr2.z; + vz -= vel.z + rot.x*wr2.y - rot.y*wr2.x; + } + var closingVel:Number = vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + if (closingVel > minClosingVel) return; + var impulse:Number = (minClosingVel - closingVel)/velByUnitImpulseN; + if (body1.movable) { + body1.applyRelPosWorldImpulse(wr1, impulseDirection, impulse); + } + if (body2 != null && body2.movable) { + body2.applyRelPosWorldImpulse(wr2, impulseDirection, -impulse); + } + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/constraints/Constraint.as b/2.0.0.0/src/alternativa/physics/constraints/Constraint.as new file mode 100644 index 0000000..51a7d71 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/constraints/Constraint.as @@ -0,0 +1,32 @@ +package alternativa.physics.constraints { + + import alternativa.physics.PhysicsScene; + + /** + * + */ + public class Constraint { + + public var satisfied:Boolean; + public var world:PhysicsScene; + + /** + * + */ + public function Constraint() { + } + + /** + * @param dt + */ + public function preProcess(dt:Number):void { + } + + /** + * @param dt + */ + public function apply(dt:Number):void { + } + + } +} \ No newline at end of file diff --git a/2.0.0.0/src/alternativa/physics/constraints/MaxDistanceConstraint.as b/2.0.0.0/src/alternativa/physics/constraints/MaxDistanceConstraint.as new file mode 100644 index 0000000..b106578 --- /dev/null +++ b/2.0.0.0/src/alternativa/physics/constraints/MaxDistanceConstraint.as @@ -0,0 +1,180 @@ +package alternativa.physics.constraints { + + import alternativa.physics.Body; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + /** + * + */ + public class MaxDistanceConstraint extends Constraint { + // Первое тело ограничения + public var body1:Body; + // Второе тело ограничения. Может быть null. + public var body2:Body; + // Первая точка крепления в системе координат первого тела + public var r1:Vector3; + // Вторая точка привязки в системе координат второго тела. В случае отсутствия второго тела, координаты задаются в мировой системе. + public var r2:Vector3; + // Максимально допустимое расстояние между точками крепления + public var maxDistance:Number; + // Радиус-вектор первой точки крепления, трансформированный без смещения в мировую систему координат + public var wr1:Vector3 = new Vector3(); + // Радиус-вектор второй точки крепления, трансформированный без смещения в мировую систему координат + public var wr2:Vector3 = new Vector3(); + // Минимальная скорость сближения для удовлетворения ограничения + private var minClosingVel:Number; + // Изменение относительной скорости на единицу импульса + private var velByUnitImpulseN:Number; + // Направление корректирующего импульса + private var impulseDirection:Vector3 = new Vector3(); + + // Вспомогательные переменные +// private static var _v:Vector3 = new Vector3(); +// private static var _v1:Vector3 = new Vector3(); +// private static var _v2:Vector3 = new Vector3(); + + /** + * @param body1 + * @param body2 + * @param r1 + * @param r2 + * @param maxDistance + */ + public function MaxDistanceConstraint(body1:Body, body2:Body, r1:Vector3, r2:Vector3, maxDistance:Number) { + super(); + this.body1 = body1; + this.body2 = body2; + this.r1 = r1.clone(); + this.r2 = r2.clone(); + this.maxDistance = maxDistance; + } + + /** + * @param dt + */ + override public function preProcess(dt:Number):void { + // Вычислим расстояние между точками + var m:Matrix3 = body1.baseMatrix; + wr1.x = m.a*r1.x + m.b*r1.y + m.c*r1.z; + wr1.y = m.e*r1.x + m.f*r1.y + m.g*r1.z; + wr1.z = m.i*r1.x + m.j*r1.y + m.k*r1.z; + if (body2 != null) { + m = body2.baseMatrix; + wr2.x = m.a*r2.x + m.b*r2.y + m.c*r2.z; + wr2.y = m.e*r2.x + m.f*r2.y + m.g*r2.z; + wr2.z = m.i*r2.x + m.j*r2.y + m.k*r2.z; + } else { + wr2.x = r2.x; + wr2.y = r2.y; + wr2.z = r2.z; + } + var p1:Vector3 = body1.state.pos; + impulseDirection.x = wr2.x - wr1.x - p1.x; + impulseDirection.y = wr2.y - wr1.y - p1.y; + impulseDirection.z = wr2.z - wr1.z - p1.z; + if (body2 != null) { + var p2:Vector3 = body2.state.pos; + impulseDirection.x += p2.x; + impulseDirection.y += p2.y; + impulseDirection.z += p2.z; + } + + var len:Number = Math.sqrt(impulseDirection.x*impulseDirection.x + impulseDirection.y*impulseDirection.y + impulseDirection.z*impulseDirection.z); + var delta:Number = len - maxDistance; + if (delta > 0) { + satisfied = false; + if (len < 0.001) { + impulseDirection.x = 1; + } else { + len = 1/len; + impulseDirection.x *= len; + impulseDirection.y *= len; + impulseDirection.z *= len; + } + minClosingVel = delta/(world.penResolutionSteps*dt); + if (minClosingVel > world.maxPenResolutionSpeed) { + minClosingVel = world.maxPenResolutionSpeed; + } + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + var x:Number; + var y:Number; + var z:Number; + var vx:Number; + var vy:Number; + var vz:Number; + velByUnitImpulseN = 0; + if (body1.movable) { +// velByUnitImpulseN += body1.invMass + _v.vCross2(wr1, impulseDirection).vTransformBy3(body1.invInertiaWorld).vCross(wr1).vDot(impulseDirection); + vx = wr1.y*impulseDirection.z - wr1.z*impulseDirection.y; + vy = wr1.z*impulseDirection.x - wr1.x*impulseDirection.z; + vz = wr1.x*impulseDirection.y - wr1.y*impulseDirection.x; + + m = body1.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr1.z - z*wr1.y; + vy = z*wr1.x - x*wr1.z; + vz = x*wr1.y - y*wr1.x; + velByUnitImpulseN += body1.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + if (body2 != null && body2.movable) { +// velByUnitImpulseN += body2.invMass + _v.vCross2(wr2, impulseDirection).vTransformBy3(body2.invInertiaWorld).vCross(wr2).vDot(impulseDirection); + vx = wr2.y*impulseDirection.z - wr2.z*impulseDirection.y; + vy = wr2.z*impulseDirection.x - wr2.x*impulseDirection.z; + vz = wr2.x*impulseDirection.y - wr2.y*impulseDirection.x; + + m = body2.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr2.z - z*wr2.y; + vy = z*wr2.x - x*wr2.z; + vz = x*wr2.y - y*wr2.x; + velByUnitImpulseN += body2.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + } else { + satisfied = true; + } + } + + /** + * @param dt + */ + override public function apply(dt:Number):void { + if (satisfied) return; + // Расчитываем проекцию скорости относительной скорости + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 + var vel:Vector3 = body1.state.velocity; + var rot:Vector3 = body1.state.rotation; +// v1Cross2(state.rotation, wr1); + var vx:Number = vel.x + rot.y*wr1.z - rot.z*wr1.y; + var vy:Number = vel.y + rot.z*wr1.x - rot.x*wr1.z; + var vz:Number = vel.z + rot.x*wr1.y - rot.y*wr1.x; + // V2 = V2_c + w2%r2 + if (body2 != null) { + vel = body2.state.velocity; + rot = body2.state.rotation; +// _v2.vCross2(state.rotation, wr2); + vx -= vel.x + rot.y*wr2.z - rot.z*wr2.y; + vy -= vel.y + rot.z*wr2.x - rot.x*wr2.z; + vz -= vel.z + rot.x*wr2.y - rot.y*wr2.x; + } + var closingVel:Number = vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + if (closingVel > minClosingVel) return; + var impulse:Number = (minClosingVel - closingVel)/velByUnitImpulseN; + if (body1.movable) { + body1.applyRelPosWorldImpulse(wr1, impulseDirection, impulse); + } + if (body2 != null && body2.movable) { + body2.applyRelPosWorldImpulse(wr2, impulseDirection, -impulse); + } + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/META-INF/.svn/all-wcprops b/2.0.1.0-SNAPSHOT/META-INF/.svn/all-wcprops new file mode 100644 index 0000000..a2eb2fb --- /dev/null +++ b/2.0.1.0-SNAPSHOT/META-INF/.svn/all-wcprops @@ -0,0 +1,11 @@ +K 25 +svn:wc:ra_dav:version-url +V 81 +/!svn/ver/29440/platform/clients/fp10/libraries/AlternativaPhysics/trunk/META-INF +END +MANIFEST.MF +K 25 +svn:wc:ra_dav:version-url +V 93 +/!svn/ver/11541/platform/clients/fp10/libraries/AlternativaPhysics/trunk/META-INF/MANIFEST.MF +END diff --git a/2.0.1.0-SNAPSHOT/META-INF/.svn/entries b/2.0.1.0-SNAPSHOT/META-INF/.svn/entries new file mode 100644 index 0000000..84dcaee --- /dev/null +++ b/2.0.1.0-SNAPSHOT/META-INF/.svn/entries @@ -0,0 +1,40 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/trunk/META-INF +http://svndev.alternativaplatform.com + + + +2010-03-09T04:55:13.477965Z +29440 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +MANIFEST.MF +file + + + + +2010-10-28T04:32:36.000000Z +7b64dceb50a6905850ff00a0bfbe77eb +2009-04-20T17:15:55.260110Z +11541 +mike + diff --git a/2.0.1.0-SNAPSHOT/META-INF/.svn/format b/2.0.1.0-SNAPSHOT/META-INF/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/META-INF/.svn/format @@ -0,0 +1 @@ +8 diff --git a/2.0.1.0-SNAPSHOT/META-INF/.svn/text-base/MANIFEST.MF.svn-base b/2.0.1.0-SNAPSHOT/META-INF/.svn/text-base/MANIFEST.MF.svn-base new file mode 100644 index 0000000..5066a55 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/META-INF/.svn/text-base/MANIFEST.MF.svn-base @@ -0,0 +1 @@ +Bundle-Name: platform.clients.fp10.libraries.AlternativaPhysics diff --git a/2.0.1.0-SNAPSHOT/META-INF/MANIFEST.MF b/2.0.1.0-SNAPSHOT/META-INF/MANIFEST.MF new file mode 100644 index 0000000..5066a55 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/META-INF/MANIFEST.MF @@ -0,0 +1 @@ +Bundle-Name: platform.clients.fp10.libraries.AlternativaPhysics diff --git a/2.0.1.0-SNAPSHOT/pom.xml b/2.0.1.0-SNAPSHOT/pom.xml new file mode 100644 index 0000000..18e729e --- /dev/null +++ b/2.0.1.0-SNAPSHOT/pom.xml @@ -0,0 +1,33 @@ + + 4.0.0 + platform.clients.fp10.libraries + AlternativaPhysics + swc + 2.0.1.0-SNAPSHOT + + platform.clients.fp10.tools.maven + BasePom + 2.0.6.0 + + + scm:svn:http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/trunk + + + + platform.clients.fp10.libraries + AlternativaOSGi + swc + external + + + + + + platform.clients.fp10.libraries + AlternativaOSGi + swc + 2.0.3.0 + + + + \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/.svn/all-wcprops b/2.0.1.0-SNAPSHOT/src/.svn/all-wcprops new file mode 100644 index 0000000..908ba58 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 76 +/!svn/ver/34889/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src +END diff --git a/2.0.1.0-SNAPSHOT/src/.svn/entries b/2.0.1.0-SNAPSHOT/src/.svn/entries new file mode 100644 index 0000000..a7cb8fa --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/.svn/entries @@ -0,0 +1,31 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src +http://svndev.alternativaplatform.com + + + +2010-05-27T15:33:01.277505Z +34889 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +alternativa +dir + diff --git a/2.0.1.0-SNAPSHOT/src/.svn/format b/2.0.1.0-SNAPSHOT/src/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/.svn/format @@ -0,0 +1 @@ +8 diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/.svn/all-wcprops b/2.0.1.0-SNAPSHOT/src/alternativa/.svn/all-wcprops new file mode 100644 index 0000000..3038fed --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/.svn/all-wcprops @@ -0,0 +1,5 @@ +K 25 +svn:wc:ra_dav:version-url +V 88 +/!svn/ver/34889/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa +END diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/.svn/entries b/2.0.1.0-SNAPSHOT/src/alternativa/.svn/entries new file mode 100644 index 0000000..7058e13 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/.svn/entries @@ -0,0 +1,34 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa +http://svndev.alternativaplatform.com + + + +2010-05-27T15:33:01.277505Z +34889 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +physics +dir + +math +dir + diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/.svn/format b/2.0.1.0-SNAPSHOT/src/alternativa/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/.svn/format @@ -0,0 +1 @@ +8 diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/math/.svn/all-wcprops b/2.0.1.0-SNAPSHOT/src/alternativa/math/.svn/all-wcprops new file mode 100644 index 0000000..40bc464 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/math/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 93 +/!svn/ver/34889/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/math +END +Quaternion.as +K 25 +svn:wc:ra_dav:version-url +V 107 +/!svn/ver/34246/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/math/Quaternion.as +END +Vector3.as +K 25 +svn:wc:ra_dav:version-url +V 104 +/!svn/ver/34889/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/math/Vector3.as +END +Matrix3.as +K 25 +svn:wc:ra_dav:version-url +V 104 +/!svn/ver/34889/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/math/Matrix3.as +END +Matrix4.as +K 25 +svn:wc:ra_dav:version-url +V 104 +/!svn/ver/34889/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/math/Matrix4.as +END diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/math/.svn/entries b/2.0.1.0-SNAPSHOT/src/alternativa/math/.svn/entries new file mode 100644 index 0000000..b8405f7 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/math/.svn/entries @@ -0,0 +1,76 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/math +http://svndev.alternativaplatform.com + + + +2010-05-27T15:33:01.277505Z +34889 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Quaternion.as +file + + + + +2010-10-28T04:32:37.000000Z +e2ea4456ad77220ef1f54c8a3fbb7f8b +2010-05-12T11:30:54.476378Z +34246 +mike + +Vector3.as +file + + + + +2010-10-28T04:32:37.000000Z +5f76527114fec00b903b4ff64c1ba7de +2010-05-27T15:33:01.277505Z +34889 +mike + +Matrix3.as +file + + + + +2010-10-28T04:32:37.000000Z +d1839928cc2f65018baed51e1d5b344c +2010-05-27T15:33:01.277505Z +34889 +mike + +Matrix4.as +file + + + + +2010-10-28T04:32:37.000000Z +dc6a0c2e10cb220fd3cd5e409b404ea8 +2010-05-27T15:33:01.277505Z +34889 +mike + diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/math/.svn/format b/2.0.1.0-SNAPSHOT/src/alternativa/math/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/math/.svn/format @@ -0,0 +1 @@ +8 diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/math/.svn/text-base/Matrix3.as.svn-base b/2.0.1.0-SNAPSHOT/src/alternativa/math/.svn/text-base/Matrix3.as.svn-base new file mode 100644 index 0000000..3645720 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/math/.svn/text-base/Matrix3.as.svn-base @@ -0,0 +1,393 @@ +package alternativa.math { + import flash.geom.Vector3D; + + /** + * Матрица 3x3, используемая для описания ориентации. + */ + public class Matrix3 { + + public static const ZERO:Matrix3 = new Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0); + public static const IDENTITY:Matrix3 = new Matrix3(); + + public var a:Number; + public var b:Number; + public var c:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix3(a:Number = 1, b:Number = 0, c:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1) { + this.a = a; + this.b = b; + this.c = c; + + this.e = e; + this.f = f; + this.g = g; + + this.i = i; + this.j = j; + this.k = k; + } + + /** + * + * @return this + */ + public function toIdentity():Matrix3 { + a = f = k = 1; + b = c = e = g = i = j = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + var det:Number = 1/(-cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk); + + a = (ff*kk - gg*jj)*det; + b = (cc*jj - bb*kk)*det; + c = (bb*gg - cc*ff)*det; + e = (gg*ii - ee*kk)*det; + f = (aa*kk - cc*ii)*det; + g = (cc*ee - aa*gg)*det; + i = (ee*jj - ff*ii)*det; + j = (bb*ii - aa*jj)*det; + k = (aa*ff - bb*ee)*det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param m правый операнд умножения + */ + public function prepend(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param m правый операнд умножения + */ + public function prependTransposed(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.b + cc*m.c; + b = aa*m.e + bb*m.f + cc*m.g; + c = aa*m.i + bb*m.j + cc*m.k; + e = ee*m.a + ff*m.b + gg*m.c; + f = ee*m.e + ff*m.f + gg*m.g; + g = ee*m.i + ff*m.j + gg*m.k; + i = ii*m.a + jj*m.b + kk*m.c; + j = ii*m.e + jj*m.f + kk*m.g; + k = ii*m.i + jj*m.j + kk*m.k; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix3):Matrix3 { + a += m.a; + b += m.b; + c += m.c; + e += m.e; + f += m.f; + g += m.g; + i += m.i; + j += m.j; + k += m.k; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix3):Matrix3 { + a -= m.a; + b -= m.b; + c -= m.c; + e -= m.e; + f -= m.f; + g -= m.g; + i -= m.i; + j -= m.j; + k -= m.k; + + return this; + } + + /** + * + * @return this + */ + public function transpose():Matrix3 { + var tmp:Number = b; + b = e; + e = tmp; + tmp = c; + c = i; + i = tmp; + tmp = g; + g = j; + j = tmp; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorTransposed(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector3To3D(vin:Vector3, vout:Vector3D):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * + * @param v + */ + public function createSkewSymmetric(v:Vector3):Matrix3 { + a = f = k = 0; + b = -v.z; + c = v.y; + e = v.z; + g = -v.x; + i = -v.y; + j = v.x; + return this; + } + + /** + * Копирование значений указанной матрицы. + * + * @param m матрица, значения которой копируются + */ + public function copy(m:Matrix3):Matrix3 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * Устанавливает вращательную часть матрицы на основе указанных эйлеровых углов. + * + * @param rx угол поворота по оси X в радианах + * @param ry угол поворота по оси Y в радианах + * @param rz угол поворота по оси Z в радианах + */ + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):void { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + } + + /** + * Метод формирует матрицу поворота на заданный угол относительно заданной оси. + * + * @param axis нормализованный вектор, задающий ось, относительно которой выполняется поворот + * @param angle угол поворота в радианах + */ + public function fromAxisAngle(axis:Vector3, angle:Number):void { + var c1:Number = Math.cos(angle); + var s:Number = Math.sin(angle); + var t:Number = 1 - c1; + var x:Number = axis.x; + var y:Number = axis.y; + var z:Number = axis.z; + + a = t*x*x + c1; + b = t*x*y - z*s; + c = t*x*z + y*s; + + e = t*x*y + z*s; + f = t*y*y + c1; + g = t*y*z - x*s; + + i = t*x*z - y*s; + j = t*y*z + x*s; + k = t*z*z + c1; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix3 { + return new Matrix3(a, b, c, e, f, g, i, j, k); + } + + /** + * Формирует строковое представление объекта. + * + * @return строковое представление объекта + */ + public function toString():String { + return "[Matrix3 (" + a + ", " + b + ", " + c + "), (" + e + ", " + f + ", " + g + "), (" + i + ", " + j + ", " + k + ")]"; + } + + /** + * @param angles + */ + public function getEulerAngles(angles:Vector3):void { + if (-1 < i && i < 1) { + angles.x = Math.atan2(j, k); + angles.y = -Math.asin(i); + angles.z = Math.atan2(e, a); + } else { + angles.x = 0; + angles.y = (i <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-b, f); + } + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/math/.svn/text-base/Matrix4.as.svn-base b/2.0.1.0-SNAPSHOT/src/alternativa/math/.svn/text-base/Matrix4.as.svn-base new file mode 100644 index 0000000..ee9cdd0 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/math/.svn/text-base/Matrix4.as.svn-base @@ -0,0 +1,572 @@ +package alternativa.math { + + /** + * + */ + public class Matrix4 { + + public static const IDENTITY:Matrix4 = new Matrix4(); + + public var a:Number; + public var b:Number; + public var c:Number; + public var d:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + public var h:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + public var l:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix4(a:Number = 1, b:Number = 0, c:Number = 0, d:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, h:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1, l:Number = 0) { + this.a = a; + this.b = b; + this.c = c; + this.d = d; + + this.e = e; + this.f = f; + this.g = g; + this.h = h; + + this.i = i; + this.j = j; + this.k = k; + this.l = l; + } + + /** + * + * @return this + */ + public function toIdentity():Matrix4 { + a = f = k = 1; + b = c = e = g = i = j = d = h = l = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + var det:Number = -cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk; + + a = (-gg*jj + ff*kk)/det; + b = (cc*jj - bb*kk)/det; + c = (-cc*ff + bb*gg)/det; + d = (dd*gg*jj - cc*hh*jj - dd*ff*kk + bb*hh*kk + cc*ff*ll - bb*gg*ll)/det; + e = (gg*ii - ee*kk)/det; + f = (-cc*ii + aa*kk)/det; + g = (cc*ee - aa*gg)/det; + h = (cc*hh*ii - dd*gg*ii + dd*ee*kk - aa*hh*kk - cc*ee*ll + aa*gg*ll)/det; + i = (-ff*ii + ee*jj)/det; + j = (bb*ii - aa*jj)/det; + k = (-bb*ee + aa*ff)/det; + l = (dd*ff*ii - bb*hh*ii - dd*ee*jj + aa*hh*jj + bb*ee*ll - aa*ff*ll)/det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + d = m.a*dd + m.b*hh + m.c*ll + m.d; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + h = m.e*dd + m.f*hh + m.g*ll + m.h; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + l = m.i*dd + m.j*hh + m.k*ll + m.l; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param m правый операнд умножения + */ + public function prepend(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + d = aa*m.d + bb*m.h + cc*m.l + dd; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + h = ee*m.d + ff*m.h + gg*m.l + hh; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + l = ii*m.d + jj*m.h + kk*m.l + ll; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix4):Matrix4 { + a += m.a; + b += m.b; + c += m.c; + d += m.d; + e += m.e; + f += m.f; + g += m.g; + h += m.h; + i += m.i; + j += m.j; + k += m.k; + l += m.l; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix4):Matrix4 { + a -= m.a; + b -= m.b; + c -= m.c; + d -= m.d; + e -= m.e; + f -= m.f; + g -= m.g; + h -= m.h; + i -= m.i; + j -= m.j; + k -= m.k; + l -= m.l; + + return this; + } + + /** + * Трансформирует координаты точки. + * + * @param vin входная точка + * @param vout вектор, в который записывается результат трансформации + */ + public function transformPoint(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Выполняет обратную трансформацию точки. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входная точка + * @param vout вектор, в который записывается результат трансформации + */ + public function transformPointTransposed(vin:Vector3, vout:Vector3):void { + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + + /** + * + * @param arrin + * @param arrout + */ + public function transformPoints(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param arrout + * @param len + */ + public function transformPointsN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param arrout + */ + public function transformPointsTransposed(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param arrin + * @param arrout + */ + public function transformPointsTransposedN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param idx + * @param axis + */ + public function getAxis(idx:int, axis:Vector3):void { + switch (idx) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + case 3: + axis.x = d; + axis.y = h; + axis.z = l; + return; + } + } + + /** + * + * @param xAxis + * @param yAxis + * @param zAxis + * @param pos + */ + public function setAxes(xAxis:Vector3, yAxis:Vector3, zAxis:Vector3, pos:Vector3):void { + a = xAxis.x; + e = xAxis.y; + i = xAxis.z; + + b = yAxis.x; + f = yAxis.y; + j = yAxis.z; + + c = zAxis.x; + g = zAxis.y; + k = zAxis.z; + + d = pos.x; + h = pos.y; + l = pos.z; + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorTransposed(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Копирование значений указанной матрицы. + * + * @param m матрица, значения которой копируются + */ + public function copy(m:Matrix4):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = m.d; + e = m.e; + f = m.f; + g = m.g; + h = m.h; + i = m.i; + j = m.j; + k = m.k; + l = m.l; + + return this; + } + + /** + * + * @param m + * @param offset + * @return this + */ + public function setFromMatrix3(m:Matrix3, offset:Vector3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = offset.x; + e = m.e; + f = m.f; + g = m.g; + h = offset.y; + i = m.i; + j = m.j; + k = m.k; + l = offset.z; + + return this; + } + + /** + * + * @param m + * @return this + */ + public function setOrientationFromMatrix3(m:Matrix3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * + * @param angles + */ + public function getEulerAngles(angles:Vector3):void { + if (-1 < i && i < 1) { + angles.x = Math.atan2(j, k); + angles.y = -Math.asin(i); + angles.z = Math.atan2(e, a); + } else { + angles.x = 0; + angles.y = (i <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-b, f); + } + } + + /** + * Устанавливает координаты матрицы. + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + d = pos.x; + h = pos.y; + l = pos.z; + } + + /** + * Устанавливает координаты матрицы. + * + * @param x + * @param y + * @param z + */ + public function setPositionXYZ(x:Number, y:Number, z:Number):void { + d = x; + h = y; + l = z; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix4 { + return new Matrix4(a, b, c, d, e, f, g, h, i, j, k, l); + } + + /** + * Формирует строковое представление объекта. + * + * @return строковое представление объекта + */ + public function toString():String { + return "[Matrix4 [" + a.toFixed(3) + " " + b.toFixed(3) + " " + c.toFixed(3) + " " + d.toFixed(3) + "] [" + e.toFixed(3) + " " + f.toFixed(3) + " " + g.toFixed(3) + " " + h.toFixed(3) + "] [" + i.toFixed(3) + " " + j.toFixed(3) + " " + k.toFixed(3) + " " + l.toFixed(3) + "]]"; + } + + /** + * + */ + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):void { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + } + + /** + * Метод формирует матрицу поворота на заданный угол относительно заданной оси. + * + * @param axis нормализованный вектор, задающий ось, относительно которой выполняется поворот + * @param angle угол поворота в радианах + */ + public function fromAxisAngle(axis:Vector3, angle:Number):void { + var c1:Number = Math.cos(angle); + var s:Number = Math.sin(angle); + var t:Number = 1 - c1; + var x:Number = axis.x; + var y:Number = axis.y; + var z:Number = axis.z; + + a = t*x*x + c1; + b = t*x*y - z*s; + c = t*x*z + y*s; + + e = t*x*y + z*s; + f = t*y*y + c1; + g = t*y*z - x*s; + + i = t*x*z - y*s; + j = t*y*z + x*s; + k = t*z*z + c1; + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/math/.svn/text-base/Quaternion.as.svn-base b/2.0.1.0-SNAPSHOT/src/alternativa/math/.svn/text-base/Quaternion.as.svn-base new file mode 100644 index 0000000..2c19cbb --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/math/.svn/text-base/Quaternion.as.svn-base @@ -0,0 +1,515 @@ +package alternativa.math { + import flash.geom.Vector3D; + + /** + * Кватернион. + */ + public class Quaternion { + + public static const IDENTITY:Quaternion = new Quaternion(); + + public var w:Number; + public var x:Number; + public var y:Number; + public var z:Number; + + public static function multiply(q1:Quaternion, q2:Quaternion, result:Quaternion):void { + result.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z; + result.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y; + result.y = q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z; + result.z = q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x; + } + + public static function createFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngle(axis, angle); + return q; + } + + public static function createFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngleComponents(x, y, z, angle); + return q; + } + + /** + * + * @param x + * @param y + * @param z + */ + public static function createFromEulerAnglesXYZ(x:Number, y:Number, z:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngleComponents(1, 0, 0, x); + + _q.setFromAxisAngleComponents(0, 1, 0, y); + q.append(_q); + q.normalize(); + + _q.setFromAxisAngleComponents(0, 0, 1, z); + q.append(_q); + q.normalize(); + return q; + } + + + private static var _q:Quaternion = new Quaternion(); + + public function Quaternion(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0) { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + } + + public function reset(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0):Quaternion { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + return this; + } + + public function normalize():Quaternion { + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * Умножает на указанный кватернион слева: this * q + * + * @param q множитель + */ + public function prepend(q:Quaternion):Quaternion { + var ww:Number = w*q.w - x*q.x - y*q.y - z*q.z; + var xx:Number = w*q.x + x*q.w + y*q.z - z*q.y; + var yy:Number = w*q.y + y*q.w + z*q.x - x*q.z; + var zz:Number = w*q.z + z*q.w + x*q.y - y*q.x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Умножает на указанный кватернион справа: q * this + * + * @param q множитель + */ + public function append(q:Quaternion):Quaternion { + var ww:Number = q.w*w - q.x*x - q.y*y - q.z*z; + var xx:Number = q.w*x + q.x*w + q.y*z - q.z*y; + var yy:Number = q.w*y + q.y*w + q.z*x - q.x*z; + var zz:Number = q.w*z + q.z*w + q.x*y - q.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * + * @param v + */ + public function rotateByVector(v:Vector3):Quaternion { + var ww:Number = -v.x*x - v.y*y - v.z*z; + var xx:Number = v.x*w + v.y*z - v.z*y; + var yy:Number = v.y*w + v.z*x - v.x*z; + var zz:Number = v.z*w + v.x*y - v.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Добавляет вращение, приданное вектором угловой скорости за указанное время. + * + * @param v + * @param scale + */ + public function addScaledVector(v:Vector3, scale:Number):Quaternion { + var vx:Number = v.x*scale; + var vy:Number = v.y*scale; + var vz:Number = v.z*scale; + var ww:Number = -x*vx - y*vy - z*vz; + var xx:Number = vx*w + vy*z - vz*y; + var yy:Number = vy*w + vz*x - vx*z; + var zz:Number = vz*w + vx*y - vy*x; + w += 0.5*ww; + x += 0.5*xx; + y += 0.5*yy; + z += 0.5*zz; + // inlined normalize + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param m + * @return this + */ + public function toMatrix3(m:Matrix3):Quaternion { + var xx2:Number = 2.0*x*x; + var yy2:Number = 2.0*y*y; + var zz2:Number = 2.0*z*z; + var xy2:Number = 2.0*x*y; + var yz2:Number = 2.0*y*z; + var zx2:Number = 2.0*z*x; + var wx2:Number = 2.0*w*x; + var wy2:Number = 2.0*w*y; + var wz2:Number = 2.0*w*z; + + m.a = 1.0 - yy2 - zz2; + m.b = xy2 - wz2; + m.c = zx2 + wy2; + + m.e = xy2 + wz2; + m.f = 1.0 - xx2 - zz2; + m.g = yz2 - wx2; + + m.i = zx2 - wy2; + m.j = yz2 + wx2; + m.k = 1.0 - xx2 - yy2; + return this; + } + + /** + * + * @param m + * @return this + */ + public function toMatrix4(m:Matrix4):Quaternion { + var xx2:Number = 2.0*x*x; + var yy2:Number = 2.0*y*y; + var zz2:Number = 2.0*z*z; + var xy2:Number = 2.0*x*y; + var yz2:Number = 2.0*y*z; + var zx2:Number = 2.0*z*x; + var wx2:Number = 2.0*w*x; + var wy2:Number = 2.0*w*y; + var wz2:Number = 2.0*w*z; + + m.a = 1.0 - yy2 - zz2; + m.b = xy2 - wz2; + m.c = zx2 + wy2; + + m.e = xy2 + wz2; + m.f = 1.0 - xx2 - zz2; + m.g = yz2 - wx2; + + m.i = zx2 - wy2; + m.j = yz2 + wx2; + m.k = 1.0 - xx2 - yy2; + return this; + } + + /** + * + * @return длина + */ + public function length():Number { + return Math.sqrt(w*w + x*x + y*y + z*z); + } + + /** + * + * @return квадрат длины + */ + public function lengthSqr():Number { + return w*w + x*x + y*y + z*z; + } + + /** + * + * @param axis + * @param angle + * @return this + */ + public function setFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z); + x = axis.x*k; + y = axis.y*k; + z = axis.z*k; + return this; + } + + /** + * + * @param x + * @param y + * @param z + * @param angle + */ + public function setFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(x*x + y*y + z*z); + this.x = x*k; + this.y = y*k; + this.z = z*k; + return this; + } + + /** + * + * @param v + */ + public function toAxisVector(v:Vector3 = null):Vector3 { + if (w < -1 || w > 1) { + normalize(); + } + if (v == null) { + v = new Vector3(); + } + if (w > -1 && w < 1) { + if (w == 0) { + v.x = x; + v.y = y; + v.z = z; + } else { + var angle:Number = 2*Math.acos(w); + var coeff:Number = 1/Math.sqrt(1 - w*w); + v.x = x*coeff*angle; + v.y = y*coeff*angle; + v.z = z*coeff*angle; + } + } else { + v.x = 0; + v.y = 0; + v.z = 0; + } + return v; + } + + /** + * + * @param angles + */ + public function getEulerAngles(angles:Vector3):Vector3 { + var qi2:Number = 2*x*x; + var qj2:Number = 2*y*y; + var qk2:Number = 2*z*z; + var qij:Number = 2*x*y; + var qjk:Number = 2*y*z; + var qki:Number = 2*z*x; + var qri:Number = 2*w*x; + var qrj:Number = 2*w*y; + var qrk:Number = 2*w*z; + + var aa:Number = 1 - qj2 - qk2; + var bb:Number = qij - qrk; + var ee:Number = qij + qrk; + var ff:Number = 1 - qi2 - qk2; + var ii:Number = qki - qrj; + var jj:Number = qjk + qri; + var kk:Number = 1 - qi2 - qj2; + + if (-1 < ii && ii < 1) { + if (angles == null) { + angles = new Vector3(Math.atan2(jj, kk), -Math.asin(ii), Math.atan2(ee, aa)); + } else { + angles.x = Math.atan2(jj, kk); + angles.y = -Math.asin(ii); + angles.z = Math.atan2(ee, aa); + } + } else { + if (angles == null) { + angles = new Vector3(0, 0.5*((ii <= -1) ? Math.PI : -Math.PI), Math.atan2(-bb, ff)); + } else { + angles.x = 0; + angles.y = (ii <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-bb, ff); + } + } + return angles; + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setFromEulerAnglesXYZ(x:Number, y:Number, z:Number):void { + setFromAxisAngleComponents(1, 0, 0, x); + + _q.setFromAxisAngleComponents(0, 1, 0, y); + append(_q); + normalize(); + + _q.setFromAxisAngleComponents(0, 0, 1, z); + append(_q); + normalize(); + } + + /** + * + */ + public function conjugate():void { + x = -x; + y = -y; + z = -z; + } + + /** + * Выполняет линейную интерполяцию. + * + * @param q1 начало отрезка + * @param q2 конец отрезка + * @param t время, обычно задаётся в интервале [0, 1] + */ + public function nlerp(q1:Quaternion, q2:Quaternion, t:Number):Quaternion { + var d:Number = 1 - t; + w = q1.w*d + q2.w*t; + x = q1.x*d + q2.x*t; + y = q1.y*d + q2.y*t; + z = q1.z*d + q2.z*t; + // inlined normalize + d = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param q + * @return this + */ + public function subtract(q:Quaternion):Quaternion { + w -= q.w; + x -= q.x; + y -= q.y; + z -= q.z; + return this; + } + + /** + * + * @param q1 + * @param q2 + * @return this + */ + public function diff(q1:Quaternion, q2:Quaternion):Quaternion { + w = q2.w - q1.w; + x = q2.x - q1.x; + y = q2.y - q1.y; + z = q2.z - q1.z; + return this; + } + + /** + * + * @param q + * @return this + */ + public function copy(q:Quaternion):Quaternion { + w = q.w; + x = q.x; + y = q.y; + z = q.z; + return this; + } + + /** + * @param result + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + result.w = w; + return result; + } + + /** + * + * @return клон + */ + public function clone():Quaternion { + return new Quaternion(w, x, y, z); + } + + /** + * Формирует строковое представление объекта. + * + * @return строковое представление объекта + */ + public function toString():String { + return "[" + w + ", " + x + ", " + y + ", " + z + "]"; + } + + /** + * Выполняет сферическую интерполяцию между двумя заданными кватернионами по наименьшему расстоянию. + * + * @param a первый кватерион + * @param b второй кватернион + * @param t параметр интерполяции, обычно принадлежит отрезку [0, 1] + * @return this + */ + public function slerp(a:Quaternion, b:Quaternion, t:Number):Quaternion { + var flip:Number = 1; + // Так как одна и та же ориентация представляется двумя значениями q и -q, нужно сменить знак одного из кватернионов + // если скалярное произведение отрицательно. Иначе будет получено интерполированное значение по наибольшему расстоянию. + var cosine:Number = a.w*b.w + a.x*b.x + a.y*b.y + a.z*b.z; + if (cosine < 0) { + cosine = -cosine; + flip = -1; + } + + if ((1 - cosine) < 0.001) { + // Вблизи нуля используется линейная интерполяция + var k1:Number = 1 - t; + var k2:Number = t*flip; + w = a.w*k1 + b.w*k2; + x = a.x*k1 + b.x*k2; + y = a.y*k1 + b.y*k2; + z = a.z*k1 + b.z*k2; + normalize(); + } else { + var theta:Number = Math.acos(cosine); + var sine:Number = Math.sin(theta); + var beta:Number = Math.sin((1 - t)*theta)/sine; + var alpha:Number = Math.sin(t*theta)/sine*flip; + w = a.w*beta + b.w*alpha; + x = a.x*beta + b.x*alpha; + y = a.y*beta + b.y*alpha; + z = a.z*beta + b.z*alpha; + } + + return this; + } + + } +} diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/math/.svn/text-base/Vector3.as.svn-base b/2.0.1.0-SNAPSHOT/src/alternativa/math/.svn/text-base/Vector3.as.svn-base new file mode 100644 index 0000000..61c0a8f --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/math/.svn/text-base/Vector3.as.svn-base @@ -0,0 +1,392 @@ +package alternativa.math { + import flash.geom.Vector3D; + + /** + * + */ + public class Vector3 { + + public static const ZERO:Vector3 = new Vector3(0, 0, 0); + public static const X_AXIS:Vector3 = new Vector3(1, 0, 0); + public static const Y_AXIS:Vector3 = new Vector3(0, 1, 0); + public static const Z_AXIS:Vector3 = new Vector3(0, 0, 1); + + public static const RIGHT:Vector3 = new Vector3(1, 0, 0); + public static const LEFT:Vector3 = new Vector3(-1, 0, 0); + + public static const FORWARD:Vector3 = new Vector3(0, 1, 0); + public static const BACK:Vector3 = new Vector3(0, -1, 0); + + public static const UP:Vector3 = new Vector3(0, 0, 1); + public static const DOWN:Vector3 = new Vector3(0, 0, -1); + + public var x:Number; + public var y:Number; + public var z:Number; + + /** + * Создаёт новый экземпляр. + * + * @param x + * @param y + * @param z + */ + public function Vector3(x:Number = 0, y:Number = 0, z:Number = 0) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * Вычисляет длину вектора. + * + * @return длина вектора + */ + public function length():Number { + return Math.sqrt(x*x + y*y + z*z); + } + + /** + * Вычисляет квадрат длины вектора. + * + * @return квадрат длины вектора + */ + public function lengthSqr():Number { + return x*x + y*y + z*z; + } + + /** + * Устанавливает длину вектора. + * + * @param length новая длина + * @return this + */ + public function setLength(length:Number):Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = length; + } else { + var k:Number = length/Math.sqrt(x*x + y*y + z*z); + x *= k; + y *= k; + z *= k; + } + return this; + } + + /** + * Нормализует вектор. + * + * @return this + */ + public function normalize():Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = 1; + } else { + d = Math.sqrt(d); + x /= d; + y /= d; + z /= d; + } + return this; + } + + /** + * Прибавляет вектор. + * + * @param v прибавляемый вектор + * @return this + */ + public function add(v:Vector3):Vector3 { + x += v.x; + y += v.y; + z += v.z; + return this; + } + + /** + * Прибавляет вектор с домножением на скаляр. + * + * @param k множитель + * @param v вектор + * @return this + */ + public function addScaled(k:Number, v:Vector3):Vector3 { + x += k*v.x; + y += k*v.y; + z += k*v.z; + return this; + } + + /** + * Вычитает вектор. + * + * @param v вычитаемый вектор + * @return this + */ + public function subtract(v:Vector3):Vector3 { + x -= v.x; + y -= v.y; + z -= v.z; + return this; + } + + /** + * Вычисляет сумму векторов и сохраняет её в текущем векторе. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + * @return this + */ + public function sum(a:Vector3, b:Vector3):Vector3 { + x = a.x + b.x; + y = a.y + b.y; + z = a.z + b.z; + return this; + } + + /** + * Вычисляет разность векторов и сохраняет её в текущем векторе. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + * @return this + */ + public function diff(a:Vector3, b:Vector3):Vector3 { + x = a.x - b.x; + y = a.y - b.y; + z = a.z - b.z; + return this; + } + + /** + * Умножает вектор на скаляр. + * + * @param k множитель + * @return this + */ + public function scale(k:Number):Vector3 { + x *= k; + y *= k; + z *= k; + return this; + } + + /** + * Инвертирует вектор. + * + * @return this + */ + public function reverse():Vector3 { + x = -x; + y = -y; + z = -z; + return this; + } + + /** + * Вычисляет скалярное произведение с вектором. + * + * @param v вектор + * @return скалярное произведение с вектором + */ + public function dot(v:Vector3):Number { + return x*v.x + y*v.y + z*v.z; + } + + /** + * Вычисляет векторное произведение с вектором и записывает результат в текущий. + * + * @param v вектор, на который умножается текущий + * @return this + */ + public function cross(v:Vector3):Vector3 { + var xx:Number = y*v.z - z*v.y; + var yy:Number = z*v.x - x*v.z; + var zz:Number = x*v.y - y*v.x; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Вычисляет векторное произведение и записывает результат в текущий вектор. + * + * @param a первый вектор произведения + * @param b второй вектор произведения + * @return this + */ + public function cross2(a:Vector3, b:Vector3):Vector3 { + x = a.y*b.z - a.z*b.y; + y = a.z*b.x - a.x*b.z; + z = a.x*b.y - a.y*b.x; + return this; + } + + /** + * Трансформирует вектор заданной матрицей 3x3. + * + * @param m матрица трансформации + * @return this + */ + public function transform3(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * Трансформирует вектор транспонированной заданной матрицей. Если матрица ортогональна, + * операция эквивалентна трансформированию матрицей, обратной к заданной. + * + * @param m матрица трансформации + * @return this + */ + public function transformTransposed3(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * Трансформирует координаты точки матрицей 4x3. + * + * @param m матрица трансформации + * @return this + */ + public function transform4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz + m.d; + y = m.e*xx + m.f*yy + m.g*zz + m.h; + z = m.i*xx + m.j*yy + m.k*zz + m.l; + return this; + } + + /** + * Выполняет обратную трансформацию координат точки. Матрица должна быть ортогональной. + * + * @param m матрица прямой трансформации + * @return this + */ + public function transformTransposed4(m:Matrix4):Vector3 { + var xx:Number = x - m.d; + var yy:Number = y - m.h; + var zz:Number = z - m.l; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * Трансформирует вектор (т.е. без учёта смещения матрицы). + * + * @param m матрица трансформации + * @return this + */ + public function transformVector4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * Установливает компоненты вектора. + * + * @return this + */ + public function reset(x:Number = 0, y:Number = 0, z:Number = 0):Vector3 { + this.x = x; + this.y = y; + this.z = z; + return this; + } + + /** + * Копирует компоненты вектора. + * + * @param v копируемый вектор + * @return this + */ + public function copy(v:Vector3):Vector3 { + x = v.x; + y = v.y; + z = v.z; + return this; + } + + /** + * Клонирует вектор. + * + * @return клонированный вектор + */ + public function clone():Vector3 { + return new Vector3(x, y, z); + } + + /** + * Копирует компоненты вектора в экземпляр класса Vector3D. + * + * @param result экземпляр, в который копируются компоненты + * @return переданный параметр + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + return result; + } + + /** + * Копирует компоненты вектора. + * + * @param source копируемый вектор + * @return this + */ + public function copyFromVector3D(source:Vector3D):Vector3 { + x = source.x; + y = source.y; + z = source.z; + return this; + } + + /** + * Вычисляет расстояние до указанной точки. + * + * @param v точка + * @return расстояние до точки + */ + public function distanceTo(v:Vector3):Number { + var dx:Number = x - v.x; + var dy:Number = y - v.y; + var dz:Number = z - v.z; + return Math.sqrt(dx*dx + dy*dy + dz*dz); + } + + /** + * Формирует строковое представление вектора. + * + * @return строковое представление вектора + */ + public function toString():String { + return "Vector3(" + x + ", " + y + ", " + z + ")"; + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/math/Matrix3.as b/2.0.1.0-SNAPSHOT/src/alternativa/math/Matrix3.as new file mode 100644 index 0000000..3645720 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/math/Matrix3.as @@ -0,0 +1,393 @@ +package alternativa.math { + import flash.geom.Vector3D; + + /** + * Матрица 3x3, используемая для описания ориентации. + */ + public class Matrix3 { + + public static const ZERO:Matrix3 = new Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0); + public static const IDENTITY:Matrix3 = new Matrix3(); + + public var a:Number; + public var b:Number; + public var c:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix3(a:Number = 1, b:Number = 0, c:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1) { + this.a = a; + this.b = b; + this.c = c; + + this.e = e; + this.f = f; + this.g = g; + + this.i = i; + this.j = j; + this.k = k; + } + + /** + * + * @return this + */ + public function toIdentity():Matrix3 { + a = f = k = 1; + b = c = e = g = i = j = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + var det:Number = 1/(-cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk); + + a = (ff*kk - gg*jj)*det; + b = (cc*jj - bb*kk)*det; + c = (bb*gg - cc*ff)*det; + e = (gg*ii - ee*kk)*det; + f = (aa*kk - cc*ii)*det; + g = (cc*ee - aa*gg)*det; + i = (ee*jj - ff*ii)*det; + j = (bb*ii - aa*jj)*det; + k = (aa*ff - bb*ee)*det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param m правый операнд умножения + */ + public function prepend(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param m правый операнд умножения + */ + public function prependTransposed(m:Matrix3):Matrix3 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + + a = aa*m.a + bb*m.b + cc*m.c; + b = aa*m.e + bb*m.f + cc*m.g; + c = aa*m.i + bb*m.j + cc*m.k; + e = ee*m.a + ff*m.b + gg*m.c; + f = ee*m.e + ff*m.f + gg*m.g; + g = ee*m.i + ff*m.j + gg*m.k; + i = ii*m.a + jj*m.b + kk*m.c; + j = ii*m.e + jj*m.f + kk*m.g; + k = ii*m.i + jj*m.j + kk*m.k; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix3):Matrix3 { + a += m.a; + b += m.b; + c += m.c; + e += m.e; + f += m.f; + g += m.g; + i += m.i; + j += m.j; + k += m.k; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix3):Matrix3 { + a -= m.a; + b -= m.b; + c -= m.c; + e -= m.e; + f -= m.f; + g -= m.g; + i -= m.i; + j -= m.j; + k -= m.k; + + return this; + } + + /** + * + * @return this + */ + public function transpose():Matrix3 { + var tmp:Number = b; + b = e; + e = tmp; + tmp = c; + c = i; + i = tmp; + tmp = g; + g = j; + j = tmp; + + return this; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * Выполняет обратную трансформацию заданного вектора. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorTransposed(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Трансформирует заданный вектор. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector3To3D(vin:Vector3, vout:Vector3D):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * + * @param v + */ + public function createSkewSymmetric(v:Vector3):Matrix3 { + a = f = k = 0; + b = -v.z; + c = v.y; + e = v.z; + g = -v.x; + i = -v.y; + j = v.x; + return this; + } + + /** + * Копирование значений указанной матрицы. + * + * @param m матрица, значения которой копируются + */ + public function copy(m:Matrix3):Matrix3 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * Устанавливает вращательную часть матрицы на основе указанных эйлеровых углов. + * + * @param rx угол поворота по оси X в радианах + * @param ry угол поворота по оси Y в радианах + * @param rz угол поворота по оси Z в радианах + */ + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):void { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + } + + /** + * Метод формирует матрицу поворота на заданный угол относительно заданной оси. + * + * @param axis нормализованный вектор, задающий ось, относительно которой выполняется поворот + * @param angle угол поворота в радианах + */ + public function fromAxisAngle(axis:Vector3, angle:Number):void { + var c1:Number = Math.cos(angle); + var s:Number = Math.sin(angle); + var t:Number = 1 - c1; + var x:Number = axis.x; + var y:Number = axis.y; + var z:Number = axis.z; + + a = t*x*x + c1; + b = t*x*y - z*s; + c = t*x*z + y*s; + + e = t*x*y + z*s; + f = t*y*y + c1; + g = t*y*z - x*s; + + i = t*x*z - y*s; + j = t*y*z + x*s; + k = t*z*z + c1; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix3 { + return new Matrix3(a, b, c, e, f, g, i, j, k); + } + + /** + * Формирует строковое представление объекта. + * + * @return строковое представление объекта + */ + public function toString():String { + return "[Matrix3 (" + a + ", " + b + ", " + c + "), (" + e + ", " + f + ", " + g + "), (" + i + ", " + j + ", " + k + ")]"; + } + + /** + * @param angles + */ + public function getEulerAngles(angles:Vector3):void { + if (-1 < i && i < 1) { + angles.x = Math.atan2(j, k); + angles.y = -Math.asin(i); + angles.z = Math.atan2(e, a); + } else { + angles.x = 0; + angles.y = (i <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-b, f); + } + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/math/Matrix4.as b/2.0.1.0-SNAPSHOT/src/alternativa/math/Matrix4.as new file mode 100644 index 0000000..ee9cdd0 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/math/Matrix4.as @@ -0,0 +1,572 @@ +package alternativa.math { + + /** + * + */ + public class Matrix4 { + + public static const IDENTITY:Matrix4 = new Matrix4(); + + public var a:Number; + public var b:Number; + public var c:Number; + public var d:Number; + + public var e:Number; + public var f:Number; + public var g:Number; + public var h:Number; + + public var i:Number; + public var j:Number; + public var k:Number; + public var l:Number; + + /** + * + * @param a + * @param b + * @param c + * @param e + * @param f + * @param g + * @param i + * @param j + * @param k + */ + public function Matrix4(a:Number = 1, b:Number = 0, c:Number = 0, d:Number = 0, e:Number = 0, f:Number = 1, g:Number = 0, h:Number = 0, i:Number = 0, j:Number = 0, k:Number = 1, l:Number = 0) { + this.a = a; + this.b = b; + this.c = c; + this.d = d; + + this.e = e; + this.f = f; + this.g = g; + this.h = h; + + this.i = i; + this.j = j; + this.k = k; + this.l = l; + } + + /** + * + * @return this + */ + public function toIdentity():Matrix4 { + a = f = k = 1; + b = c = e = g = i = j = d = h = l = 0; + return this; + } + + /** + * Преобразование матрицы в обратную. + */ + public function invert():Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + var det:Number = -cc*ff*ii + bb*gg*ii + cc*ee*jj - aa*gg*jj - bb*ee*kk + aa*ff*kk; + + a = (-gg*jj + ff*kk)/det; + b = (cc*jj - bb*kk)/det; + c = (-cc*ff + bb*gg)/det; + d = (dd*gg*jj - cc*hh*jj - dd*ff*kk + bb*hh*kk + cc*ff*ll - bb*gg*ll)/det; + e = (gg*ii - ee*kk)/det; + f = (-cc*ii + aa*kk)/det; + g = (cc*ee - aa*gg)/det; + h = (cc*hh*ii - dd*gg*ii + dd*ee*kk - aa*hh*kk - cc*ee*ll + aa*gg*ll)/det; + i = (-ff*ii + ee*jj)/det; + j = (bb*ii - aa*jj)/det; + k = (-bb*ee + aa*ff)/det; + l = (dd*ff*ii - bb*hh*ii - dd*ee*jj + aa*hh*jj + bb*ee*ll - aa*ff*ll)/det; + + return this; + } + + /** + * Умножение на матрицу справа: M * this + * + * @param m левый операнд умножения + */ + public function append(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = m.a*aa + m.b*ee + m.c*ii; + b = m.a*bb + m.b*ff + m.c*jj; + c = m.a*cc + m.b*gg + m.c*kk; + d = m.a*dd + m.b*hh + m.c*ll + m.d; + e = m.e*aa + m.f*ee + m.g*ii; + f = m.e*bb + m.f*ff + m.g*jj; + g = m.e*cc + m.f*gg + m.g*kk; + h = m.e*dd + m.f*hh + m.g*ll + m.h; + i = m.i*aa + m.j*ee + m.k*ii; + j = m.i*bb + m.j*ff + m.k*jj; + k = m.i*cc + m.j*gg + m.k*kk; + l = m.i*dd + m.j*hh + m.k*ll + m.l; + + return this; + } + + /** + * Умножение на матрицу слева: this * M + * + * @param m правый операнд умножения + */ + public function prepend(m:Matrix4):Matrix4 { + var aa:Number = a; + var bb:Number = b; + var cc:Number = c; + var dd:Number = d; + var ee:Number = e; + var ff:Number = f; + var gg:Number = g; + var hh:Number = h; + var ii:Number = i; + var jj:Number = j; + var kk:Number = k; + var ll:Number = l; + + a = aa*m.a + bb*m.e + cc*m.i; + b = aa*m.b + bb*m.f + cc*m.j; + c = aa*m.c + bb*m.g + cc*m.k; + d = aa*m.d + bb*m.h + cc*m.l + dd; + e = ee*m.a + ff*m.e + gg*m.i; + f = ee*m.b + ff*m.f + gg*m.j; + g = ee*m.c + ff*m.g + gg*m.k; + h = ee*m.d + ff*m.h + gg*m.l + hh; + i = ii*m.a + jj*m.e + kk*m.i; + j = ii*m.b + jj*m.f + kk*m.j; + k = ii*m.c + jj*m.g + kk*m.k; + l = ii*m.d + jj*m.h + kk*m.l + ll; + + return this; + } + + /** + * + * @param m + */ + public function add(m:Matrix4):Matrix4 { + a += m.a; + b += m.b; + c += m.c; + d += m.d; + e += m.e; + f += m.f; + g += m.g; + h += m.h; + i += m.i; + j += m.j; + k += m.k; + l += m.l; + + return this; + } + + /** + * + * @param m + */ + public function subtract(m:Matrix4):Matrix4 { + a -= m.a; + b -= m.b; + c -= m.c; + d -= m.d; + e -= m.e; + f -= m.f; + g -= m.g; + h -= m.h; + i -= m.i; + j -= m.j; + k -= m.k; + l -= m.l; + + return this; + } + + /** + * Трансформирует координаты точки. + * + * @param vin входная точка + * @param vout вектор, в который записывается результат трансформации + */ + public function transformPoint(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + + /** + * Выполняет обратную трансформацию точки. При этом подразумевается, что квадратная матрица содержит только вращения. + * + * @param vin входная точка + * @param vout вектор, в который записывается результат трансформации + */ + public function transformPointTransposed(vin:Vector3, vout:Vector3):void { + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + + /** + * + * @param arrin + * @param arrout + */ + public function transformPoints(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param arrout + * @param len + */ + public function transformPointsN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + vout.x = a*vin.x + b*vin.y + c*vin.z + d; + vout.y = e*vin.x + f*vin.y + g*vin.z + h; + vout.z = i*vin.x + j*vin.y + k*vin.z + l; + } + } + + /** + * + * @param arrin + * @param arrout + */ + public function transformPointsTransposed(arrin:Vector., arrout:Vector.):void { + var len:int = arrin.length; + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param arrin + * @param arrout + */ + public function transformPointsTransposedN(arrin:Vector., arrout:Vector., len:int):void { + var vin:Vector3; + var vout:Vector3; + for (var idx:int = 0; idx < len; idx++) { + vin = arrin[idx]; + vout = arrout[idx]; + var xx:Number = vin.x - d; + var yy:Number = vin.y - h; + var zz:Number = vin.z - l; + vout.x = a*xx + e*yy + i*zz; + vout.y = b*xx + f*yy + j*zz; + vout.z = c*xx + g*yy + k*zz; + } + } + + /** + * + * @param idx + * @param axis + */ + public function getAxis(idx:int, axis:Vector3):void { + switch (idx) { + case 0: + axis.x = a; + axis.y = e; + axis.z = i; + return; + case 1: + axis.x = b; + axis.y = f; + axis.z = j; + return; + case 2: + axis.x = c; + axis.y = g; + axis.z = k; + return; + case 3: + axis.x = d; + axis.y = h; + axis.z = l; + return; + } + } + + /** + * + * @param xAxis + * @param yAxis + * @param zAxis + * @param pos + */ + public function setAxes(xAxis:Vector3, yAxis:Vector3, zAxis:Vector3, pos:Vector3):void { + a = xAxis.x; + e = xAxis.y; + i = xAxis.z; + + b = yAxis.x; + f = yAxis.y; + j = yAxis.z; + + c = zAxis.x; + g = zAxis.y; + k = zAxis.z; + + d = pos.x; + h = pos.y; + l = pos.z; + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVector(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + b*vin.y + c*vin.z; + vout.y = e*vin.x + f*vin.y + g*vin.z; + vout.z = i*vin.x + j*vin.y + k*vin.z; + } + + /** + * Трансформирует заданный вектор без учёта смещения центра матрицы. + * + * @param vin входной вектор + * @param vout вектор, в который записывается результат трансформации + */ + public function transformVectorTransposed(vin:Vector3, vout:Vector3):void { + vout.x = a*vin.x + e*vin.y + i*vin.z; + vout.y = b*vin.x + f*vin.y + j*vin.z; + vout.z = c*vin.x + g*vin.y + k*vin.z; + } + + /** + * Копирование значений указанной матрицы. + * + * @param m матрица, значения которой копируются + */ + public function copy(m:Matrix4):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = m.d; + e = m.e; + f = m.f; + g = m.g; + h = m.h; + i = m.i; + j = m.j; + k = m.k; + l = m.l; + + return this; + } + + /** + * + * @param m + * @param offset + * @return this + */ + public function setFromMatrix3(m:Matrix3, offset:Vector3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + d = offset.x; + e = m.e; + f = m.f; + g = m.g; + h = offset.y; + i = m.i; + j = m.j; + k = m.k; + l = offset.z; + + return this; + } + + /** + * + * @param m + * @return this + */ + public function setOrientationFromMatrix3(m:Matrix3):Matrix4 { + a = m.a; + b = m.b; + c = m.c; + e = m.e; + f = m.f; + g = m.g; + i = m.i; + j = m.j; + k = m.k; + + return this; + } + + /** + * + * @param angles + */ + public function getEulerAngles(angles:Vector3):void { + if (-1 < i && i < 1) { + angles.x = Math.atan2(j, k); + angles.y = -Math.asin(i); + angles.z = Math.atan2(e, a); + } else { + angles.x = 0; + angles.y = (i <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-b, f); + } + } + + /** + * Устанавливает координаты матрицы. + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + d = pos.x; + h = pos.y; + l = pos.z; + } + + /** + * Устанавливает координаты матрицы. + * + * @param x + * @param y + * @param z + */ + public function setPositionXYZ(x:Number, y:Number, z:Number):void { + d = x; + h = y; + l = z; + } + + /** + * Клонирование матрицы. + * + * @return клон матрицы + */ + public function clone():Matrix4 { + return new Matrix4(a, b, c, d, e, f, g, h, i, j, k, l); + } + + /** + * Формирует строковое представление объекта. + * + * @return строковое представление объекта + */ + public function toString():String { + return "[Matrix4 [" + a.toFixed(3) + " " + b.toFixed(3) + " " + c.toFixed(3) + " " + d.toFixed(3) + "] [" + e.toFixed(3) + " " + f.toFixed(3) + " " + g.toFixed(3) + " " + h.toFixed(3) + "] [" + i.toFixed(3) + " " + j.toFixed(3) + " " + k.toFixed(3) + " " + l.toFixed(3) + "]]"; + } + + /** + * + */ + public function setRotationMatrix(rx:Number, ry:Number, rz:Number):void { + var cosX:Number = Math.cos(rx); + var sinX:Number = Math.sin(rx); + var cosY:Number = Math.cos(ry); + var sinY:Number = Math.sin(ry); + var cosZ:Number = Math.cos(rz); + var sinZ:Number = Math.sin(rz); + + var cosZsinY:Number = cosZ*sinY; + var sinZsinY:Number = sinZ*sinY; + + a = cosZ*cosY; + b = cosZsinY*sinX - sinZ*cosX; + c = cosZsinY*cosX + sinZ*sinX; + + e = sinZ*cosY; + f = sinZsinY*sinX + cosZ*cosX; + g = sinZsinY*cosX - cosZ*sinX; + + i = -sinY; + j = cosY*sinX; + k = cosY*cosX; + } + + /** + * Метод формирует матрицу поворота на заданный угол относительно заданной оси. + * + * @param axis нормализованный вектор, задающий ось, относительно которой выполняется поворот + * @param angle угол поворота в радианах + */ + public function fromAxisAngle(axis:Vector3, angle:Number):void { + var c1:Number = Math.cos(angle); + var s:Number = Math.sin(angle); + var t:Number = 1 - c1; + var x:Number = axis.x; + var y:Number = axis.y; + var z:Number = axis.z; + + a = t*x*x + c1; + b = t*x*y - z*s; + c = t*x*z + y*s; + + e = t*x*y + z*s; + f = t*y*y + c1; + g = t*y*z - x*s; + + i = t*x*z - y*s; + j = t*y*z + x*s; + k = t*z*z + c1; + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/math/Quaternion.as b/2.0.1.0-SNAPSHOT/src/alternativa/math/Quaternion.as new file mode 100644 index 0000000..2c19cbb --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/math/Quaternion.as @@ -0,0 +1,515 @@ +package alternativa.math { + import flash.geom.Vector3D; + + /** + * Кватернион. + */ + public class Quaternion { + + public static const IDENTITY:Quaternion = new Quaternion(); + + public var w:Number; + public var x:Number; + public var y:Number; + public var z:Number; + + public static function multiply(q1:Quaternion, q2:Quaternion, result:Quaternion):void { + result.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z; + result.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y; + result.y = q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z; + result.z = q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x; + } + + public static function createFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngle(axis, angle); + return q; + } + + public static function createFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngleComponents(x, y, z, angle); + return q; + } + + /** + * + * @param x + * @param y + * @param z + */ + public static function createFromEulerAnglesXYZ(x:Number, y:Number, z:Number):Quaternion { + var q:Quaternion = new Quaternion(); + q.setFromAxisAngleComponents(1, 0, 0, x); + + _q.setFromAxisAngleComponents(0, 1, 0, y); + q.append(_q); + q.normalize(); + + _q.setFromAxisAngleComponents(0, 0, 1, z); + q.append(_q); + q.normalize(); + return q; + } + + + private static var _q:Quaternion = new Quaternion(); + + public function Quaternion(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0) { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + } + + public function reset(w:Number = 1, x:Number = 0, y:Number = 0, z:Number = 0):Quaternion { + this.w = w; + this.x = x; + this.y = y; + this.z = z; + return this; + } + + public function normalize():Quaternion { + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * Умножает на указанный кватернион слева: this * q + * + * @param q множитель + */ + public function prepend(q:Quaternion):Quaternion { + var ww:Number = w*q.w - x*q.x - y*q.y - z*q.z; + var xx:Number = w*q.x + x*q.w + y*q.z - z*q.y; + var yy:Number = w*q.y + y*q.w + z*q.x - x*q.z; + var zz:Number = w*q.z + z*q.w + x*q.y - y*q.x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Умножает на указанный кватернион справа: q * this + * + * @param q множитель + */ + public function append(q:Quaternion):Quaternion { + var ww:Number = q.w*w - q.x*x - q.y*y - q.z*z; + var xx:Number = q.w*x + q.x*w + q.y*z - q.z*y; + var yy:Number = q.w*y + q.y*w + q.z*x - q.x*z; + var zz:Number = q.w*z + q.z*w + q.x*y - q.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * + * @param v + */ + public function rotateByVector(v:Vector3):Quaternion { + var ww:Number = -v.x*x - v.y*y - v.z*z; + var xx:Number = v.x*w + v.y*z - v.z*y; + var yy:Number = v.y*w + v.z*x - v.x*z; + var zz:Number = v.z*w + v.x*y - v.y*x; + w = ww; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Добавляет вращение, приданное вектором угловой скорости за указанное время. + * + * @param v + * @param scale + */ + public function addScaledVector(v:Vector3, scale:Number):Quaternion { + var vx:Number = v.x*scale; + var vy:Number = v.y*scale; + var vz:Number = v.z*scale; + var ww:Number = -x*vx - y*vy - z*vz; + var xx:Number = vx*w + vy*z - vz*y; + var yy:Number = vy*w + vz*x - vx*z; + var zz:Number = vz*w + vx*y - vy*x; + w += 0.5*ww; + x += 0.5*xx; + y += 0.5*yy; + z += 0.5*zz; + // inlined normalize + var d:Number = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param m + * @return this + */ + public function toMatrix3(m:Matrix3):Quaternion { + var xx2:Number = 2.0*x*x; + var yy2:Number = 2.0*y*y; + var zz2:Number = 2.0*z*z; + var xy2:Number = 2.0*x*y; + var yz2:Number = 2.0*y*z; + var zx2:Number = 2.0*z*x; + var wx2:Number = 2.0*w*x; + var wy2:Number = 2.0*w*y; + var wz2:Number = 2.0*w*z; + + m.a = 1.0 - yy2 - zz2; + m.b = xy2 - wz2; + m.c = zx2 + wy2; + + m.e = xy2 + wz2; + m.f = 1.0 - xx2 - zz2; + m.g = yz2 - wx2; + + m.i = zx2 - wy2; + m.j = yz2 + wx2; + m.k = 1.0 - xx2 - yy2; + return this; + } + + /** + * + * @param m + * @return this + */ + public function toMatrix4(m:Matrix4):Quaternion { + var xx2:Number = 2.0*x*x; + var yy2:Number = 2.0*y*y; + var zz2:Number = 2.0*z*z; + var xy2:Number = 2.0*x*y; + var yz2:Number = 2.0*y*z; + var zx2:Number = 2.0*z*x; + var wx2:Number = 2.0*w*x; + var wy2:Number = 2.0*w*y; + var wz2:Number = 2.0*w*z; + + m.a = 1.0 - yy2 - zz2; + m.b = xy2 - wz2; + m.c = zx2 + wy2; + + m.e = xy2 + wz2; + m.f = 1.0 - xx2 - zz2; + m.g = yz2 - wx2; + + m.i = zx2 - wy2; + m.j = yz2 + wx2; + m.k = 1.0 - xx2 - yy2; + return this; + } + + /** + * + * @return длина + */ + public function length():Number { + return Math.sqrt(w*w + x*x + y*y + z*z); + } + + /** + * + * @return квадрат длины + */ + public function lengthSqr():Number { + return w*w + x*x + y*y + z*z; + } + + /** + * + * @param axis + * @param angle + * @return this + */ + public function setFromAxisAngle(axis:Vector3, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z); + x = axis.x*k; + y = axis.y*k; + z = axis.z*k; + return this; + } + + /** + * + * @param x + * @param y + * @param z + * @param angle + */ + public function setFromAxisAngleComponents(x:Number, y:Number, z:Number, angle:Number):Quaternion { + w = Math.cos(0.5*angle); + var k:Number = Math.sin(0.5*angle)/Math.sqrt(x*x + y*y + z*z); + this.x = x*k; + this.y = y*k; + this.z = z*k; + return this; + } + + /** + * + * @param v + */ + public function toAxisVector(v:Vector3 = null):Vector3 { + if (w < -1 || w > 1) { + normalize(); + } + if (v == null) { + v = new Vector3(); + } + if (w > -1 && w < 1) { + if (w == 0) { + v.x = x; + v.y = y; + v.z = z; + } else { + var angle:Number = 2*Math.acos(w); + var coeff:Number = 1/Math.sqrt(1 - w*w); + v.x = x*coeff*angle; + v.y = y*coeff*angle; + v.z = z*coeff*angle; + } + } else { + v.x = 0; + v.y = 0; + v.z = 0; + } + return v; + } + + /** + * + * @param angles + */ + public function getEulerAngles(angles:Vector3):Vector3 { + var qi2:Number = 2*x*x; + var qj2:Number = 2*y*y; + var qk2:Number = 2*z*z; + var qij:Number = 2*x*y; + var qjk:Number = 2*y*z; + var qki:Number = 2*z*x; + var qri:Number = 2*w*x; + var qrj:Number = 2*w*y; + var qrk:Number = 2*w*z; + + var aa:Number = 1 - qj2 - qk2; + var bb:Number = qij - qrk; + var ee:Number = qij + qrk; + var ff:Number = 1 - qi2 - qk2; + var ii:Number = qki - qrj; + var jj:Number = qjk + qri; + var kk:Number = 1 - qi2 - qj2; + + if (-1 < ii && ii < 1) { + if (angles == null) { + angles = new Vector3(Math.atan2(jj, kk), -Math.asin(ii), Math.atan2(ee, aa)); + } else { + angles.x = Math.atan2(jj, kk); + angles.y = -Math.asin(ii); + angles.z = Math.atan2(ee, aa); + } + } else { + if (angles == null) { + angles = new Vector3(0, 0.5*((ii <= -1) ? Math.PI : -Math.PI), Math.atan2(-bb, ff)); + } else { + angles.x = 0; + angles.y = (ii <= -1) ? Math.PI : -Math.PI; + angles.y *= 0.5; + angles.z = Math.atan2(-bb, ff); + } + } + return angles; + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setFromEulerAnglesXYZ(x:Number, y:Number, z:Number):void { + setFromAxisAngleComponents(1, 0, 0, x); + + _q.setFromAxisAngleComponents(0, 1, 0, y); + append(_q); + normalize(); + + _q.setFromAxisAngleComponents(0, 0, 1, z); + append(_q); + normalize(); + } + + /** + * + */ + public function conjugate():void { + x = -x; + y = -y; + z = -z; + } + + /** + * Выполняет линейную интерполяцию. + * + * @param q1 начало отрезка + * @param q2 конец отрезка + * @param t время, обычно задаётся в интервале [0, 1] + */ + public function nlerp(q1:Quaternion, q2:Quaternion, t:Number):Quaternion { + var d:Number = 1 - t; + w = q1.w*d + q2.w*t; + x = q1.x*d + q2.x*t; + y = q1.y*d + q2.y*t; + z = q1.z*d + q2.z*t; + // inlined normalize + d = w*w + x*x + y*y + z*z; + if (d == 0) { + w = 1; + } else { + d = 1/Math.sqrt(d); + w *= d; + x *= d; + y *= d; + z *= d; + } + return this; + } + + /** + * + * @param q + * @return this + */ + public function subtract(q:Quaternion):Quaternion { + w -= q.w; + x -= q.x; + y -= q.y; + z -= q.z; + return this; + } + + /** + * + * @param q1 + * @param q2 + * @return this + */ + public function diff(q1:Quaternion, q2:Quaternion):Quaternion { + w = q2.w - q1.w; + x = q2.x - q1.x; + y = q2.y - q1.y; + z = q2.z - q1.z; + return this; + } + + /** + * + * @param q + * @return this + */ + public function copy(q:Quaternion):Quaternion { + w = q.w; + x = q.x; + y = q.y; + z = q.z; + return this; + } + + /** + * @param result + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + result.w = w; + return result; + } + + /** + * + * @return клон + */ + public function clone():Quaternion { + return new Quaternion(w, x, y, z); + } + + /** + * Формирует строковое представление объекта. + * + * @return строковое представление объекта + */ + public function toString():String { + return "[" + w + ", " + x + ", " + y + ", " + z + "]"; + } + + /** + * Выполняет сферическую интерполяцию между двумя заданными кватернионами по наименьшему расстоянию. + * + * @param a первый кватерион + * @param b второй кватернион + * @param t параметр интерполяции, обычно принадлежит отрезку [0, 1] + * @return this + */ + public function slerp(a:Quaternion, b:Quaternion, t:Number):Quaternion { + var flip:Number = 1; + // Так как одна и та же ориентация представляется двумя значениями q и -q, нужно сменить знак одного из кватернионов + // если скалярное произведение отрицательно. Иначе будет получено интерполированное значение по наибольшему расстоянию. + var cosine:Number = a.w*b.w + a.x*b.x + a.y*b.y + a.z*b.z; + if (cosine < 0) { + cosine = -cosine; + flip = -1; + } + + if ((1 - cosine) < 0.001) { + // Вблизи нуля используется линейная интерполяция + var k1:Number = 1 - t; + var k2:Number = t*flip; + w = a.w*k1 + b.w*k2; + x = a.x*k1 + b.x*k2; + y = a.y*k1 + b.y*k2; + z = a.z*k1 + b.z*k2; + normalize(); + } else { + var theta:Number = Math.acos(cosine); + var sine:Number = Math.sin(theta); + var beta:Number = Math.sin((1 - t)*theta)/sine; + var alpha:Number = Math.sin(t*theta)/sine*flip; + w = a.w*beta + b.w*alpha; + x = a.x*beta + b.x*alpha; + y = a.y*beta + b.y*alpha; + z = a.z*beta + b.z*alpha; + } + + return this; + } + + } +} diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/math/Vector3.as b/2.0.1.0-SNAPSHOT/src/alternativa/math/Vector3.as new file mode 100644 index 0000000..61c0a8f --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/math/Vector3.as @@ -0,0 +1,392 @@ +package alternativa.math { + import flash.geom.Vector3D; + + /** + * + */ + public class Vector3 { + + public static const ZERO:Vector3 = new Vector3(0, 0, 0); + public static const X_AXIS:Vector3 = new Vector3(1, 0, 0); + public static const Y_AXIS:Vector3 = new Vector3(0, 1, 0); + public static const Z_AXIS:Vector3 = new Vector3(0, 0, 1); + + public static const RIGHT:Vector3 = new Vector3(1, 0, 0); + public static const LEFT:Vector3 = new Vector3(-1, 0, 0); + + public static const FORWARD:Vector3 = new Vector3(0, 1, 0); + public static const BACK:Vector3 = new Vector3(0, -1, 0); + + public static const UP:Vector3 = new Vector3(0, 0, 1); + public static const DOWN:Vector3 = new Vector3(0, 0, -1); + + public var x:Number; + public var y:Number; + public var z:Number; + + /** + * Создаёт новый экземпляр. + * + * @param x + * @param y + * @param z + */ + public function Vector3(x:Number = 0, y:Number = 0, z:Number = 0) { + this.x = x; + this.y = y; + this.z = z; + } + + /** + * Вычисляет длину вектора. + * + * @return длина вектора + */ + public function length():Number { + return Math.sqrt(x*x + y*y + z*z); + } + + /** + * Вычисляет квадрат длины вектора. + * + * @return квадрат длины вектора + */ + public function lengthSqr():Number { + return x*x + y*y + z*z; + } + + /** + * Устанавливает длину вектора. + * + * @param length новая длина + * @return this + */ + public function setLength(length:Number):Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = length; + } else { + var k:Number = length/Math.sqrt(x*x + y*y + z*z); + x *= k; + y *= k; + z *= k; + } + return this; + } + + /** + * Нормализует вектор. + * + * @return this + */ + public function normalize():Vector3 { + var d:Number = x*x + y*y + z*z; + if (d == 0) { + x = 1; + } else { + d = Math.sqrt(d); + x /= d; + y /= d; + z /= d; + } + return this; + } + + /** + * Прибавляет вектор. + * + * @param v прибавляемый вектор + * @return this + */ + public function add(v:Vector3):Vector3 { + x += v.x; + y += v.y; + z += v.z; + return this; + } + + /** + * Прибавляет вектор с домножением на скаляр. + * + * @param k множитель + * @param v вектор + * @return this + */ + public function addScaled(k:Number, v:Vector3):Vector3 { + x += k*v.x; + y += k*v.y; + z += k*v.z; + return this; + } + + /** + * Вычитает вектор. + * + * @param v вычитаемый вектор + * @return this + */ + public function subtract(v:Vector3):Vector3 { + x -= v.x; + y -= v.y; + z -= v.z; + return this; + } + + /** + * Вычисляет сумму векторов и сохраняет её в текущем векторе. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + * @return this + */ + public function sum(a:Vector3, b:Vector3):Vector3 { + x = a.x + b.x; + y = a.y + b.y; + z = a.z + b.z; + return this; + } + + /** + * Вычисляет разность векторов и сохраняет её в текущем векторе. + * + * @param a уменьшаемый вектор + * @param b вычитаемый вектор + * @return this + */ + public function diff(a:Vector3, b:Vector3):Vector3 { + x = a.x - b.x; + y = a.y - b.y; + z = a.z - b.z; + return this; + } + + /** + * Умножает вектор на скаляр. + * + * @param k множитель + * @return this + */ + public function scale(k:Number):Vector3 { + x *= k; + y *= k; + z *= k; + return this; + } + + /** + * Инвертирует вектор. + * + * @return this + */ + public function reverse():Vector3 { + x = -x; + y = -y; + z = -z; + return this; + } + + /** + * Вычисляет скалярное произведение с вектором. + * + * @param v вектор + * @return скалярное произведение с вектором + */ + public function dot(v:Vector3):Number { + return x*v.x + y*v.y + z*v.z; + } + + /** + * Вычисляет векторное произведение с вектором и записывает результат в текущий. + * + * @param v вектор, на который умножается текущий + * @return this + */ + public function cross(v:Vector3):Vector3 { + var xx:Number = y*v.z - z*v.y; + var yy:Number = z*v.x - x*v.z; + var zz:Number = x*v.y - y*v.x; + x = xx; + y = yy; + z = zz; + return this; + } + + /** + * Вычисляет векторное произведение и записывает результат в текущий вектор. + * + * @param a первый вектор произведения + * @param b второй вектор произведения + * @return this + */ + public function cross2(a:Vector3, b:Vector3):Vector3 { + x = a.y*b.z - a.z*b.y; + y = a.z*b.x - a.x*b.z; + z = a.x*b.y - a.y*b.x; + return this; + } + + /** + * Трансформирует вектор заданной матрицей 3x3. + * + * @param m матрица трансформации + * @return this + */ + public function transform3(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * Трансформирует вектор транспонированной заданной матрицей. Если матрица ортогональна, + * операция эквивалентна трансформированию матрицей, обратной к заданной. + * + * @param m матрица трансформации + * @return this + */ + public function transformTransposed3(m:Matrix3):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * Трансформирует координаты точки матрицей 4x3. + * + * @param m матрица трансформации + * @return this + */ + public function transform4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz + m.d; + y = m.e*xx + m.f*yy + m.g*zz + m.h; + z = m.i*xx + m.j*yy + m.k*zz + m.l; + return this; + } + + /** + * Выполняет обратную трансформацию координат точки. Матрица должна быть ортогональной. + * + * @param m матрица прямой трансформации + * @return this + */ + public function transformTransposed4(m:Matrix4):Vector3 { + var xx:Number = x - m.d; + var yy:Number = y - m.h; + var zz:Number = z - m.l; + x = m.a*xx + m.e*yy + m.i*zz; + y = m.b*xx + m.f*yy + m.j*zz; + z = m.c*xx + m.g*yy + m.k*zz; + return this; + } + + /** + * Трансформирует вектор (т.е. без учёта смещения матрицы). + * + * @param m матрица трансформации + * @return this + */ + public function transformVector4(m:Matrix4):Vector3 { + var xx:Number = x; + var yy:Number = y; + var zz:Number = z; + x = m.a*xx + m.b*yy + m.c*zz; + y = m.e*xx + m.f*yy + m.g*zz; + z = m.i*xx + m.j*yy + m.k*zz; + return this; + } + + /** + * Установливает компоненты вектора. + * + * @return this + */ + public function reset(x:Number = 0, y:Number = 0, z:Number = 0):Vector3 { + this.x = x; + this.y = y; + this.z = z; + return this; + } + + /** + * Копирует компоненты вектора. + * + * @param v копируемый вектор + * @return this + */ + public function copy(v:Vector3):Vector3 { + x = v.x; + y = v.y; + z = v.z; + return this; + } + + /** + * Клонирует вектор. + * + * @return клонированный вектор + */ + public function clone():Vector3 { + return new Vector3(x, y, z); + } + + /** + * Копирует компоненты вектора в экземпляр класса Vector3D. + * + * @param result экземпляр, в который копируются компоненты + * @return переданный параметр + */ + public function toVector3D(result:Vector3D):Vector3D { + result.x = x; + result.y = y; + result.z = z; + return result; + } + + /** + * Копирует компоненты вектора. + * + * @param source копируемый вектор + * @return this + */ + public function copyFromVector3D(source:Vector3D):Vector3 { + x = source.x; + y = source.y; + z = source.z; + return this; + } + + /** + * Вычисляет расстояние до указанной точки. + * + * @param v точка + * @return расстояние до точки + */ + public function distanceTo(v:Vector3):Number { + var dx:Number = x - v.x; + var dy:Number = y - v.y; + var dz:Number = z - v.z; + return Math.sqrt(dx*dx + dy*dy + dz*dz); + } + + /** + * Формирует строковое представление вектора. + * + * @return строковое представление вектора + */ + public function toString():String { + return "Vector3(" + x + ", " + y + ", " + z + ")"; + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/.svn/all-wcprops b/2.0.1.0-SNAPSHOT/src/alternativa/physics/.svn/all-wcprops new file mode 100644 index 0000000..8f95dbe --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/.svn/all-wcprops @@ -0,0 +1,71 @@ +K 25 +svn:wc:ra_dav:version-url +V 96 +/!svn/ver/34889/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics +END +Body.as +K 25 +svn:wc:ra_dav:version-url +V 104 +/!svn/ver/34246/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/Body.as +END +BodyState.as +K 25 +svn:wc:ra_dav:version-url +V 109 +/!svn/ver/29719/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/BodyState.as +END +CollisionPrimitiveListItem.as +K 25 +svn:wc:ra_dav:version-url +V 126 +/!svn/ver/22253/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/CollisionPrimitiveListItem.as +END +CollisionPrimitiveList.as +K 25 +svn:wc:ra_dav:version-url +V 122 +/!svn/ver/22955/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/CollisionPrimitiveList.as +END +PhysicsScene.as +K 25 +svn:wc:ra_dav:version-url +V 112 +/!svn/ver/34889/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/PhysicsScene.as +END +PhysicsUtils.as +K 25 +svn:wc:ra_dav:version-url +V 112 +/!svn/ver/23758/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/PhysicsUtils.as +END +ContactPoint.as +K 25 +svn:wc:ra_dav:version-url +V 112 +/!svn/ver/29719/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/ContactPoint.as +END +BodyListItem.as +K 25 +svn:wc:ra_dav:version-url +V 112 +/!svn/ver/22253/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/BodyListItem.as +END +BodyList.as +K 25 +svn:wc:ra_dav:version-url +V 108 +/!svn/ver/22955/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/BodyList.as +END +Contact.as +K 25 +svn:wc:ra_dav:version-url +V 107 +/!svn/ver/28424/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/Contact.as +END +BodyMaterial.as +K 25 +svn:wc:ra_dav:version-url +V 112 +/!svn/ver/22253/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/BodyMaterial.as +END diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/.svn/entries b/2.0.1.0-SNAPSHOT/src/alternativa/physics/.svn/entries new file mode 100644 index 0000000..91cc6a6 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/.svn/entries @@ -0,0 +1,166 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics +http://svndev.alternativaplatform.com + + + +2010-05-27T15:33:01.277505Z +34889 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Body.as +file + + + + +2010-10-28T04:32:37.000000Z +c325edce8bb1addbaf2167322948f2b3 +2010-05-12T11:30:54.476378Z +34246 +mike + +BodyState.as +file + + + + +2010-10-28T04:32:37.000000Z +b3105d13f93c5d6d8d2bf3ce02c7e8d9 +2010-03-12T16:45:48.613787Z +29719 +mike + +CollisionPrimitiveListItem.as +file + + + + +2010-10-28T04:32:37.000000Z +cec5dbfefef812d87481478455f367b1 +2009-10-19T07:17:33.113306Z +22253 +mike + +CollisionPrimitiveList.as +file + + + + +2010-10-28T04:32:37.000000Z +5752b1f4909c5146720b58fbfffed468 +2009-11-01T12:35:52.270571Z +22955 +mike + +PhysicsScene.as +file + + + + +2010-10-28T04:32:37.000000Z +1562a46e7c26e82eb1059469a550d48c +2010-05-27T15:33:01.277505Z +34889 +mike + +PhysicsUtils.as +file + + + + +2010-10-28T04:32:37.000000Z +dce0a94f8a861291a2a8942ed0ba54a5 +2009-11-24T08:44:15.941480Z +23758 +mike + +constraints +dir + +ContactPoint.as +file + + + + +2010-10-28T04:32:37.000000Z +9eedaf1a541ae335b614fcc4170012f2 +2010-03-12T16:45:48.613787Z +29719 +mike + +BodyListItem.as +file + + + + +2010-10-28T04:32:37.000000Z +2fc2b598310b24aef3f8b90e14a8ba4f +2009-10-19T07:17:33.113306Z +22253 +mike + +collision +dir + +BodyList.as +file + + + + +2010-10-28T04:32:37.000000Z +08a90332660bd0273728006d1402dd25 +2009-11-01T12:35:52.270571Z +22955 +mike + +Contact.as +file + + + + +2010-10-28T04:32:37.000000Z +eb89f4f37bcf2d21eadc733f244fcf64 +2010-02-12T12:34:25.192477Z +28424 +mike + +BodyMaterial.as +file + + + + +2010-10-28T04:32:37.000000Z +9b700692ebb8017882f3e8cec095e1a9 +2009-10-19T07:17:33.113306Z +22253 +mike + diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/.svn/format b/2.0.1.0-SNAPSHOT/src/alternativa/physics/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/.svn/format @@ -0,0 +1 @@ +8 diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/.svn/text-base/Body.as.svn-base b/2.0.1.0-SNAPSHOT/src/alternativa/physics/.svn/text-base/Body.as.svn-base new file mode 100644 index 0000000..28c9b1a --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/.svn/text-base/Body.as.svn-base @@ -0,0 +1,461 @@ +package alternativa.physics { + + import alternativa.math.Matrix3; + import alternativa.math.Matrix4; + import alternativa.math.Quaternion; + import alternativa.math.Vector3; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.IBodyCollisionPredicate; + import alternativa.physics.collision.types.BoundBox; + + /** + * + */ + public class Body { + + public static var linDamping:Number = 0.997; + public static var rotDamping:Number = 0.997; + + // Идентификатор тела, уникальный в пределах мира + public var id:int; + // Имя тела + public var name:String; + // Мир, в котором находится тело + public var world:PhysicsScene; + // Флаг подвижности тела + public var movable:Boolean = true; + // Флаг указывает, может ли тело быть заморожено + public var canFreeze:Boolean = false; + + public var freezeCounter:int; + public var frozen:Boolean = false; + + public var useGravity:Boolean = true; + + // Ограничивающий бокс тела + public var aabb:BoundBox = new BoundBox(); + // Предикат, через который тело получает сообщения о столкновениях + public var postCollisionPredicate:IBodyCollisionPredicate; + + // Текущее состояние тела + public var state:BodyState = new BodyState(); + // Предыдущее состояние тела + public var prevState:BodyState = new BodyState(); + // Линейное ускорение тела на текущем шаге симуляции + public var accel:Vector3 = new Vector3(); + // Угловое ускорение тела на текущем шаге симуляции + public var angleAccel:Vector3 = new Vector3(); + // Физический материал тела + public var material:BodyMaterial = new BodyMaterial(); + // Обратная масса тела + public var invMass:Number = 1; + // Обратная матрица тензора инерции в локальных координатах + public var invInertia:Matrix3 = new Matrix3(); + // Обратная матрица тензора инерции в мировых координатах + public var invInertiaWorld:Matrix3 = new Matrix3(); + // Базисная матрица тела в мировых координатах + public var baseMatrix:Matrix3 = new Matrix3(); + + public const MAX_CONTACTS:int = 20; + public var contacts:Vector. = new Vector.(MAX_CONTACTS); + public var contactsNum:int; + + public var collisionPrimitives:CollisionPrimitiveList; + + // Аккумулятор сил + public var forceAccum:Vector3 = new Vector3(); + // Аккумулятор моментов + public var torqueAccum:Vector3 = new Vector3(); + + // Внутренние переменные для избежания создания экземпляров + private static var _r:Vector3 = new Vector3(); + private static var _f:Vector3 = new Vector3(); + private static var _q:Quaternion = new Quaternion(); + + /** + * + * @param invMass + * @param invInertia + */ + public function Body(invMass:Number, invInertia:Matrix3) { + this.invMass = invMass; + this.invInertia.copy(invInertia); + } + + /** + * @param primitive + * @param localTransform + */ + public function addCollisionPrimitive(primitive:CollisionPrimitive, localTransform:Matrix4 = null):void { + if (primitive == null) { + throw new ArgumentError("Primitive cannot be null"); + } + if (collisionPrimitives == null) { + collisionPrimitives = new CollisionPrimitiveList(); + } + collisionPrimitives.append(primitive); + primitive.setBody(this, localTransform); + } + + /** + * + * @param primitive + */ + public function removeCollisionPrimitive(primitive:CollisionPrimitive):void { + if (collisionPrimitives == null) return; + primitive.setBody(null); + collisionPrimitives.remove(primitive); + if (collisionPrimitives.size == 0) { + collisionPrimitives = null; + } + } + + /** + * + * @param t + * @param pos + * @param orientation + */ + public function interpolate(t:Number, pos:Vector3, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.w = prevState.orientation.w*t1 + state.orientation.w*t; + orientation.x = prevState.orientation.x*t1 + state.orientation.x*t; + orientation.y = prevState.orientation.y*t1 + state.orientation.y*t; + orientation.z = prevState.orientation.z*t1 + state.orientation.z*t; + } + + /** + * + * @param t + * @param pos + * @param orientation + */ + public function interpolateSLERP(t:Number, pos:Vector3, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.slerp(prevState.orientation, state.orientation, t); + } + + /** + * + * @param t + * @param m + */ + public function interpolateToMatrix(t:Number, m:Matrix4):void { + var t1:Number = 1 - t; + m.d = prevState.pos.x*t1 + state.pos.x*t; + m.h = prevState.pos.y*t1 + state.pos.y*t; + m.l = prevState.pos.z*t1 + state.pos.z*t; + _q.w = prevState.orientation.w*t1 + state.orientation.w*t; + _q.x = prevState.orientation.x*t1 + state.orientation.x*t; + _q.y = prevState.orientation.y*t1 + state.orientation.y*t; + _q.z = prevState.orientation.z*t1 + state.orientation.z*t; + _q.normalize(); + _q.toMatrix4(m); + } + + /** + * + * @param t + * @param m + */ + public function interpolateToMatrixSLERP(t:Number, m:Matrix4):void { + var t1:Number = 1 - t; + m.d = prevState.pos.x*t1 + state.pos.x*t; + m.h = prevState.pos.y*t1 + state.pos.y*t; + m.l = prevState.pos.z*t1 + state.pos.z*t; + _q.slerp(prevState.orientation, state.orientation, t); + _q.normalize(); + _q.toMatrix4(m); + } + + /** + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + state.pos.copy(pos); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setPositionXYZ(x:Number, y:Number, z:Number):void { + state.pos.reset(x, y, z); + } + + /** + * + * @param vel + */ + public function setVelocity(vel:Vector3):void { + state.velocity.copy(vel); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setVelocityXYZ(x:Number, y:Number, z:Number):void { + state.velocity.reset(x, y, z); + } + + /** + * + * @param rot + */ + public function setRotation(rot:Vector3):void { + state.rotation.copy(rot); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setRotationXYZ(x:Number, y:Number, z:Number):void { + state.rotation.reset(x, y, z); + } + + /** + * + * @param q + */ + public function setOrientation(q:Quaternion):void { + state.orientation.copy(q); + } + + /** + * @param r + * @param dir + * @param magnitude + */ + public function applyRelPosWorldImpulse(r:Vector3, dir:Vector3, magnitude:Number):void { + var d:Number = magnitude*invMass; + // Линейная часть + state.velocity.x += d*dir.x; + state.velocity.y += d*dir.y; + state.velocity.z += d*dir.z; + + // Вращательная часть + var x:Number = (r.y*dir.z - r.z*dir.y)*magnitude; + var y:Number = (r.z*dir.x - r.x*dir.z)*magnitude; + var z:Number = (r.x*dir.y - r.y*dir.x)*magnitude; + + state.rotation.x += invInertiaWorld.a*x + invInertiaWorld.b*y + invInertiaWorld.c*z; + state.rotation.y += invInertiaWorld.e*x + invInertiaWorld.f*y + invInertiaWorld.g*z; + state.rotation.z += invInertiaWorld.i*x + invInertiaWorld.j*y + invInertiaWorld.k*z; + } + + /** + * @param f + */ + public function addForce(f:Vector3):void { + forceAccum.add(f); + } + + /** + * + * @param fx + * @param fy + * @param fz + */ + public function addForceXYZ(fx:Number, fy:Number, fz:Number):void { + forceAccum.x += fx; + forceAccum.y += fy; + forceAccum.z += fz; + } + + /** + * + * @param px + * @param py + * @param pz + * @param fx + * @param fy + * @param fz + */ + public function addWorldForceXYZ(px:Number, py:Number, pz:Number, fx:Number, fy:Number, fz:Number):void { + forceAccum.x += fx; + forceAccum.y += fy; + forceAccum.z += fz; + + var pos:Vector3 = state.pos; + var rx:Number = px - pos.x; + var ry:Number = py - pos.y; + var rz:Number = pz - pos.z; + +// var x:Number = ry*fz - rz*fy; +// var y:Number = rz*fx - rx*fz; +// var z:Number = rx*fy - ry*fx; + + torqueAccum.x += ry*fz - rz*fy; + torqueAccum.y += rz*fx - rx*fz; + torqueAccum.z += rx*fy - ry*fx; + } + + /** + * @param pos + * @param f + */ + public function addWorldForce(pos:Vector3, force:Vector3):void { + forceAccum.add(force); + torqueAccum.add(_r.diff(pos, state.pos).cross(force)); + } + + /** + * @param pos + * @param f + */ + public function addWorldForceScaled(pos:Vector3, force:Vector3, scale:Number):void { + _f.x = scale*force.x; + _f.y = scale*force.y; + _f.z = scale*force.z; + forceAccum.add(_f); + torqueAccum.add(_r.diff(pos, state.pos).cross(_f)); + } + + /** + * @param pos + * @param f + */ + public function addLocalForce(pos:Vector3, force:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(pos, _r); + // Трансформируем вектор силы в мировую систему + baseMatrix.transformVector(force, _f); + // Добавляем силу и момент + forceAccum.add(_f); + torqueAccum.add(_r.cross(_f)); + } + + /** + * + * @param localPos + * @param worldForce + */ + public function addWorldForceAtLocalPoint(localPos:Vector3, worldForce:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(localPos, _r); + // Добавляем силу и момент + forceAccum.add(worldForce); + torqueAccum.add(_r.cross(worldForce)); + } + + /** + * @param dt + */ + public function beforePhysicsStep(dt:Number):void { + } + + /** + * @param t + */ + public function addTorque(t:Vector3):void { + torqueAccum.add(t); + } + + /** + * + */ + internal function clearAccumulators():void { + forceAccum.x = forceAccum.y = forceAccum.z = 0; + torqueAccum.x = torqueAccum.y = torqueAccum.z = 0; + } + + /** + * + */ + internal function calcAccelerations():void { + accel.x = forceAccum.x*invMass; + accel.y = forceAccum.y*invMass; + accel.z = forceAccum.z*invMass; + angleAccel.x = invInertiaWorld.a*torqueAccum.x + invInertiaWorld.b*torqueAccum.y + invInertiaWorld.c*torqueAccum.z; + angleAccel.y = invInertiaWorld.e*torqueAccum.x + invInertiaWorld.f*torqueAccum.y + invInertiaWorld.g*torqueAccum.z; + angleAccel.z = invInertiaWorld.i*torqueAccum.x + invInertiaWorld.j*torqueAccum.y + invInertiaWorld.k*torqueAccum.z; + } + + /** + * Вычисляет производные данные. + */ + public function calcDerivedData():void { + // Вычисление базисной матрицы и обратного тензора инерции в мировых координатах + state.orientation.toMatrix3(baseMatrix); + invInertiaWorld.copy(invInertia).append(baseMatrix).prependTransposed(baseMatrix); + if (collisionPrimitives != null) { + aabb.infinity(); + var item:CollisionPrimitiveListItem = collisionPrimitives.head; + while (item != null) { + var primitive:CollisionPrimitive = item.primitive; + primitive.transform.setFromMatrix3(baseMatrix, state.pos); + if (primitive.localTransform != null) { + primitive.transform.prepend(primitive.localTransform); + } + primitive.calculateAABB(); + aabb.addBoundBox(primitive.aabb); + item = item.next; + } + } + } + + /** + * + */ + public function saveState():void { + prevState.copy(state); + } + + /** + * + */ + internal function restoreState():void { + state.copy(prevState); + } + + /** + * @param dt + */ + internal function integrateVelocity(dt:Number):void { + // v = v + a*t + state.velocity.x += accel.x*dt; + state.velocity.y += accel.y*dt; + state.velocity.z += accel.z*dt; + // rot = rot + eps*t + state.rotation.x += angleAccel.x*dt; + state.rotation.y += angleAccel.y*dt; + state.rotation.z += angleAccel.z*dt; + + state.velocity.x *= linDamping; + state.velocity.y *= linDamping; + state.velocity.z *= linDamping; + + state.rotation.x *= rotDamping; + state.rotation.y *= rotDamping; + state.rotation.z *= rotDamping; + } + + /** + * + */ + internal function integratePosition(dt:Number):void { + // pos = pos + v*t + state.pos.x += state.velocity.x*dt; + state.pos.y += state.velocity.y*dt; + state.pos.z += state.velocity.z*dt; + // q = q + 0.5*rot*q + state.orientation.addScaledVector(state.rotation, dt); + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/.svn/text-base/BodyList.as.svn-base b/2.0.1.0-SNAPSHOT/src/alternativa/physics/.svn/text-base/BodyList.as.svn-base new file mode 100644 index 0000000..aa8f79f --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/.svn/text-base/BodyList.as.svn-base @@ -0,0 +1,76 @@ +package alternativa.physics { + + /** + * + */ + public class BodyList { + + public var head:BodyListItem; + public var tail:BodyListItem; + public var size:int; + + /** + * + */ + public function BodyList() { + } + + /** + * + * @param body + */ + public function append(body:Body):void { + var item:BodyListItem = BodyListItem.create(body); + if (head == null) { + head = tail = item; + } else { + tail.next = item; + item.prev = tail; + tail = item; + } + size++; + } + + /** + * + * @param body + */ + public function remove(body:Body):Boolean { + var item:BodyListItem = findItem(body); + if (item == null) return false; + if (item == head) { + if (size == 1) { + head = tail = null; + } else { + head = item.next; + head.prev = null; + } + } else { + if (item == tail) { + tail = item.prev; + tail.next = null; + } else { + item.prev.next = item.next; + item.next.prev = item.prev; + } + } + item.dispose(); + size--; + return true; + } + + /** + * + * @param body + * @return + */ + public function findItem(body:Body):BodyListItem { + var item:BodyListItem = head; + while (item != null && item.body != body) { + item = item.next; + } + return item; + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/.svn/text-base/BodyListItem.as.svn-base b/2.0.1.0-SNAPSHOT/src/alternativa/physics/.svn/text-base/BodyListItem.as.svn-base new file mode 100644 index 0000000..382e11c --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/.svn/text-base/BodyListItem.as.svn-base @@ -0,0 +1,74 @@ +package alternativa.physics { + + /** + * + */ + public class BodyListItem { + + // Верхний элемент хранилища + private static var poolTop:BodyListItem; + + /** + * Создаёт новый элемент списка. + * + * @param primitive примитив, содержащийся в элементе + * @return новый элемент списка + */ + public static function create(body:Body):BodyListItem { + var item:BodyListItem; + if (poolTop == null) { + item = new BodyListItem(body); + } else { + item = poolTop; + poolTop = item.next; + item.next = null; + item.body = body; + } + return item; + } + + /** + * Очищает хранилище. + */ + public static function clearPool():void { + var item:BodyListItem = poolTop; + while (item != null) { + poolTop = item.next; + item.next = null; + item = poolTop; + } + } + + /** + * + */ + public var body:Body; + /** + * + */ + public var next:BodyListItem; + /** + * + */ + public var prev:BodyListItem; + + /** + * + * @param body + */ + public function BodyListItem(body:Body) { + this.body = body; + } + + /** + * + */ + public function dispose():void { + body = null; + prev = null; + next = poolTop; + poolTop = this; + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/.svn/text-base/BodyMaterial.as.svn-base b/2.0.1.0-SNAPSHOT/src/alternativa/physics/.svn/text-base/BodyMaterial.as.svn-base new file mode 100644 index 0000000..7a7c47d --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/.svn/text-base/BodyMaterial.as.svn-base @@ -0,0 +1,11 @@ +package alternativa.physics { + + public class BodyMaterial { + + public var restitution:Number = 0; + public var friction:Number = 0.3; +// public var dynamicFriction:Number = 0.2; +// public var dynamicFriction:Number = 0.2; + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/.svn/text-base/BodyState.as.svn-base b/2.0.1.0-SNAPSHOT/src/alternativa/physics/.svn/text-base/BodyState.as.svn-base new file mode 100644 index 0000000..f397afe --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/.svn/text-base/BodyState.as.svn-base @@ -0,0 +1,39 @@ +package alternativa.physics { + import alternativa.math.Quaternion; + import alternativa.math.Vector3; + + /** + * Класс описывает состояние твёрдого тела. + */ + public class BodyState { + /** + * Положение тела. + */ + public var pos:Vector3 = new Vector3(); + /** + * Ориентация тела. + */ + public var orientation:Quaternion = new Quaternion(); + /** + * Скорость тела. + */ + public var velocity:Vector3 = new Vector3(); + /** + * Угловая скорость тела. + */ + public var rotation:Vector3 = new Vector3(); + + /** + * Копирует значение указанного объекта. + * + * @param state + */ + public function copy(state:BodyState):void { + pos.copy(state.pos); + orientation.copy(state.orientation); + velocity.copy(state.velocity); + rotation.copy(state.rotation); + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/.svn/text-base/CollisionPrimitiveList.as.svn-base b/2.0.1.0-SNAPSHOT/src/alternativa/physics/.svn/text-base/CollisionPrimitiveList.as.svn-base new file mode 100644 index 0000000..5d5304f --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/.svn/text-base/CollisionPrimitiveList.as.svn-base @@ -0,0 +1,89 @@ +package alternativa.physics { + import alternativa.physics.collision.CollisionPrimitive; + + /** + * + */ + public class CollisionPrimitiveList { + + public var head:CollisionPrimitiveListItem; + public var tail:CollisionPrimitiveListItem; + public var size:int; + + /** + * + */ + public function CollisionPrimitiveList() { + } + + /** + * + * @param primitive + */ + public function append(primitive:CollisionPrimitive):void { + var item:CollisionPrimitiveListItem = CollisionPrimitiveListItem.create(primitive); + if (head == null) { + head = tail = item; + } else { + tail.next = item; + item.prev = tail; + tail = item; + } + size++; + } + + /** + * + * @param primitve + */ + public function remove(primitve:CollisionPrimitive):void { + var item:CollisionPrimitiveListItem = findItem(primitve); + if (item == null) return; + if (item == head) { + if (size == 1) { + head = tail = null; + } else { + head = item.next; + head.prev = null; + } + } else { + if (item == tail) { + tail = tail.prev; + tail.next = null; + } else { + item.prev.next = item.next; + item.next.prev = item.prev; + } + } + item.dispose(); + size--; + } + + /** + * + * @param primitive + * @return + */ + public function findItem(primitive:CollisionPrimitive):CollisionPrimitiveListItem { + var item:CollisionPrimitiveListItem = head; + while (item != null && item.primitive != primitive) { + item = item.next; + } + return item; + } + + /** + * + */ + public function clear():void { + while (head != null) { + var item:CollisionPrimitiveListItem = head; + head = head.next; + item.dispose(); + } + tail = null; + size = 0; + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/.svn/text-base/CollisionPrimitiveListItem.as.svn-base b/2.0.1.0-SNAPSHOT/src/alternativa/physics/.svn/text-base/CollisionPrimitiveListItem.as.svn-base new file mode 100644 index 0000000..d204532 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/.svn/text-base/CollisionPrimitiveListItem.as.svn-base @@ -0,0 +1,76 @@ +package alternativa.physics { + import alternativa.physics.collision.CollisionPrimitive; + + /** + * Элемент списка примитивов. + */ + public class CollisionPrimitiveListItem { + + // Верхний элемент хранилища + private static var poolTop:CollisionPrimitiveListItem; + + /** + * Создаёт новый элемент списка. + * + * @param primitive примитив, содержащийся в элементе + * @return новый элемент списка + */ + public static function create(primitive:CollisionPrimitive):CollisionPrimitiveListItem { + var item:CollisionPrimitiveListItem; + if (poolTop == null) { + item = new CollisionPrimitiveListItem(primitive); + } else { + item = poolTop; + item.primitive = primitive; + poolTop = item.next; + item.next = null; + } + return item; + } + + /** + * Очищает хранилище. + */ + public static function clearPool():void { + var curr:CollisionPrimitiveListItem = poolTop; + while (curr != null) { + poolTop = curr.next; + curr.next = null; + curr = poolTop; + } + } + + /** + * Примитив, хранящийся в элементе списка. + */ + public var primitive:CollisionPrimitive; + /** + * Ссылка не следующий элемент списка. + */ + public var next:CollisionPrimitiveListItem; + /** + * Ссылка не предыдущий элемент списка. + */ + public var prev:CollisionPrimitiveListItem; + + /** + * Создаёт новый экземпляр. + * + * @param primitive примитив, хранящийся в элементе списка + */ + public function CollisionPrimitiveListItem(primitive:CollisionPrimitive) { + this.primitive = primitive; + } + + /** + * Очищает внутренние ссылки и помещает элемент в хранилище для дальнейшего использования. + */ + public function dispose():void { + primitive = null; + prev = null; + next = poolTop; + poolTop = this; + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/.svn/text-base/Contact.as.svn-base b/2.0.1.0-SNAPSHOT/src/alternativa/physics/.svn/text-base/Contact.as.svn-base new file mode 100644 index 0000000..3f010ec --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/.svn/text-base/Contact.as.svn-base @@ -0,0 +1,69 @@ +package alternativa.physics { + + + import alternativa.math.Vector3; + + /** + * Описывает контакт одного или двух тел. + */ + public class Contact { + /** + * Первое тело контакта, не может быть равно null. + */ + public var body1:Body; + /** + * Второе тело контакта, может быть равно null. + */ + public var body2:Body; + /** + * Предрассчитанный взаимный коэффициент отскока. + */ + public var restitution:Number; + /** + * Предрассчитанный взаимный коэффициент трения. + */ + public var friction:Number; + /** + * Нормаль контакта. Направлена к первому телу. + */ + public var normal:Vector3 = new Vector3(); + /** + * Список точек контакта. + */ + public var points:Vector. = new Vector.(MAX_POINTS, true); + /** + * Количество точек контакта. + */ + public var pcount:int; + /** + * Максимальная глубина пересечения. + */ + public var maxPenetration:Number = 0; + /** + * Флаг показывает, разрешён контакт или нет. + */ + public var satisfied:Boolean; + /** + * Следующий контакт в списке. + */ + public var next:Contact; + /** + * Индекс контакта. Первый контакт в списке имеет индекс 0. + */ + public var index:int; + + // Максимальное количество точек контакта + private const MAX_POINTS:int = 8; + + /** + * + */ + public function Contact(index:int) { + this.index = index; + for (var i:int = 0; i < MAX_POINTS; i++) { + points[i] = new ContactPoint(); + } + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/.svn/text-base/ContactPoint.as.svn-base b/2.0.1.0-SNAPSHOT/src/alternativa/physics/.svn/text-base/ContactPoint.as.svn-base new file mode 100644 index 0000000..d1d8eb6 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/.svn/text-base/ContactPoint.as.svn-base @@ -0,0 +1,55 @@ +package alternativa.physics { + import alternativa.math.Vector3; + + /** + * + */ + public class ContactPoint { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + + // Величины, расчитываемые перед началом фазы решения контактов + + // Требуемая проекция конечной скорости на нормаль для упругого контакта + public var normalVel:Number; + // Минимальная скорость разделения неупругого контакта + public var minSepVel:Number; + // Изменение проекции скорости на единицу нормального импульса + public var velByUnitImpulseN:Number; + + public var angularInertia1:Number; + public var angularInertia2:Number; + + // Радиус-вектор точки контакта относительно центра первого тела + public var r1:Vector3 = new Vector3(); + // Радиус-вектор точки контакта относительно центра второго тела + public var r2:Vector3 = new Vector3(); + + // Величины, накапливаемые во время фазы решения контактов + + // Накопленный импульс, применяемый для получения требуемой относительной скорости в точке контакта + public var accumImpulseN:Number; + // Накопленный импульс, применяемый для разделения тел в точке контакта. Не создаёт момента. +// public var accumSepImpulse:Number; + + public var satisfied:Boolean; + + /** + * + * @param cp + */ + public function copyFrom(cp:ContactPoint):void { + pos.copy(cp.pos); + penetration = cp.penetration; + feature1 = cp.feature1; + feature2 = cp.feature2; + r1.copy(cp.r1); + r2.copy(cp.r2); + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/.svn/text-base/PhysicsScene.as.svn-base b/2.0.1.0-SNAPSHOT/src/alternativa/physics/.svn/text-base/PhysicsScene.as.svn-base new file mode 100644 index 0000000..a1328a0 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/.svn/text-base/PhysicsScene.as.svn-base @@ -0,0 +1,549 @@ +package alternativa.physics { + + import alternativa.physics.collision.ICollisionDetector; + import alternativa.physics.collision.KdTreeCollisionDetector; + import alternativa.physics.constraints.Constraint; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + /** + * Класс реализует физическую симуляцию поведения твёрдых тел. + */ + public class PhysicsScene { + + private static var lastBodyId:int; + + // Максимальное количество контактов + public const MAX_CONTACTS:int = 1000; + + ////////////////////////////////////////////////////////////////// + // Настроечные параметры симуляции. Могут быть изменены в любой + // момент времени без нарушения корректной работы симуляции. + ////////////////////////////////////////////////////////////////// + // Количество шагов, за которое пересекающиеся тела должны разделиться + public var penResolutionSteps:int = 10; + // Величина допустимой глубины пересечения + public var allowedPenetration:Number = 0.1; + // Максимальная скорость, добавляемая с целью разделения тел + public var maxPenResolutionSpeed:Number = 0.5; + // Количество итераций для обработки упругих контактов + public var collisionIterations:int = 5; + // Количество итераций для обработки неупругих контактов + public var contactIterations:int = 5; + // Флаг использования предсказания состояний + public var usePrediction:Boolean = false; + + public var freezeSteps:int = 10; + public var linSpeedFreezeLimit:Number = 1; + public var angSpeedFreezeLimit:Number = 0.01; + + // Вектор гравитации + public var _gravity:Vector3 = new Vector3(0, 0, -9.8); + // Модуль вектора гравитации + public var _gravityMagnitude:Number = 9.8; + + // Использующийся детектор столкновений + public var collisionDetector:ICollisionDetector; + + // Список тел, участвующих в симуляции + public var bodies:BodyList = new BodyList(); + + // Список контактов на текущем шаге симуляции + public var contacts:Contact; + // Список ограничений + public var constraints:Vector. = new Vector.(); + // Количество ограничений + public var constraintsNum:int; + // Временная метка. Число прошедших шагов с начала симуляции. + public var frame:int; + // Время с начала симуляции, мс + public var time:int; + // Первый неиспользованный контакт на текущем шаге симуляции + private var borderContact:Contact; + + // Временные переменные для избежания создания экземпляров + private var _t:Vector3 = new Vector3(); + private var _v:Vector3 = new Vector3(); + + /** + * + */ + public function PhysicsScene() { + contacts = new Contact(0); + var contact:Contact = contacts; + for (var i:int = 1; i < MAX_CONTACTS; i++) { + contact.next = new Contact(i); + contact = contact.next; + } + collisionDetector = new KdTreeCollisionDetector(); + } + + /** + * Вектор гравитации. + */ + public function get gravity():Vector3 { + return _gravity.clone(); + } + + /** + * @private + */ + public function set gravity(value:Vector3):void { + _gravity.copy(value); + _gravityMagnitude = _gravity.length(); + } + + /** + * Добавляет тело в симуляцию. + * + * @param body + */ + public function addBody(body:Body):void { + body.id = lastBodyId++; + body.world = this; + bodies.append(body); + } + + /** + * Удаляет тело из симуляции. + * @param body + */ + public function removeBody(body:Body):void { + if (bodies.remove(body)) { + body.world = null; + } + } + + /** + * Добавляет ограничение. + * @param c + */ + public function addConstraint(c:Constraint):void { + constraints[constraintsNum++] = c; + c.world = this; + } + + /** + * Удаляет ограничение. + * @param c + */ + public function removeConstraint(c:Constraint):Boolean { + var idx:int = constraints.indexOf(c); + if (idx < 0) return false; + constraints.splice(idx, 1); + constraintsNum--; + c.world = null; + return true; + } + + /** + * Применяет к телам действующте на них силы. Аккумуляторы сил и моментов тел очищаются после завершения шага + * симуляции, поэтому на момент вызова метода могут уже содержать некоторые значения. + * + * @param dt промежуток времени, в течении которого действуют силы + */ + private function applyForces(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + body.beforePhysicsStep(dt); + body.calcAccelerations(); + // Ускорение свободного падения применяется только к подвижным телам во избежание некорректного изменения + // фиктивной скорости неподвижных тел. + if (body.useGravity && body.movable && !body.frozen) { + body.accel.x += _gravity.x; + body.accel.y += _gravity.y; + body.accel.z += _gravity.z; + } + item = item.next; + } + } + + /** + * Определяет все столкновения на текущем шаге симуляции и заполняет список получившихся контактов. + * + * @param dt длительность шага симуляции + */ + private function detectCollisions(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + if (!body.frozen) { + body.contactsNum = 0; + body.saveState(); + // При включённом режиме предсказания состояние тел интегрируется на один шаг вперёд + if (usePrediction) { + body.integrateVelocity(dt); + body.integratePosition(dt); + } + body.calcDerivedData(); + } + item = item.next; + } + + borderContact = collisionDetector.getAllContacts(contacts); + + // Расчёт относительных векторов точки контакта вынесен сюда из-за необходимости учитывать + // положение тел в предсказанном состоянии + var contact:Contact = contacts; + while (contact != borderContact) { + var b1:Body = contact.body1; + var b2:Body = contact.body2; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + var bPos:Vector3 = b1.state.pos; + cp.r1.x = cp.pos.x - bPos.x; + cp.r1.y = cp.pos.y - bPos.y; + cp.r1.z = cp.pos.z - bPos.z; + if (b2 != null) { + bPos = b2.state.pos; + cp.r2.x = cp.pos.x - bPos.x; + cp.r2.y = cp.pos.y - bPos.y; + cp.r2.z = cp.pos.z - bPos.z; + } + } + contact = contact.next; + } + + // Восстановление состояния тел + if (usePrediction) { + item = bodies.head; + while (item != null) { + body = item.body; + if (!body.frozen) { + body.restoreState(); + body.calcDerivedData(); + } + item = item.next; + } + } + } + + /** + * Подготваливает полученные из детектора столкновений контакты, расчитывая значения, не меняющиеся + * в ходе шага симуляции. + */ + private function preProcessContacts(dt:Number):void { + var contact:Contact = contacts; + while (contact != borderContact) { + var b1:Body = contact.body1; + var b2:Body = contact.body2; + // Столкнувшиеся тела размораживаются + if (b1.frozen) { + b1.frozen = false; + b1.freezeCounter = 0; + } + if (b2 != null && b2.frozen) { + b2.frozen = false; + b2.freezeCounter = 0; + } + contact.restitution = b1.material.restitution; + if (b2 != null && b2.material.restitution < contact.restitution) contact.restitution = b2.material.restitution; + contact.friction = b1.material.friction; + if (b2 != null && b2.material.friction < contact.friction) contact.friction = b2.material.friction; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.accumImpulseN = 0; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + cp.velByUnitImpulseN = 0; + if (b1.movable) { + cp.angularInertia1 = _v.cross2(cp.r1, contact.normal).transform3(b1.invInertiaWorld).cross(cp.r1).dot(contact.normal); + cp.velByUnitImpulseN += b1.invMass + cp.angularInertia1; + } + if (b2 != null && b2.movable) { + cp.angularInertia2 = _v.cross2(cp.r2, contact.normal).transform3(b2.invInertiaWorld).cross(cp.r2).dot(contact.normal); + cp.velByUnitImpulseN += b2.invMass + cp.angularInertia2; + } + // Расчёт требуемой конечной скорости для упругого контакта + calcSepVelocity(b1, b2, cp, _v); + cp.normalVel = _v.dot(contact.normal); + if (cp.normalVel < 0) cp.normalVel = - contact.restitution*cp.normalVel; + // Скорость разделения неупругого контакта + cp.minSepVel = cp.penetration > allowedPenetration ? (cp.penetration - allowedPenetration)/(penResolutionSteps*dt) : 0; + if (cp.minSepVel > maxPenResolutionSpeed) cp.minSepVel = maxPenResolutionSpeed; + } + contact = contact.next; + } + for (var i:int = 0; i < constraintsNum; i++) { + var constraint:Constraint = constraints[i]; + constraint.preProcess(dt); + } + } + + /** + * + * @param dt + * @param forceInelastic + */ + private function processContacts(dt:Number, forceInelastic:Boolean):void { + var iterNum:int = forceInelastic ? contactIterations : collisionIterations; + var i:int; + var forwardLoop:Boolean = false; + for (var iter:int = 0; iter < iterNum; iter++) { + forwardLoop = !forwardLoop; + var contact:Contact = contacts; + while (contact != borderContact) { + resolveContact(contact, forceInelastic, forwardLoop); + contact = contact.next; + } + // Ограничения + for (i = 0; i < constraintsNum; i++) { + var constraint:Constraint = constraints[i]; + constraint.apply(dt); + } + } + } + + /** + * + * @param contactInfo + * @param forceInelastic + * @param forwardLoop + */ + private function resolveContact(contactInfo:Contact, forceInelastic:Boolean, forwardLoop:Boolean):void { + var b1:Body = contactInfo.body1; + var b2:Body = contactInfo.body2; + var normal:Vector3 = contactInfo.normal; + var i:int; + if (forwardLoop) { + for (i = 0; i < contactInfo.pcount; i++) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } else { + for (i = contactInfo.pcount - 1; i >= 0; i--) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } + } + + /** + * + * @param idx + * @param b1 + * @param b2 + * @param contact + * @param normal + * @param forceInelastic + */ + private function resolveContactPoint(idx:int, b1:Body, b2:Body, contact:Contact, normal:Vector3, forceInelastic:Boolean):void { + var cp:ContactPoint = contact.points[idx]; + if (!forceInelastic) { + cp.satisfied = true; + } + + var newVel:Number = 0; + calcSepVelocity(b1, b2, cp, _v); + var cnormal:Vector3 = contact.normal; + var sepVel:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + if (forceInelastic) { + var minSpeVel:Number = cp.minSepVel; + if (sepVel < minSpeVel) { + cp.satisfied = false; + } else if (cp.satisfied) return; + newVel = minSpeVel; + } else { + newVel = cp.normalVel; + } + var deltaVel:Number = newVel - sepVel; + var impulse:Number = deltaVel/cp.velByUnitImpulseN; + var accumImpulse:Number = cp.accumImpulseN + impulse; + if (accumImpulse < 0) { + accumImpulse = 0; + } + var deltaImpulse:Number = accumImpulse - cp.accumImpulseN; + cp.accumImpulseN = accumImpulse; + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, normal, deltaImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, normal, -deltaImpulse); + } + + // Учёт силы трения + calcSepVelocity(b1, b2, cp, _v); + // Расчитываем изменение касательной скорости на единицу касательного импульса + var tanSpeedByUnitImpulse:Number = 0; + +// _v.vAddScaled(-_v.vDot(contact.normal), contact.normal); + var dot:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + _v.x -= dot*cnormal.x; + _v.y -= dot*cnormal.y; + _v.z -= dot*cnormal.z; + + var tanSpeed:Number = _v.length(); + if (tanSpeed < 0.001) return; + +// _t.vCopy(_v).vNormalize().vReverse(); + _t.x = -_v.x; + _t.y = -_v.y; + _t.z = -_v.z; + _t.normalize(); + + var r:Vector3; + var m:Matrix3; + var xx:Number; + var yy:Number; + var zz:Number; + // dV = b.invMass + ((invI * (r % t)) % r) * t + if (b1.movable) { +// _v.vCross2(cp.r1, _t).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1); + r = cp.r1; + m = b1.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b1.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + if (b2 != null && b2.movable) { +// _v.vCross2(cp.r2, _t).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2); + + r = cp.r2; + m = b2.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b2.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + + var tanImpulse:Number = tanSpeed/tanSpeedByUnitImpulse; + var max:Number = contact.friction*cp.accumImpulseN; + if (max < 0) { + if (tanImpulse < max) tanImpulse = max; + } else { + if (tanImpulse > max) tanImpulse = max; + } + + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, _t, tanImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, _t, -tanImpulse); + } + } + + /** + * + * @param body1 + * @param body2 + * @param cp + * @param result + */ + private function calcSepVelocity(body1:Body, body2:Body, cp:ContactPoint, result:Vector3):void { + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 +// result.vCopy(body1.state.velocity).vAdd(_v1.vCross2(body1.state.rotation, cp.r1)); + var rot:Vector3 = body1.state.rotation; + var v:Vector3 = cp.r1; + var x:Number = rot.y*v.z - rot.z*v.y; + var y:Number = rot.z*v.x - rot.x*v.z; + var z:Number = rot.x*v.y - rot.y*v.x; + v = body1.state.velocity; + result.x = v.x + x; + result.y = v.y + y; + result.z = v.z + z; + // V2 = V2_c + w2%r2 + if (body2 != null) { +// result.vSubtract(body2.state.velocity).vSubtract(_v2.vCross2(body2.state.rotation, cp.r2)); + rot = body2.state.rotation; + v = cp.r2; + x = rot.y*v.z - rot.z*v.y; + y = rot.z*v.x - rot.x*v.z; + z = rot.x*v.y - rot.y*v.x; + v = body2.state.velocity; + result.x -= v.x + x; + result.y -= v.y + y; + result.z -= v.z + z; + } + } + + /** + * + * @param dt + */ + private function intergateVelocities(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + item.body.integrateVelocity(dt); + item = item.next; + } + } + + /** + * + * @param dt + */ + private function integratePositions(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + if (body.movable && !body.frozen) { + body.integratePosition(dt); + } + item = item.next; + } + } + + /** + * + */ + private function postPhysics():void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + body.clearAccumulators(); + body.calcDerivedData(); + if (body.canFreeze) { + if (body.state.velocity.length() < linSpeedFreezeLimit && body.state.rotation.length() < angSpeedFreezeLimit) { + if (!body.frozen) { + body.freezeCounter++; + if (body.freezeCounter >= freezeSteps) + body.frozen = true; + } + } else { + body.freezeCounter = 0; + body.frozen = false; + } + } + item = item.next; + } + } + + /** + * + * @param delta + */ + public function update(delta:int):void { + frame++; + time += delta; + var dt:Number = 0.001*delta; + applyForces(dt); + detectCollisions(dt); + preProcessContacts(dt); + processContacts(dt, false); + intergateVelocities(dt); + processContacts(dt, true); + integratePositions(dt); + postPhysics(); + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/.svn/text-base/PhysicsUtils.as.svn-base b/2.0.1.0-SNAPSHOT/src/alternativa/physics/.svn/text-base/PhysicsUtils.as.svn-base new file mode 100644 index 0000000..5a2e46f --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/.svn/text-base/PhysicsUtils.as.svn-base @@ -0,0 +1,49 @@ +package alternativa.physics { + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + public class PhysicsUtils { + + /** + * @param mass + * @param halfSize + * @param result + */ + public static function getBoxInvInertia(mass:Number, halfSize:Vector3, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + result.a = 3/(mass*(yy + zz)); + result.f = 3/(mass*(zz + xx)); + result.k = 3/(mass*(xx + yy)); + } + + /** + * @param mass + * @param r + * @param h + * @param result + */ + public static function getCylinderInvInertia(mass:Number, r:Number, h:Number, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + + result.a = result.f = 1/(mass*(h*h/12 + r*r/4)); + result.k = 2/(mass*r*r); + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/Body.as b/2.0.1.0-SNAPSHOT/src/alternativa/physics/Body.as new file mode 100644 index 0000000..28c9b1a --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/Body.as @@ -0,0 +1,461 @@ +package alternativa.physics { + + import alternativa.math.Matrix3; + import alternativa.math.Matrix4; + import alternativa.math.Quaternion; + import alternativa.math.Vector3; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.IBodyCollisionPredicate; + import alternativa.physics.collision.types.BoundBox; + + /** + * + */ + public class Body { + + public static var linDamping:Number = 0.997; + public static var rotDamping:Number = 0.997; + + // Идентификатор тела, уникальный в пределах мира + public var id:int; + // Имя тела + public var name:String; + // Мир, в котором находится тело + public var world:PhysicsScene; + // Флаг подвижности тела + public var movable:Boolean = true; + // Флаг указывает, может ли тело быть заморожено + public var canFreeze:Boolean = false; + + public var freezeCounter:int; + public var frozen:Boolean = false; + + public var useGravity:Boolean = true; + + // Ограничивающий бокс тела + public var aabb:BoundBox = new BoundBox(); + // Предикат, через который тело получает сообщения о столкновениях + public var postCollisionPredicate:IBodyCollisionPredicate; + + // Текущее состояние тела + public var state:BodyState = new BodyState(); + // Предыдущее состояние тела + public var prevState:BodyState = new BodyState(); + // Линейное ускорение тела на текущем шаге симуляции + public var accel:Vector3 = new Vector3(); + // Угловое ускорение тела на текущем шаге симуляции + public var angleAccel:Vector3 = new Vector3(); + // Физический материал тела + public var material:BodyMaterial = new BodyMaterial(); + // Обратная масса тела + public var invMass:Number = 1; + // Обратная матрица тензора инерции в локальных координатах + public var invInertia:Matrix3 = new Matrix3(); + // Обратная матрица тензора инерции в мировых координатах + public var invInertiaWorld:Matrix3 = new Matrix3(); + // Базисная матрица тела в мировых координатах + public var baseMatrix:Matrix3 = new Matrix3(); + + public const MAX_CONTACTS:int = 20; + public var contacts:Vector. = new Vector.(MAX_CONTACTS); + public var contactsNum:int; + + public var collisionPrimitives:CollisionPrimitiveList; + + // Аккумулятор сил + public var forceAccum:Vector3 = new Vector3(); + // Аккумулятор моментов + public var torqueAccum:Vector3 = new Vector3(); + + // Внутренние переменные для избежания создания экземпляров + private static var _r:Vector3 = new Vector3(); + private static var _f:Vector3 = new Vector3(); + private static var _q:Quaternion = new Quaternion(); + + /** + * + * @param invMass + * @param invInertia + */ + public function Body(invMass:Number, invInertia:Matrix3) { + this.invMass = invMass; + this.invInertia.copy(invInertia); + } + + /** + * @param primitive + * @param localTransform + */ + public function addCollisionPrimitive(primitive:CollisionPrimitive, localTransform:Matrix4 = null):void { + if (primitive == null) { + throw new ArgumentError("Primitive cannot be null"); + } + if (collisionPrimitives == null) { + collisionPrimitives = new CollisionPrimitiveList(); + } + collisionPrimitives.append(primitive); + primitive.setBody(this, localTransform); + } + + /** + * + * @param primitive + */ + public function removeCollisionPrimitive(primitive:CollisionPrimitive):void { + if (collisionPrimitives == null) return; + primitive.setBody(null); + collisionPrimitives.remove(primitive); + if (collisionPrimitives.size == 0) { + collisionPrimitives = null; + } + } + + /** + * + * @param t + * @param pos + * @param orientation + */ + public function interpolate(t:Number, pos:Vector3, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.w = prevState.orientation.w*t1 + state.orientation.w*t; + orientation.x = prevState.orientation.x*t1 + state.orientation.x*t; + orientation.y = prevState.orientation.y*t1 + state.orientation.y*t; + orientation.z = prevState.orientation.z*t1 + state.orientation.z*t; + } + + /** + * + * @param t + * @param pos + * @param orientation + */ + public function interpolateSLERP(t:Number, pos:Vector3, orientation:Quaternion):void { + var t1:Number = 1 - t; + pos.x = prevState.pos.x*t1 + state.pos.x*t; + pos.y = prevState.pos.y*t1 + state.pos.y*t; + pos.z = prevState.pos.z*t1 + state.pos.z*t; + orientation.slerp(prevState.orientation, state.orientation, t); + } + + /** + * + * @param t + * @param m + */ + public function interpolateToMatrix(t:Number, m:Matrix4):void { + var t1:Number = 1 - t; + m.d = prevState.pos.x*t1 + state.pos.x*t; + m.h = prevState.pos.y*t1 + state.pos.y*t; + m.l = prevState.pos.z*t1 + state.pos.z*t; + _q.w = prevState.orientation.w*t1 + state.orientation.w*t; + _q.x = prevState.orientation.x*t1 + state.orientation.x*t; + _q.y = prevState.orientation.y*t1 + state.orientation.y*t; + _q.z = prevState.orientation.z*t1 + state.orientation.z*t; + _q.normalize(); + _q.toMatrix4(m); + } + + /** + * + * @param t + * @param m + */ + public function interpolateToMatrixSLERP(t:Number, m:Matrix4):void { + var t1:Number = 1 - t; + m.d = prevState.pos.x*t1 + state.pos.x*t; + m.h = prevState.pos.y*t1 + state.pos.y*t; + m.l = prevState.pos.z*t1 + state.pos.z*t; + _q.slerp(prevState.orientation, state.orientation, t); + _q.normalize(); + _q.toMatrix4(m); + } + + /** + * + * @param pos + */ + public function setPosition(pos:Vector3):void { + state.pos.copy(pos); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setPositionXYZ(x:Number, y:Number, z:Number):void { + state.pos.reset(x, y, z); + } + + /** + * + * @param vel + */ + public function setVelocity(vel:Vector3):void { + state.velocity.copy(vel); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setVelocityXYZ(x:Number, y:Number, z:Number):void { + state.velocity.reset(x, y, z); + } + + /** + * + * @param rot + */ + public function setRotation(rot:Vector3):void { + state.rotation.copy(rot); + } + + /** + * + * @param x + * @param y + * @param z + */ + public function setRotationXYZ(x:Number, y:Number, z:Number):void { + state.rotation.reset(x, y, z); + } + + /** + * + * @param q + */ + public function setOrientation(q:Quaternion):void { + state.orientation.copy(q); + } + + /** + * @param r + * @param dir + * @param magnitude + */ + public function applyRelPosWorldImpulse(r:Vector3, dir:Vector3, magnitude:Number):void { + var d:Number = magnitude*invMass; + // Линейная часть + state.velocity.x += d*dir.x; + state.velocity.y += d*dir.y; + state.velocity.z += d*dir.z; + + // Вращательная часть + var x:Number = (r.y*dir.z - r.z*dir.y)*magnitude; + var y:Number = (r.z*dir.x - r.x*dir.z)*magnitude; + var z:Number = (r.x*dir.y - r.y*dir.x)*magnitude; + + state.rotation.x += invInertiaWorld.a*x + invInertiaWorld.b*y + invInertiaWorld.c*z; + state.rotation.y += invInertiaWorld.e*x + invInertiaWorld.f*y + invInertiaWorld.g*z; + state.rotation.z += invInertiaWorld.i*x + invInertiaWorld.j*y + invInertiaWorld.k*z; + } + + /** + * @param f + */ + public function addForce(f:Vector3):void { + forceAccum.add(f); + } + + /** + * + * @param fx + * @param fy + * @param fz + */ + public function addForceXYZ(fx:Number, fy:Number, fz:Number):void { + forceAccum.x += fx; + forceAccum.y += fy; + forceAccum.z += fz; + } + + /** + * + * @param px + * @param py + * @param pz + * @param fx + * @param fy + * @param fz + */ + public function addWorldForceXYZ(px:Number, py:Number, pz:Number, fx:Number, fy:Number, fz:Number):void { + forceAccum.x += fx; + forceAccum.y += fy; + forceAccum.z += fz; + + var pos:Vector3 = state.pos; + var rx:Number = px - pos.x; + var ry:Number = py - pos.y; + var rz:Number = pz - pos.z; + +// var x:Number = ry*fz - rz*fy; +// var y:Number = rz*fx - rx*fz; +// var z:Number = rx*fy - ry*fx; + + torqueAccum.x += ry*fz - rz*fy; + torqueAccum.y += rz*fx - rx*fz; + torqueAccum.z += rx*fy - ry*fx; + } + + /** + * @param pos + * @param f + */ + public function addWorldForce(pos:Vector3, force:Vector3):void { + forceAccum.add(force); + torqueAccum.add(_r.diff(pos, state.pos).cross(force)); + } + + /** + * @param pos + * @param f + */ + public function addWorldForceScaled(pos:Vector3, force:Vector3, scale:Number):void { + _f.x = scale*force.x; + _f.y = scale*force.y; + _f.z = scale*force.z; + forceAccum.add(_f); + torqueAccum.add(_r.diff(pos, state.pos).cross(_f)); + } + + /** + * @param pos + * @param f + */ + public function addLocalForce(pos:Vector3, force:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(pos, _r); + // Трансформируем вектор силы в мировую систему + baseMatrix.transformVector(force, _f); + // Добавляем силу и момент + forceAccum.add(_f); + torqueAccum.add(_r.cross(_f)); + } + + /** + * + * @param localPos + * @param worldForce + */ + public function addWorldForceAtLocalPoint(localPos:Vector3, worldForce:Vector3):void { + // Трансформируем точку приложения в мировую систему координат + baseMatrix.transformVector(localPos, _r); + // Добавляем силу и момент + forceAccum.add(worldForce); + torqueAccum.add(_r.cross(worldForce)); + } + + /** + * @param dt + */ + public function beforePhysicsStep(dt:Number):void { + } + + /** + * @param t + */ + public function addTorque(t:Vector3):void { + torqueAccum.add(t); + } + + /** + * + */ + internal function clearAccumulators():void { + forceAccum.x = forceAccum.y = forceAccum.z = 0; + torqueAccum.x = torqueAccum.y = torqueAccum.z = 0; + } + + /** + * + */ + internal function calcAccelerations():void { + accel.x = forceAccum.x*invMass; + accel.y = forceAccum.y*invMass; + accel.z = forceAccum.z*invMass; + angleAccel.x = invInertiaWorld.a*torqueAccum.x + invInertiaWorld.b*torqueAccum.y + invInertiaWorld.c*torqueAccum.z; + angleAccel.y = invInertiaWorld.e*torqueAccum.x + invInertiaWorld.f*torqueAccum.y + invInertiaWorld.g*torqueAccum.z; + angleAccel.z = invInertiaWorld.i*torqueAccum.x + invInertiaWorld.j*torqueAccum.y + invInertiaWorld.k*torqueAccum.z; + } + + /** + * Вычисляет производные данные. + */ + public function calcDerivedData():void { + // Вычисление базисной матрицы и обратного тензора инерции в мировых координатах + state.orientation.toMatrix3(baseMatrix); + invInertiaWorld.copy(invInertia).append(baseMatrix).prependTransposed(baseMatrix); + if (collisionPrimitives != null) { + aabb.infinity(); + var item:CollisionPrimitiveListItem = collisionPrimitives.head; + while (item != null) { + var primitive:CollisionPrimitive = item.primitive; + primitive.transform.setFromMatrix3(baseMatrix, state.pos); + if (primitive.localTransform != null) { + primitive.transform.prepend(primitive.localTransform); + } + primitive.calculateAABB(); + aabb.addBoundBox(primitive.aabb); + item = item.next; + } + } + } + + /** + * + */ + public function saveState():void { + prevState.copy(state); + } + + /** + * + */ + internal function restoreState():void { + state.copy(prevState); + } + + /** + * @param dt + */ + internal function integrateVelocity(dt:Number):void { + // v = v + a*t + state.velocity.x += accel.x*dt; + state.velocity.y += accel.y*dt; + state.velocity.z += accel.z*dt; + // rot = rot + eps*t + state.rotation.x += angleAccel.x*dt; + state.rotation.y += angleAccel.y*dt; + state.rotation.z += angleAccel.z*dt; + + state.velocity.x *= linDamping; + state.velocity.y *= linDamping; + state.velocity.z *= linDamping; + + state.rotation.x *= rotDamping; + state.rotation.y *= rotDamping; + state.rotation.z *= rotDamping; + } + + /** + * + */ + internal function integratePosition(dt:Number):void { + // pos = pos + v*t + state.pos.x += state.velocity.x*dt; + state.pos.y += state.velocity.y*dt; + state.pos.z += state.velocity.z*dt; + // q = q + 0.5*rot*q + state.orientation.addScaledVector(state.rotation, dt); + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/BodyList.as b/2.0.1.0-SNAPSHOT/src/alternativa/physics/BodyList.as new file mode 100644 index 0000000..aa8f79f --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/BodyList.as @@ -0,0 +1,76 @@ +package alternativa.physics { + + /** + * + */ + public class BodyList { + + public var head:BodyListItem; + public var tail:BodyListItem; + public var size:int; + + /** + * + */ + public function BodyList() { + } + + /** + * + * @param body + */ + public function append(body:Body):void { + var item:BodyListItem = BodyListItem.create(body); + if (head == null) { + head = tail = item; + } else { + tail.next = item; + item.prev = tail; + tail = item; + } + size++; + } + + /** + * + * @param body + */ + public function remove(body:Body):Boolean { + var item:BodyListItem = findItem(body); + if (item == null) return false; + if (item == head) { + if (size == 1) { + head = tail = null; + } else { + head = item.next; + head.prev = null; + } + } else { + if (item == tail) { + tail = item.prev; + tail.next = null; + } else { + item.prev.next = item.next; + item.next.prev = item.prev; + } + } + item.dispose(); + size--; + return true; + } + + /** + * + * @param body + * @return + */ + public function findItem(body:Body):BodyListItem { + var item:BodyListItem = head; + while (item != null && item.body != body) { + item = item.next; + } + return item; + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/BodyListItem.as b/2.0.1.0-SNAPSHOT/src/alternativa/physics/BodyListItem.as new file mode 100644 index 0000000..382e11c --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/BodyListItem.as @@ -0,0 +1,74 @@ +package alternativa.physics { + + /** + * + */ + public class BodyListItem { + + // Верхний элемент хранилища + private static var poolTop:BodyListItem; + + /** + * Создаёт новый элемент списка. + * + * @param primitive примитив, содержащийся в элементе + * @return новый элемент списка + */ + public static function create(body:Body):BodyListItem { + var item:BodyListItem; + if (poolTop == null) { + item = new BodyListItem(body); + } else { + item = poolTop; + poolTop = item.next; + item.next = null; + item.body = body; + } + return item; + } + + /** + * Очищает хранилище. + */ + public static function clearPool():void { + var item:BodyListItem = poolTop; + while (item != null) { + poolTop = item.next; + item.next = null; + item = poolTop; + } + } + + /** + * + */ + public var body:Body; + /** + * + */ + public var next:BodyListItem; + /** + * + */ + public var prev:BodyListItem; + + /** + * + * @param body + */ + public function BodyListItem(body:Body) { + this.body = body; + } + + /** + * + */ + public function dispose():void { + body = null; + prev = null; + next = poolTop; + poolTop = this; + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/BodyMaterial.as b/2.0.1.0-SNAPSHOT/src/alternativa/physics/BodyMaterial.as new file mode 100644 index 0000000..7a7c47d --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/BodyMaterial.as @@ -0,0 +1,11 @@ +package alternativa.physics { + + public class BodyMaterial { + + public var restitution:Number = 0; + public var friction:Number = 0.3; +// public var dynamicFriction:Number = 0.2; +// public var dynamicFriction:Number = 0.2; + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/BodyState.as b/2.0.1.0-SNAPSHOT/src/alternativa/physics/BodyState.as new file mode 100644 index 0000000..f397afe --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/BodyState.as @@ -0,0 +1,39 @@ +package alternativa.physics { + import alternativa.math.Quaternion; + import alternativa.math.Vector3; + + /** + * Класс описывает состояние твёрдого тела. + */ + public class BodyState { + /** + * Положение тела. + */ + public var pos:Vector3 = new Vector3(); + /** + * Ориентация тела. + */ + public var orientation:Quaternion = new Quaternion(); + /** + * Скорость тела. + */ + public var velocity:Vector3 = new Vector3(); + /** + * Угловая скорость тела. + */ + public var rotation:Vector3 = new Vector3(); + + /** + * Копирует значение указанного объекта. + * + * @param state + */ + public function copy(state:BodyState):void { + pos.copy(state.pos); + orientation.copy(state.orientation); + velocity.copy(state.velocity); + rotation.copy(state.rotation); + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/CollisionPrimitiveList.as b/2.0.1.0-SNAPSHOT/src/alternativa/physics/CollisionPrimitiveList.as new file mode 100644 index 0000000..5d5304f --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/CollisionPrimitiveList.as @@ -0,0 +1,89 @@ +package alternativa.physics { + import alternativa.physics.collision.CollisionPrimitive; + + /** + * + */ + public class CollisionPrimitiveList { + + public var head:CollisionPrimitiveListItem; + public var tail:CollisionPrimitiveListItem; + public var size:int; + + /** + * + */ + public function CollisionPrimitiveList() { + } + + /** + * + * @param primitive + */ + public function append(primitive:CollisionPrimitive):void { + var item:CollisionPrimitiveListItem = CollisionPrimitiveListItem.create(primitive); + if (head == null) { + head = tail = item; + } else { + tail.next = item; + item.prev = tail; + tail = item; + } + size++; + } + + /** + * + * @param primitve + */ + public function remove(primitve:CollisionPrimitive):void { + var item:CollisionPrimitiveListItem = findItem(primitve); + if (item == null) return; + if (item == head) { + if (size == 1) { + head = tail = null; + } else { + head = item.next; + head.prev = null; + } + } else { + if (item == tail) { + tail = tail.prev; + tail.next = null; + } else { + item.prev.next = item.next; + item.next.prev = item.prev; + } + } + item.dispose(); + size--; + } + + /** + * + * @param primitive + * @return + */ + public function findItem(primitive:CollisionPrimitive):CollisionPrimitiveListItem { + var item:CollisionPrimitiveListItem = head; + while (item != null && item.primitive != primitive) { + item = item.next; + } + return item; + } + + /** + * + */ + public function clear():void { + while (head != null) { + var item:CollisionPrimitiveListItem = head; + head = head.next; + item.dispose(); + } + tail = null; + size = 0; + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/CollisionPrimitiveListItem.as b/2.0.1.0-SNAPSHOT/src/alternativa/physics/CollisionPrimitiveListItem.as new file mode 100644 index 0000000..d204532 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/CollisionPrimitiveListItem.as @@ -0,0 +1,76 @@ +package alternativa.physics { + import alternativa.physics.collision.CollisionPrimitive; + + /** + * Элемент списка примитивов. + */ + public class CollisionPrimitiveListItem { + + // Верхний элемент хранилища + private static var poolTop:CollisionPrimitiveListItem; + + /** + * Создаёт новый элемент списка. + * + * @param primitive примитив, содержащийся в элементе + * @return новый элемент списка + */ + public static function create(primitive:CollisionPrimitive):CollisionPrimitiveListItem { + var item:CollisionPrimitiveListItem; + if (poolTop == null) { + item = new CollisionPrimitiveListItem(primitive); + } else { + item = poolTop; + item.primitive = primitive; + poolTop = item.next; + item.next = null; + } + return item; + } + + /** + * Очищает хранилище. + */ + public static function clearPool():void { + var curr:CollisionPrimitiveListItem = poolTop; + while (curr != null) { + poolTop = curr.next; + curr.next = null; + curr = poolTop; + } + } + + /** + * Примитив, хранящийся в элементе списка. + */ + public var primitive:CollisionPrimitive; + /** + * Ссылка не следующий элемент списка. + */ + public var next:CollisionPrimitiveListItem; + /** + * Ссылка не предыдущий элемент списка. + */ + public var prev:CollisionPrimitiveListItem; + + /** + * Создаёт новый экземпляр. + * + * @param primitive примитив, хранящийся в элементе списка + */ + public function CollisionPrimitiveListItem(primitive:CollisionPrimitive) { + this.primitive = primitive; + } + + /** + * Очищает внутренние ссылки и помещает элемент в хранилище для дальнейшего использования. + */ + public function dispose():void { + primitive = null; + prev = null; + next = poolTop; + poolTop = this; + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/Contact.as b/2.0.1.0-SNAPSHOT/src/alternativa/physics/Contact.as new file mode 100644 index 0000000..3f010ec --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/Contact.as @@ -0,0 +1,69 @@ +package alternativa.physics { + + + import alternativa.math.Vector3; + + /** + * Описывает контакт одного или двух тел. + */ + public class Contact { + /** + * Первое тело контакта, не может быть равно null. + */ + public var body1:Body; + /** + * Второе тело контакта, может быть равно null. + */ + public var body2:Body; + /** + * Предрассчитанный взаимный коэффициент отскока. + */ + public var restitution:Number; + /** + * Предрассчитанный взаимный коэффициент трения. + */ + public var friction:Number; + /** + * Нормаль контакта. Направлена к первому телу. + */ + public var normal:Vector3 = new Vector3(); + /** + * Список точек контакта. + */ + public var points:Vector. = new Vector.(MAX_POINTS, true); + /** + * Количество точек контакта. + */ + public var pcount:int; + /** + * Максимальная глубина пересечения. + */ + public var maxPenetration:Number = 0; + /** + * Флаг показывает, разрешён контакт или нет. + */ + public var satisfied:Boolean; + /** + * Следующий контакт в списке. + */ + public var next:Contact; + /** + * Индекс контакта. Первый контакт в списке имеет индекс 0. + */ + public var index:int; + + // Максимальное количество точек контакта + private const MAX_POINTS:int = 8; + + /** + * + */ + public function Contact(index:int) { + this.index = index; + for (var i:int = 0; i < MAX_POINTS; i++) { + points[i] = new ContactPoint(); + } + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/ContactPoint.as b/2.0.1.0-SNAPSHOT/src/alternativa/physics/ContactPoint.as new file mode 100644 index 0000000..d1d8eb6 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/ContactPoint.as @@ -0,0 +1,55 @@ +package alternativa.physics { + import alternativa.math.Vector3; + + /** + * + */ + public class ContactPoint { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + + // Величины, расчитываемые перед началом фазы решения контактов + + // Требуемая проекция конечной скорости на нормаль для упругого контакта + public var normalVel:Number; + // Минимальная скорость разделения неупругого контакта + public var minSepVel:Number; + // Изменение проекции скорости на единицу нормального импульса + public var velByUnitImpulseN:Number; + + public var angularInertia1:Number; + public var angularInertia2:Number; + + // Радиус-вектор точки контакта относительно центра первого тела + public var r1:Vector3 = new Vector3(); + // Радиус-вектор точки контакта относительно центра второго тела + public var r2:Vector3 = new Vector3(); + + // Величины, накапливаемые во время фазы решения контактов + + // Накопленный импульс, применяемый для получения требуемой относительной скорости в точке контакта + public var accumImpulseN:Number; + // Накопленный импульс, применяемый для разделения тел в точке контакта. Не создаёт момента. +// public var accumSepImpulse:Number; + + public var satisfied:Boolean; + + /** + * + * @param cp + */ + public function copyFrom(cp:ContactPoint):void { + pos.copy(cp.pos); + penetration = cp.penetration; + feature1 = cp.feature1; + feature2 = cp.feature2; + r1.copy(cp.r1); + r2.copy(cp.r2); + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/PhysicsScene.as b/2.0.1.0-SNAPSHOT/src/alternativa/physics/PhysicsScene.as new file mode 100644 index 0000000..a1328a0 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/PhysicsScene.as @@ -0,0 +1,549 @@ +package alternativa.physics { + + import alternativa.physics.collision.ICollisionDetector; + import alternativa.physics.collision.KdTreeCollisionDetector; + import alternativa.physics.constraints.Constraint; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + /** + * Класс реализует физическую симуляцию поведения твёрдых тел. + */ + public class PhysicsScene { + + private static var lastBodyId:int; + + // Максимальное количество контактов + public const MAX_CONTACTS:int = 1000; + + ////////////////////////////////////////////////////////////////// + // Настроечные параметры симуляции. Могут быть изменены в любой + // момент времени без нарушения корректной работы симуляции. + ////////////////////////////////////////////////////////////////// + // Количество шагов, за которое пересекающиеся тела должны разделиться + public var penResolutionSteps:int = 10; + // Величина допустимой глубины пересечения + public var allowedPenetration:Number = 0.1; + // Максимальная скорость, добавляемая с целью разделения тел + public var maxPenResolutionSpeed:Number = 0.5; + // Количество итераций для обработки упругих контактов + public var collisionIterations:int = 5; + // Количество итераций для обработки неупругих контактов + public var contactIterations:int = 5; + // Флаг использования предсказания состояний + public var usePrediction:Boolean = false; + + public var freezeSteps:int = 10; + public var linSpeedFreezeLimit:Number = 1; + public var angSpeedFreezeLimit:Number = 0.01; + + // Вектор гравитации + public var _gravity:Vector3 = new Vector3(0, 0, -9.8); + // Модуль вектора гравитации + public var _gravityMagnitude:Number = 9.8; + + // Использующийся детектор столкновений + public var collisionDetector:ICollisionDetector; + + // Список тел, участвующих в симуляции + public var bodies:BodyList = new BodyList(); + + // Список контактов на текущем шаге симуляции + public var contacts:Contact; + // Список ограничений + public var constraints:Vector. = new Vector.(); + // Количество ограничений + public var constraintsNum:int; + // Временная метка. Число прошедших шагов с начала симуляции. + public var frame:int; + // Время с начала симуляции, мс + public var time:int; + // Первый неиспользованный контакт на текущем шаге симуляции + private var borderContact:Contact; + + // Временные переменные для избежания создания экземпляров + private var _t:Vector3 = new Vector3(); + private var _v:Vector3 = new Vector3(); + + /** + * + */ + public function PhysicsScene() { + contacts = new Contact(0); + var contact:Contact = contacts; + for (var i:int = 1; i < MAX_CONTACTS; i++) { + contact.next = new Contact(i); + contact = contact.next; + } + collisionDetector = new KdTreeCollisionDetector(); + } + + /** + * Вектор гравитации. + */ + public function get gravity():Vector3 { + return _gravity.clone(); + } + + /** + * @private + */ + public function set gravity(value:Vector3):void { + _gravity.copy(value); + _gravityMagnitude = _gravity.length(); + } + + /** + * Добавляет тело в симуляцию. + * + * @param body + */ + public function addBody(body:Body):void { + body.id = lastBodyId++; + body.world = this; + bodies.append(body); + } + + /** + * Удаляет тело из симуляции. + * @param body + */ + public function removeBody(body:Body):void { + if (bodies.remove(body)) { + body.world = null; + } + } + + /** + * Добавляет ограничение. + * @param c + */ + public function addConstraint(c:Constraint):void { + constraints[constraintsNum++] = c; + c.world = this; + } + + /** + * Удаляет ограничение. + * @param c + */ + public function removeConstraint(c:Constraint):Boolean { + var idx:int = constraints.indexOf(c); + if (idx < 0) return false; + constraints.splice(idx, 1); + constraintsNum--; + c.world = null; + return true; + } + + /** + * Применяет к телам действующте на них силы. Аккумуляторы сил и моментов тел очищаются после завершения шага + * симуляции, поэтому на момент вызова метода могут уже содержать некоторые значения. + * + * @param dt промежуток времени, в течении которого действуют силы + */ + private function applyForces(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + body.beforePhysicsStep(dt); + body.calcAccelerations(); + // Ускорение свободного падения применяется только к подвижным телам во избежание некорректного изменения + // фиктивной скорости неподвижных тел. + if (body.useGravity && body.movable && !body.frozen) { + body.accel.x += _gravity.x; + body.accel.y += _gravity.y; + body.accel.z += _gravity.z; + } + item = item.next; + } + } + + /** + * Определяет все столкновения на текущем шаге симуляции и заполняет список получившихся контактов. + * + * @param dt длительность шага симуляции + */ + private function detectCollisions(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + if (!body.frozen) { + body.contactsNum = 0; + body.saveState(); + // При включённом режиме предсказания состояние тел интегрируется на один шаг вперёд + if (usePrediction) { + body.integrateVelocity(dt); + body.integratePosition(dt); + } + body.calcDerivedData(); + } + item = item.next; + } + + borderContact = collisionDetector.getAllContacts(contacts); + + // Расчёт относительных векторов точки контакта вынесен сюда из-за необходимости учитывать + // положение тел в предсказанном состоянии + var contact:Contact = contacts; + while (contact != borderContact) { + var b1:Body = contact.body1; + var b2:Body = contact.body2; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + var bPos:Vector3 = b1.state.pos; + cp.r1.x = cp.pos.x - bPos.x; + cp.r1.y = cp.pos.y - bPos.y; + cp.r1.z = cp.pos.z - bPos.z; + if (b2 != null) { + bPos = b2.state.pos; + cp.r2.x = cp.pos.x - bPos.x; + cp.r2.y = cp.pos.y - bPos.y; + cp.r2.z = cp.pos.z - bPos.z; + } + } + contact = contact.next; + } + + // Восстановление состояния тел + if (usePrediction) { + item = bodies.head; + while (item != null) { + body = item.body; + if (!body.frozen) { + body.restoreState(); + body.calcDerivedData(); + } + item = item.next; + } + } + } + + /** + * Подготваливает полученные из детектора столкновений контакты, расчитывая значения, не меняющиеся + * в ходе шага симуляции. + */ + private function preProcessContacts(dt:Number):void { + var contact:Contact = contacts; + while (contact != borderContact) { + var b1:Body = contact.body1; + var b2:Body = contact.body2; + // Столкнувшиеся тела размораживаются + if (b1.frozen) { + b1.frozen = false; + b1.freezeCounter = 0; + } + if (b2 != null && b2.frozen) { + b2.frozen = false; + b2.freezeCounter = 0; + } + contact.restitution = b1.material.restitution; + if (b2 != null && b2.material.restitution < contact.restitution) contact.restitution = b2.material.restitution; + contact.friction = b1.material.friction; + if (b2 != null && b2.material.friction < contact.friction) contact.friction = b2.material.friction; + for (var j:int = 0; j < contact.pcount; j++) { + var cp:ContactPoint = contact.points[j]; + cp.accumImpulseN = 0; + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + cp.velByUnitImpulseN = 0; + if (b1.movable) { + cp.angularInertia1 = _v.cross2(cp.r1, contact.normal).transform3(b1.invInertiaWorld).cross(cp.r1).dot(contact.normal); + cp.velByUnitImpulseN += b1.invMass + cp.angularInertia1; + } + if (b2 != null && b2.movable) { + cp.angularInertia2 = _v.cross2(cp.r2, contact.normal).transform3(b2.invInertiaWorld).cross(cp.r2).dot(contact.normal); + cp.velByUnitImpulseN += b2.invMass + cp.angularInertia2; + } + // Расчёт требуемой конечной скорости для упругого контакта + calcSepVelocity(b1, b2, cp, _v); + cp.normalVel = _v.dot(contact.normal); + if (cp.normalVel < 0) cp.normalVel = - contact.restitution*cp.normalVel; + // Скорость разделения неупругого контакта + cp.minSepVel = cp.penetration > allowedPenetration ? (cp.penetration - allowedPenetration)/(penResolutionSteps*dt) : 0; + if (cp.minSepVel > maxPenResolutionSpeed) cp.minSepVel = maxPenResolutionSpeed; + } + contact = contact.next; + } + for (var i:int = 0; i < constraintsNum; i++) { + var constraint:Constraint = constraints[i]; + constraint.preProcess(dt); + } + } + + /** + * + * @param dt + * @param forceInelastic + */ + private function processContacts(dt:Number, forceInelastic:Boolean):void { + var iterNum:int = forceInelastic ? contactIterations : collisionIterations; + var i:int; + var forwardLoop:Boolean = false; + for (var iter:int = 0; iter < iterNum; iter++) { + forwardLoop = !forwardLoop; + var contact:Contact = contacts; + while (contact != borderContact) { + resolveContact(contact, forceInelastic, forwardLoop); + contact = contact.next; + } + // Ограничения + for (i = 0; i < constraintsNum; i++) { + var constraint:Constraint = constraints[i]; + constraint.apply(dt); + } + } + } + + /** + * + * @param contactInfo + * @param forceInelastic + * @param forwardLoop + */ + private function resolveContact(contactInfo:Contact, forceInelastic:Boolean, forwardLoop:Boolean):void { + var b1:Body = contactInfo.body1; + var b2:Body = contactInfo.body2; + var normal:Vector3 = contactInfo.normal; + var i:int; + if (forwardLoop) { + for (i = 0; i < contactInfo.pcount; i++) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } else { + for (i = contactInfo.pcount - 1; i >= 0; i--) resolveContactPoint(i, b1, b2, contactInfo, normal, forceInelastic); + } + } + + /** + * + * @param idx + * @param b1 + * @param b2 + * @param contact + * @param normal + * @param forceInelastic + */ + private function resolveContactPoint(idx:int, b1:Body, b2:Body, contact:Contact, normal:Vector3, forceInelastic:Boolean):void { + var cp:ContactPoint = contact.points[idx]; + if (!forceInelastic) { + cp.satisfied = true; + } + + var newVel:Number = 0; + calcSepVelocity(b1, b2, cp, _v); + var cnormal:Vector3 = contact.normal; + var sepVel:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + if (forceInelastic) { + var minSpeVel:Number = cp.minSepVel; + if (sepVel < minSpeVel) { + cp.satisfied = false; + } else if (cp.satisfied) return; + newVel = minSpeVel; + } else { + newVel = cp.normalVel; + } + var deltaVel:Number = newVel - sepVel; + var impulse:Number = deltaVel/cp.velByUnitImpulseN; + var accumImpulse:Number = cp.accumImpulseN + impulse; + if (accumImpulse < 0) { + accumImpulse = 0; + } + var deltaImpulse:Number = accumImpulse - cp.accumImpulseN; + cp.accumImpulseN = accumImpulse; + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, normal, deltaImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, normal, -deltaImpulse); + } + + // Учёт силы трения + calcSepVelocity(b1, b2, cp, _v); + // Расчитываем изменение касательной скорости на единицу касательного импульса + var tanSpeedByUnitImpulse:Number = 0; + +// _v.vAddScaled(-_v.vDot(contact.normal), contact.normal); + var dot:Number = _v.x*cnormal.x + _v.y*cnormal.y + _v.z*cnormal.z; + _v.x -= dot*cnormal.x; + _v.y -= dot*cnormal.y; + _v.z -= dot*cnormal.z; + + var tanSpeed:Number = _v.length(); + if (tanSpeed < 0.001) return; + +// _t.vCopy(_v).vNormalize().vReverse(); + _t.x = -_v.x; + _t.y = -_v.y; + _t.z = -_v.z; + _t.normalize(); + + var r:Vector3; + var m:Matrix3; + var xx:Number; + var yy:Number; + var zz:Number; + // dV = b.invMass + ((invI * (r % t)) % r) * t + if (b1.movable) { +// _v.vCross2(cp.r1, _t).vTransformBy3(b1.invInertiaWorld).vCross(cp.r1); + r = cp.r1; + m = b1.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b1.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + if (b2 != null && b2.movable) { +// _v.vCross2(cp.r2, _t).vTransformBy3(b2.invInertiaWorld).vCross(cp.r2); + + r = cp.r2; + m = b2.invInertiaWorld; + + _v.x = r.y*_t.z - r.z*_t.y; + _v.y = r.z*_t.x - r.x*_t.z; + _v.z = r.x*_t.y - r.y*_t.x; + + xx = m.a*_v.x + m.b*_v.y + m.c*_v.z; + yy = m.e*_v.x + m.f*_v.y + m.g*_v.z; + zz = m.i*_v.x + m.j*_v.y + m.k*_v.z; + + _v.x = yy*r.z - zz*r.y; + _v.y = zz*r.x - xx*r.z; + _v.z = xx*r.y - yy*r.x; + + tanSpeedByUnitImpulse += b2.invMass + _v.x*_t.x + _v.y*_t.y + _v.z*_t.z; + } + + var tanImpulse:Number = tanSpeed/tanSpeedByUnitImpulse; + var max:Number = contact.friction*cp.accumImpulseN; + if (max < 0) { + if (tanImpulse < max) tanImpulse = max; + } else { + if (tanImpulse > max) tanImpulse = max; + } + + // Применяем импульс к телам + if (b1.movable) { + b1.applyRelPosWorldImpulse(cp.r1, _t, tanImpulse); + } + if (b2 != null && b2.movable) { + b2.applyRelPosWorldImpulse(cp.r2, _t, -tanImpulse); + } + } + + /** + * + * @param body1 + * @param body2 + * @param cp + * @param result + */ + private function calcSepVelocity(body1:Body, body2:Body, cp:ContactPoint, result:Vector3):void { + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 +// result.vCopy(body1.state.velocity).vAdd(_v1.vCross2(body1.state.rotation, cp.r1)); + var rot:Vector3 = body1.state.rotation; + var v:Vector3 = cp.r1; + var x:Number = rot.y*v.z - rot.z*v.y; + var y:Number = rot.z*v.x - rot.x*v.z; + var z:Number = rot.x*v.y - rot.y*v.x; + v = body1.state.velocity; + result.x = v.x + x; + result.y = v.y + y; + result.z = v.z + z; + // V2 = V2_c + w2%r2 + if (body2 != null) { +// result.vSubtract(body2.state.velocity).vSubtract(_v2.vCross2(body2.state.rotation, cp.r2)); + rot = body2.state.rotation; + v = cp.r2; + x = rot.y*v.z - rot.z*v.y; + y = rot.z*v.x - rot.x*v.z; + z = rot.x*v.y - rot.y*v.x; + v = body2.state.velocity; + result.x -= v.x + x; + result.y -= v.y + y; + result.z -= v.z + z; + } + } + + /** + * + * @param dt + */ + private function intergateVelocities(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + item.body.integrateVelocity(dt); + item = item.next; + } + } + + /** + * + * @param dt + */ + private function integratePositions(dt:Number):void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + if (body.movable && !body.frozen) { + body.integratePosition(dt); + } + item = item.next; + } + } + + /** + * + */ + private function postPhysics():void { + var item:BodyListItem = bodies.head; + while (item != null) { + var body:Body = item.body; + body.clearAccumulators(); + body.calcDerivedData(); + if (body.canFreeze) { + if (body.state.velocity.length() < linSpeedFreezeLimit && body.state.rotation.length() < angSpeedFreezeLimit) { + if (!body.frozen) { + body.freezeCounter++; + if (body.freezeCounter >= freezeSteps) + body.frozen = true; + } + } else { + body.freezeCounter = 0; + body.frozen = false; + } + } + item = item.next; + } + } + + /** + * + * @param delta + */ + public function update(delta:int):void { + frame++; + time += delta; + var dt:Number = 0.001*delta; + applyForces(dt); + detectCollisions(dt); + preProcessContacts(dt); + processContacts(dt, false); + intergateVelocities(dt); + processContacts(dt, true); + integratePositions(dt); + postPhysics(); + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/PhysicsUtils.as b/2.0.1.0-SNAPSHOT/src/alternativa/physics/PhysicsUtils.as new file mode 100644 index 0000000..5a2e46f --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/PhysicsUtils.as @@ -0,0 +1,49 @@ +package alternativa.physics { + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + public class PhysicsUtils { + + /** + * @param mass + * @param halfSize + * @param result + */ + public static function getBoxInvInertia(mass:Number, halfSize:Vector3, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + /* Момент инерции бокса: + + m*(hy*hy + hz*hz)/3 0 0 + 0 m*(hz*hz + hx*hx)/3 0 + 0 0 m*(hx*hx + hy*hy)/3 + + hx, hy, hz -- половина размера бокса вдоль соответствующей оси + */ + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + var xx:Number = halfSize.x*halfSize.x; + var yy:Number = halfSize.y*halfSize.y; + var zz:Number = halfSize.z*halfSize.z; + result.a = 3/(mass*(yy + zz)); + result.f = 3/(mass*(zz + xx)); + result.k = 3/(mass*(xx + yy)); + } + + /** + * @param mass + * @param r + * @param h + * @param result + */ + public static function getCylinderInvInertia(mass:Number, r:Number, h:Number, result:Matrix3):void { + if (mass <= 0) throw new ArgumentError(); + + result.copy(Matrix3.ZERO); + if (mass == Infinity) return; + + result.a = result.f = 1/(mass*(h*h/12 + r*r/4)); + result.k = 2/(mass*r*r); + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/.svn/all-wcprops b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/.svn/all-wcprops new file mode 100644 index 0000000..363f4f2 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/.svn/all-wcprops @@ -0,0 +1,65 @@ +K 25 +svn:wc:ra_dav:version-url +V 106 +/!svn/ver/34889/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/collision +END +ICollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 128 +/!svn/ver/30874/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/collision/ICollisionDetector.as +END +CollisionKdNode.as +K 25 +svn:wc:ra_dav:version-url +V 125 +/!svn/ver/28424/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/collision/CollisionKdNode.as +END +CollisionKdTree2D.as +K 25 +svn:wc:ra_dav:version-url +V 127 +/!svn/ver/28424/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/collision/CollisionKdTree2D.as +END +IRayCollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 132 +/!svn/ver/22253/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/collision/IRayCollisionPredicate.as +END +IBodyCollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 133 +/!svn/ver/22253/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/collision/IBodyCollisionPredicate.as +END +CollisionPrimitive.as +K 25 +svn:wc:ra_dav:version-url +V 128 +/!svn/ver/30874/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/collision/CollisionPrimitive.as +END +KdTreeCollisionDetector.as +K 25 +svn:wc:ra_dav:version-url +V 133 +/!svn/ver/30874/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/collision/KdTreeCollisionDetector.as +END +ICollisionPredicate.as +K 25 +svn:wc:ra_dav:version-url +V 129 +/!svn/ver/22253/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/collision/ICollisionPredicate.as +END +ICollider.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/22253/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/collision/ICollider.as +END +CollisionKdTree.as +K 25 +svn:wc:ra_dav:version-url +V 125 +/!svn/ver/30874/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/collision/CollisionKdTree.as +END diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/.svn/entries b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/.svn/entries new file mode 100644 index 0000000..24ea8bf --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/.svn/entries @@ -0,0 +1,157 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/collision +http://svndev.alternativaplatform.com + + + +2010-05-27T15:33:01.277505Z +34889 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +colliders +dir + +ICollisionDetector.as +file + + + + +2010-10-28T04:32:37.000000Z +60fce7ab667a5c17f27ea92335b56c3d +2010-04-01T08:37:11.976264Z +30874 +mike + +CollisionKdNode.as +file + + + + +2010-10-28T04:32:37.000000Z +a6fab491cd9bd45a5472bf4ffc321e08 +2010-02-12T12:34:25.192477Z +28424 +mike + +CollisionKdTree2D.as +file + + + + +2010-10-28T04:32:37.000000Z +82bb636d8b49cdfbc22694dd8b01e71d +2010-02-12T12:34:25.192477Z +28424 +mike + +IRayCollisionPredicate.as +file + + + + +2010-10-28T04:32:37.000000Z +32f9b1eaeacfcb40e7ae1927f52b8ab8 +2009-10-19T07:17:33.113306Z +22253 +mike + +types +dir + +IBodyCollisionPredicate.as +file + + + + +2010-10-28T04:32:37.000000Z +618f907a588979a8d06d4f4bcf6618a0 +2009-10-19T07:17:33.113306Z +22253 +mike + +CollisionPrimitive.as +file + + + + +2010-10-28T04:32:37.000000Z +cbd09b4defca3baf37421bd77f6bd223 +2010-04-01T08:37:11.976264Z +30874 +mike + +KdTreeCollisionDetector.as +file + + + + +2010-10-28T04:32:37.000000Z +9830dea81d6f25e16ccd8780d92f3c07 +2010-04-01T08:37:11.976264Z +30874 +mike + +ICollisionPredicate.as +file + + + + +2010-10-28T04:32:37.000000Z +65f3250d36556fffc457c740fb7d38b3 +2009-10-19T07:17:33.113306Z +22253 +mike + +ICollider.as +file + + + + +2010-10-28T04:32:37.000000Z +128a7d7479e13caeccecb45161b27b22 +2009-10-19T07:17:33.113306Z +22253 +mike + +primitives +dir + +CollisionKdTree.as +file + + + + +2010-10-28T04:32:37.000000Z +77d47ba10fe3bd9d1a003da13f295a36 +2010-04-01T08:37:11.976264Z +30874 +mike + diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/.svn/format b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/.svn/format @@ -0,0 +1 @@ +8 diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base new file mode 100644 index 0000000..f450f1e --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/.svn/text-base/CollisionKdNode.as.svn-base @@ -0,0 +1,18 @@ +package alternativa.physics.collision { + + + import alternativa.physics.collision.types.BoundBox; + + public class CollisionKdNode { + public var indices:Vector.; + public var splitIndices:Vector.; + public var boundBox:BoundBox; + public var parent:CollisionKdNode; + public var splitTree:CollisionKdTree2D; + + public var axis:int = -1; // 0 - x, 1 - y, 2 - z + public var coord:Number; + public var positiveNode:CollisionKdNode; + public var negativeNode:CollisionKdNode; + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base new file mode 100644 index 0000000..672563a --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree.as.svn-base @@ -0,0 +1,264 @@ +package alternativa.physics.collision { + + + + import alternativa.physics.collision.types.BoundBox; + + /** + * @author mike + */ + public class CollisionKdTree { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + public var rootNode:CollisionKdNode; + public var staticChildren:Vector.; + public var numStaticChildren:int; + public var staticBoundBoxes:Vector. = new Vector.(); + + private var splitAxis:int; + private var splitCoord:Number; + private var splitCost:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + + /** + * @param boundBox + */ + public function createTree(collisionPrimitives:Vector., boundBox:BoundBox = null):void { + staticChildren = collisionPrimitives.concat(); + numStaticChildren = staticChildren.length; + // Создаём корневую ноду + rootNode = new CollisionKdNode(); + rootNode.indices = new Vector.(); + // Расчитываем баунды объектов и рутовой ноды + var rootNodeBoundBox:BoundBox = rootNode.boundBox = (boundBox != null) ? boundBox : new BoundBox(); + for (var i:int = 0; i < numStaticChildren; ++i) { + var child:CollisionPrimitive = staticChildren[i]; + var childBoundBox:BoundBox = staticBoundBoxes[i] = child.calculateAABB(); + rootNodeBoundBox.addBoundBox(childBoundBox); + rootNode.indices[i] = i; + } + staticBoundBoxes.length = numStaticChildren; + // Разделяем рутовую ноду + splitNode(rootNode); + + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + var indices:Vector. = node.indices; + var numPrimitives:int = indices.length; + if (numPrimitives <= minPrimitivesPerNode) return; + + // Подготовка баунда с погрешностями + var nodeBoundBox:BoundBox = node.boundBox; + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + // Собираем опорные координаты + var i:int; + var j:int; + var numSplitCoordsX:int = 0 + var numSplitCoordsY:int = 0; + var numSplitCoordsZ:int = 0; + for (i = 0; i < numPrimitives; ++i) { + var boundBox:BoundBox = staticBoundBoxes[indices[i]]; + + if (boundBox.maxX - boundBox.minX <= doubleThreshold) { + if (boundBox.minX <= nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.minX; + else if (boundBox.maxX >= nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.maxX; + else splitCoordsX[numSplitCoordsX++] = (boundBox.minX + boundBox.maxX)*0.5; + } else { + if (boundBox.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = boundBox.minX; + if (boundBox.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = boundBox.maxX; + } + + if (boundBox.maxY - boundBox.minY <= doubleThreshold) { + if (boundBox.minY <= nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.minY; + else if (boundBox.maxY >= nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.maxY; + else splitCoordsY[numSplitCoordsY++] = (boundBox.minY + boundBox.maxY)*0.5; + } else { + if (boundBox.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = boundBox.minY; + if (boundBox.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = boundBox.maxY; + } + + if (boundBox.maxZ - boundBox.minZ <= doubleThreshold) { + if (boundBox.minZ <= nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.minZ; + else if (boundBox.maxZ >= nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.maxZ; + else splitCoordsZ[numSplitCoordsZ++] = (boundBox.minZ + boundBox.maxZ)*0.5; + } else { + if (boundBox.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.minZ; + if (boundBox.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numPrimitives; ++i) { + boundBox = staticBoundBoxes[indices[i]]; + var min:Number = axisX ? boundBox.minX : (axisY ? boundBox.minY : boundBox.minZ); + var max:Number = axisX ? boundBox.maxX : (axisY ? boundBox.maxY : boundBox.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(indices[i]); + indices[i] = -1; + } else { + if (node.splitIndices == null) node.splitIndices = new Vector.(); + node.splitIndices.push(indices[i]); + indices[i] = -1; + } + } else { + if (min >= coordMin) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(indices[i]); + indices[i] = -1; + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numPrimitives; ++i) { + if (indices[i] >= 0) indices[j++] = indices[i]; + } + if (j > 0) indices.length = j; + else node.indices = null; + + if (node.splitIndices != null) { + node.splitTree = new CollisionKdTree2D(this, node); + node.splitTree.createTree(); + } + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { +// trace(maxCoord - minCoord, _bb[axis + 3] - _bb[axis]); + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict) { + if (cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + /** + * + */ + public function traceTree():void { + traceNode("", rootNode); + } + + /** + * @param str + * @param node + */ + private function traceNode(str:String, node:CollisionKdNode):void { + if (node == null) return; + trace(str, node.axis == -1 ? "end" : ((node.axis == 0) ? "X" : ((node.axis == 1) ? "Y" : "Z")), "splitCoord=" + splitCoord, "bound", node.boundBox, "objs:", node.indices); + traceNode(str + "-", node.negativeNode); + traceNode(str + "+", node.positiveNode); + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base new file mode 100644 index 0000000..2621517 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/.svn/text-base/CollisionKdTree2D.as.svn-base @@ -0,0 +1,226 @@ +package alternativa.physics.collision { + + + import alternativa.physics.collision.types.BoundBox; + + /** + * + */ + public class CollisionKdTree2D { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + + public var parentTree:CollisionKdTree; + public var parentNode:CollisionKdNode; + public var rootNode:CollisionKdNode; + + private var splitAxis:int; + private var splitCost:Number; + private var splitCoord:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + /** + * + * @param parentTree + * @param parentNode + */ + public function CollisionKdTree2D(parentTree:CollisionKdTree, parentNode:CollisionKdNode) { + this.parentTree = parentTree; + this.parentNode = parentNode; + } + + /** + * + */ + public function createTree():void { + rootNode = new CollisionKdNode(); + rootNode.boundBox = parentNode.boundBox.clone(); + rootNode.indices = new Vector.(); + var numObjects:int = parentNode.splitIndices.length; + for (var i:int = 0; i < numObjects; ++i) rootNode.indices[i] = parentNode.splitIndices[i]; + + splitNode(rootNode); + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + if (node.indices.length <= minPrimitivesPerNode) return; + + var objects:Vector. = node.indices; + var i:int; + var j:int; + + var nodeBoundBox:BoundBox = node.boundBox; + + // Подготовка баунда с погрешностями + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + // Собираем опорные координаты + var numSplitCoordsX:int; + var numSplitCoordsY:int; + var numSplitCoordsZ:int; + var numObjects:int = objects.length; + for (i = 0; i < numObjects; ++i) { + var bb:BoundBox = staticBoundBoxes[objects[i]]; + + if (parentNode.axis != 0) { + if (bb.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = bb.minX; + if (bb.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = bb.maxX; + } + + if (parentNode.axis != 1) { + if (bb.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = bb.minY; + if (bb.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = bb.maxY; + } + + if (parentNode.axis != 2) { + if (bb.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = bb.minZ; + if (bb.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = bb.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + if (parentNode.axis != 0) checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + if (parentNode.axis != 1) checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + if (parentNode.axis != 2) checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numObjects; ++i) { + bb = staticBoundBoxes[objects[i]]; + var min:Number = axisX ? bb.minX : (axisY ? bb.minY : bb.minZ); + var max:Number = axisX ? bb.maxX : (axisY ? bb.maxY : bb.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(objects[i]); + objects[i] = -1; + } + } else { + if (min >= coordMin) { + if (max > coordMax) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(objects[i]); + objects[i] = -1; + } + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numObjects; ++i) { + if (objects[i] >= 0) objects[j++] = objects[i]; + } + if (j > 0) objects.length = j; + else node.indices = null; + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/.svn/text-base/CollisionPrimitive.as.svn-base b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/.svn/text-base/CollisionPrimitive.as.svn-base new file mode 100644 index 0000000..8516aa4 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/.svn/text-base/CollisionPrimitive.as.svn-base @@ -0,0 +1,140 @@ +package alternativa.physics.collision { + + import alternativa.physics.Body; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * Базовый класс для примитивов, использующихся детектором столкновений. + */ + public class CollisionPrimitive { + // Константы типов примитива + public static const BOX:int = 1; + public static const PLANE:int = 2; + public static const SPHERE:int = 3; + public static const RECT:int = 4; + public static const TRIANGLE:int = 5; + + // Тип примитива + public var type:int; + // Группы примитива. Каждая группа определяется установленным битом. Столкновения проверяются только для примитивов, + // имеющих хотя бы одну общую группу. + public var collisionGroup:int; + // Предикат примитива, вызывающийся детектором при нахождении столкновения. В зависимости от возвращённого + // предикатом результата столкновение либо регистрируется, либо игнорируется. + // Nullable + public var postCollisionPredicate:ICollisionPredicate; + // Тело, владеющее примитивом. Поле сделано открытым для быстрого доступа на чтение. Установка значения должна выполняться вызовом метода setBody(). + // Nullable + public var body:Body; + // Трансформация примитива в системе координат тела, если оно указано. Не допускается масштабирование матрицы. + public var localTransform:Matrix4; + // Полная трансформация примитива. Не допускается масштабирование матрицы. + public var transform:Matrix4 = new Matrix4(); + // AABB в мировой системе координат. Расчитывается системой вызовом функции calculateBoundBox(). + public var aabb:BoundBox = new BoundBox(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param type тип примитива + * @param collisionGroup группа примитива + */ + public function CollisionPrimitive(type:int, collisionGroup:int) { + this.type = type; + this.collisionGroup = collisionGroup; + } + + /** + * Устанавливает тело, владеющее примитивом. + * + * @param body тело, которое владеет примитивом + * @param localTransform трансформация примитива в системе координат тела. Указание значения null равносильно + * заданию единичной матрицы, однако в первом случае не будет дополнительного умножения матриц при вычислении полной трансформации примитива. + */ + public function setBody(body:Body, localTransform:Matrix4 = null):void { + if (this.body == body) return; + this.body = body; + if (body != null) { + if (localTransform != null) { + if (this.localTransform == null) { + this.localTransform = new Matrix4(); + } + this.localTransform.copy(localTransform); + } else { + this.localTransform = null; + } + } + } + + /** + * Рассчитывает AABB примитива. Наследники должны переопределять этот метод, реализуя в нём корректный рассчёт. + * + * @return ссылка на свой AABB + */ + public function calculateAABB():BoundBox { + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + public function raycast(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + return -1; + } + + /** + * Клонирует примитив. Переопределять не рекомендуется. Вместо этого переопределяются методы createPrimitive() и copyFrom(). + * + * @return клон примитива + */ + public function clone():CollisionPrimitive { + var p:CollisionPrimitive = createPrimitive(); + return p.copyFrom(this); + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + if (source == null) { + throw new ArgumentError("Parameter source cannot be null"); + } + type = source.type; + transform.copy(source.transform); + collisionGroup = source.collisionGroup; + setBody(source.body, source.localTransform); + aabb.copyFrom(source.aabb); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + public function toString():String { + return "[CollisionPrimitive type=" + type + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + protected function createPrimitive():CollisionPrimitive { + return new CollisionPrimitive(type, collisionGroup); + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base new file mode 100644 index 0000000..4ace681 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/.svn/text-base/IBodyCollisionPredicate.as.svn-base @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.Body; + + public interface IBodyCollisionPredicate { + function considerBodies(body1:Body, body2:Body):Boolean; + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base new file mode 100644 index 0000000..e6aac2a --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/.svn/text-base/ICollider.as.svn-base @@ -0,0 +1,29 @@ +package alternativa.physics.collision { + + import alternativa.physics.Contact; + + /** + * Интерфейс определителя столкновений между двумя примитивами. + */ + public interface ICollider { + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base new file mode 100644 index 0000000..53e70e7 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/.svn/text-base/ICollisionDetector.as.svn-base @@ -0,0 +1,64 @@ +package alternativa.physics.collision { + import alternativa.physics.Contact; + import alternativa.physics.collision.types.RayHit; + import alternativa.math.Vector3; + + /** + * Интерфейс детектора столкновений. + */ + public interface ICollisionDetector { + + /** + * Получает все столкновения в текущей конфигурации физической геометрии. + * + * @param contacts список контактов, в кторые будет записана информация о столкновении + * @return количество найденных столкновений + */ + function getAllContacts(contacts:Contact):Contact; + + /** + * Тестирует луч на пересечение с физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param direction направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function raycast(origin:Vector3, direction:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayHit):Boolean; + + /** + * + * @param origin + * @param direction + * @param collisionGroup + * @param maxTime + * @param predicate + * @param result + * @return + */ + function raycastStatic(origin:Vector3, direction:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayHit):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @return + */ + function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base new file mode 100644 index 0000000..33d30ca --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/.svn/text-base/ICollisionPredicate.as.svn-base @@ -0,0 +1,8 @@ +package alternativa.physics.collision { + + public interface ICollisionPredicate { + + function considerCollision(primitive:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base new file mode 100644 index 0000000..b319f62 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/.svn/text-base/IRayCollisionPredicate.as.svn-base @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.Body; + + public interface IRayCollisionPredicate { + function considerBody(body:Body):Boolean; + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base new file mode 100644 index 0000000..1c35356 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/.svn/text-base/KdTreeCollisionDetector.as.svn-base @@ -0,0 +1,482 @@ +package alternativa.physics.collision { + + import alternativa.physics.Body; + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + 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.RayHit; + import alternativa.math.Vector3; + + /** + * Детектор, хранящий статическую геометрию в kD-дереве и использующий дерево для ускорения тестов на пересечения. + */ + public class KdTreeCollisionDetector implements ICollisionDetector { + + public var tree:CollisionKdTree; + public var dynamicPrimitives:Vector.; + public var dynamicPrimitivesNum:int; + public 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:RayHit = new RayHit(); + + /** + * + */ + public function KdTreeCollisionDetector() { + tree = new CollisionKdTree(); + dynamicPrimitives = new Vector.(); + + 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.):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 contacts; + } + + /** + * @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 raycast(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayHit):Boolean { + var hasStaticIntersection:Boolean = raycastStatic(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 raycastStatic(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayHit):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. = tree.staticChildren; +// var indices:Vector. = 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:RayHit):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.raycast(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:RayHit):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., indices:Vector., predicate:IRayCollisionPredicate, intersection:RayHit):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.raycast(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; + } \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/CollisionKdNode.as b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/CollisionKdNode.as new file mode 100644 index 0000000..f450f1e --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/CollisionKdNode.as @@ -0,0 +1,18 @@ +package alternativa.physics.collision { + + + import alternativa.physics.collision.types.BoundBox; + + public class CollisionKdNode { + public var indices:Vector.; + public var splitIndices:Vector.; + public var boundBox:BoundBox; + public var parent:CollisionKdNode; + public var splitTree:CollisionKdTree2D; + + public var axis:int = -1; // 0 - x, 1 - y, 2 - z + public var coord:Number; + public var positiveNode:CollisionKdNode; + public var negativeNode:CollisionKdNode; + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/CollisionKdTree.as b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/CollisionKdTree.as new file mode 100644 index 0000000..672563a --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/CollisionKdTree.as @@ -0,0 +1,264 @@ +package alternativa.physics.collision { + + + + import alternativa.physics.collision.types.BoundBox; + + /** + * @author mike + */ + public class CollisionKdTree { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + public var rootNode:CollisionKdNode; + public var staticChildren:Vector.; + public var numStaticChildren:int; + public var staticBoundBoxes:Vector. = new Vector.(); + + private var splitAxis:int; + private var splitCoord:Number; + private var splitCost:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + + /** + * @param boundBox + */ + public function createTree(collisionPrimitives:Vector., boundBox:BoundBox = null):void { + staticChildren = collisionPrimitives.concat(); + numStaticChildren = staticChildren.length; + // Создаём корневую ноду + rootNode = new CollisionKdNode(); + rootNode.indices = new Vector.(); + // Расчитываем баунды объектов и рутовой ноды + var rootNodeBoundBox:BoundBox = rootNode.boundBox = (boundBox != null) ? boundBox : new BoundBox(); + for (var i:int = 0; i < numStaticChildren; ++i) { + var child:CollisionPrimitive = staticChildren[i]; + var childBoundBox:BoundBox = staticBoundBoxes[i] = child.calculateAABB(); + rootNodeBoundBox.addBoundBox(childBoundBox); + rootNode.indices[i] = i; + } + staticBoundBoxes.length = numStaticChildren; + // Разделяем рутовую ноду + splitNode(rootNode); + + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + var indices:Vector. = node.indices; + var numPrimitives:int = indices.length; + if (numPrimitives <= minPrimitivesPerNode) return; + + // Подготовка баунда с погрешностями + var nodeBoundBox:BoundBox = node.boundBox; + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + // Собираем опорные координаты + var i:int; + var j:int; + var numSplitCoordsX:int = 0 + var numSplitCoordsY:int = 0; + var numSplitCoordsZ:int = 0; + for (i = 0; i < numPrimitives; ++i) { + var boundBox:BoundBox = staticBoundBoxes[indices[i]]; + + if (boundBox.maxX - boundBox.minX <= doubleThreshold) { + if (boundBox.minX <= nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.minX; + else if (boundBox.maxX >= nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = nodeBoundBox.maxX; + else splitCoordsX[numSplitCoordsX++] = (boundBox.minX + boundBox.maxX)*0.5; + } else { + if (boundBox.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = boundBox.minX; + if (boundBox.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = boundBox.maxX; + } + + if (boundBox.maxY - boundBox.minY <= doubleThreshold) { + if (boundBox.minY <= nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.minY; + else if (boundBox.maxY >= nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = nodeBoundBox.maxY; + else splitCoordsY[numSplitCoordsY++] = (boundBox.minY + boundBox.maxY)*0.5; + } else { + if (boundBox.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = boundBox.minY; + if (boundBox.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = boundBox.maxY; + } + + if (boundBox.maxZ - boundBox.minZ <= doubleThreshold) { + if (boundBox.minZ <= nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.minZ; + else if (boundBox.maxZ >= nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = nodeBoundBox.maxZ; + else splitCoordsZ[numSplitCoordsZ++] = (boundBox.minZ + boundBox.maxZ)*0.5; + } else { + if (boundBox.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.minZ; + if (boundBox.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = boundBox.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numPrimitives; ++i) { + boundBox = staticBoundBoxes[indices[i]]; + var min:Number = axisX ? boundBox.minX : (axisY ? boundBox.minY : boundBox.minZ); + var max:Number = axisX ? boundBox.maxX : (axisY ? boundBox.maxY : boundBox.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(indices[i]); + indices[i] = -1; + } else { + if (node.splitIndices == null) node.splitIndices = new Vector.(); + node.splitIndices.push(indices[i]); + indices[i] = -1; + } + } else { + if (min >= coordMin) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(indices[i]); + indices[i] = -1; + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numPrimitives; ++i) { + if (indices[i] >= 0) indices[j++] = indices[i]; + } + if (j > 0) indices.length = j; + else node.indices = null; + + if (node.splitIndices != null) { + node.splitTree = new CollisionKdTree2D(this, node); + node.splitTree.createTree(); + } + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { +// trace(maxCoord - minCoord, _bb[axis + 3] - _bb[axis]); + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict) { + if (cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + /** + * + */ + public function traceTree():void { + traceNode("", rootNode); + } + + /** + * @param str + * @param node + */ + private function traceNode(str:String, node:CollisionKdNode):void { + if (node == null) return; + trace(str, node.axis == -1 ? "end" : ((node.axis == 0) ? "X" : ((node.axis == 1) ? "Y" : "Z")), "splitCoord=" + splitCoord, "bound", node.boundBox, "objs:", node.indices); + traceNode(str + "-", node.negativeNode); + traceNode(str + "+", node.positiveNode); + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/CollisionKdTree2D.as b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/CollisionKdTree2D.as new file mode 100644 index 0000000..2621517 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/CollisionKdTree2D.as @@ -0,0 +1,226 @@ +package alternativa.physics.collision { + + + import alternativa.physics.collision.types.BoundBox; + + /** + * + */ + public class CollisionKdTree2D { + + public var threshold:Number = 0.1; + public var minPrimitivesPerNode:int = 1; + + public var parentTree:CollisionKdTree; + public var parentNode:CollisionKdNode; + public var rootNode:CollisionKdNode; + + private var splitAxis:int; + private var splitCost:Number; + private var splitCoord:Number; + + private static const nodeBoundBoxThreshold:BoundBox = new BoundBox(); + private static const splitCoordsX:Vector. = new Vector.(); + private static const splitCoordsY:Vector. = new Vector.(); + private static const splitCoordsZ:Vector. = new Vector.(); + private static const _nodeBB:Vector. = new Vector.(6); + private static const _bb:Vector. = new Vector.(6); + /** + * + * @param parentTree + * @param parentNode + */ + public function CollisionKdTree2D(parentTree:CollisionKdTree, parentNode:CollisionKdNode) { + this.parentTree = parentTree; + this.parentNode = parentNode; + } + + /** + * + */ + public function createTree():void { + rootNode = new CollisionKdNode(); + rootNode.boundBox = parentNode.boundBox.clone(); + rootNode.indices = new Vector.(); + var numObjects:int = parentNode.splitIndices.length; + for (var i:int = 0; i < numObjects; ++i) rootNode.indices[i] = parentNode.splitIndices[i]; + + splitNode(rootNode); + splitCoordsX.length = splitCoordsY.length = splitCoordsZ.length = 0; + } + + /** + * @param node + */ + private function splitNode(node:CollisionKdNode):void { + if (node.indices.length <= minPrimitivesPerNode) return; + + var objects:Vector. = node.indices; + var i:int; + var j:int; + + var nodeBoundBox:BoundBox = node.boundBox; + + // Подготовка баунда с погрешностями + nodeBoundBoxThreshold.minX = nodeBoundBox.minX + threshold; + nodeBoundBoxThreshold.minY = nodeBoundBox.minY + threshold; + nodeBoundBoxThreshold.minZ = nodeBoundBox.minZ + threshold; + nodeBoundBoxThreshold.maxX = nodeBoundBox.maxX - threshold; + nodeBoundBoxThreshold.maxY = nodeBoundBox.maxY - threshold; + nodeBoundBoxThreshold.maxZ = nodeBoundBox.maxZ - threshold; + var doubleThreshold:Number = threshold*2; + + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + // Собираем опорные координаты + var numSplitCoordsX:int; + var numSplitCoordsY:int; + var numSplitCoordsZ:int; + var numObjects:int = objects.length; + for (i = 0; i < numObjects; ++i) { + var bb:BoundBox = staticBoundBoxes[objects[i]]; + + if (parentNode.axis != 0) { + if (bb.minX > nodeBoundBoxThreshold.minX) splitCoordsX[numSplitCoordsX++] = bb.minX; + if (bb.maxX < nodeBoundBoxThreshold.maxX) splitCoordsX[numSplitCoordsX++] = bb.maxX; + } + + if (parentNode.axis != 1) { + if (bb.minY > nodeBoundBoxThreshold.minY) splitCoordsY[numSplitCoordsY++] = bb.minY; + if (bb.maxY < nodeBoundBoxThreshold.maxY) splitCoordsY[numSplitCoordsY++] = bb.maxY; + } + + if (parentNode.axis != 2) { + if (bb.minZ > nodeBoundBoxThreshold.minZ) splitCoordsZ[numSplitCoordsZ++] = bb.minZ; + if (bb.maxZ < nodeBoundBoxThreshold.maxZ) splitCoordsZ[numSplitCoordsZ++] = bb.maxZ; + } + } + + // Поиск наилучшего сплита + splitAxis = -1; + splitCost = 1e308; + _nodeBB[0] = nodeBoundBox.minX; + _nodeBB[1] = nodeBoundBox.minY; + _nodeBB[2] = nodeBoundBox.minZ; + _nodeBB[3] = nodeBoundBox.maxX; + _nodeBB[4] = nodeBoundBox.maxY; + _nodeBB[5] = nodeBoundBox.maxZ; + if (parentNode.axis != 0) checkNodeAxis(node, 0, numSplitCoordsX, splitCoordsX, _nodeBB); + if (parentNode.axis != 1) checkNodeAxis(node, 1, numSplitCoordsY, splitCoordsY, _nodeBB); + if (parentNode.axis != 2) checkNodeAxis(node, 2, numSplitCoordsZ, splitCoordsZ, _nodeBB); + + // Если сплит не найден, выходим + if (splitAxis < 0) return; + + // Сплиттер найден. Разделение узла. + var axisX:Boolean = splitAxis == 0 + var axisY:Boolean = splitAxis == 1; + node.axis = splitAxis; + node.coord = splitCoord; + // Создаём дочерние ноды + node.negativeNode = new CollisionKdNode(); + node.negativeNode.parent = node; + node.negativeNode.boundBox = nodeBoundBox.clone(); + node.positiveNode = new CollisionKdNode(); + node.positiveNode.parent = node; + node.positiveNode.boundBox = nodeBoundBox.clone(); + if (axisX) node.negativeNode.boundBox.maxX = node.positiveNode.boundBox.minX = splitCoord; + else if (axisY) node.negativeNode.boundBox.maxY = node.positiveNode.boundBox.minY = splitCoord; + else node.negativeNode.boundBox.maxZ = node.positiveNode.boundBox.minZ = splitCoord; + + // Распределяем объекты по дочерним нодам + var coordMin:Number = splitCoord - threshold; + var coordMax:Number = splitCoord + threshold; + for (i = 0; i < numObjects; ++i) { + bb = staticBoundBoxes[objects[i]]; + var min:Number = axisX ? bb.minX : (axisY ? bb.minY : bb.minZ); + var max:Number = axisX ? bb.maxX : (axisY ? bb.maxY : bb.maxZ); + if (max <= coordMax) { + if (min < coordMin) { + // Объект в негативной стороне + if (node.negativeNode.indices == null) node.negativeNode.indices = new Vector.(); + node.negativeNode.indices.push(objects[i]); + objects[i] = -1; + } + } else { + if (min >= coordMin) { + if (max > coordMax) { + // Объект в положительной стороне + if (node.positiveNode.indices == null) node.positiveNode.indices = new Vector.(); + node.positiveNode.indices.push(objects[i]); + objects[i] = -1; + } + } + } + } + + // Очистка списка объектов + for (i = 0, j = 0; i < numObjects; ++i) { + if (objects[i] >= 0) objects[j++] = objects[i]; + } + if (j > 0) objects.length = j; + else node.indices = null; + + // Разделение дочерних нод + if (node.negativeNode.indices != null) splitNode(node.negativeNode); + if (node.positiveNode.indices != null) splitNode(node.positiveNode); + } + + /** + * + * @param node + * @param axis + * @param numSplitCoords + * @param splitCoords + * @param bb + */ + private function checkNodeAxis(node:CollisionKdNode, axis:int, numSplitCoords:int, splitCoords:Vector., bb:Vector.):void { + var axis1:int = (axis + 1)%3; + var axis2:int = (axis + 2)%3; + var area:Number = (bb[axis1 + 3] - bb[axis1])*(bb[axis2 + 3] - bb[axis2]); + var staticBoundBoxes:Vector. = parentTree.staticBoundBoxes; + for (var i:int = 0; i < numSplitCoords; ++i) { + var currSplitCoord:Number = splitCoords[i]; + if (isNaN(currSplitCoord)) continue; + var minCoord:Number = currSplitCoord - threshold; + var maxCoord:Number = currSplitCoord + threshold; + var areaNegative:Number = area*(currSplitCoord - bb[axis]); + var areaPositive:Number = area*(bb[int(axis + 3)] - currSplitCoord); + var numNegative:int = 0; + var numPositive:int = 0; + var conflict:Boolean = false; + // Проверяем объекты + var numObjects:int = node.indices.length; + for (var j:int = 0; j < numObjects; j++) { + var boundBox:BoundBox = staticBoundBoxes[node.indices[j]]; + _bb[0] = boundBox.minX; + _bb[1] = boundBox.minY; + _bb[2] = boundBox.minZ; + _bb[3] = boundBox.maxX; + _bb[4] = boundBox.maxY; + _bb[5] = boundBox.maxZ; + if (_bb[axis + 3] <= maxCoord) { + if (_bb[axis] < minCoord) numNegative++; + } else { + if (_bb[axis] >= minCoord) { + numPositive++; + } else { + conflict = true; + break; + } + } + } + // Если хороший сплит, сохраняем + var cost:Number = areaNegative*numNegative + areaPositive*numPositive; + if (!conflict && cost < splitCost) { + splitAxis = axis; + splitCost = cost; + splitCoord = currSplitCoord; + } + for (j = i + 1; j < numSplitCoords; ++j) { + if (splitCoords[j] >= currSplitCoord - threshold && splitCoords[j] <= currSplitCoord + threshold) splitCoords[j] = NaN; + } + } + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/CollisionPrimitive.as b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/CollisionPrimitive.as new file mode 100644 index 0000000..8516aa4 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/CollisionPrimitive.as @@ -0,0 +1,140 @@ +package alternativa.physics.collision { + + import alternativa.physics.Body; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * Базовый класс для примитивов, использующихся детектором столкновений. + */ + public class CollisionPrimitive { + // Константы типов примитива + public static const BOX:int = 1; + public static const PLANE:int = 2; + public static const SPHERE:int = 3; + public static const RECT:int = 4; + public static const TRIANGLE:int = 5; + + // Тип примитива + public var type:int; + // Группы примитива. Каждая группа определяется установленным битом. Столкновения проверяются только для примитивов, + // имеющих хотя бы одну общую группу. + public var collisionGroup:int; + // Предикат примитива, вызывающийся детектором при нахождении столкновения. В зависимости от возвращённого + // предикатом результата столкновение либо регистрируется, либо игнорируется. + // Nullable + public var postCollisionPredicate:ICollisionPredicate; + // Тело, владеющее примитивом. Поле сделано открытым для быстрого доступа на чтение. Установка значения должна выполняться вызовом метода setBody(). + // Nullable + public var body:Body; + // Трансформация примитива в системе координат тела, если оно указано. Не допускается масштабирование матрицы. + public var localTransform:Matrix4; + // Полная трансформация примитива. Не допускается масштабирование матрицы. + public var transform:Matrix4 = new Matrix4(); + // AABB в мировой системе координат. Расчитывается системой вызовом функции calculateBoundBox(). + public var aabb:BoundBox = new BoundBox(); + + /** + * Создаёт новый экземпляр примитива. + * + * @param type тип примитива + * @param collisionGroup группа примитива + */ + public function CollisionPrimitive(type:int, collisionGroup:int) { + this.type = type; + this.collisionGroup = collisionGroup; + } + + /** + * Устанавливает тело, владеющее примитивом. + * + * @param body тело, которое владеет примитивом + * @param localTransform трансформация примитива в системе координат тела. Указание значения null равносильно + * заданию единичной матрицы, однако в первом случае не будет дополнительного умножения матриц при вычислении полной трансформации примитива. + */ + public function setBody(body:Body, localTransform:Matrix4 = null):void { + if (this.body == body) return; + this.body = body; + if (body != null) { + if (localTransform != null) { + if (this.localTransform == null) { + this.localTransform = new Matrix4(); + } + this.localTransform.copy(localTransform); + } else { + this.localTransform = null; + } + } + } + + /** + * Рассчитывает AABB примитива. Наследники должны переопределять этот метод, реализуя в нём корректный рассчёт. + * + * @return ссылка на свой AABB + */ + public function calculateAABB():BoundBox { + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + public function raycast(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + return -1; + } + + /** + * Клонирует примитив. Переопределять не рекомендуется. Вместо этого переопределяются методы createPrimitive() и copyFrom(). + * + * @return клон примитива + */ + public function clone():CollisionPrimitive { + var p:CollisionPrimitive = createPrimitive(); + return p.copyFrom(this); + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + if (source == null) { + throw new ArgumentError("Parameter source cannot be null"); + } + type = source.type; + transform.copy(source.transform); + collisionGroup = source.collisionGroup; + setBody(source.body, source.localTransform); + aabb.copyFrom(source.aabb); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + public function toString():String { + return "[CollisionPrimitive type=" + type + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + protected function createPrimitive():CollisionPrimitive { + return new CollisionPrimitive(type, collisionGroup); + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/IBodyCollisionPredicate.as b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/IBodyCollisionPredicate.as new file mode 100644 index 0000000..4ace681 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/IBodyCollisionPredicate.as @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.Body; + + public interface IBodyCollisionPredicate { + function considerBodies(body1:Body, body2:Body):Boolean; + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/ICollider.as b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/ICollider.as new file mode 100644 index 0000000..e6aac2a --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/ICollider.as @@ -0,0 +1,29 @@ +package alternativa.physics.collision { + + import alternativa.physics.Contact; + + /** + * Интерфейс определителя столкновений между двумя примитивами. + */ + public interface ICollider { + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/ICollisionDetector.as b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/ICollisionDetector.as new file mode 100644 index 0000000..53e70e7 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/ICollisionDetector.as @@ -0,0 +1,64 @@ +package alternativa.physics.collision { + import alternativa.physics.Contact; + import alternativa.physics.collision.types.RayHit; + import alternativa.math.Vector3; + + /** + * Интерфейс детектора столкновений. + */ + public interface ICollisionDetector { + + /** + * Получает все столкновения в текущей конфигурации физической геометрии. + * + * @param contacts список контактов, в кторые будет записана информация о столкновении + * @return количество найденных столкновений + */ + function getAllContacts(contacts:Contact):Contact; + + /** + * Тестирует луч на пересечение с физической геометрией. Подразумевается, что детектор содержит набор примитивов, для которых выполняется проверка. + * В случае наличия нескольких пересечений, метод должен возвращать ближайшее к началу луча пересечение. + * + * @param origin начальная точка луча в мировых координатах + * @param direction направляющий вектор луча в мировых координатах. Длина вектора должна быть отлична от нуля. + * @param collisionGroup идентификатор группы + * @param maxTime параметр, задающий длину проверяемого сегмента. Единица соответствует одной длине направлящего вектора. + * @param predicate предикат, применяемый к столкновениям + * @param result переменная для записи информации о столкновении в случае положительного теста. В случае отрицательного результата сохранность начальных данных в + * переданной структуре не гарантируется. + * @return true в случае наличия пересечения, иначе false + */ + function raycast(origin:Vector3, direction:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayHit):Boolean; + + /** + * + * @param origin + * @param direction + * @param collisionGroup + * @param maxTime + * @param predicate + * @param result + * @return + */ + function raycastStatic(origin:Vector3, direction:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayHit):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @param contact + * @return + */ + function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean; + + /** + * + * @param prim1 + * @param prim2 + * @return + */ + function testCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/ICollisionPredicate.as b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/ICollisionPredicate.as new file mode 100644 index 0000000..33d30ca --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/ICollisionPredicate.as @@ -0,0 +1,8 @@ +package alternativa.physics.collision { + + public interface ICollisionPredicate { + + function considerCollision(primitive:CollisionPrimitive):Boolean; + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/IRayCollisionPredicate.as b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/IRayCollisionPredicate.as new file mode 100644 index 0000000..b319f62 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/IRayCollisionPredicate.as @@ -0,0 +1,7 @@ +package alternativa.physics.collision { + import alternativa.physics.Body; + + public interface IRayCollisionPredicate { + function considerBody(body:Body):Boolean; + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/KdTreeCollisionDetector.as b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/KdTreeCollisionDetector.as new file mode 100644 index 0000000..1c35356 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/KdTreeCollisionDetector.as @@ -0,0 +1,482 @@ +package alternativa.physics.collision { + + import alternativa.physics.Body; + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + 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.RayHit; + import alternativa.math.Vector3; + + /** + * Детектор, хранящий статическую геометрию в kD-дереве и использующий дерево для ускорения тестов на пересечения. + */ + public class KdTreeCollisionDetector implements ICollisionDetector { + + public var tree:CollisionKdTree; + public var dynamicPrimitives:Vector.; + public var dynamicPrimitivesNum:int; + public 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:RayHit = new RayHit(); + + /** + * + */ + public function KdTreeCollisionDetector() { + tree = new CollisionKdTree(); + dynamicPrimitives = new Vector.(); + + 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.):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 contacts; + } + + /** + * @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 raycast(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayHit):Boolean { + var hasStaticIntersection:Boolean = raycastStatic(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 raycastStatic(origin:Vector3, dir:Vector3, collisionGroup:int, maxTime:Number, predicate:IRayCollisionPredicate, result:RayHit):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. = tree.staticChildren; +// var indices:Vector. = 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:RayHit):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.raycast(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:RayHit):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., indices:Vector., predicate:IRayCollisionPredicate, intersection:RayHit):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.raycast(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; + } \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/.svn/all-wcprops b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/.svn/all-wcprops new file mode 100644 index 0000000..8c93a78 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/.svn/all-wcprops @@ -0,0 +1,53 @@ +K 25 +svn:wc:ra_dav:version-url +V 116 +/!svn/ver/34889/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/collision/colliders +END +BoxSphereCollider.as +K 25 +svn:wc:ra_dav:version-url +V 137 +/!svn/ver/34889/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/collision/colliders/BoxSphereCollider.as +END +BoxBoxCollider.as +K 25 +svn:wc:ra_dav:version-url +V 134 +/!svn/ver/34889/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/collision/colliders/BoxBoxCollider.as +END +BoxRectCollider.as +K 25 +svn:wc:ra_dav:version-url +V 135 +/!svn/ver/34889/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/collision/colliders/BoxRectCollider.as +END +SpherePlaneCollider.as +K 25 +svn:wc:ra_dav:version-url +V 139 +/!svn/ver/23758/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/collision/colliders/SpherePlaneCollider.as +END +BoxCollider.as +K 25 +svn:wc:ra_dav:version-url +V 131 +/!svn/ver/28424/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/collision/colliders/BoxCollider.as +END +BoxPlaneCollider.as +K 25 +svn:wc:ra_dav:version-url +V 136 +/!svn/ver/28424/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/collision/colliders/BoxPlaneCollider.as +END +SphereSphereCollider.as +K 25 +svn:wc:ra_dav:version-url +V 140 +/!svn/ver/30874/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/collision/colliders/SphereSphereCollider.as +END +BoxTriangleCollider.as +K 25 +svn:wc:ra_dav:version-url +V 139 +/!svn/ver/34889/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/collision/colliders/BoxTriangleCollider.as +END diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/.svn/entries b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/.svn/entries new file mode 100644 index 0000000..7008d01 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/.svn/entries @@ -0,0 +1,124 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/collision/colliders +http://svndev.alternativaplatform.com + + + +2010-05-27T15:33:01.277505Z +34889 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +BoxSphereCollider.as +file + + + + +2010-10-28T04:32:37.000000Z +4f9e1e3880d3bb13719ac8bdac5da789 +2010-05-27T15:33:01.277505Z +34889 +mike + +BoxBoxCollider.as +file + + + + +2010-10-28T04:32:37.000000Z +5b314e136dcefc9cda2debc6d4fd4375 +2010-05-27T15:33:01.277505Z +34889 +mike + +BoxRectCollider.as +file + + + + +2010-10-28T04:32:37.000000Z +7ea40daa1bd605f6e07c6c84ab2a80ad +2010-05-27T15:33:01.277505Z +34889 +mike + +SpherePlaneCollider.as +file + + + + +2010-10-28T04:32:37.000000Z +67621ed47d7e90b72498acf2802c4e2d +2009-11-24T08:44:15.941480Z +23758 +mike + +BoxCollider.as +file + + + + +2010-10-28T04:32:37.000000Z +52e72ed30a24a3a43ce16a19930546b5 +2010-02-12T12:34:25.192477Z +28424 +mike + +BoxPlaneCollider.as +file + + + + +2010-10-28T04:32:37.000000Z +204c7b02a9a1913269c558658bad4262 +2010-02-12T12:34:25.192477Z +28424 +mike + +SphereSphereCollider.as +file + + + + +2010-10-28T04:32:37.000000Z +dba9cde04a5c73c74f8096ee3e86873b +2010-04-01T08:37:11.976264Z +30874 +mike + +BoxTriangleCollider.as +file + + + + +2010-10-28T04:32:37.000000Z +47e74bba7e9929e9422e3674d7b348cc +2010-05-27T15:33:01.277505Z +34889 +mike + diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/.svn/format b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/.svn/format @@ -0,0 +1 @@ +8 diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/.svn/text-base/BoxBoxCollider.as.svn-base b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/.svn/text-base/BoxBoxCollider.as.svn-base new file mode 100644 index 0000000..f17a96f --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/.svn/text-base/BoxBoxCollider.as.svn-base @@ -0,0 +1,585 @@ +package alternativa.physics.collision.colliders { + + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * Расчитывает точки контакта двух боксов. Нормаль контакта направляется в сторону бокса с меньшим ID. + */ + public class BoxBoxCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + private var vectorToBox1:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var bestAxisIndex:int; + private var minOverlap:Number; + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + private var tmpPoints:Vector. = new Vector.(8, true); + private var pcount:int; + + /** + * + */ + public function BoxBoxCollider() { + for (var i:int = 0; i < 8; i++) { + tmpPoints[i] = new ContactPoint(); + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + + if (bestAxisIndex < 6) { + // Контакт грань-(грань|ребро|вершина) + if (!findFaceContactPoints(box1, box2, vectorToBox1, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 6; + findEdgesIntersection(box1, box2, vectorToBox1, int(bestAxisIndex/3), bestAxisIndex%3, contact); + } + contact.body1 = box1.body; + contact.body2 = box2.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + // Вектор из центра второго бокса в центр первого + vectorToBox1.x = transform1.d - transform2.d; + vectorToBox1.y = transform1.h - transform2.h; + vectorToBox1.z = transform1.l - transform2.l; + + // Проверка пересечения по основным осям + axis10.x = transform1.a; + axis10.y = transform1.e; + axis10.z = transform1.i; + if (!testMainAxis(box1, box2, axis10, 0, vectorToBox1)) return false; + axis11.x = transform1.b; + axis11.y = transform1.f; + axis11.z = transform1.j; + if (!testMainAxis(box1, box2, axis11, 1, vectorToBox1)) return false; + axis12.x = transform1.c; + axis12.y = transform1.g; + axis12.z = transform1.k; + if (!testMainAxis(box1, box2, axis12, 2, vectorToBox1)) return false; + + axis20.x = transform2.a; + axis20.y = transform2.e; + axis20.z = transform2.i; + if (!testMainAxis(box1, box2, axis20, 3, vectorToBox1)) return false; + axis21.x = transform2.b; + axis21.y = transform2.f; + axis21.z = transform2.j; + if (!testMainAxis(box1, box2, axis21, 4, vectorToBox1)) return false; + axis22.x = transform2.c; + axis22.y = transform2.g; + axis22.z = transform2.k; + if (!testMainAxis(box1, box2, axis22, 5, vectorToBox1)) return false; + + // Проверка производных осей + if (!testDerivedAxis(box1, box2, axis10, axis20, 6, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis21, 7, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis22, 8, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis11, axis20, 9, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis21, 10, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis22, 11, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis12, axis20, 12, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis21, 13, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis22, 14, vectorToBox1)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта грани одного бокса с гранью/ребром/вершиной другого. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param faceAxisIdx индекс оси первого бокса, перпендикулярной грани, с которой произошло столкновение + * @param contactInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var swapNormal:Boolean = false; + if (faceAxisIdx > 2) { + // Столкновение с гранью второго бокса. Для дальнейших расчётов боксы меняются местами, + // но нормаль контакта всё равно должна быть направлена в сторону первоначального box1 + var tmpBox:CollisionBox = box1; + box1 = box2; + box2 = tmpBox; + vectorToBox1.x = -vectorToBox1.x; + vectorToBox1.y = -vectorToBox1.y; + vectorToBox1.z = -vectorToBox1.z; + faceAxisIdx -= 3; + swapNormal = true; + } + + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + var colAxis:Vector3 = contact.normal; + + transform1.getAxis(faceAxisIdx, colAxis); + var negativeFace:Boolean = colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z > 0; + + var code:int = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box1.excludedFaces) != 0) return false; + + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + // Ищем ось второго бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + transform2.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisDot = dot; + incidentAxisIdx = axisIdx; + } + } + // Получаем список вершин грани второго бокса, переводим их в систему координат первого бокса и выполняем обрезку + // по грани первого бокса. Таким образом получается список потенциальных точек контакта. + transform2.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box2.hs, incidentAxisIdx, incidentAxisDot < 0, points1); + + // TODO: Вычислить результирующую матрицу, затем преобразовать векторы за один заход + transform2.transformPointsN(points1, points2, 4); + transform1.transformPointsTransposedN(points2, points1, 4); + + var pnum:int = clip(box1.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points1[i]; + if ((pen = getPointBoxPenetration(box1.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + var cp:ContactPoint = tmpPoints[pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = transform1.a*v.x + transform1.b*v.y + transform1.c*v.z + transform1.d; + cpPos.y = transform1.e*v.x + transform1.f*v.y + transform1.g*v.z + transform1.h; + cpPos.z = transform1.i*v.x + transform1.j*v.y + transform1.k*v.z + transform1.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + + cp.penetration = pen; + } + } + if (swapNormal) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (pcount > 4) { + reducePoints(); + } + for (i = 0; i < pcount; i++) { + cp = contact.points[i]; + cp.copyFrom(tmpPoints[i]); + } + contact.pcount = pcount; + return true; + } + + /** + * + * @param contactInfo + */ + private function reducePoints():void { + var i:int; + var minIdx:int; + var cp1:ContactPoint; + var cp2:ContactPoint; + while (pcount > 4) { + var minLen:Number = 1e10; + var p1:Vector3 = (tmpPoints[int(pcount - 1)] as ContactPoint).pos; + var p2:Vector3; + for (i = 0; i < pcount; i++) { + p2 = (tmpPoints[i] as ContactPoint).pos; + var dx:Number = p2.x - p1.x; + var dy:Number = p2.y - p1.y; + var dz:Number = p2.z - p1.z; + var len:Number = dx*dx + dy*dy + dz*dz; + if (len < minLen) { + minLen = len; + minIdx = i; + } + p1 = p2; + } + var ii:int = minIdx == 0 ? (pcount - 1) : (minIdx - 1); + cp1 = tmpPoints[ii]; + cp2 = tmpPoints[minIdx]; + p1 = cp1.pos; + p2 = cp2.pos; + p2.x = 0.5*(p1.x + p2.x); + p2.y = 0.5*(p1.y + p2.y); + p2.z = 0.5*(p1.z + p2.z); + cp2.penetration = 0.5*(cp1.penetration + cp2.penetration); + if (minIdx > 0) { + for (i = minIdx; i < pcount; i++) { + tmpPoints[int(i - 1)] = tmpPoints[i]; + } + tmpPoints[int(pcount - 1)] = cp1; + } + pcount--; + } + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clip(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param contactInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):void { + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + transform1.getAxis(axisIdx1, axis10); + transform2.getAxis(axisIdx2, axis20); + + var colAxis:Vector3 = contact.normal; + + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону первого бокса + if (colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + + var tx:Number = box1.hs.x; + var ty:Number = box1.hs.y; + var tz:Number = box1.hs.z; + + var x2:Number = box2.hs.x; + var y2:Number = box2.hs.y; + var z2:Number = box2.hs.z; + // x + if (axisIdx1 == 0) { + tx = 0; + halfLen1 = box1.hs.x; + } else { + if (colAxis.x*transform1.a + colAxis.y*transform1.e + colAxis.z*transform1.i > 0) { + tx = -tx; + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = box2.hs.x; + } else { + if (colAxis.x*transform2.a + colAxis.y*transform2.e + colAxis.z*transform2.i < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + ty = 0; + halfLen1 = box1.hs.y; + } else { + if (colAxis.x*transform1.b + colAxis.y*transform1.f + colAxis.z*transform1.j > 0) { + ty = -ty; + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = box2.hs.y; + } else { + if (colAxis.x*transform2.b + colAxis.y*transform2.f + colAxis.z*transform2.j < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + tz = 0; + halfLen1 = box1.hs.z; + } else { + if (colAxis.x*transform1.c + colAxis.y*transform1.g + colAxis.z*transform1.k > 0) { + tz = -tz; + } + } + if (axisIdx2 == 2) { + z2 = 0; + halfLen2 = box2.hs.z; + } else { + if (colAxis.x*transform2.c + colAxis.y*transform2.g + colAxis.z*transform2.k < 0) { + z2 = -z2; + } + } + // Получаем глобальные координаты средних точек рёбер + var x1:Number = transform1.a*tx + transform1.b*ty + transform1.c*tz + transform1.d; + var y1:Number = transform1.e*tx + transform1.f*ty + transform1.g*tz + transform1.h; + var z1:Number = transform1.i*tx + transform1.j*ty + transform1.k*tz + transform1.l; + tx = x2; + ty = y2; + tz = z2; + x2 = transform2.a*tx + transform2.b*ty + transform2.c*tz + transform2.d; + y2 = transform2.e*tx + transform2.f*ty + transform2.g*tz + transform2.h; + z2 = transform2.i*tx + transform2.j*ty + transform2.k*tz + transform2.l; + + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + tx = x2 - x1; + ty = y2 - y1; + tz = z2 - z1; + var c1:Number = axis10.x*tx + axis10.y*ty + axis10.z*tz; + var c2:Number = axis20.x*tx + axis20.y*ty + axis20.z*tz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cp.pos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cp.pos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение боксов вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box1 + * @param box2 + * @param axis + * @param axisIndex + * @param toBox1 + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box1 + * @param box2 + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox1 + * @return + */ + private function testDerivedAxis(box1:CollisionBox, box2:CollisionBox, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия двух боксов вдоль заданной оси. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param axis ось + * @param vectorToBox1 вектор, соединяющий центр второго бокса с центром первого + * @return величина перекрытия боксов вдоль оси + */ + public function overlapOnAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, vectorToBox1:Vector3):Number { + var m:Matrix4 = box1.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box1.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box1.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box1.hs.z; + if (d < 0) d = -d; + projection += d; + + m = box2.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box2.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box2.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box2.hs.z; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox1.x*axis.x + vectorToBox1.y*axis.y + vectorToBox1.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} + import alternativa.math.Vector3; + + class CollisionPointTmp { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + } diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/.svn/text-base/BoxCollider.as.svn-base b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/.svn/text-base/BoxCollider.as.svn-base new file mode 100644 index 0000000..2429bb1 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/.svn/text-base/BoxCollider.as.svn-base @@ -0,0 +1,427 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + /** + * + */ + public class BoxCollider implements ICollider { + + /** + * + */ + public function BoxCollider() { + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + return false; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + protected function getFaceVertsByAxis(hs:Vector3, axisIdx:int, negativeFace:Boolean, result:Vector.):void { + var v:Vector3; + switch (axisIdx) { + case 0: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + } + break; + case 1: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + } + break; + case 2: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + } else { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } + break; + } + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/.svn/text-base/BoxPlaneCollider.as.svn-base b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/.svn/text-base/BoxPlaneCollider.as.svn-base new file mode 100644 index 0000000..3bebdc3 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/.svn/text-base/BoxPlaneCollider.as.svn-base @@ -0,0 +1,88 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + public class BoxPlaneCollider implements ICollider { + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var normal:Vector3 = new Vector3(); + + public function BoxPlaneCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var box:RigidBox = body1 as RigidBox; +// var plane:RigidPlane; +// if (box == null) { +// box = body2 as RigidBox; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные координаты вершин бокса +// var sx:Number = box.halfSize.x; +// var sy:Number = box.halfSize.y; +// var sz:Number = box.halfSize.z; +// (verts1[0] as Vector3).reset(-sx, -sy, -sz); +// (verts1[1] as Vector3).reset(sx, -sy, -sz); +// (verts1[2] as Vector3).reset(sx, sy, -sz); +// (verts1[3] as Vector3).reset(-sx, sy, -sz); +// (verts1[4] as Vector3).reset(-sx, -sy, sz); +// (verts1[5] as Vector3).reset(sx, -sy, sz); +// (verts1[6] as Vector3).reset(sx, sy, sz); +// (verts1[7] as Vector3).reset(-sx, sy, sz); +// +// box.transform.transformVectors(verts1, verts2); +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// // Проверяем наличие столкновений с каждой вершиной +// collisionInfo.pcount = 0; +// for (var i:int = 0; i < 8; i++) { +// // Вершина добавляется в список точек столкновения, если лежит под плоскостью +// var dist:Number = (verts2[i] as Vector3).dot(normal); +// if (dist < offset) { +// var cp:ContactPoint; +// if (collisionInfo.pcount == collisionInfo.points.length) { +// cp = new ContactPoint(); +// collisionInfo.points[collisionInfo.pcount] = cp; +// } else { +// cp = collisionInfo.points[collisionInfo.pcount]; +// } +// cp.pos.copy(verts2[i]); +// cp.r1.diff(cp.pos, box.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); +// cp.penetration = offset - dist; +// collisionInfo.pcount++; +// } +// } +// if (collisionInfo.pcount > 0) { +// collisionInfo.body1 = box; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// return true; +// } + return false; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/.svn/text-base/BoxRectCollider.as.svn-base b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/.svn/text-base/BoxRectCollider.as.svn-base new file mode 100644 index 0000000..7ce47be --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/.svn/text-base/BoxRectCollider.as.svn-base @@ -0,0 +1,573 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * + */ + public class BoxRectCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + + private var vectorToBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxRectCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, rect, vectorToBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + if (!findEdgesIntersection(box, rect, vectorToBox, int(bestAxisIndex/2), bestAxisIndex%2, contact)) { + return false; + } + } + contact.body1 = box.body; + contact.body2 = rect.body; + + // Хак для танков, чтобы исключить утыкания танков в стыки статических примитивов + if (rect.transform.k > 0.99999) { + contact.normal.x = 0; + contact.normal.y = 0; + contact.normal.z = 1; + } + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + // Вектор из центра прямоугольника в центр бокса + vectorToBox.x = boxTransform.d - rectTransform.d; + vectorToBox.y = boxTransform.h - rectTransform.h; + vectorToBox.z = boxTransform.l - rectTransform.l; + + // Проверка пересечения по нормали прямоугольника + rectTransform.getAxis(2, axis22); + if (!testMainAxis(box, rect, axis22, 0, vectorToBox)) return false; + + // Проверка пересечения по основным осям бокса + boxTransform.getAxis(0, axis10); + if (!testMainAxis(box, rect, axis10, 1, vectorToBox)) return false; + boxTransform.getAxis(1, axis11); + if (!testMainAxis(box, rect, axis11, 2, vectorToBox)) return false; + boxTransform.getAxis(2, axis12); + if (!testMainAxis(box, rect, axis12, 3, vectorToBox)) return false; + + // Получаем направляющие рёбер прямоугольника + rectTransform.getAxis(0, axis20); + rectTransform.getAxis(1, axis21); + + // Проверка производных осей + if (!testDerivedAxis(box, rect, axis10, axis20, 4, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis10, axis21, 5, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis11, axis20, 6, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis11, axis21, 7, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis12, axis20, 8, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis12, axis21, 9, vectorToBox)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта бокса с прямоугольником. + * + * @param box бокс + * @param rect прямоугольник + * @param vectorToBox вектор, направленный из центра прямоугольника в центр бокса + * @param faceAxisIdx индекс оси, идентифицирующей полскость столкновения (грань бокса или полскость прямоугольника) + * @param colInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var pnum:int; + var i:int; + var v:Vector3; + var cp:ContactPoint; + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + var colAxis:Vector3 = contact.normal; + + var negativeFace:Boolean; + var code:int; + + if (faceAxisIdx == 0) { + // Столкновение с плоскостью прямоугольника + + // Проверим положение бокса относительно плоскости прямоугольника + colAxis.x = rectTransform.c; + colAxis.y = rectTransform.g; + colAxis.z = rectTransform.k; + +// var offset:Number = colAxis.x*rectTransform.d + colAxis.y*rectTransform.h + colAxis.z*rectTransform.l; +// if (bbPos.vDot(colAxis) < offset) return false; + + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisIdx = axisIdx; + incidentAxisDot = dot; + } + } + negativeFace = incidentAxisDot > 0; + + code = 1 << (incidentAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + + // Получаем список вершин грани бокса, переводим их в систему координат прямоугольника и выполняем обрезку + // по прямоугольнику. Таким образом получается список потенциальных точек контакта. + boxTransform.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box.hs, incidentAxisIdx, negativeFace, points1); + boxTransform.transformPointsN(points1, points2, 4); + rectTransform.transformPointsTransposedN(points2, points1, 4); + pnum = clipByRect(rect.hs); + // Проверяем каждую потенциальную точку на принадлежность нижней полуплоскости прямоугольника и добавляем такие точки в список контактов + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if (v.z < epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = -v.z; + var cpPos:Vector3 = cp.pos; + cpPos.x = rectTransform.a*v.x + rectTransform.b*v.y + rectTransform.c*v.z + rectTransform.d; + cpPos.y = rectTransform.e*v.x + rectTransform.f*v.y + rectTransform.g*v.z + rectTransform.h; + cpPos.z = rectTransform.i*v.x + rectTransform.j*v.y + rectTransform.k*v.z + rectTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } else { + // Столкновение с гранью бокса + faceAxisIdx--; + boxTransform.getAxis(faceAxisIdx, colAxis); + negativeFace = colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z > 0; + + code = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) { + return false; + } + + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (rectTransform.c*colAxis.x + rectTransform.g*colAxis.y + rectTransform.k*colAxis.z < 0) return false; + + getFaceVertsByAxis(rect.hs, 2, false, points1); + rectTransform.transformPointsN(points1, points2, 4); + boxTransform.transformPointsTransposedN(points2, points1, 4); + pnum = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if ((pen = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = pen; + cpPos = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } + return true; + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * + * @param hs + * @return + */ + private function clipByRect(hs:Vector3):int { + var pnum:int = 4; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box первый бокс + * @param rect второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param colInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + boxTransform.getAxis(axisIdx1, axis10); + rectTransform.getAxis(axisIdx2, axis20); + var colAxis:Vector3 = contact.normal; + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону бокса + if (colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + var vx:Number = box.hs.x; + var vy:Number = box.hs.y; + var vz:Number = box.hs.z; + var x2:Number = rect.hs.x; + var y2:Number = rect.hs.y; + var z2:Number = rect.hs.z; + // x + if (axisIdx1 == 0) { + vx = 0; + halfLen1 = box.hs.x; + } else { + if (boxTransform.a*colAxis.x + boxTransform.e*colAxis.y + boxTransform.i*colAxis.z > 0) { + vx = -vx; + if ((box.excludedFaces & 2) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 1) != 0) { + return false; + } + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = rect.hs.x; + } else { + if (rectTransform.a*colAxis.x + rectTransform.e*colAxis.y + rectTransform.i*colAxis.z < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + vy = 0; + halfLen1 = box.hs.y; + } else { + if (boxTransform.b*colAxis.x + boxTransform.f*colAxis.y + boxTransform.j*colAxis.z > 0) { + vy = -vy; + if ((box.excludedFaces & 8) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 4) != 0) { + return false; + } + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = rect.hs.y; + } else { + if (rectTransform.b*colAxis.x + rectTransform.f*colAxis.y + rectTransform.j*colAxis.z < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + vz = 0; + halfLen1 = box.hs.z; + } else { + if (boxTransform.c*colAxis.x + boxTransform.g*colAxis.y + boxTransform.k*colAxis.z > 0) { + vz = -vz; + if ((box.excludedFaces & 32) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 16) != 0) { + return false; + } + } + } + // Получаем глобальные координаты средних точек рёбер + + var x1:Number = boxTransform.a*vx + boxTransform.b*vy + boxTransform.c*vz + boxTransform.d; + var y1:Number = boxTransform.e*vx + boxTransform.f*vy + boxTransform.g*vz + boxTransform.h; + var z1:Number = boxTransform.i*vx + boxTransform.j*vy + boxTransform.k*vz + boxTransform.l; + vx = x2; + vy = y2; + vz = z2; + x2 = rectTransform.a*vx + rectTransform.b*vy + rectTransform.c*vz + rectTransform.d; + y2 = rectTransform.e*vx + rectTransform.f*vy + rectTransform.g*vz + rectTransform.h; + z2 = rectTransform.i*vx + rectTransform.j*vy + rectTransform.k*vz + rectTransform.l; + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + vx = x2 - x1; + vy = y2 - y1; + vz = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cpPos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cpPos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cpPos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var v:Vector3 = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + return true; + } + + /** + * Проверяет пересечение вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box + * @param rect + * @param axis + * @param axisIndex + * @param vectorToBox + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param rect + * @param axis1 + * @param axis2 + * @param axisIndex + * @param vectorToBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, rect:CollisionRect, axis1:Vector3, axis2:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия вдоль заданной оси. + * + * @param box бокс + * @param rect прямоугольник + * @param axis ось + * @param vectorToBox вектор, соединяющий центр прямоугольника с центром бокса + * @return величина перекрытия вдоль оси + */ + public function overlapOnAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, vectorToBox:Vector3):Number { + var m:Matrix4 = box.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box.hs.z; + if (d < 0) d = -d; + projection += d; + + m = rect.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*rect.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*rect.hs.y; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox.x*axis.x + vectorToBox.y*axis.y + vectorToBox.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/.svn/text-base/BoxSphereCollider.as.svn-base b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/.svn/text-base/BoxSphereCollider.as.svn-base new file mode 100644 index 0000000..78e1920 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/.svn/text-base/BoxSphereCollider.as.svn-base @@ -0,0 +1,161 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.math.Vector3; + + /** + * + */ + public class BoxSphereCollider implements ICollider { + + private var center:Vector3 = new Vector3(); + private var closestPt:Vector3 = new Vector3(); + + private var bPos:Vector3 = new Vector3(); + private var sPos:Vector3 = new Vector3(); + + /** + * + */ + public function BoxSphereCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformPointTransposed(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.subtract(closestPt).lengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + // Зафиксированно столкновение + contact.body1 = sphere.body; + contact.body2 = box.body; + contact.normal.copy(closestPt).transform4(box.transform).subtract(sPos).normalize().reverse(); + contact.pcount = 1; + + var cp:ContactPoint = contact.points[0]; + cp.penetration = sphere.r - Math.sqrt(distSqr); + cp.pos.copy(contact.normal).scale(-sphere.r).add(sPos); + cp.r1.diff(cp.pos, sPos); + cp.r2.diff(cp.pos, bPos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformPointTransposed(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + var distSqr:Number = center.subtract(closestPt).lengthSqr(); + return distSqr <= sphere.r*sphere.r; + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/.svn/text-base/BoxTriangleCollider.as.svn-base b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/.svn/text-base/BoxTriangleCollider.as.svn-base new file mode 100644 index 0000000..762355c --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/.svn/text-base/BoxTriangleCollider.as.svn-base @@ -0,0 +1,725 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionTriangle; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * + */ + public class BoxTriangleCollider extends BoxCollider { + + public var epsilon:Number = 0.001; + + private var bestAxisIndex:int; + private var minOverlap:Number; + private var toBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var colNormal:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxTriangleCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, tri, toBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + if (!findEdgesIntersection(box, tri, toBox, bestAxisIndex%3, int(bestAxisIndex/3), contact)) return false; + } + + contact.body1 = box.body; + contact.body2 = tri.body; + + // Хак для танков, чтобы исключить утыкания танков в стыки статических примитивов + if (tri.transform.k > 0.99999) { + contact.normal.x = 0; + contact.normal.y = 0; + contact.normal.z = 1; + } + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + toBox.x = boxTransform.d - triTransform.d; + toBox.y = boxTransform.h - triTransform.h; + toBox.z = boxTransform.l - triTransform.l; + + minOverlap = 1e308; + + // Сначала проверяется нормаль треугольника + axis.x = triTransform.c; + axis.y = triTransform.g; + axis.z = triTransform.k; + if (!testMainAxis(box, tri, axis, 0, toBox)) return false; + + // Проверка основных осей бокса + axis10.x = boxTransform.a; + axis10.y = boxTransform.e; + axis10.z = boxTransform.i; + if (!testMainAxis(box, tri, axis10, 1, toBox)) return false; + axis11.x = boxTransform.b; + axis11.y = boxTransform.f; + axis11.z = boxTransform.j; + if (!testMainAxis(box, tri, axis11, 2, toBox)) return false; + axis12.x = boxTransform.c; + axis12.y = boxTransform.g; + axis12.z = boxTransform.k; + if (!testMainAxis(box, tri, axis12, 3, toBox)) return false; + + // Проверка производных осей + // TODO: заменить вычисления векторных произведений инлайнами + var v:Vector3 = tri.e0; + axis20.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis20.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis20.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis20, 4, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis20, 5, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis20, 6, toBox)) return false; + + v = tri.e1; + axis21.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis21.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis21.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis21, 7, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis21, 8, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis21, 9, toBox)) return false; + + v = tri.e2; + axis22.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis22.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis22.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis22, 10, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis22, 11, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis22, 12, toBox)) return false; + + return true; + } + + /** + * Тестирует пересечение примитивов вдоль заданной оси. + * + * @param box бокс + * @param tri треугольник + * @param axis ось + * @param axisIndex индекс оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return true, если примитивы имеют перекрытие проекций вдоль указанной оси, иначе false + */ + private function testMainAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, axisIndex:int, toBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param tri + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, tri:CollisionTriangle, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Рассчитывает величину перекрытия проекций бокса и треугольника на заданную ось. + * + * @param box бокс + * @param tri треугольник + * @param axis единичный направляющий вектор оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return величина перекрытия. Положительное значение указывает на наличие перекрытия, нулевое или отрицательное значение указывает на отсутствие перекрытия. + */ + private function overlapOnAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, toBox:Vector3):Number { + var t:Matrix4 = box.transform; + var projection:Number = (t.a*axis.x + t.e*axis.y + t.i*axis.z)*box.hs.x; + if (projection < 0) projection = -projection; + var d:Number = (t.b*axis.x + t.f*axis.y + t.j*axis.z)*box.hs.y; + projection += d < 0 ? -d : d; + d = (t.c*axis.x + t.g*axis.y + t.k*axis.z)*box.hs.z; + projection += d < 0 ? -d : d; + + var vectorProjection:Number = toBox.x*axis.x + toBox.y*axis.y + toBox.z*axis.z; + // Для оптимизации ось преобразуется в систему треугольника, а не его вершины в мировую систему + t = tri.transform; + var ax:Number = t.a*axis.x + t.e*axis.y + t.i*axis.z; + var ay:Number = t.b*axis.x + t.f*axis.y + t.j*axis.z; + var az:Number = t.c*axis.x + t.g*axis.y + t.k*axis.z; + var max:Number = 0; + if (vectorProjection < 0) { + vectorProjection = -vectorProjection; + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d < max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d < max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d < max) max = d; + max = -max; + } else { + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d > max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d > max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d > max) max = d; + } + + return projection + max - vectorProjection; + } + + /** + * Определяет точки контакта бокса и треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function findFaceContactPoints(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIndex:int, contact:Contact):Boolean { + if (faceAxisIndex == 0) { + // Столкновение с плоскостью треугольника + return getBoxToTriContact(box, tri, toBox, contact); + } else { + // Столкновение с гранью бокса + return getTriToBoxContact(box, tri, toBox, faceAxisIndex, contact); + } + } + + /** + * Определяет точки контакта бокса с плоскостью треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getBoxToTriContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + colNormal.x = triTransform.c; + colNormal.y = triTransform.g; + colNormal.z = triTransform.k; + + var over:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + if (!over) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var incAxisDot:Number = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colNormal.x + axis.y*colNormal.y + axis.z*colNormal.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incAxisDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Обрезка грани + var negativeFace:Boolean = incAxisDot > 0; + + var code:int = 1 << (incFaceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + getFaceVertsByAxis(box.hs, incFaceAxisIdx, negativeFace, points1); + boxTransform.transformPointsN(points1, points2, 4); + triTransform.transformPointsTransposedN(points2, points1, 4); + var pnum:int = clipByTriangle(tri); + // Среди конечного списка точек определяются лежащие под плоскостью треугольника + var cp:ContactPoint; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points2[i]; + if ((over && v.z < 0) || (!over && v.z > 0)) { + cp = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z + triTransform.d; + cpPos.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z + triTransform.h; + cpPos.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z + triTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = over ? -v.z : v.z; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Оперделяет точки контакта треугольника с гранью бокса. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getTriToBoxContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + faceAxisIdx--; + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + boxTransform.getAxis(faceAxisIdx, colNormal); + var negativeFace:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + + var code:int = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + if (!negativeFace) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + var v:Vector3 = points1[0]; + v.x = tri.v0.x; + v.y = tri.v0.y; + v.z = tri.v0.z; + + v = points1[1]; + v.x = tri.v1.x; + v.y = tri.v1.y; + v.z = tri.v1.z; + + v = points1[2]; + v.x = tri.v2.x; + v.y = tri.v2.y; + v.z = tri.v2.z; + + triTransform.transformPointsN(points1, points2, 3); + boxTransform.transformPointsTransposedN(points2, points1, 3); + + var pnum:int = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность боксу и добавляем такие точки в список контактов + var penetration:Number; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + v = points1[i]; + penetration = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace); + if (penetration > -epsilon) { + var cp:ContactPoint = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = penetration; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Вычисляет величину проникновения точки в бокс. + * + * @param hs вектор половинных размеров бокса + * @param p точка в системе координат бокса + * @param axisIndex индекс оси + * @param negativeFace если true, проверяется нижняя грань + * @return величина проникновения точки в бокс + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, negativeFace:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (negativeFace) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (negativeFace) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (negativeFace) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, вершины которой заданы в списке points1. Результат сохраняется в этом же списке. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 3; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Обрезает грань прямоугольника сторонами треугольника. Обрезка выполняется в системе координат и в проекции на + * плоскость треугольника. Входные вершины грани в количестве четырёх штук должны находиться в списке points1. + * Конечные вершины сохраняются в списке points2. + * + * @param tri треугольник + * @return количество врешин в конечном списке + */ + private function clipByTriangle(tri:CollisionTriangle):int { + var vnum:int = 4; + vnum = clipByLine(tri.v0, tri.e0, points1, vnum, points2); + if (vnum == 0) return 0; + vnum = clipByLine(tri.v1, tri.e1, points2, vnum, points1); + if (vnum == 0) return 0; + return clipByLine(tri.v2, tri.e2, points1, vnum, points2);; + } + + /** + * Обрезает полигон указанной прямой. + * + * @param linePoint точка на прямой + * @param lineDir единичный направляющий вектор прямой + * @param verticesIn список вершин исходного полигона + * @param vnum количество вершин исходного полигона + * @param verticesOut список, куда будут записаны вершины конечного полигона + * @return количество вершин конечного полигона + */ + private function clipByLine(linePoint:Vector3, lineDir:Vector3, verticesIn:Vector., vnum:int, verticesOut:Vector.):int { + var nx:Number = -lineDir.y; + var ny:Number = lineDir.x; + var offset:Number = linePoint.x*nx + linePoint.y*ny; + var v1:Vector3 = verticesIn[int(vnum - 1)]; + var offset1:Number = v1.x*nx + v1.y*ny; + var t:Number; + var v:Vector3; + var num:int = 0; + for (var i:int = 0; i < vnum; i++) { + var v2:Vector3 = verticesIn[i]; + var offset2:Number = v2.x*nx + v2.y*ny; + if (offset1 < offset) { + // Первая точка ребра во внешней полуплоскости + if (offset2 > offset) { + // Вторая точка ребра во внутренней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } else { + // Первая точка ребра во внутренней полуплоскости. Добавляем её в конечный список. + v = verticesOut[num]; + v.x = v1.x; + v.y = v1.y; + v.z = v1.z; + num++; + if (offset2 < offset) { + // Вторая точка ребра во внешней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } + v1 = v2; + offset1 = offset2; + } + return num; + } + + /** + * + * @param box + * @param tri + * @param toBox + * @param boxAxisIdx + * @param triAxisIdx + * @param contact + */ + private function findEdgesIntersection(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, boxAxisIdx:int, triAxisIdx:int, contact:Contact):Boolean { + // Определение точки и направляющего вектора ребра треугольника + var tmpx1:Number; + var tmpy1:Number; + var tmpz1:Number; + var tmpx2:Number; + var tmpy2:Number; + var tmpz2:Number; + switch (triAxisIdx) { + case 0: + tmpx1 = tri.e0.x; + tmpy1 = tri.e0.y; + tmpz1 = tri.e0.z; + tmpx2 = tri.v0.x; + tmpy2 = tri.v0.y; + tmpz2 = tri.v0.z; + break; + case 1: + tmpx1 = tri.e1.x; + tmpy1 = tri.e1.y; + tmpz1 = tri.e1.z; + tmpx2 = tri.v1.x; + tmpy2 = tri.v1.y; + tmpz2 = tri.v1.z; + break; + case 2: + tmpx1 = tri.e2.x; + tmpy1 = tri.e2.y; + tmpz1 = tri.e2.z; + tmpx2 = tri.v2.x; + tmpy2 = tri.v2.y; + tmpz2 = tri.v2.z; + break; + } + var triTransform:Matrix4 = tri.transform; + axis20.x = triTransform.a*tmpx1 + triTransform.b*tmpy1 + triTransform.c*tmpz1; + axis20.y = triTransform.e*tmpx1 + triTransform.f*tmpy1 + triTransform.g*tmpz1; + axis20.z = triTransform.i*tmpx1 + triTransform.j*tmpy1 + triTransform.k*tmpz1; + var x2:Number = triTransform.a*tmpx2 + triTransform.b*tmpy2 + triTransform.c*tmpz2 + triTransform.d; + var y2:Number = triTransform.e*tmpx2 + triTransform.f*tmpy2 + triTransform.g*tmpz2 + triTransform.h; + var z2:Number = triTransform.i*tmpx2 + triTransform.j*tmpy2 + triTransform.k*tmpz2 + triTransform.l; + + // Определение нормали контакта, точки и направляющего вектора ребра бокса + var boxTransform:Matrix4 = box.transform; + boxTransform.getAxis(boxAxisIdx, axis10); + + // Нормаль контакта + var v:Vector3 = contact.normal; + v.x = axis10.y*axis20.z - axis10.z*axis20.y; + v.y = axis10.z*axis20.x - axis10.x*axis20.z; + v.z = axis10.x*axis20.y - axis10.y*axis20.x; + k = 1/Math.sqrt(v.x*v.x + v.y*v.y + v.z*v.z); + v.x *= k; + v.y *= k; + v.z *= k; + // Разворот нормали в сторону бокса + if (v.x*toBox.x + v.y*toBox.y + v.z*toBox.z < 0) { + v.x = -v.x; + v.y = -v.y; + v.z = -v.z; + } + + var boxHalfLen:Number; + tmpx1 = box.hs.x; + tmpy1 = box.hs.y; + tmpz1 = box.hs.z; + // X + if (boxAxisIdx == 0) { + tmpx1 = 0; + boxHalfLen = box.hs.x; + } else { + if (boxTransform.a*v.x + boxTransform.e*v.y + boxTransform.i*v.z > 0) { + tmpx1 = -tmpx1; + if ((box.excludedFaces & 2) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 1) != 0) { + return false; + } + } + } + // Y + if (boxAxisIdx == 1) { + tmpy1 = 0; + boxHalfLen = box.hs.y; + } else { + if (boxTransform.b*v.x + boxTransform.f*v.y + boxTransform.j*v.z > 0) { + tmpy1 = -tmpy1; + if ((box.excludedFaces & 8) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 4) != 0) { + return false; + } + } + } + // Z + if (boxAxisIdx == 2) { + tmpz1 = 0; + boxHalfLen = box.hs.z; + } else { + if (boxTransform.c*v.x + boxTransform.g*v.y + boxTransform.k*v.z > 0) { + tmpz1 = -tmpz1; + if ((box.excludedFaces & 32) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 16) != 0) { + return false; + } + } + } + var x1:Number = boxTransform.a*tmpx1 + boxTransform.b*tmpy1 + boxTransform.c*tmpz1 + boxTransform.d; + var y1:Number = boxTransform.e*tmpx1 + boxTransform.f*tmpy1 + boxTransform.g*tmpz1 + boxTransform.h; + var z1:Number = boxTransform.i*tmpx1 + boxTransform.j*tmpy1 + boxTransform.k*tmpz1 + boxTransform.l; + + // Находим точку пересечения рёбер, решая систему уравнений + // axis10 - направляющий вектор ребра бокса + // x1, y1, z1 - средняя точка ребра бокса + // axis20 - направляющий вектор ребра треугольника + // x2, y2, z2 - начальная точка ребра треугольника + var k:Number = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + var vx:Number = x2 - x1; + var vy:Number = y2 - y1; + var vz:Number = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + + // Запись данных о контакте + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + v = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + v.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + v.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + v.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = v.x - boxTransform.d; + r.y = v.y - boxTransform.h; + r.z = v.z - boxTransform.l; + r = cp.r2; + r.x = v.x - triTransform.d; + r.y = v.y - triTransform.h; + r.z = v.z - triTransform.l; + return true; + } + + } +} diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/.svn/text-base/SpherePlaneCollider.as.svn-base b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/.svn/text-base/SpherePlaneCollider.as.svn-base new file mode 100644 index 0000000..30cd55f --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/.svn/text-base/SpherePlaneCollider.as.svn-base @@ -0,0 +1,68 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + /** + * + */ + public class SpherePlaneCollider implements ICollider { + + private var normal:Vector3 = new Vector3(); + + /** + * + */ + public function SpherePlaneCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var sphere:RigidSphere = body1 as RigidSphere; +// var plane:RigidPlane; +// if (sphere == null) { +// sphere = body2 as RigidSphere; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// +// var dist:Number = sphere.state.pos.dot(normal) - offset; +// if (dist > sphere.r) return false; +// +// collisionInfo.body1 = sphere; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// collisionInfo.pcount = 1; +// +// var cp:ContactPoint = collisionInfo.points[0]; +// cp.penetration = sphere.r - dist; +// cp.pos.copy(normal).reverse().scale(sphere.r).add(sphere.state.pos); +// cp.r1.diff(cp.pos, sphere.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/.svn/text-base/SphereSphereCollider.as.svn-base b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/.svn/text-base/SphereSphereCollider.as.svn-base new file mode 100644 index 0000000..228d6c9 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/.svn/text-base/SphereSphereCollider.as.svn-base @@ -0,0 +1,68 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.math.Vector3; + + public class SphereSphereCollider implements ICollider { + + private var p1:Vector3 = new Vector3(); + private var p2:Vector3 = new Vector3(); + + public function SphereSphereCollider() { + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var s1:CollisionSphere; + var s2:CollisionSphere; + if (prim1.body != null) { + s1 = prim1 as CollisionSphere; + s2 = prim2 as CollisionSphere; + } else { + s1 = prim2 as CollisionSphere; + s2 = prim1 as CollisionSphere; + } + + s1.transform.getAxis(3, p1); + s2.transform.getAxis(3, p2); + var dx:Number = p1.x - p2.x; + var dy:Number = p1.y - p2.y; + var dz:Number = p1.z - p2.z; + var len:Number = dx*dx + dy*dy + dz*dz; + var sum:Number = s1.r + s2.r; + if (len > sum*sum) return false; + len = Math.sqrt(len); + dx /= len; + dy /= len; + dz /= len; + + contact.body1 = s1.body; + contact.body2 = s2.body; + contact.normal.x = dx; + contact.normal.y = dy; + contact.normal.z = dz; + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = sum - len; + cp.pos.x = p1.x - dx*s1.r; + cp.pos.y = p1.y - dy*s1.r; + cp.pos.z = p1.z - dz*s1.r; + cp.r1.diff(cp.pos, p1); + cp.r2.diff(cp.pos, p2); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/BoxBoxCollider.as b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/BoxBoxCollider.as new file mode 100644 index 0000000..f17a96f --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/BoxBoxCollider.as @@ -0,0 +1,585 @@ +package alternativa.physics.collision.colliders { + + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * Расчитывает точки контакта двух боксов. Нормаль контакта направляется в сторону бокса с меньшим ID. + */ + public class BoxBoxCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + private var vectorToBox1:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var bestAxisIndex:int; + private var minOverlap:Number; + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + private var tmpPoints:Vector. = new Vector.(8, true); + private var pcount:int; + + /** + * + */ + public function BoxBoxCollider() { + for (var i:int = 0; i < 8; i++) { + tmpPoints[i] = new ContactPoint(); + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + + if (bestAxisIndex < 6) { + // Контакт грань-(грань|ребро|вершина) + if (!findFaceContactPoints(box1, box2, vectorToBox1, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 6; + findEdgesIntersection(box1, box2, vectorToBox1, int(bestAxisIndex/3), bestAxisIndex%3, contact); + } + contact.body1 = box1.body; + contact.body2 = box2.body; + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box1:CollisionBox; + var box2:CollisionBox; + if (prim1.body != null) { + box1 = prim1 as CollisionBox; + box2 = prim2 as CollisionBox; + } else { + box1 = prim2 as CollisionBox; + box2 = prim1 as CollisionBox; + } + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + // Вектор из центра второго бокса в центр первого + vectorToBox1.x = transform1.d - transform2.d; + vectorToBox1.y = transform1.h - transform2.h; + vectorToBox1.z = transform1.l - transform2.l; + + // Проверка пересечения по основным осям + axis10.x = transform1.a; + axis10.y = transform1.e; + axis10.z = transform1.i; + if (!testMainAxis(box1, box2, axis10, 0, vectorToBox1)) return false; + axis11.x = transform1.b; + axis11.y = transform1.f; + axis11.z = transform1.j; + if (!testMainAxis(box1, box2, axis11, 1, vectorToBox1)) return false; + axis12.x = transform1.c; + axis12.y = transform1.g; + axis12.z = transform1.k; + if (!testMainAxis(box1, box2, axis12, 2, vectorToBox1)) return false; + + axis20.x = transform2.a; + axis20.y = transform2.e; + axis20.z = transform2.i; + if (!testMainAxis(box1, box2, axis20, 3, vectorToBox1)) return false; + axis21.x = transform2.b; + axis21.y = transform2.f; + axis21.z = transform2.j; + if (!testMainAxis(box1, box2, axis21, 4, vectorToBox1)) return false; + axis22.x = transform2.c; + axis22.y = transform2.g; + axis22.z = transform2.k; + if (!testMainAxis(box1, box2, axis22, 5, vectorToBox1)) return false; + + // Проверка производных осей + if (!testDerivedAxis(box1, box2, axis10, axis20, 6, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis21, 7, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis10, axis22, 8, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis11, axis20, 9, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis21, 10, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis11, axis22, 11, vectorToBox1)) return false; + + if (!testDerivedAxis(box1, box2, axis12, axis20, 12, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis21, 13, vectorToBox1)) return false; + if (!testDerivedAxis(box1, box2, axis12, axis22, 14, vectorToBox1)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта грани одного бокса с гранью/ребром/вершиной другого. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param faceAxisIdx индекс оси первого бокса, перпендикулярной грани, с которой произошло столкновение + * @param contactInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var swapNormal:Boolean = false; + if (faceAxisIdx > 2) { + // Столкновение с гранью второго бокса. Для дальнейших расчётов боксы меняются местами, + // но нормаль контакта всё равно должна быть направлена в сторону первоначального box1 + var tmpBox:CollisionBox = box1; + box1 = box2; + box2 = tmpBox; + vectorToBox1.x = -vectorToBox1.x; + vectorToBox1.y = -vectorToBox1.y; + vectorToBox1.z = -vectorToBox1.z; + faceAxisIdx -= 3; + swapNormal = true; + } + + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + + var colAxis:Vector3 = contact.normal; + + transform1.getAxis(faceAxisIdx, colAxis); + var negativeFace:Boolean = colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z > 0; + + var code:int = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box1.excludedFaces) != 0) return false; + + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + // Ищем ось второго бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + transform2.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisDot = dot; + incidentAxisIdx = axisIdx; + } + } + // Получаем список вершин грани второго бокса, переводим их в систему координат первого бокса и выполняем обрезку + // по грани первого бокса. Таким образом получается список потенциальных точек контакта. + transform2.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box2.hs, incidentAxisIdx, incidentAxisDot < 0, points1); + + // TODO: Вычислить результирующую матрицу, затем преобразовать векторы за один заход + transform2.transformPointsN(points1, points2, 4); + transform1.transformPointsTransposedN(points2, points1, 4); + + var pnum:int = clip(box1.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points1[i]; + if ((pen = getPointBoxPenetration(box1.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + var cp:ContactPoint = tmpPoints[pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = transform1.a*v.x + transform1.b*v.y + transform1.c*v.z + transform1.d; + cpPos.y = transform1.e*v.x + transform1.f*v.y + transform1.g*v.z + transform1.h; + cpPos.z = transform1.i*v.x + transform1.j*v.y + transform1.k*v.z + transform1.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + + cp.penetration = pen; + } + } + if (swapNormal) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (pcount > 4) { + reducePoints(); + } + for (i = 0; i < pcount; i++) { + cp = contact.points[i]; + cp.copyFrom(tmpPoints[i]); + } + contact.pcount = pcount; + return true; + } + + /** + * + * @param contactInfo + */ + private function reducePoints():void { + var i:int; + var minIdx:int; + var cp1:ContactPoint; + var cp2:ContactPoint; + while (pcount > 4) { + var minLen:Number = 1e10; + var p1:Vector3 = (tmpPoints[int(pcount - 1)] as ContactPoint).pos; + var p2:Vector3; + for (i = 0; i < pcount; i++) { + p2 = (tmpPoints[i] as ContactPoint).pos; + var dx:Number = p2.x - p1.x; + var dy:Number = p2.y - p1.y; + var dz:Number = p2.z - p1.z; + var len:Number = dx*dx + dy*dy + dz*dz; + if (len < minLen) { + minLen = len; + minIdx = i; + } + p1 = p2; + } + var ii:int = minIdx == 0 ? (pcount - 1) : (minIdx - 1); + cp1 = tmpPoints[ii]; + cp2 = tmpPoints[minIdx]; + p1 = cp1.pos; + p2 = cp2.pos; + p2.x = 0.5*(p1.x + p2.x); + p2.y = 0.5*(p1.y + p2.y); + p2.z = 0.5*(p1.z + p2.z); + cp2.penetration = 0.5*(cp1.penetration + cp2.penetration); + if (minIdx > 0) { + for (i = minIdx; i < pcount; i++) { + tmpPoints[int(i - 1)] = tmpPoints[i]; + } + tmpPoints[int(pcount - 1)] = cp1; + } + pcount--; + } + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clip(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param contactInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box1:CollisionBox, box2:CollisionBox, vectorToBox1:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):void { + var transform1:Matrix4 = box1.transform; + var transform2:Matrix4 = box2.transform; + transform1.getAxis(axisIdx1, axis10); + transform2.getAxis(axisIdx2, axis20); + + var colAxis:Vector3 = contact.normal; + + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону первого бокса + if (colAxis.x*vectorToBox1.x + colAxis.y*vectorToBox1.y + colAxis.z*vectorToBox1.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + + var tx:Number = box1.hs.x; + var ty:Number = box1.hs.y; + var tz:Number = box1.hs.z; + + var x2:Number = box2.hs.x; + var y2:Number = box2.hs.y; + var z2:Number = box2.hs.z; + // x + if (axisIdx1 == 0) { + tx = 0; + halfLen1 = box1.hs.x; + } else { + if (colAxis.x*transform1.a + colAxis.y*transform1.e + colAxis.z*transform1.i > 0) { + tx = -tx; + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = box2.hs.x; + } else { + if (colAxis.x*transform2.a + colAxis.y*transform2.e + colAxis.z*transform2.i < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + ty = 0; + halfLen1 = box1.hs.y; + } else { + if (colAxis.x*transform1.b + colAxis.y*transform1.f + colAxis.z*transform1.j > 0) { + ty = -ty; + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = box2.hs.y; + } else { + if (colAxis.x*transform2.b + colAxis.y*transform2.f + colAxis.z*transform2.j < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + tz = 0; + halfLen1 = box1.hs.z; + } else { + if (colAxis.x*transform1.c + colAxis.y*transform1.g + colAxis.z*transform1.k > 0) { + tz = -tz; + } + } + if (axisIdx2 == 2) { + z2 = 0; + halfLen2 = box2.hs.z; + } else { + if (colAxis.x*transform2.c + colAxis.y*transform2.g + colAxis.z*transform2.k < 0) { + z2 = -z2; + } + } + // Получаем глобальные координаты средних точек рёбер + var x1:Number = transform1.a*tx + transform1.b*ty + transform1.c*tz + transform1.d; + var y1:Number = transform1.e*tx + transform1.f*ty + transform1.g*tz + transform1.h; + var z1:Number = transform1.i*tx + transform1.j*ty + transform1.k*tz + transform1.l; + tx = x2; + ty = y2; + tz = z2; + x2 = transform2.a*tx + transform2.b*ty + transform2.c*tz + transform2.d; + y2 = transform2.e*tx + transform2.f*ty + transform2.g*tz + transform2.h; + z2 = transform2.i*tx + transform2.j*ty + transform2.k*tz + transform2.l; + + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + tx = x2 - x1; + ty = y2 - y1; + tz = z2 - z1; + var c1:Number = axis10.x*tx + axis10.y*ty + axis10.z*tz; + var c2:Number = axis20.x*tx + axis20.y*ty + axis20.z*tz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cp.pos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cp.pos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cp.pos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = cpPos.x - transform1.d; + r.y = cpPos.y - transform1.h; + r.z = cpPos.z - transform1.l; + r = cp.r2; + r.x = cpPos.x - transform2.d; + r.y = cpPos.y - transform2.h; + r.z = cpPos.z - transform2.l; + cp.penetration = minOverlap; + } + + /** + * Проверяет пересечение боксов вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box1 + * @param box2 + * @param axis + * @param axisIndex + * @param toBox1 + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box1 + * @param box2 + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox1 + * @return + */ + private function testDerivedAxis(box1:CollisionBox, box2:CollisionBox, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox1:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box1, box2, axis, toBox1); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия двух боксов вдоль заданной оси. + * + * @param box1 первый бокс + * @param box2 второй бокс + * @param axis ось + * @param vectorToBox1 вектор, соединяющий центр второго бокса с центром первого + * @return величина перекрытия боксов вдоль оси + */ + public function overlapOnAxis(box1:CollisionBox, box2:CollisionBox, axis:Vector3, vectorToBox1:Vector3):Number { + var m:Matrix4 = box1.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box1.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box1.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box1.hs.z; + if (d < 0) d = -d; + projection += d; + + m = box2.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box2.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box2.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box2.hs.z; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox1.x*axis.x + vectorToBox1.y*axis.y + vectorToBox1.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} + import alternativa.math.Vector3; + + class CollisionPointTmp { + + public var pos:Vector3 = new Vector3(); + public var penetration:Number; + + public var feature1:int; + public var feature2:int; + } diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/BoxCollider.as b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/BoxCollider.as new file mode 100644 index 0000000..2429bb1 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/BoxCollider.as @@ -0,0 +1,427 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + /** + * + */ + public class BoxCollider implements ICollider { + + /** + * + */ + public function BoxCollider() { + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + return false; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + /** + * Формирует список вершин грани бокса, заданной нормальной к грани осью. Вершины перечисляются против часовой стрелки. + * + * @param box бокс, в котором ишутся вершины + * @param axisIdx индекс нормальной оси + * @param reverse если указано значение true, возвращаются вершины противоположной грани + * @param result список, в который помещаются вершины + */ + protected function getFaceVertsByAxis(hs:Vector3, axisIdx:int, negativeFace:Boolean, result:Vector.):void { + var v:Vector3; + switch (axisIdx) { + case 0: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + } + break; + case 1: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + } else { + v = result[0]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + } + break; + case 2: + if (negativeFace) { + v = result[0]; v.x = -hs.x; v.y = hs.y; v.z = -hs.z; + v = result[1]; v.x = hs.x; v.y = hs.y; v.z = -hs.z; + v = result[2]; v.x = hs.x; v.y = -hs.y; v.z = -hs.z; + v = result[3]; v.x = -hs.x; v.y = -hs.y; v.z = -hs.z; + } else { + v = result[0]; v.x = -hs.x; v.y = -hs.y; v.z = hs.z; + v = result[1]; v.x = hs.x; v.y = -hs.y; v.z = hs.z; + v = result[2]; v.x = hs.x; v.y = hs.y; v.z = hs.z; + v = result[3]; v.x = -hs.x; v.y = hs.y; v.z = hs.z; + } + break; + } + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x > x1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighX(x:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var x1:Number = x + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.x < x1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.x > x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.x < x1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (x - p1.x)/dx; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y > y1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighY(y:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var y1:Number = y + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.y < y1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.y > y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.y < y1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (y - p1.y)/dy; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipLowZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z - epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z > z1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + /** + * + * @param x + * @param pnum + * @param points + * @param result + * @return + */ + protected function clipHighZ(z:Number, pnum:int, points:Vector., result:Vector., epsilon:Number):int { + var z1:Number = z + epsilon; + var num:int = 0; + var p1:Vector3 = points[int(pnum - 1)]; + var p2:Vector3; + + var dx:Number; + var dy:Number; + var dz:Number; + var t:Number; + var v:Vector3; + + for (var i:int = 0; i < pnum; i++) { + p2 = points[i]; + if (p1.z < z1) { + v = result[num]; + num++; + v.x = p1.x; + v.y = p1.y; + v.z = p1.z; + if (p2.z > z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + } else if (p2.z < z1) { + dx = p2.x - p1.x; + dy = p2.y - p1.y; + dz = p2.z - p1.z; + t = (z - p1.z)/dz; + v = result[num]; + num++; + v.x = p1.x + t*dx; + v.y = p1.y + t*dy; + v.z = p1.z + t*dz; + } + p1 = p2; + } + return num; + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/BoxPlaneCollider.as b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/BoxPlaneCollider.as new file mode 100644 index 0000000..3bebdc3 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/BoxPlaneCollider.as @@ -0,0 +1,88 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + public class BoxPlaneCollider implements ICollider { + + private var verts1:Vector. = new Vector.(8, true); + private var verts2:Vector. = new Vector.(8, true); + private var normal:Vector3 = new Vector3(); + + public function BoxPlaneCollider() { + for (var i:int = 0; i < 8; i++) { + verts1[i] = new Vector3(); + verts2[i] = new Vector3(); + } + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var box:RigidBox = body1 as RigidBox; +// var plane:RigidPlane; +// if (box == null) { +// box = body2 as RigidBox; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные координаты вершин бокса +// var sx:Number = box.halfSize.x; +// var sy:Number = box.halfSize.y; +// var sz:Number = box.halfSize.z; +// (verts1[0] as Vector3).reset(-sx, -sy, -sz); +// (verts1[1] as Vector3).reset(sx, -sy, -sz); +// (verts1[2] as Vector3).reset(sx, sy, -sz); +// (verts1[3] as Vector3).reset(-sx, sy, -sz); +// (verts1[4] as Vector3).reset(-sx, -sy, sz); +// (verts1[5] as Vector3).reset(sx, -sy, sz); +// (verts1[6] as Vector3).reset(sx, sy, sz); +// (verts1[7] as Vector3).reset(-sx, sy, sz); +// +// box.transform.transformVectors(verts1, verts2); +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// // Проверяем наличие столкновений с каждой вершиной +// collisionInfo.pcount = 0; +// for (var i:int = 0; i < 8; i++) { +// // Вершина добавляется в список точек столкновения, если лежит под плоскостью +// var dist:Number = (verts2[i] as Vector3).dot(normal); +// if (dist < offset) { +// var cp:ContactPoint; +// if (collisionInfo.pcount == collisionInfo.points.length) { +// cp = new ContactPoint(); +// collisionInfo.points[collisionInfo.pcount] = cp; +// } else { +// cp = collisionInfo.points[collisionInfo.pcount]; +// } +// cp.pos.copy(verts2[i]); +// cp.r1.diff(cp.pos, box.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); +// cp.penetration = offset - dist; +// collisionInfo.pcount++; +// } +// } +// if (collisionInfo.pcount > 0) { +// collisionInfo.body1 = box; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// return true; +// } + return false; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/BoxRectCollider.as b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/BoxRectCollider.as new file mode 100644 index 0000000..7ce47be --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/BoxRectCollider.as @@ -0,0 +1,573 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionRect; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * + */ + public class BoxRectCollider extends BoxCollider { + + private var epsilon:Number = 0.001; + + private var vectorToBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + + private var bestAxisIndex:int; + private var minOverlap:Number; + + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxRectCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, rect, vectorToBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + if (!findEdgesIntersection(box, rect, vectorToBox, int(bestAxisIndex/2), bestAxisIndex%2, contact)) { + return false; + } + } + contact.body1 = box.body; + contact.body2 = rect.body; + + // Хак для танков, чтобы исключить утыкания танков в стыки статических примитивов + if (rect.transform.k > 0.99999) { + contact.normal.x = 0; + contact.normal.y = 0; + contact.normal.z = 1; + } + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + minOverlap = 1e10; + var box:CollisionBox = prim1 as CollisionBox; + var rect:CollisionRect; + if (box == null) { + box = prim2 as CollisionBox; + rect = prim1 as CollisionRect; + } else { + rect = prim2 as CollisionRect; + } + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + // Вектор из центра прямоугольника в центр бокса + vectorToBox.x = boxTransform.d - rectTransform.d; + vectorToBox.y = boxTransform.h - rectTransform.h; + vectorToBox.z = boxTransform.l - rectTransform.l; + + // Проверка пересечения по нормали прямоугольника + rectTransform.getAxis(2, axis22); + if (!testMainAxis(box, rect, axis22, 0, vectorToBox)) return false; + + // Проверка пересечения по основным осям бокса + boxTransform.getAxis(0, axis10); + if (!testMainAxis(box, rect, axis10, 1, vectorToBox)) return false; + boxTransform.getAxis(1, axis11); + if (!testMainAxis(box, rect, axis11, 2, vectorToBox)) return false; + boxTransform.getAxis(2, axis12); + if (!testMainAxis(box, rect, axis12, 3, vectorToBox)) return false; + + // Получаем направляющие рёбер прямоугольника + rectTransform.getAxis(0, axis20); + rectTransform.getAxis(1, axis21); + + // Проверка производных осей + if (!testDerivedAxis(box, rect, axis10, axis20, 4, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis10, axis21, 5, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis11, axis20, 6, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis11, axis21, 7, vectorToBox)) return false; + + if (!testDerivedAxis(box, rect, axis12, axis20, 8, vectorToBox)) return false; + if (!testDerivedAxis(box, rect, axis12, axis21, 9, vectorToBox)) return false; + + return true; + } + + /** + * Выполняет поиск точек контакта бокса с прямоугольником. + * + * @param box бокс + * @param rect прямоугольник + * @param vectorToBox вектор, направленный из центра прямоугольника в центр бокса + * @param faceAxisIdx индекс оси, идентифицирующей полскость столкновения (грань бокса или полскость прямоугольника) + * @param colInfo структура, в которую записывается информация о точках контакта + */ + private function findFaceContactPoints(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + var pnum:int; + var i:int; + var v:Vector3; + var cp:ContactPoint; + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + var colAxis:Vector3 = contact.normal; + + var negativeFace:Boolean; + var code:int; + + if (faceAxisIdx == 0) { + // Столкновение с плоскостью прямоугольника + + // Проверим положение бокса относительно плоскости прямоугольника + colAxis.x = rectTransform.c; + colAxis.y = rectTransform.g; + colAxis.z = rectTransform.k; + +// var offset:Number = colAxis.x*rectTransform.d + colAxis.y*rectTransform.h + colAxis.z*rectTransform.l; +// if (bbPos.vDot(colAxis) < offset) return false; + + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incidentAxisIdx:int = 0; + var incidentAxisDot:Number; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colAxis.x + axis.y*colAxis.y + axis.z*colAxis.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incidentAxisIdx = axisIdx; + incidentAxisDot = dot; + } + } + negativeFace = incidentAxisDot > 0; + + code = 1 << (incidentAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + + // Получаем список вершин грани бокса, переводим их в систему координат прямоугольника и выполняем обрезку + // по прямоугольнику. Таким образом получается список потенциальных точек контакта. + boxTransform.getAxis(incidentAxisIdx, axis); + getFaceVertsByAxis(box.hs, incidentAxisIdx, negativeFace, points1); + boxTransform.transformPointsN(points1, points2, 4); + rectTransform.transformPointsTransposedN(points2, points1, 4); + pnum = clipByRect(rect.hs); + // Проверяем каждую потенциальную точку на принадлежность нижней полуплоскости прямоугольника и добавляем такие точки в список контактов + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if (v.z < epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = -v.z; + var cpPos:Vector3 = cp.pos; + cpPos.x = rectTransform.a*v.x + rectTransform.b*v.y + rectTransform.c*v.z + rectTransform.d; + cpPos.y = rectTransform.e*v.x + rectTransform.f*v.y + rectTransform.g*v.z + rectTransform.h; + cpPos.z = rectTransform.i*v.x + rectTransform.j*v.y + rectTransform.k*v.z + rectTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } else { + // Столкновение с гранью бокса + faceAxisIdx--; + boxTransform.getAxis(faceAxisIdx, colAxis); + negativeFace = colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z > 0; + + code = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) { + return false; + } + + if (!negativeFace) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + if (rectTransform.c*colAxis.x + rectTransform.g*colAxis.y + rectTransform.k*colAxis.z < 0) return false; + + getFaceVertsByAxis(rect.hs, 2, false, points1); + rectTransform.transformPointsN(points1, points2, 4); + boxTransform.transformPointsTransposedN(points2, points1, 4); + pnum = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность первому боксу и добавляем такие точки в список контактов + var pen:Number; + contact.pcount = 0; + for (i = 0; i < pnum; i++) { + v = points1[i]; + if ((pen = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace)) > -epsilon) { + cp = contact.points[contact.pcount++]; + cp.penetration = pen; + cpPos = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + v = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + } + } + } + return true; + } + + /** + * + * @param hs + * @param p + * @param axisIndex + * @param reverse + * @return + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, reverse:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (reverse) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (reverse) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (reverse) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, заданной списком вершин в поле объекта verts1. Результат сохраняется в этом же поле. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани, заданной вершинами в поле verts1 + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 4; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * + * @param hs + * @return + */ + private function clipByRect(hs:Vector3):int { + var pnum:int = 4; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + + /** + * Вычисляет точку столкновения рёбер двух боксов. + * + * @param box первый бокс + * @param rect второй бокс + * @param vectorToBox1 вектор, направленный из центра второго бокса в центр первого + * @param axisIdx1 индекс направляющей оси ребра первого бокса + * @param axisIdx2 индекс направляющей оси ребра второго бокса + * @param colInfo структура, в которую записывается информация о столкновении + */ + private function findEdgesIntersection(box:CollisionBox, rect:CollisionRect, vectorToBox:Vector3, axisIdx1:int, axisIdx2:int, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var rectTransform:Matrix4 = rect.transform; + + boxTransform.getAxis(axisIdx1, axis10); + rectTransform.getAxis(axisIdx2, axis20); + var colAxis:Vector3 = contact.normal; + colAxis.x = axis10.y*axis20.z - axis10.z*axis20.y; + colAxis.y = axis10.z*axis20.x - axis10.x*axis20.z; + colAxis.z = axis10.x*axis20.y - axis10.y*axis20.x; + var k:Number = 1/Math.sqrt(colAxis.x*colAxis.x + colAxis.y*colAxis.y + colAxis.z*colAxis.z); + colAxis.x *= k; + colAxis.y *= k; + colAxis.z *= k; + + // Разворот оси в сторону бокса + if (colAxis.x*vectorToBox.x + colAxis.y*vectorToBox.y + colAxis.z*vectorToBox.z < 0) { + colAxis.x = -colAxis.x; + colAxis.y = -colAxis.y; + colAxis.z = -colAxis.z; + } + + // Находим среднюю точку на каждом из пересекающихся рёбер + var halfLen1:Number; + var halfLen2:Number; + var vx:Number = box.hs.x; + var vy:Number = box.hs.y; + var vz:Number = box.hs.z; + var x2:Number = rect.hs.x; + var y2:Number = rect.hs.y; + var z2:Number = rect.hs.z; + // x + if (axisIdx1 == 0) { + vx = 0; + halfLen1 = box.hs.x; + } else { + if (boxTransform.a*colAxis.x + boxTransform.e*colAxis.y + boxTransform.i*colAxis.z > 0) { + vx = -vx; + if ((box.excludedFaces & 2) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 1) != 0) { + return false; + } + } + } + if (axisIdx2 == 0) { + x2 = 0; + halfLen2 = rect.hs.x; + } else { + if (rectTransform.a*colAxis.x + rectTransform.e*colAxis.y + rectTransform.i*colAxis.z < 0) { + x2 = -x2; + } + } + // y + if (axisIdx1 == 1) { + vy = 0; + halfLen1 = box.hs.y; + } else { + if (boxTransform.b*colAxis.x + boxTransform.f*colAxis.y + boxTransform.j*colAxis.z > 0) { + vy = -vy; + if ((box.excludedFaces & 8) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 4) != 0) { + return false; + } + } + } + if (axisIdx2 == 1) { + y2 = 0; + halfLen2 = rect.hs.y; + } else { + if (rectTransform.b*colAxis.x + rectTransform.f*colAxis.y + rectTransform.j*colAxis.z < 0) { + y2 = -y2; + } + } + // z + if (axisIdx1 == 2) { + vz = 0; + halfLen1 = box.hs.z; + } else { + if (boxTransform.c*colAxis.x + boxTransform.g*colAxis.y + boxTransform.k*colAxis.z > 0) { + vz = -vz; + if ((box.excludedFaces & 32) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 16) != 0) { + return false; + } + } + } + // Получаем глобальные координаты средних точек рёбер + + var x1:Number = boxTransform.a*vx + boxTransform.b*vy + boxTransform.c*vz + boxTransform.d; + var y1:Number = boxTransform.e*vx + boxTransform.f*vy + boxTransform.g*vz + boxTransform.h; + var z1:Number = boxTransform.i*vx + boxTransform.j*vy + boxTransform.k*vz + boxTransform.l; + vx = x2; + vy = y2; + vz = z2; + x2 = rectTransform.a*vx + rectTransform.b*vy + rectTransform.c*vz + rectTransform.d; + y2 = rectTransform.e*vx + rectTransform.f*vy + rectTransform.g*vz + rectTransform.h; + z2 = rectTransform.i*vx + rectTransform.j*vy + rectTransform.k*vz + rectTransform.l; + // Находим точку пересечения рёбер, решая систему уравнений + k = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + vx = x2 - x1; + vy = y2 - y1; + vz = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + // Запись данных о столкновении + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + var cpPos:Vector3 = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + cpPos.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + cpPos.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + cpPos.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var v:Vector3 = cp.r1; + v.x = cpPos.x - boxTransform.d; + v.y = cpPos.y - boxTransform.h; + v.z = cpPos.z - boxTransform.l; + v = cp.r2; + v.x = cpPos.x - rectTransform.d; + v.y = cpPos.y - rectTransform.h; + v.z = cpPos.z - rectTransform.l; + return true; + } + + /** + * Проверяет пересечение вдоль заданной оси. При наличии пересечения сохраняется глубина пересечения, если она минимальна. + * + * @param box + * @param rect + * @param axis + * @param axisIndex + * @param vectorToBox + * @param bestAxis + * @return true в случае, если проекции боксов на заданную ось пересекаются, иначе false + */ + private function testMainAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param rect + * @param axis1 + * @param axis2 + * @param axisIndex + * @param vectorToBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, rect:CollisionRect, axis1:Vector3, axis2:Vector3, axisIndex:int, vectorToBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, rect, axis, vectorToBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Вычисляет глубину перекрытия вдоль заданной оси. + * + * @param box бокс + * @param rect прямоугольник + * @param axis ось + * @param vectorToBox вектор, соединяющий центр прямоугольника с центром бокса + * @return величина перекрытия вдоль оси + */ + public function overlapOnAxis(box:CollisionBox, rect:CollisionRect, axis:Vector3, vectorToBox:Vector3):Number { + var m:Matrix4 = box.transform; + var d:Number = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*box.hs.x; + if (d < 0) d = -d; + var projection:Number = d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*box.hs.y; + if (d < 0) d = -d; + projection += d; + d = (m.c*axis.x + m.g*axis.y + m.k*axis.z)*box.hs.z; + if (d < 0) d = -d; + projection += d; + + m = rect.transform; + d = (m.a*axis.x + m.e*axis.y + m.i*axis.z)*rect.hs.x; + if (d < 0) d = -d; + projection += d; + d = (m.b*axis.x + m.f*axis.y + m.j*axis.z)*rect.hs.y; + if (d < 0) d = -d; + projection += d; + + d = vectorToBox.x*axis.x + vectorToBox.y*axis.y + vectorToBox.z*axis.z; + if (d < 0) d = -d; + + return projection - d; + } + + } +} diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/BoxSphereCollider.as b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/BoxSphereCollider.as new file mode 100644 index 0000000..78e1920 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/BoxSphereCollider.as @@ -0,0 +1,161 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.math.Vector3; + + /** + * + */ + public class BoxSphereCollider implements ICollider { + + private var center:Vector3 = new Vector3(); + private var closestPt:Vector3 = new Vector3(); + + private var bPos:Vector3 = new Vector3(); + private var sPos:Vector3 = new Vector3(); + + /** + * + */ + public function BoxSphereCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformPointTransposed(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + // TODO: Предусмотреть обработку случая, когда центр сферы внутри бокса + + var distSqr:Number = center.subtract(closestPt).lengthSqr(); + if (distSqr > sphere.r*sphere.r) { + return false; + } + // Зафиксированно столкновение + contact.body1 = sphere.body; + contact.body2 = box.body; + contact.normal.copy(closestPt).transform4(box.transform).subtract(sPos).normalize().reverse(); + contact.pcount = 1; + + var cp:ContactPoint = contact.points[0]; + cp.penetration = sphere.r - Math.sqrt(distSqr); + cp.pos.copy(contact.normal).scale(-sphere.r).add(sPos); + cp.r1.diff(cp.pos, sPos); + cp.r2.diff(cp.pos, bPos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + var sphere:CollisionSphere = prim1 as CollisionSphere; + var box:CollisionBox; + if (sphere == null) { + sphere = prim2 as CollisionSphere; + box = prim1 as CollisionBox; + } else { + box = prim2 as CollisionBox; + } + // Трансформируем центр сферы в систему координат бокса + sphere.transform.getAxis(3, sPos); + box.transform.getAxis(3, bPos); + box.transform.transformPointTransposed(sPos, center); + // Выполняем поиск разделяющей оси + var hs:Vector3 = box.hs; + var sx:Number = hs.x + sphere.r; + var sy:Number = hs.y + sphere.r; + var sz:Number = hs.z + sphere.r; + if (center.x > sx || center.x < -sx + || center.y > sy || center.y < -sy + || center.z > sz || center.z < -sz) { + return false; + } + // Находим ближайшую к сфере точку на боксе + if (center.x > hs.x) { + closestPt.x = hs.x; + } else if (center.x < -hs.x) { + closestPt.x = -hs.x; + } else { + closestPt.x = center.x; + } + + if (center.y > hs.y) { + closestPt.y = hs.y; + } else if (center.y < -hs.y) { + closestPt.y = -hs.y; + } else { + closestPt.y = center.y; + } + + if (center.z > hs.z) { + closestPt.z = hs.z; + } else if (center.z < -hs.z) { + closestPt.z = -hs.z; + } else { + closestPt.z = center.z; + } + + var distSqr:Number = center.subtract(closestPt).lengthSqr(); + return distSqr <= sphere.r*sphere.r; + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/BoxTriangleCollider.as b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/BoxTriangleCollider.as new file mode 100644 index 0000000..762355c --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/BoxTriangleCollider.as @@ -0,0 +1,725 @@ +package alternativa.physics.collision.colliders { + + + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.primitives.CollisionBox; + import alternativa.physics.collision.primitives.CollisionTriangle; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * + */ + public class BoxTriangleCollider extends BoxCollider { + + public var epsilon:Number = 0.001; + + private var bestAxisIndex:int; + private var minOverlap:Number; + private var toBox:Vector3 = new Vector3(); + private var axis:Vector3 = new Vector3(); + private var colNormal:Vector3 = new Vector3(); + private var axis10:Vector3 = new Vector3(); + private var axis11:Vector3 = new Vector3(); + private var axis12:Vector3 = new Vector3(); + private var axis20:Vector3 = new Vector3(); + private var axis21:Vector3 = new Vector3(); + private var axis22:Vector3 = new Vector3(); + private var point1:Vector3 = new Vector3(); + private var point2:Vector3 = new Vector3(); + private var vector:Vector3 = new Vector3(); + private var points1:Vector. = new Vector.(8, true); + private var points2:Vector. = new Vector.(8, true); + + /** + * + */ + public function BoxTriangleCollider() { + for (var i:int = 0; i < 8; i++) { + points1[i] = new Vector3(); + points2[i] = new Vector3(); + } + } + + /** + * Проверяет наличие пересечения примитивов. Если пересечение существует, заполняется информация о контакте. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + override public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + if (!haveCollision(prim1, prim2)) return false; + + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + if (bestAxisIndex < 4) { + // Контакт вдоль одной из основных осей + if (!findFaceContactPoints(box, tri, toBox, bestAxisIndex, contact)) return false; + } else { + // Контакт ребро-ребро + bestAxisIndex -= 4; + if (!findEdgesIntersection(box, tri, toBox, bestAxisIndex%3, int(bestAxisIndex/3), contact)) return false; + } + + contact.body1 = box.body; + contact.body2 = tri.body; + + // Хак для танков, чтобы исключить утыкания танков в стыки статических примитивов + if (tri.transform.k > 0.99999) { + contact.normal.x = 0; + contact.normal.y = 0; + contact.normal.z = 1; + } + + return true; + } + + /** + * Выполняет быстрый тест на наличие пересечения двух примитивов. + * + * @param prim1 первый примитив + * @param prim2 второй примитив + * @return true, если пересечение существует, иначе false + */ + override public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + var tri:CollisionTriangle = prim1 as CollisionTriangle; + var box:CollisionBox; + if (tri == null) { + box = CollisionBox(prim1); + tri = CollisionTriangle(prim2); + } else { + box = CollisionBox(prim2); + } + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + toBox.x = boxTransform.d - triTransform.d; + toBox.y = boxTransform.h - triTransform.h; + toBox.z = boxTransform.l - triTransform.l; + + minOverlap = 1e308; + + // Сначала проверяется нормаль треугольника + axis.x = triTransform.c; + axis.y = triTransform.g; + axis.z = triTransform.k; + if (!testMainAxis(box, tri, axis, 0, toBox)) return false; + + // Проверка основных осей бокса + axis10.x = boxTransform.a; + axis10.y = boxTransform.e; + axis10.z = boxTransform.i; + if (!testMainAxis(box, tri, axis10, 1, toBox)) return false; + axis11.x = boxTransform.b; + axis11.y = boxTransform.f; + axis11.z = boxTransform.j; + if (!testMainAxis(box, tri, axis11, 2, toBox)) return false; + axis12.x = boxTransform.c; + axis12.y = boxTransform.g; + axis12.z = boxTransform.k; + if (!testMainAxis(box, tri, axis12, 3, toBox)) return false; + + // Проверка производных осей + // TODO: заменить вычисления векторных произведений инлайнами + var v:Vector3 = tri.e0; + axis20.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis20.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis20.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis20, 4, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis20, 5, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis20, 6, toBox)) return false; + + v = tri.e1; + axis21.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis21.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis21.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis21, 7, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis21, 8, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis21, 9, toBox)) return false; + + v = tri.e2; + axis22.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z; + axis22.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z; + axis22.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z; + if (!testDerivedAxis(box, tri, axis10, axis22, 10, toBox)) return false; + if (!testDerivedAxis(box, tri, axis11, axis22, 11, toBox)) return false; + if (!testDerivedAxis(box, tri, axis12, axis22, 12, toBox)) return false; + + return true; + } + + /** + * Тестирует пересечение примитивов вдоль заданной оси. + * + * @param box бокс + * @param tri треугольник + * @param axis ось + * @param axisIndex индекс оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return true, если примитивы имеют перекрытие проекций вдоль указанной оси, иначе false + */ + private function testMainAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, axisIndex:int, toBox:Vector3):Boolean { + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * + * @param box + * @param tri + * @param axis1 + * @param axis2 + * @param axisIndex + * @param toBox + * @return + * + */ + private function testDerivedAxis(box:CollisionBox, tri:CollisionTriangle, axis1:Vector3, axis2:Vector3, axisIndex:int, toBox:Vector3):Boolean { + // axis = axis1 cross axis2 + axis.x = axis1.y*axis2.z - axis1.z*axis2.y; + axis.y = axis1.z*axis2.x - axis1.x*axis2.z; + axis.z = axis1.x*axis2.y - axis1.y*axis2.x; + var lenSqr:Number = axis.x*axis.x + axis.y*axis.y + axis.z*axis.z; + if (lenSqr < 0.0001) return true; + var k:Number = 1/Math.sqrt(lenSqr); + axis.x *= k; + axis.y *= k; + axis.z *= k; + var overlap:Number = overlapOnAxis(box, tri, axis, toBox); + if (overlap < -epsilon) return false; + if (overlap + epsilon < minOverlap) { + minOverlap = overlap; + bestAxisIndex = axisIndex; + } + return true; + } + + /** + * Рассчитывает величину перекрытия проекций бокса и треугольника на заданную ось. + * + * @param box бокс + * @param tri треугольник + * @param axis единичный направляющий вектор оси + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @return величина перекрытия. Положительное значение указывает на наличие перекрытия, нулевое или отрицательное значение указывает на отсутствие перекрытия. + */ + private function overlapOnAxis(box:CollisionBox, tri:CollisionTriangle, axis:Vector3, toBox:Vector3):Number { + var t:Matrix4 = box.transform; + var projection:Number = (t.a*axis.x + t.e*axis.y + t.i*axis.z)*box.hs.x; + if (projection < 0) projection = -projection; + var d:Number = (t.b*axis.x + t.f*axis.y + t.j*axis.z)*box.hs.y; + projection += d < 0 ? -d : d; + d = (t.c*axis.x + t.g*axis.y + t.k*axis.z)*box.hs.z; + projection += d < 0 ? -d : d; + + var vectorProjection:Number = toBox.x*axis.x + toBox.y*axis.y + toBox.z*axis.z; + // Для оптимизации ось преобразуется в систему треугольника, а не его вершины в мировую систему + t = tri.transform; + var ax:Number = t.a*axis.x + t.e*axis.y + t.i*axis.z; + var ay:Number = t.b*axis.x + t.f*axis.y + t.j*axis.z; + var az:Number = t.c*axis.x + t.g*axis.y + t.k*axis.z; + var max:Number = 0; + if (vectorProjection < 0) { + vectorProjection = -vectorProjection; + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d < max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d < max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d < max) max = d; + max = -max; + } else { + d = tri.v0.x*ax + tri.v0.y*ay + tri.v0.z*az; + if (d > max) max = d; + d = tri.v1.x*ax + tri.v1.y*ay + tri.v1.z*az; + if (d > max) max = d; + d = tri.v2.x*ax + tri.v2.y*ay + tri.v2.z*az; + if (d > max) max = d; + } + + return projection + max - vectorProjection; + } + + /** + * Определяет точки контакта бокса и треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function findFaceContactPoints(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIndex:int, contact:Contact):Boolean { + if (faceAxisIndex == 0) { + // Столкновение с плоскостью треугольника + return getBoxToTriContact(box, tri, toBox, contact); + } else { + // Столкновение с гранью бокса + return getTriToBoxContact(box, tri, toBox, faceAxisIndex, contact); + } + } + + /** + * Определяет точки контакта бокса с плоскостью треугольника. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getBoxToTriContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, contact:Contact):Boolean { + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + colNormal.x = triTransform.c; + colNormal.y = triTransform.g; + colNormal.z = triTransform.k; + + var over:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + if (!over) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + // Ищем ось бокса, определяющую наиболее антипараллельную грань + var incFaceAxisIdx:int = 0; + var incAxisDot:Number = 0; + var maxDot:Number = 0; + for (var axisIdx:int = 0; axisIdx < 3; axisIdx++) { + boxTransform.getAxis(axisIdx, axis); + var dot:Number = axis.x*colNormal.x + axis.y*colNormal.y + axis.z*colNormal.z; + var absDot:Number = dot < 0 ? -dot : dot; + if (absDot > maxDot) { + maxDot = absDot; + incAxisDot = dot; + incFaceAxisIdx = axisIdx; + } + } + // Обрезка грани + var negativeFace:Boolean = incAxisDot > 0; + + var code:int = 1 << (incFaceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + getFaceVertsByAxis(box.hs, incFaceAxisIdx, negativeFace, points1); + boxTransform.transformPointsN(points1, points2, 4); + triTransform.transformPointsTransposedN(points2, points1, 4); + var pnum:int = clipByTriangle(tri); + // Среди конечного списка точек определяются лежащие под плоскостью треугольника + var cp:ContactPoint; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + var v:Vector3 = points2[i]; + if ((over && v.z < 0) || (!over && v.z > 0)) { + cp = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = triTransform.a*v.x + triTransform.b*v.y + triTransform.c*v.z + triTransform.d; + cpPos.y = triTransform.e*v.x + triTransform.f*v.y + triTransform.g*v.z + triTransform.h; + cpPos.z = triTransform.i*v.x + triTransform.j*v.y + triTransform.k*v.z + triTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = over ? -v.z : v.z; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Оперделяет точки контакта треугольника с гранью бокса. + * + * @param box бокс + * @param tri треугольник + * @param toBox вектор, соединяющий центр треугольника с центром бокса + * @param faceAxisIndex индекс оси, вдоль которой перекрытие минимально + * @param contact переменная, в которую записывается информация о контакте, если пересечение существует + * @return true, если пересечение существует, иначе false + */ + private function getTriToBoxContact(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, faceAxisIdx:int, contact:Contact):Boolean { + faceAxisIdx--; + + var boxTransform:Matrix4 = box.transform; + var triTransform:Matrix4 = tri.transform; + + boxTransform.getAxis(faceAxisIdx, colNormal); + var negativeFace:Boolean = toBox.x*colNormal.x + toBox.y*colNormal.y + toBox.z*colNormal.z > 0; + + var code:int = 1 << (faceAxisIdx << 1); + if (negativeFace) { + code <<= 1; + } + if ((code & box.excludedFaces) != 0) return false; + + if (!negativeFace) { + colNormal.x = -colNormal.x; + colNormal.y = -colNormal.y; + colNormal.z = -colNormal.z; + } + var v:Vector3 = points1[0]; + v.x = tri.v0.x; + v.y = tri.v0.y; + v.z = tri.v0.z; + + v = points1[1]; + v.x = tri.v1.x; + v.y = tri.v1.y; + v.z = tri.v1.z; + + v = points1[2]; + v.x = tri.v2.x; + v.y = tri.v2.y; + v.z = tri.v2.z; + + triTransform.transformPointsN(points1, points2, 3); + boxTransform.transformPointsTransposedN(points2, points1, 3); + + var pnum:int = clipByBox(box.hs, faceAxisIdx); + // Проверяем каждую потенциальную точку на принадлежность боксу и добавляем такие точки в список контактов + var penetration:Number; + contact.pcount = 0; + for (var i:int = 0; i < pnum; i++) { + v = points1[i]; + penetration = getPointBoxPenetration(box.hs, v, faceAxisIdx, negativeFace); + if (penetration > -epsilon) { + var cp:ContactPoint = contact.points[contact.pcount++]; + var cpPos:Vector3 = cp.pos; + cpPos.x = boxTransform.a*v.x + boxTransform.b*v.y + boxTransform.c*v.z + boxTransform.d; + cpPos.y = boxTransform.e*v.x + boxTransform.f*v.y + boxTransform.g*v.z + boxTransform.h; + cpPos.z = boxTransform.i*v.x + boxTransform.j*v.y + boxTransform.k*v.z + boxTransform.l; + var r:Vector3 = cp.r1; + r.x = cpPos.x - boxTransform.d; + r.y = cpPos.y - boxTransform.h; + r.z = cpPos.z - boxTransform.l; + r = cp.r2; + r.x = cpPos.x - triTransform.d; + r.y = cpPos.y - triTransform.h; + r.z = cpPos.z - triTransform.l; + cp.penetration = penetration; + } + } + contact.normal.x = colNormal.x; + contact.normal.y = colNormal.y; + contact.normal.z = colNormal.z; + return true; + } + + /** + * Вычисляет величину проникновения точки в бокс. + * + * @param hs вектор половинных размеров бокса + * @param p точка в системе координат бокса + * @param axisIndex индекс оси + * @param negativeFace если true, проверяется нижняя грань + * @return величина проникновения точки в бокс + */ + private function getPointBoxPenetration(hs:Vector3, p:Vector3, faceAxisIdx:int, negativeFace:Boolean):Number { + switch (faceAxisIdx) { + case 0: + if (negativeFace) return p.x + hs.x; + else return hs.x - p.x; + case 1: + if (negativeFace) return p.y + hs.y; + else return hs.y - p.y; + case 2: + if (negativeFace) return p.z + hs.z; + else return hs.z - p.z; + } + return 0; + } + + /** + * Выполняет обрезку грани, вершины которой заданы в списке points1. Результат сохраняется в этом же списке. + * + * @param hs вектор половинных размеров бокса, гранью которого обрезается грань второго бокса + * @param faceAxisIdx индекс нормальной оси грани, по которой выполняется обрезка + * @return количество вершин, получившихся в результате обрезки грани + */ + private function clipByBox(hs:Vector3, faceAxisIdx:int):int { + var pnum:int = 3; + switch (faceAxisIdx) { + case 0: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + case 1: + if ((pnum = clipLowZ(-hs.z, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighZ(hs.z, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighX(hs.x, pnum, points2, points1, epsilon); + case 2: + if ((pnum = clipLowX(-hs.x, pnum, points1, points2, epsilon)) == 0) return 0; + if ((pnum = clipHighX(hs.x, pnum, points2, points1, epsilon)) == 0) return 0; + if ((pnum = clipLowY(-hs.y, pnum, points1, points2, epsilon)) == 0) return 0; + return clipHighY(hs.y, pnum, points2, points1, epsilon); + } + return 0; + } + + /** + * Обрезает грань прямоугольника сторонами треугольника. Обрезка выполняется в системе координат и в проекции на + * плоскость треугольника. Входные вершины грани в количестве четырёх штук должны находиться в списке points1. + * Конечные вершины сохраняются в списке points2. + * + * @param tri треугольник + * @return количество врешин в конечном списке + */ + private function clipByTriangle(tri:CollisionTriangle):int { + var vnum:int = 4; + vnum = clipByLine(tri.v0, tri.e0, points1, vnum, points2); + if (vnum == 0) return 0; + vnum = clipByLine(tri.v1, tri.e1, points2, vnum, points1); + if (vnum == 0) return 0; + return clipByLine(tri.v2, tri.e2, points1, vnum, points2);; + } + + /** + * Обрезает полигон указанной прямой. + * + * @param linePoint точка на прямой + * @param lineDir единичный направляющий вектор прямой + * @param verticesIn список вершин исходного полигона + * @param vnum количество вершин исходного полигона + * @param verticesOut список, куда будут записаны вершины конечного полигона + * @return количество вершин конечного полигона + */ + private function clipByLine(linePoint:Vector3, lineDir:Vector3, verticesIn:Vector., vnum:int, verticesOut:Vector.):int { + var nx:Number = -lineDir.y; + var ny:Number = lineDir.x; + var offset:Number = linePoint.x*nx + linePoint.y*ny; + var v1:Vector3 = verticesIn[int(vnum - 1)]; + var offset1:Number = v1.x*nx + v1.y*ny; + var t:Number; + var v:Vector3; + var num:int = 0; + for (var i:int = 0; i < vnum; i++) { + var v2:Vector3 = verticesIn[i]; + var offset2:Number = v2.x*nx + v2.y*ny; + if (offset1 < offset) { + // Первая точка ребра во внешней полуплоскости + if (offset2 > offset) { + // Вторая точка ребра во внутренней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } else { + // Первая точка ребра во внутренней полуплоскости. Добавляем её в конечный список. + v = verticesOut[num]; + v.x = v1.x; + v.y = v1.y; + v.z = v1.z; + num++; + if (offset2 < offset) { + // Вторая точка ребра во внешней полуплоскости, в конечный список добавляется точка на пересечении + t = (offset - offset1)/(offset2 - offset1); + v = verticesOut[num]; + v.x = v1.x + t*(v2.x - v1.x); + v.y = v1.y + t*(v2.y - v1.y); + v.z = v1.z + t*(v2.z - v1.z); + num++; + } + } + v1 = v2; + offset1 = offset2; + } + return num; + } + + /** + * + * @param box + * @param tri + * @param toBox + * @param boxAxisIdx + * @param triAxisIdx + * @param contact + */ + private function findEdgesIntersection(box:CollisionBox, tri:CollisionTriangle, toBox:Vector3, boxAxisIdx:int, triAxisIdx:int, contact:Contact):Boolean { + // Определение точки и направляющего вектора ребра треугольника + var tmpx1:Number; + var tmpy1:Number; + var tmpz1:Number; + var tmpx2:Number; + var tmpy2:Number; + var tmpz2:Number; + switch (triAxisIdx) { + case 0: + tmpx1 = tri.e0.x; + tmpy1 = tri.e0.y; + tmpz1 = tri.e0.z; + tmpx2 = tri.v0.x; + tmpy2 = tri.v0.y; + tmpz2 = tri.v0.z; + break; + case 1: + tmpx1 = tri.e1.x; + tmpy1 = tri.e1.y; + tmpz1 = tri.e1.z; + tmpx2 = tri.v1.x; + tmpy2 = tri.v1.y; + tmpz2 = tri.v1.z; + break; + case 2: + tmpx1 = tri.e2.x; + tmpy1 = tri.e2.y; + tmpz1 = tri.e2.z; + tmpx2 = tri.v2.x; + tmpy2 = tri.v2.y; + tmpz2 = tri.v2.z; + break; + } + var triTransform:Matrix4 = tri.transform; + axis20.x = triTransform.a*tmpx1 + triTransform.b*tmpy1 + triTransform.c*tmpz1; + axis20.y = triTransform.e*tmpx1 + triTransform.f*tmpy1 + triTransform.g*tmpz1; + axis20.z = triTransform.i*tmpx1 + triTransform.j*tmpy1 + triTransform.k*tmpz1; + var x2:Number = triTransform.a*tmpx2 + triTransform.b*tmpy2 + triTransform.c*tmpz2 + triTransform.d; + var y2:Number = triTransform.e*tmpx2 + triTransform.f*tmpy2 + triTransform.g*tmpz2 + triTransform.h; + var z2:Number = triTransform.i*tmpx2 + triTransform.j*tmpy2 + triTransform.k*tmpz2 + triTransform.l; + + // Определение нормали контакта, точки и направляющего вектора ребра бокса + var boxTransform:Matrix4 = box.transform; + boxTransform.getAxis(boxAxisIdx, axis10); + + // Нормаль контакта + var v:Vector3 = contact.normal; + v.x = axis10.y*axis20.z - axis10.z*axis20.y; + v.y = axis10.z*axis20.x - axis10.x*axis20.z; + v.z = axis10.x*axis20.y - axis10.y*axis20.x; + k = 1/Math.sqrt(v.x*v.x + v.y*v.y + v.z*v.z); + v.x *= k; + v.y *= k; + v.z *= k; + // Разворот нормали в сторону бокса + if (v.x*toBox.x + v.y*toBox.y + v.z*toBox.z < 0) { + v.x = -v.x; + v.y = -v.y; + v.z = -v.z; + } + + var boxHalfLen:Number; + tmpx1 = box.hs.x; + tmpy1 = box.hs.y; + tmpz1 = box.hs.z; + // X + if (boxAxisIdx == 0) { + tmpx1 = 0; + boxHalfLen = box.hs.x; + } else { + if (boxTransform.a*v.x + boxTransform.e*v.y + boxTransform.i*v.z > 0) { + tmpx1 = -tmpx1; + if ((box.excludedFaces & 2) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 1) != 0) { + return false; + } + } + } + // Y + if (boxAxisIdx == 1) { + tmpy1 = 0; + boxHalfLen = box.hs.y; + } else { + if (boxTransform.b*v.x + boxTransform.f*v.y + boxTransform.j*v.z > 0) { + tmpy1 = -tmpy1; + if ((box.excludedFaces & 8) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 4) != 0) { + return false; + } + } + } + // Z + if (boxAxisIdx == 2) { + tmpz1 = 0; + boxHalfLen = box.hs.z; + } else { + if (boxTransform.c*v.x + boxTransform.g*v.y + boxTransform.k*v.z > 0) { + tmpz1 = -tmpz1; + if ((box.excludedFaces & 32) != 0) { + return false; + } + } else { + if ((box.excludedFaces & 16) != 0) { + return false; + } + } + } + var x1:Number = boxTransform.a*tmpx1 + boxTransform.b*tmpy1 + boxTransform.c*tmpz1 + boxTransform.d; + var y1:Number = boxTransform.e*tmpx1 + boxTransform.f*tmpy1 + boxTransform.g*tmpz1 + boxTransform.h; + var z1:Number = boxTransform.i*tmpx1 + boxTransform.j*tmpy1 + boxTransform.k*tmpz1 + boxTransform.l; + + // Находим точку пересечения рёбер, решая систему уравнений + // axis10 - направляющий вектор ребра бокса + // x1, y1, z1 - средняя точка ребра бокса + // axis20 - направляющий вектор ребра треугольника + // x2, y2, z2 - начальная точка ребра треугольника + var k:Number = axis10.x*axis20.x + axis10.y*axis20.y + axis10.z*axis20.z; + var det:Number = k*k - 1; + var vx:Number = x2 - x1; + var vy:Number = y2 - y1; + var vz:Number = z2 - z1; + var c1:Number = axis10.x*vx + axis10.y*vy + axis10.z*vz; + var c2:Number = axis20.x*vx + axis20.y*vy + axis20.z*vz; + var t1:Number = (c2*k - c1)/det; + var t2:Number = (c2 - c1*k)/det; + + // Запись данных о контакте + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = minOverlap; + v = cp.pos; + // Точка столкновения вычисляется как среднее между ближайшими точками на рёбрах + v.x = 0.5*(x1 + axis10.x*t1 + x2 + axis20.x*t2); + v.y = 0.5*(y1 + axis10.y*t1 + y2 + axis20.y*t2); + v.z = 0.5*(z1 + axis10.z*t1 + z2 + axis20.z*t2); + var r:Vector3 = cp.r1; + r.x = v.x - boxTransform.d; + r.y = v.y - boxTransform.h; + r.z = v.z - boxTransform.l; + r = cp.r2; + r.x = v.x - triTransform.d; + r.y = v.y - triTransform.h; + r.z = v.z - triTransform.l; + return true; + } + + } +} diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/SpherePlaneCollider.as b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/SpherePlaneCollider.as new file mode 100644 index 0000000..30cd55f --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/SpherePlaneCollider.as @@ -0,0 +1,68 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.math.Vector3; + + /** + * + */ + public class SpherePlaneCollider implements ICollider { + + private var normal:Vector3 = new Vector3(); + + /** + * + */ + public function SpherePlaneCollider() { + } + + /** + * + * @param body1 + * @param body2 + * @param collisionInfo + * @return + */ + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { +// var sphere:RigidSphere = body1 as RigidSphere; +// var plane:RigidPlane; +// if (sphere == null) { +// sphere = body2 as RigidSphere; +// plane = body1 as RigidPlane; +// } else { +// plane = body2 as RigidPlane; +// } +// +// // Вычисляем глобальные нормаль и смещение плоскости +// plane.baseMatrix.transformVector(plane.normal, normal); +// var offset:Number = plane.offset + normal.x*plane.transform.d + normal.y*plane.transform.h + normal.z*plane.transform.l; +// +// var dist:Number = sphere.state.pos.dot(normal) - offset; +// if (dist > sphere.r) return false; +// +// collisionInfo.body1 = sphere; +// collisionInfo.body2 = plane; +// collisionInfo.normal.copy(normal); +// collisionInfo.pcount = 1; +// +// var cp:ContactPoint = collisionInfo.points[0]; +// cp.penetration = sphere.r - dist; +// cp.pos.copy(normal).reverse().scale(sphere.r).add(sphere.state.pos); +// cp.r1.diff(cp.pos, sphere.state.pos); +// cp.r2.diff(cp.pos, plane.state.pos); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/SphereSphereCollider.as b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/SphereSphereCollider.as new file mode 100644 index 0000000..228d6c9 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/colliders/SphereSphereCollider.as @@ -0,0 +1,68 @@ +package alternativa.physics.collision.colliders { + import alternativa.physics.Contact; + import alternativa.physics.ContactPoint; + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.ICollider; + import alternativa.physics.collision.primitives.CollisionSphere; + import alternativa.math.Vector3; + + public class SphereSphereCollider implements ICollider { + + private var p1:Vector3 = new Vector3(); + private var p2:Vector3 = new Vector3(); + + public function SphereSphereCollider() { + } + + public function getContact(prim1:CollisionPrimitive, prim2:CollisionPrimitive, contact:Contact):Boolean { + var s1:CollisionSphere; + var s2:CollisionSphere; + if (prim1.body != null) { + s1 = prim1 as CollisionSphere; + s2 = prim2 as CollisionSphere; + } else { + s1 = prim2 as CollisionSphere; + s2 = prim1 as CollisionSphere; + } + + s1.transform.getAxis(3, p1); + s2.transform.getAxis(3, p2); + var dx:Number = p1.x - p2.x; + var dy:Number = p1.y - p2.y; + var dz:Number = p1.z - p2.z; + var len:Number = dx*dx + dy*dy + dz*dz; + var sum:Number = s1.r + s2.r; + if (len > sum*sum) return false; + len = Math.sqrt(len); + dx /= len; + dy /= len; + dz /= len; + + contact.body1 = s1.body; + contact.body2 = s2.body; + contact.normal.x = dx; + contact.normal.y = dy; + contact.normal.z = dz; + contact.pcount = 1; + var cp:ContactPoint = contact.points[0]; + cp.penetration = sum - len; + cp.pos.x = p1.x - dx*s1.r; + cp.pos.y = p1.y - dy*s1.r; + cp.pos.z = p1.z - dz*s1.r; + cp.r1.diff(cp.pos, p1); + cp.r2.diff(cp.pos, p2); + + return true; + } + + /** + * @param prim1 + * @param prim2 + * @return + */ + public function haveCollision(prim1:CollisionPrimitive, prim2:CollisionPrimitive):Boolean { + return false; + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/primitives/.svn/all-wcprops b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/primitives/.svn/all-wcprops new file mode 100644 index 0000000..9538ab2 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/primitives/.svn/all-wcprops @@ -0,0 +1,29 @@ +K 25 +svn:wc:ra_dav:version-url +V 117 +/!svn/ver/30874/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/collision/primitives +END +CollisionTriangle.as +K 25 +svn:wc:ra_dav:version-url +V 138 +/!svn/ver/30874/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/collision/primitives/CollisionTriangle.as +END +CollisionSphere.as +K 25 +svn:wc:ra_dav:version-url +V 136 +/!svn/ver/30874/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/collision/primitives/CollisionSphere.as +END +CollisionBox.as +K 25 +svn:wc:ra_dav:version-url +V 133 +/!svn/ver/30874/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/collision/primitives/CollisionBox.as +END +CollisionRect.as +K 25 +svn:wc:ra_dav:version-url +V 134 +/!svn/ver/30874/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/collision/primitives/CollisionRect.as +END diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/primitives/.svn/entries b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/primitives/.svn/entries new file mode 100644 index 0000000..8459d05 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/primitives/.svn/entries @@ -0,0 +1,76 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/collision/primitives +http://svndev.alternativaplatform.com + + + +2010-04-01T08:37:11.976264Z +30874 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +CollisionTriangle.as +file + + + + +2010-10-28T04:32:37.000000Z +8a9c2eea3bc27e016889d59eeb2b5669 +2010-04-01T08:37:11.976264Z +30874 +mike + +CollisionSphere.as +file + + + + +2010-10-28T04:32:37.000000Z +9e28a33c4fc342dc016b7b53ac018e62 +2010-04-01T08:37:11.976264Z +30874 +mike + +CollisionBox.as +file + + + + +2010-10-28T04:32:37.000000Z +b0bec39d69669900e723a5ce9dacbabf +2010-04-01T08:37:11.976264Z +30874 +mike + +CollisionRect.as +file + + + + +2010-10-28T04:32:37.000000Z +ae1c608cdba8f05b23ecb9ab1cd66cee +2010-04-01T08:37:11.976264Z +30874 +mike + diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/primitives/.svn/format b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/primitives/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/primitives/.svn/format @@ -0,0 +1 @@ +8 diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base new file mode 100644 index 0000000..ac62c8b --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionBox.as.svn-base @@ -0,0 +1,202 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * Ориентированный бокс. + */ + public class CollisionBox extends CollisionPrimitive { + + // Половинные размеры вдоль каждой из осей + public var hs:Vector3 = new Vector3(); + + public var excludedFaces:int; + + /** + * @param hs + * @param collisionGroup + */ + public function CollisionBox(hs:Vector3, collisionGroup:int) { + super(BOX, collisionGroup); + this.hs.copy(hs); + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + var t:Matrix4 = transform; + + var xx:Number; + var yy:Number; + var zz:Number; + + xx = t.a < 0 ? -t.a : t.a; + yy = t.b < 0 ? -t.b : t.b; + zz = t.c < 0 ? -t.c : t.c; + aabb.maxX = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minX = -aabb.maxX; + + xx = t.e < 0 ? -t.e : t.e; + yy = t.f < 0 ? -t.f : t.f; + zz = t.g < 0 ? -t.g : t.g; + aabb.maxY = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minY = -aabb.maxY; + + xx = t.i < 0 ? -t.i : t.i; + yy = t.j < 0 ? -t.j : t.j; + zz = t.k < 0 ? -t.k : t.k; + aabb.maxZ = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var box:CollisionBox = source as CollisionBox; + if (box == null) return this; + super.copyFrom(box); + hs.copy(box.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionBox(hs, collisionGroup); + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function raycast(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + var tMin:Number = -1; + var tMax:Number = 1e308; + var t1:Number; + var t2:Number; + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + // X + if (vx < epsilon && vx > -epsilon) { + if (ox < -hs.x || ox > hs.x) return -1; + } else { + t1 = (-hs.x - ox)/vx; + t2 = (hs.x - ox)/vx; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.x = -1; + normal.y = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.x = 1; + normal.y = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Y + if (vy < epsilon && vy > -epsilon) { + if (oy < -hs.y || oy > hs.y) return -1; + } else { + t1 = (-hs.y - oy)/vy; + t2 = (hs.y - oy)/vy; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.y = -1; + normal.x = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.y = 1; + normal.x = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Z + if (vz < epsilon && vz > -epsilon) { + if (oz < -hs.z || oz > hs.z) return -1; + } else { + t1 = (-hs.z - oz)/vz; + t2 = (hs.z - oz)/vz; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.z = -1; + normal.x = normal.y = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.z = 1; + normal.x = normal.y = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Перевод нормали в мировую систему координат + // Inlined normal.vDeltaTransformBy4(transform); + vx = normal.x; + vy = normal.y; + vz = normal.z; + normal.x = transform.a*vx + transform.b*vy + transform.c*vz; + normal.y = transform.e*vx + transform.f*vy + transform.g*vz; + normal.z = transform.i*vx + transform.j*vy + transform.k*vz; + + return tMin; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionBox hs=" + hs + "]"; + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base new file mode 100644 index 0000000..f070566 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionRect.as.svn-base @@ -0,0 +1,141 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * Ориентированный прямоугольник. Задаётся половинами размеров вдоль осей X (ширина) и Y (длина) локальной системы + * координат примитива. Таким образом, прямоугольник лежит в плоскости XY, его стороны параллельны осям этой + * плоскости, а нормаль направлена вдоль локальной оси Z. + * + * Прямоугольник может быть одно- или двусторонним. В случае одностороннего прямоугольника, + * столкновения с внутренней стороны не регистрируются. + */ + public class CollisionRect extends CollisionPrimitive { + // Половинные размеры прямоугольника вдоль осей X и Y. Нормаль направлена вдоль оси Z. + public var hs:Vector3 = new Vector3(); + // Флаг указывает, является примитив одно- или двусторонним + public var twoSided:Boolean = true; + + // Малое значение. Используется для указания фиктивной высоты примитива и в функции определения пересечения с сегментом. + private static const EPSILON:Number = 0.005; + + /** + * Создаёт новый экземпляр примитива. + * + * @param hs половинные размерв прямоугольника вдоль осей X и Y. Значение z игнорируется. + * @param collisionGroup группа примитива + */ + public function CollisionRect(hs:Vector3, collisionGroup:int) { + super(RECT, collisionGroup); + this.hs.copy(hs); + } + + /** + * Расчитывает ограничивающий бокс прямоугольника. Для избежания проблем высота примитива принимается равной + * не нулю, а малому значению. + * + * @return + */ + override public function calculateAABB():BoundBox { + // Баунд бокс прямоугольника имеет минимальную высоту, отличную от нуля во избежание проблем с построением kd-дерева + var t:Matrix4 = transform; + var xx:Number = t.a < 0 ? -t.a : t.a; + var yy:Number = t.b < 0 ? -t.b : t.b; + var zz:Number = t.c < 0 ? -t.c : t.c; + aabb.maxX = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minX = -aabb.maxX; + + xx = t.e < 0 ? -t.e : t.e; + yy = t.f < 0 ? -t.f : t.f; + zz = t.g < 0 ? -t.g : t.g; + aabb.maxY = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minY = -aabb.maxY; + + xx = t.i < 0 ? -t.i : t.i; + yy = t.j < 0 ? -t.j : t.j; + zz = t.k < 0 ? -t.k : t.k; + aabb.maxZ = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var rect:CollisionRect = source as CollisionRect; + if (rect == null) return this; + super.copyFrom(rect); + hs.copy(rect.hs); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionRect hs=" + hs + "]"; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionRect(hs, collisionGroup); + } + + /** + * @param origin + * @param vector + * @param collisionGroup + * @param threshold + * @param normal + * @return + */ + override public function raycast(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + + // Проверка параллельности сегмента и плоскости примитива + if (vz > -threshold && vz < threshold) return -1; + var t:Number = -oz/vz; + if (t < 0) return -1; + // Проверка вхождения точки пересечения в прямоугольник + ox += vx*t; + oy += vy*t; + oz = 0; + if (ox < (-hs.x - threshold) || ox > (hs.x + threshold) || oy < (-hs.y - threshold) || oy > (hs.y + threshold)) return -1; + + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + return t; + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base new file mode 100644 index 0000000..e6f4e00 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionSphere.as.svn-base @@ -0,0 +1,78 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Vector3; + + /** + * Сфера. + */ + public class CollisionSphere extends CollisionPrimitive { + + // Радиус сферы + public var r:Number = 0; + + /** + * + * @param r + * @param collisionGroup + */ + public function CollisionSphere(r:Number, collisionGroup:int) { + super(SPHERE, collisionGroup); + this.r = r; + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + aabb.maxX = transform.d + r; + aabb.minX = transform.d - r; + + aabb.maxY = transform.h + r; + aabb.minY = transform.h - r; + + aabb.maxZ = transform.l + r; + aabb.minZ = transform.l - r; + + return aabb; + } + + /** + * @param origin + * @param vector + * @param threshold + * @param normal + * @return + */ + override public function raycast(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var px:Number = origin.x - transform.d; + var py:Number = origin.y - transform.h; + var pz:Number = origin.z - transform.l; + var k:Number = vector.x*px + vector.y*py + vector.z*pz; + if (k > 0) return -1; + var a:Number = vector.x*vector.x + vector.y*vector.y + vector.z*vector.z; + var D:Number = k*k - a*(px*px + py*py + pz*pz - r*r); + if (D < 0) return -1; + return -(k + Math.sqrt(D))/a; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var sphere:CollisionSphere = source as CollisionSphere; + if (sphere == null) return this; + super.copyFrom(sphere); + r = sphere.r; + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionSphere(r, collisionGroup); + } + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base new file mode 100644 index 0000000..2611feb --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/primitives/.svn/text-base/CollisionTriangle.as.svn-base @@ -0,0 +1,247 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Vector3; + + /** + * Примитив треугольник. + */ + public class CollisionTriangle extends CollisionPrimitive { + + public var v0:Vector3 = new Vector3(); + public var v1:Vector3 = new Vector3(); + public var v2:Vector3 = new Vector3(); + + public var e0:Vector3 = new Vector3(); + public var e1:Vector3 = new Vector3(); + public var e2:Vector3 = new Vector3(); + +// public var len0:Number; +// public var len1:Number; +// public var len2:Number; +// + /** + * + * @param v0 + * @param v1 + * @param v2 + * @param collisionGroup + */ + public function CollisionTriangle(v0:Vector3, v1:Vector3, v2:Vector3, collisionGroup:int) { + super(TRIANGLE, collisionGroup); + initVertices(v0, v1, v2); + } + + /** + * Рассчитывает AABB примитива. + * + * @return ссылка на свой AABB + */ + override public function calculateAABB():BoundBox { + var epsilon:Number = 0.005; + var a:Number; + var b:Number; + var eps_c:Number = epsilon*transform.c; + var eps_g:Number = epsilon*transform.g; + var eps_k:Number = epsilon*transform.k; + + // Вершина 0 + // Ось X + a = v0.x*transform.a + v0.y*transform.b; + aabb.minX = aabb.maxX = a + eps_c; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v0.x*transform.e + v0.y*transform.f; + aabb.minY = aabb.maxY = a + eps_g; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v0.x*transform.i + v0.y*transform.j; + aabb.minZ = aabb.maxZ = a + eps_k; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 1 + // Ось X + a = v1.x*transform.a + v1.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v1.x*transform.e + v1.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v1.x*transform.i + v1.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 2 + // Ось X + a = v2.x*transform.a + v2.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v2.x*transform.e + v2.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v2.x*transform.i + v2.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + aabb.minX += transform.d; + aabb.maxX += transform.d; + + aabb.minY += transform.h; + aabb.maxY += transform.h; + + aabb.minZ += transform.l; + aabb.maxZ += transform.l; + + if (isNaN(aabb.minX) || isNaN(aabb.minY) || isNaN(aabb.minZ) || isNaN(aabb.maxX) || isNaN(aabb.maxY) || isNaN(aabb.maxZ)) { + var aa:int = 1; + trace(aabb); + } + + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function raycast(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + // Луч трансформируется в систему координат примитива, затем проверяется пересечение + + var vz:Number = vector.x*transform.c + vector.y*transform.g + vector.z*transform.k; + // Если луч параллелен плоскости птимитива, то пересечения нет + if (vz < epsilon && vz > -epsilon) return -1; + + var tx:Number = origin.x - transform.d; + var ty:Number = origin.y - transform.h; + var tz:Number = origin.z - transform.l; + + var oz:Number = tx*transform.c + ty*transform.g + tz*transform.k; + var t:Number = -oz/vz; + if (t < 0) return -1; + + var ox:Number = tx*transform.a + ty*transform.e + tz*transform.i; + var oy:Number = tx*transform.b + ty*transform.f + tz*transform.j; + + tx = ox + t*(vector.x*transform.a + vector.y*transform.e + vector.z*transform.i); + ty = oy + t*(vector.x*transform.b + vector.y*transform.f + vector.z*transform.j); + tz = oz + t*vz; + + // Проверка вхождения точки в треугольник + if ((e0.x*(ty - v0.y) - e0.y*(tx - v0.x) < 0) || (e1.x*(ty - v1.y) - e1.y*(tx - v1.x) < 0) || (e2.x*(ty - v2.y) - e2.y*(tx - v2.x) < 0)) return -1; + + // Запись нормали + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + + return t; + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + super.copyFrom(source); + var tri:CollisionTriangle = source as CollisionTriangle; + if (tri != null) { + v0.copy(tri.v0); + v1.copy(tri.v1); + v2.copy(tri.v2); + + e0.copy(tri.e0); + e1.copy(tri.e1); + e2.copy(tri.e2); + +// len0 = tri.len0; +// len1 = tri.len1; +// len2 = tri.len2; + } + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionTriangle v0=" + v0 + ", v1=" + v1 + ", v2=" + v2 + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionTriangle(v0, v1, v2, collisionGroup); + } + + /** + * + * @param v0 + * @param v1 + * @param v2 + */ + private function initVertices(v0:Vector3, v1:Vector3, v2:Vector3):void { + this.v0.copy(v0); + this.v1.copy(v1); + this.v2.copy(v2); + + e0.diff(v1, v0); +// len0 = e0.vLength(); + e0.normalize(); + + e1.diff(v2, v1); +// len1 = e1.vLength(); + e1.normalize(); + + e2.diff(v0, v2); +// len2 = e2.vLength(); + e2.normalize(); + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/primitives/CollisionBox.as b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/primitives/CollisionBox.as new file mode 100644 index 0000000..ac62c8b --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/primitives/CollisionBox.as @@ -0,0 +1,202 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * Ориентированный бокс. + */ + public class CollisionBox extends CollisionPrimitive { + + // Половинные размеры вдоль каждой из осей + public var hs:Vector3 = new Vector3(); + + public var excludedFaces:int; + + /** + * @param hs + * @param collisionGroup + */ + public function CollisionBox(hs:Vector3, collisionGroup:int) { + super(BOX, collisionGroup); + this.hs.copy(hs); + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + var t:Matrix4 = transform; + + var xx:Number; + var yy:Number; + var zz:Number; + + xx = t.a < 0 ? -t.a : t.a; + yy = t.b < 0 ? -t.b : t.b; + zz = t.c < 0 ? -t.c : t.c; + aabb.maxX = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minX = -aabb.maxX; + + xx = t.e < 0 ? -t.e : t.e; + yy = t.f < 0 ? -t.f : t.f; + zz = t.g < 0 ? -t.g : t.g; + aabb.maxY = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minY = -aabb.maxY; + + xx = t.i < 0 ? -t.i : t.i; + yy = t.j < 0 ? -t.j : t.j; + zz = t.k < 0 ? -t.k : t.k; + aabb.maxZ = hs.x*xx + hs.y*yy + hs.z*zz; + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var box:CollisionBox = source as CollisionBox; + if (box == null) return this; + super.copyFrom(box); + hs.copy(box.hs); + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionBox(hs, collisionGroup); + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function raycast(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + var tMin:Number = -1; + var tMax:Number = 1e308; + var t1:Number; + var t2:Number; + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + // X + if (vx < epsilon && vx > -epsilon) { + if (ox < -hs.x || ox > hs.x) return -1; + } else { + t1 = (-hs.x - ox)/vx; + t2 = (hs.x - ox)/vx; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.x = -1; + normal.y = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.x = 1; + normal.y = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Y + if (vy < epsilon && vy > -epsilon) { + if (oy < -hs.y || oy > hs.y) return -1; + } else { + t1 = (-hs.y - oy)/vy; + t2 = (hs.y - oy)/vy; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.y = -1; + normal.x = normal.z = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.y = 1; + normal.x = normal.z = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Z + if (vz < epsilon && vz > -epsilon) { + if (oz < -hs.z || oz > hs.z) return -1; + } else { + t1 = (-hs.z - oz)/vz; + t2 = (hs.z - oz)/vz; + if (t1 < t2) { + if (t1 > tMin) { + tMin = t1; + normal.z = -1; + normal.x = normal.y = 0; + } + if (t2 < tMax) tMax = t2; + } else { + if (t2 > tMin) { + tMin = t2; + normal.z = 1; + normal.x = normal.y = 0; + } + if (t1 < tMax) tMax = t1; + } + if (tMax < tMin) return -1; + } + // Перевод нормали в мировую систему координат + // Inlined normal.vDeltaTransformBy4(transform); + vx = normal.x; + vy = normal.y; + vz = normal.z; + normal.x = transform.a*vx + transform.b*vy + transform.c*vz; + normal.y = transform.e*vx + transform.f*vy + transform.g*vz; + normal.z = transform.i*vx + transform.j*vy + transform.k*vz; + + return tMin; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionBox hs=" + hs + "]"; + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/primitives/CollisionRect.as b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/primitives/CollisionRect.as new file mode 100644 index 0000000..f070566 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/primitives/CollisionRect.as @@ -0,0 +1,141 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Matrix4; + import alternativa.math.Vector3; + + /** + * Ориентированный прямоугольник. Задаётся половинами размеров вдоль осей X (ширина) и Y (длина) локальной системы + * координат примитива. Таким образом, прямоугольник лежит в плоскости XY, его стороны параллельны осям этой + * плоскости, а нормаль направлена вдоль локальной оси Z. + * + * Прямоугольник может быть одно- или двусторонним. В случае одностороннего прямоугольника, + * столкновения с внутренней стороны не регистрируются. + */ + public class CollisionRect extends CollisionPrimitive { + // Половинные размеры прямоугольника вдоль осей X и Y. Нормаль направлена вдоль оси Z. + public var hs:Vector3 = new Vector3(); + // Флаг указывает, является примитив одно- или двусторонним + public var twoSided:Boolean = true; + + // Малое значение. Используется для указания фиктивной высоты примитива и в функции определения пересечения с сегментом. + private static const EPSILON:Number = 0.005; + + /** + * Создаёт новый экземпляр примитива. + * + * @param hs половинные размерв прямоугольника вдоль осей X и Y. Значение z игнорируется. + * @param collisionGroup группа примитива + */ + public function CollisionRect(hs:Vector3, collisionGroup:int) { + super(RECT, collisionGroup); + this.hs.copy(hs); + } + + /** + * Расчитывает ограничивающий бокс прямоугольника. Для избежания проблем высота примитива принимается равной + * не нулю, а малому значению. + * + * @return + */ + override public function calculateAABB():BoundBox { + // Баунд бокс прямоугольника имеет минимальную высоту, отличную от нуля во избежание проблем с построением kd-дерева + var t:Matrix4 = transform; + var xx:Number = t.a < 0 ? -t.a : t.a; + var yy:Number = t.b < 0 ? -t.b : t.b; + var zz:Number = t.c < 0 ? -t.c : t.c; + aabb.maxX = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minX = -aabb.maxX; + + xx = t.e < 0 ? -t.e : t.e; + yy = t.f < 0 ? -t.f : t.f; + zz = t.g < 0 ? -t.g : t.g; + aabb.maxY = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minY = -aabb.maxY; + + xx = t.i < 0 ? -t.i : t.i; + yy = t.j < 0 ? -t.j : t.j; + zz = t.k < 0 ? -t.k : t.k; + aabb.maxZ = hs.x*xx + hs.y*yy + EPSILON*zz; + aabb.minZ = -aabb.maxZ; + + aabb.minX += t.d; + aabb.maxX += t.d; + + aabb.minY += t.h; + aabb.maxY += t.h; + + aabb.minZ += t.l; + aabb.maxZ += t.l; + + return aabb; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var rect:CollisionRect = source as CollisionRect; + if (rect == null) return this; + super.copyFrom(rect); + hs.copy(rect.hs); + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionRect hs=" + hs + "]"; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionRect(hs, collisionGroup); + } + + /** + * @param origin + * @param vector + * @param collisionGroup + * @param threshold + * @param normal + * @return + */ + override public function raycast(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + // Перевод параметров сегмента в систему координат примитива + // Inlined transform.transformVectorInverse(origin, _o); + var vx:Number = origin.x - transform.d; + var vy:Number = origin.y - transform.h; + var vz:Number = origin.z - transform.l; + var ox:Number = transform.a*vx + transform.e*vy + transform.i*vz; + var oy:Number = transform.b*vx + transform.f*vy + transform.j*vz; + var oz:Number = transform.c*vx + transform.g*vy + transform.k*vz; + // Inlined transform.deltaTransformVectorInverse(vector, _v); + vx = transform.a*vector.x + transform.e*vector.y + transform.i*vector.z; + vy = transform.b*vector.x + transform.f*vector.y + transform.j*vector.z; + vz = transform.c*vector.x + transform.g*vector.y + transform.k*vector.z; + + // Проверка параллельности сегмента и плоскости примитива + if (vz > -threshold && vz < threshold) return -1; + var t:Number = -oz/vz; + if (t < 0) return -1; + // Проверка вхождения точки пересечения в прямоугольник + ox += vx*t; + oy += vy*t; + oz = 0; + if (ox < (-hs.x - threshold) || ox > (hs.x + threshold) || oy < (-hs.y - threshold) || oy > (hs.y + threshold)) return -1; + + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + return t; + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/primitives/CollisionSphere.as b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/primitives/CollisionSphere.as new file mode 100644 index 0000000..e6f4e00 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/primitives/CollisionSphere.as @@ -0,0 +1,78 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Vector3; + + /** + * Сфера. + */ + public class CollisionSphere extends CollisionPrimitive { + + // Радиус сферы + public var r:Number = 0; + + /** + * + * @param r + * @param collisionGroup + */ + public function CollisionSphere(r:Number, collisionGroup:int) { + super(SPHERE, collisionGroup); + this.r = r; + } + + /** + * @return + */ + override public function calculateAABB():BoundBox { + aabb.maxX = transform.d + r; + aabb.minX = transform.d - r; + + aabb.maxY = transform.h + r; + aabb.minY = transform.h - r; + + aabb.maxZ = transform.l + r; + aabb.minZ = transform.l - r; + + return aabb; + } + + /** + * @param origin + * @param vector + * @param threshold + * @param normal + * @return + */ + override public function raycast(origin:Vector3, vector:Vector3, threshold:Number, normal:Vector3):Number { + var px:Number = origin.x - transform.d; + var py:Number = origin.y - transform.h; + var pz:Number = origin.z - transform.l; + var k:Number = vector.x*px + vector.y*py + vector.z*pz; + if (k > 0) return -1; + var a:Number = vector.x*vector.x + vector.y*vector.y + vector.z*vector.z; + var D:Number = k*k - a*(px*px + py*py + pz*pz - r*r); + if (D < 0) return -1; + return -(k + Math.sqrt(D))/a; + } + + /** + * @param source + * @return + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + var sphere:CollisionSphere = source as CollisionSphere; + if (sphere == null) return this; + super.copyFrom(sphere); + r = sphere.r; + return this; + } + + /** + * @return + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionSphere(r, collisionGroup); + } + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/primitives/CollisionTriangle.as b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/primitives/CollisionTriangle.as new file mode 100644 index 0000000..2611feb --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/primitives/CollisionTriangle.as @@ -0,0 +1,247 @@ +package alternativa.physics.collision.primitives { + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.physics.collision.types.BoundBox; + import alternativa.math.Vector3; + + /** + * Примитив треугольник. + */ + public class CollisionTriangle extends CollisionPrimitive { + + public var v0:Vector3 = new Vector3(); + public var v1:Vector3 = new Vector3(); + public var v2:Vector3 = new Vector3(); + + public var e0:Vector3 = new Vector3(); + public var e1:Vector3 = new Vector3(); + public var e2:Vector3 = new Vector3(); + +// public var len0:Number; +// public var len1:Number; +// public var len2:Number; +// + /** + * + * @param v0 + * @param v1 + * @param v2 + * @param collisionGroup + */ + public function CollisionTriangle(v0:Vector3, v1:Vector3, v2:Vector3, collisionGroup:int) { + super(TRIANGLE, collisionGroup); + initVertices(v0, v1, v2); + } + + /** + * Рассчитывает AABB примитива. + * + * @return ссылка на свой AABB + */ + override public function calculateAABB():BoundBox { + var epsilon:Number = 0.005; + var a:Number; + var b:Number; + var eps_c:Number = epsilon*transform.c; + var eps_g:Number = epsilon*transform.g; + var eps_k:Number = epsilon*transform.k; + + // Вершина 0 + // Ось X + a = v0.x*transform.a + v0.y*transform.b; + aabb.minX = aabb.maxX = a + eps_c; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v0.x*transform.e + v0.y*transform.f; + aabb.minY = aabb.maxY = a + eps_g; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v0.x*transform.i + v0.y*transform.j; + aabb.minZ = aabb.maxZ = a + eps_k; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 1 + // Ось X + a = v1.x*transform.a + v1.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v1.x*transform.e + v1.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v1.x*transform.i + v1.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + // Вершина 2 + // Ось X + a = v2.x*transform.a + v2.y*transform.b; + b = a + eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + b = a - eps_c; + if (b > aabb.maxX) aabb.maxX = b; + else if (b < aabb.minX) aabb.minX = b; + // Ось Y + a = v2.x*transform.e + v2.y*transform.f; + b = a + eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + b = a - eps_g; + if (b > aabb.maxY) aabb.maxY = b; + else if (b < aabb.minY) aabb.minY = b; + // Ось Z + a = v2.x*transform.i + v2.y*transform.j; + b = a + eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + b = a - eps_k; + if (b > aabb.maxZ) aabb.maxZ = b; + else if (b < aabb.minZ) aabb.minZ = b; + + aabb.minX += transform.d; + aabb.maxX += transform.d; + + aabb.minY += transform.h; + aabb.maxY += transform.h; + + aabb.minZ += transform.l; + aabb.maxZ += transform.l; + + if (isNaN(aabb.minX) || isNaN(aabb.minY) || isNaN(aabb.minZ) || isNaN(aabb.maxX) || isNaN(aabb.maxY) || isNaN(aabb.maxZ)) { + var aa:int = 1; + trace(aabb); + } + + return aabb; + } + + /** + * Вычисляет пересечение луча с примитивом. + * + * @param origin начальная точка луча в мировых координатах + * @param vector направляющий вектор луча в мировых координатах. Вектор может быть любой отличной от нуля длины. + * @param epsilon погрешность измерения расстояния. Величина, не превышающая по абсолютному значению указанную погрешность, считается равной нулю. + * @param normal если пересечение существует, в этот параметр записывается нормаль к примитиву в точке пересечения + * @return если пересечение существует, возвращается неотрицательное время точки пересечения, в противном случае возвращается -1. + */ + override public function raycast(origin:Vector3, vector:Vector3, epsilon:Number, normal:Vector3):Number { + // Луч трансформируется в систему координат примитива, затем проверяется пересечение + + var vz:Number = vector.x*transform.c + vector.y*transform.g + vector.z*transform.k; + // Если луч параллелен плоскости птимитива, то пересечения нет + if (vz < epsilon && vz > -epsilon) return -1; + + var tx:Number = origin.x - transform.d; + var ty:Number = origin.y - transform.h; + var tz:Number = origin.z - transform.l; + + var oz:Number = tx*transform.c + ty*transform.g + tz*transform.k; + var t:Number = -oz/vz; + if (t < 0) return -1; + + var ox:Number = tx*transform.a + ty*transform.e + tz*transform.i; + var oy:Number = tx*transform.b + ty*transform.f + tz*transform.j; + + tx = ox + t*(vector.x*transform.a + vector.y*transform.e + vector.z*transform.i); + ty = oy + t*(vector.x*transform.b + vector.y*transform.f + vector.z*transform.j); + tz = oz + t*vz; + + // Проверка вхождения точки в треугольник + if ((e0.x*(ty - v0.y) - e0.y*(tx - v0.x) < 0) || (e1.x*(ty - v1.y) - e1.y*(tx - v1.x) < 0) || (e2.x*(ty - v2.y) - e2.y*(tx - v2.x) < 0)) return -1; + + // Запись нормали + normal.x = transform.c; + normal.y = transform.g; + normal.z = transform.k; + + return t; + } + + /** + * Копирует параметры указанного примитива. Объекты копируются по значению. + * + * @param source примитив, чьи параметры копируются + * @return this + */ + override public function copyFrom(source:CollisionPrimitive):CollisionPrimitive { + super.copyFrom(source); + var tri:CollisionTriangle = source as CollisionTriangle; + if (tri != null) { + v0.copy(tri.v0); + v1.copy(tri.v1); + v2.copy(tri.v2); + + e0.copy(tri.e0); + e1.copy(tri.e1); + e2.copy(tri.e2); + +// len0 = tri.len0; +// len1 = tri.len1; +// len2 = tri.len2; + } + return this; + } + + /** + * Создаёт строковое представление объекта. + * + * @return строковое представление объекта + */ + override public function toString():String { + return "[CollisionTriangle v0=" + v0 + ", v1=" + v1 + ", v2=" + v2 + "]"; + } + + /** + * Создаёт новый экземпляр примитива соответствующего типа. + * + * @return новый экземпляр примитива + */ + override protected function createPrimitive():CollisionPrimitive { + return new CollisionTriangle(v0, v1, v2, collisionGroup); + } + + /** + * + * @param v0 + * @param v1 + * @param v2 + */ + private function initVertices(v0:Vector3, v1:Vector3, v2:Vector3):void { + this.v0.copy(v0); + this.v1.copy(v1); + this.v2.copy(v2); + + e0.diff(v1, v0); +// len0 = e0.vLength(); + e0.normalize(); + + e1.diff(v2, v1); +// len1 = e1.vLength(); + e1.normalize(); + + e2.diff(v0, v2); +// len2 = e2.vLength(); + e2.normalize(); + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/types/.svn/all-wcprops b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/types/.svn/all-wcprops new file mode 100644 index 0000000..79353b2 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/types/.svn/all-wcprops @@ -0,0 +1,23 @@ +K 25 +svn:wc:ra_dav:version-url +V 112 +/!svn/ver/34246/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/collision/types +END +BoundBox.as +K 25 +svn:wc:ra_dav:version-url +V 124 +/!svn/ver/19633/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/collision/types/BoundBox.as +END +RayHit.as +K 25 +svn:wc:ra_dav:version-url +V 122 +/!svn/ver/34246/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/collision/types/RayHit.as +END +Ray.as +K 25 +svn:wc:ra_dav:version-url +V 119 +/!svn/ver/30874/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/collision/types/Ray.as +END diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/types/.svn/entries b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/types/.svn/entries new file mode 100644 index 0000000..68db96a --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/types/.svn/entries @@ -0,0 +1,64 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/collision/types +http://svndev.alternativaplatform.com + + + +2010-05-12T11:30:54.476378Z +34246 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +BoundBox.as +file + + + + +2010-10-28T04:32:37.000000Z +d92d4ab3dde698feb674fb9488f59678 +2009-09-09T14:02:48.134744Z +19633 +mike + +RayHit.as +file + + + + +2010-10-28T04:32:37.000000Z +ac9441a2e2f03f116092fa83c4f91149 +2010-05-12T11:30:54.476378Z +34246 +mike + +Ray.as +file + + + + +2010-10-28T04:32:37.000000Z +852d765db9177ad511768635d0f5c564 +2010-04-01T08:37:11.976264Z +30874 +mike + diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/types/.svn/format b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/types/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/types/.svn/format @@ -0,0 +1 @@ +8 diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base new file mode 100644 index 0000000..b4d5278 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/types/.svn/text-base/BoundBox.as.svn-base @@ -0,0 +1,75 @@ +package alternativa.physics.collision.types { + + /** + * Ограничивающий параллелепипед. + */ + public class BoundBox { + + public var minX:Number = 1e308; + public var minY:Number = 1e308; + public var minZ:Number = 1e308; + public var maxX:Number = -1e308; + public var maxY:Number = -1e308; + public var maxZ:Number = -1e308; + + public function setSize(minX:Number, minY:Number, minZ:Number, maxX:Number, maxY:Number, maxZ:Number):void { + this.minX = minX; + this.minY = minY; + this.minZ = minZ; + this.maxX = maxX; + this.maxY = maxY; + this.maxZ = maxZ; + } + + public function addBoundBox(boundBox:BoundBox):void { + minX = (boundBox.minX < minX) ? boundBox.minX : minX; + minY = (boundBox.minY < minY) ? boundBox.minY : minY; + minZ = (boundBox.minZ < minZ) ? boundBox.minZ : minZ; + maxX = (boundBox.maxX > maxX) ? boundBox.maxX : maxX; + maxY = (boundBox.maxY > maxY) ? boundBox.maxY : maxY; + maxZ = (boundBox.maxZ > maxZ) ? boundBox.maxZ : maxZ; + } + + public function addPoint(x:Number, y:Number, z:Number):void { + if (x < minX) minX = x; + if (x > maxX) maxX = x; + if (y < minY) minY = y; + if (y > maxY) maxY = y; + if (z < minZ) minZ = z; + if (z > maxZ) maxZ = z; + } + + public function infinity():void { + minX = 1e308; + minY = 1e308; + minZ = 1e308; + maxX = -1e308; + maxY = -1e308; + maxZ = -1e308; + } + + public function intersects(bb:BoundBox, epsilon:Number):Boolean { + return !(minX > bb.maxX + epsilon || maxX < bb.minX - epsilon || minY > bb.maxY + epsilon || maxY < bb.minY - epsilon || minZ > bb.maxZ + epsilon || maxZ < bb.minZ - epsilon); + } + + public function copyFrom(boundBox:BoundBox):void { + minX = boundBox.minX; + minY = boundBox.minY; + minZ = boundBox.minZ; + maxX = boundBox.maxX; + maxY = boundBox.maxY; + maxZ = boundBox.maxZ; + } + + public function clone():BoundBox { + var clone:BoundBox = new BoundBox(); + clone.copyFrom(this); + return clone; + } + + public function toString():String { + return "BoundBox [" + minX + ", " + minY + ", " + minZ + " : " + maxX + ", " + maxY + ", " + maxZ + "]"; + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base new file mode 100644 index 0000000..26938ad --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/types/.svn/text-base/Ray.as.svn-base @@ -0,0 +1,13 @@ +package alternativa.physics.collision.types { + import alternativa.math.Vector3; + + /** + * Структура, описывающая луч и сегмент прямой в пространстве. + */ + public class Ray { + // Начало луча. Начальная точка сегмента на прямой луча. + public var origin:Vector3 = new Vector3(); + // Направляющий вектор луча. Длина вектора задаёт длину сегмента на луче. Конечная точка сегмента вычисляется как origin + dir. + public var vector:Vector3 = new Vector3(); + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/types/.svn/text-base/RayHit.as.svn-base b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/types/.svn/text-base/RayHit.as.svn-base new file mode 100644 index 0000000..f908bc4 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/types/.svn/text-base/RayHit.as.svn-base @@ -0,0 +1,30 @@ +package alternativa.physics.collision.types { + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.math.Vector3; + + /** + * Структура описывает точку пересечения луча с физической геометрией. + */ + public class RayHit { + // Физический примитив, с которым пересекается луч + public var primitive:CollisionPrimitive; + // Глобальные координаты точки пересечения + public var pos:Vector3 = new Vector3(); + // Глобальная нормаль поверхности физического примитива в точке пересечения + public var normal:Vector3 = new Vector3(); + // Время пересечения (|pos - ray.origin|/|ray.dir|) + public var t:Number = 0; + + /** + * Копирует данные объекта-источника. + * + * @param source источник данных + */ + public function copy(source:RayHit):void { + primitive = source.primitive; + pos.copy(source.pos); + normal.copy(source.normal); + t = source.t; + } + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/types/BoundBox.as b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/types/BoundBox.as new file mode 100644 index 0000000..b4d5278 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/types/BoundBox.as @@ -0,0 +1,75 @@ +package alternativa.physics.collision.types { + + /** + * Ограничивающий параллелепипед. + */ + public class BoundBox { + + public var minX:Number = 1e308; + public var minY:Number = 1e308; + public var minZ:Number = 1e308; + public var maxX:Number = -1e308; + public var maxY:Number = -1e308; + public var maxZ:Number = -1e308; + + public function setSize(minX:Number, minY:Number, minZ:Number, maxX:Number, maxY:Number, maxZ:Number):void { + this.minX = minX; + this.minY = minY; + this.minZ = minZ; + this.maxX = maxX; + this.maxY = maxY; + this.maxZ = maxZ; + } + + public function addBoundBox(boundBox:BoundBox):void { + minX = (boundBox.minX < minX) ? boundBox.minX : minX; + minY = (boundBox.minY < minY) ? boundBox.minY : minY; + minZ = (boundBox.minZ < minZ) ? boundBox.minZ : minZ; + maxX = (boundBox.maxX > maxX) ? boundBox.maxX : maxX; + maxY = (boundBox.maxY > maxY) ? boundBox.maxY : maxY; + maxZ = (boundBox.maxZ > maxZ) ? boundBox.maxZ : maxZ; + } + + public function addPoint(x:Number, y:Number, z:Number):void { + if (x < minX) minX = x; + if (x > maxX) maxX = x; + if (y < minY) minY = y; + if (y > maxY) maxY = y; + if (z < minZ) minZ = z; + if (z > maxZ) maxZ = z; + } + + public function infinity():void { + minX = 1e308; + minY = 1e308; + minZ = 1e308; + maxX = -1e308; + maxY = -1e308; + maxZ = -1e308; + } + + public function intersects(bb:BoundBox, epsilon:Number):Boolean { + return !(minX > bb.maxX + epsilon || maxX < bb.minX - epsilon || minY > bb.maxY + epsilon || maxY < bb.minY - epsilon || minZ > bb.maxZ + epsilon || maxZ < bb.minZ - epsilon); + } + + public function copyFrom(boundBox:BoundBox):void { + minX = boundBox.minX; + minY = boundBox.minY; + minZ = boundBox.minZ; + maxX = boundBox.maxX; + maxY = boundBox.maxY; + maxZ = boundBox.maxZ; + } + + public function clone():BoundBox { + var clone:BoundBox = new BoundBox(); + clone.copyFrom(this); + return clone; + } + + public function toString():String { + return "BoundBox [" + minX + ", " + minY + ", " + minZ + " : " + maxX + ", " + maxY + ", " + maxZ + "]"; + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/types/Ray.as b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/types/Ray.as new file mode 100644 index 0000000..26938ad --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/types/Ray.as @@ -0,0 +1,13 @@ +package alternativa.physics.collision.types { + import alternativa.math.Vector3; + + /** + * Структура, описывающая луч и сегмент прямой в пространстве. + */ + public class Ray { + // Начало луча. Начальная точка сегмента на прямой луча. + public var origin:Vector3 = new Vector3(); + // Направляющий вектор луча. Длина вектора задаёт длину сегмента на луче. Конечная точка сегмента вычисляется как origin + dir. + public var vector:Vector3 = new Vector3(); + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/types/RayHit.as b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/types/RayHit.as new file mode 100644 index 0000000..f908bc4 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/collision/types/RayHit.as @@ -0,0 +1,30 @@ +package alternativa.physics.collision.types { + import alternativa.physics.collision.CollisionPrimitive; + import alternativa.math.Vector3; + + /** + * Структура описывает точку пересечения луча с физической геометрией. + */ + public class RayHit { + // Физический примитив, с которым пересекается луч + public var primitive:CollisionPrimitive; + // Глобальные координаты точки пересечения + public var pos:Vector3 = new Vector3(); + // Глобальная нормаль поверхности физического примитива в точке пересечения + public var normal:Vector3 = new Vector3(); + // Время пересечения (|pos - ray.origin|/|ray.dir|) + public var t:Number = 0; + + /** + * Копирует данные объекта-источника. + * + * @param source источник данных + */ + public function copy(source:RayHit):void { + primitive = source.primitive; + pos.copy(source.pos); + normal.copy(source.normal); + t = source.t; + } + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/constraints/.svn/all-wcprops b/2.0.1.0-SNAPSHOT/src/alternativa/physics/constraints/.svn/all-wcprops new file mode 100644 index 0000000..8cf6f21 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/constraints/.svn/all-wcprops @@ -0,0 +1,17 @@ +K 25 +svn:wc:ra_dav:version-url +V 108 +/!svn/ver/30874/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/constraints +END +Constraint.as +K 25 +svn:wc:ra_dav:version-url +V 122 +/!svn/ver/30874/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/constraints/Constraint.as +END +MaxDistanceConstraint.as +K 25 +svn:wc:ra_dav:version-url +V 133 +/!svn/ver/30874/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/constraints/MaxDistanceConstraint.as +END diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/constraints/.svn/entries b/2.0.1.0-SNAPSHOT/src/alternativa/physics/constraints/.svn/entries new file mode 100644 index 0000000..6e97440 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/constraints/.svn/entries @@ -0,0 +1,52 @@ +8 + +dir +46043 +http://svndev.alternativaplatform.com/platform/clients/fp10/libraries/AlternativaPhysics/trunk/src/alternativa/physics/constraints +http://svndev.alternativaplatform.com + + + +2010-04-01T08:37:11.976264Z +30874 +mike + + +svn:special svn:externals svn:needs-lock + + + + + + + + + + + +d9e2387a-1f3e-40e2-b57f-9df5970a2fa5 + +Constraint.as +file + + + + +2010-10-28T04:32:37.000000Z +34b6cfa491d92dc4089c03b867461a2e +2010-04-01T08:37:11.976264Z +30874 +mike + +MaxDistanceConstraint.as +file + + + + +2010-10-28T04:32:37.000000Z +d75e28da7563213b2cd3a3f005ac8946 +2010-04-01T08:37:11.976264Z +30874 +mike + diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/constraints/.svn/format b/2.0.1.0-SNAPSHOT/src/alternativa/physics/constraints/.svn/format new file mode 100644 index 0000000..45a4fb7 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/constraints/.svn/format @@ -0,0 +1 @@ +8 diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/constraints/.svn/text-base/Constraint.as.svn-base b/2.0.1.0-SNAPSHOT/src/alternativa/physics/constraints/.svn/text-base/Constraint.as.svn-base new file mode 100644 index 0000000..51a7d71 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/constraints/.svn/text-base/Constraint.as.svn-base @@ -0,0 +1,32 @@ +package alternativa.physics.constraints { + + import alternativa.physics.PhysicsScene; + + /** + * + */ + public class Constraint { + + public var satisfied:Boolean; + public var world:PhysicsScene; + + /** + * + */ + public function Constraint() { + } + + /** + * @param dt + */ + public function preProcess(dt:Number):void { + } + + /** + * @param dt + */ + public function apply(dt:Number):void { + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base b/2.0.1.0-SNAPSHOT/src/alternativa/physics/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base new file mode 100644 index 0000000..b106578 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/constraints/.svn/text-base/MaxDistanceConstraint.as.svn-base @@ -0,0 +1,180 @@ +package alternativa.physics.constraints { + + import alternativa.physics.Body; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + /** + * + */ + public class MaxDistanceConstraint extends Constraint { + // Первое тело ограничения + public var body1:Body; + // Второе тело ограничения. Может быть null. + public var body2:Body; + // Первая точка крепления в системе координат первого тела + public var r1:Vector3; + // Вторая точка привязки в системе координат второго тела. В случае отсутствия второго тела, координаты задаются в мировой системе. + public var r2:Vector3; + // Максимально допустимое расстояние между точками крепления + public var maxDistance:Number; + // Радиус-вектор первой точки крепления, трансформированный без смещения в мировую систему координат + public var wr1:Vector3 = new Vector3(); + // Радиус-вектор второй точки крепления, трансформированный без смещения в мировую систему координат + public var wr2:Vector3 = new Vector3(); + // Минимальная скорость сближения для удовлетворения ограничения + private var minClosingVel:Number; + // Изменение относительной скорости на единицу импульса + private var velByUnitImpulseN:Number; + // Направление корректирующего импульса + private var impulseDirection:Vector3 = new Vector3(); + + // Вспомогательные переменные +// private static var _v:Vector3 = new Vector3(); +// private static var _v1:Vector3 = new Vector3(); +// private static var _v2:Vector3 = new Vector3(); + + /** + * @param body1 + * @param body2 + * @param r1 + * @param r2 + * @param maxDistance + */ + public function MaxDistanceConstraint(body1:Body, body2:Body, r1:Vector3, r2:Vector3, maxDistance:Number) { + super(); + this.body1 = body1; + this.body2 = body2; + this.r1 = r1.clone(); + this.r2 = r2.clone(); + this.maxDistance = maxDistance; + } + + /** + * @param dt + */ + override public function preProcess(dt:Number):void { + // Вычислим расстояние между точками + var m:Matrix3 = body1.baseMatrix; + wr1.x = m.a*r1.x + m.b*r1.y + m.c*r1.z; + wr1.y = m.e*r1.x + m.f*r1.y + m.g*r1.z; + wr1.z = m.i*r1.x + m.j*r1.y + m.k*r1.z; + if (body2 != null) { + m = body2.baseMatrix; + wr2.x = m.a*r2.x + m.b*r2.y + m.c*r2.z; + wr2.y = m.e*r2.x + m.f*r2.y + m.g*r2.z; + wr2.z = m.i*r2.x + m.j*r2.y + m.k*r2.z; + } else { + wr2.x = r2.x; + wr2.y = r2.y; + wr2.z = r2.z; + } + var p1:Vector3 = body1.state.pos; + impulseDirection.x = wr2.x - wr1.x - p1.x; + impulseDirection.y = wr2.y - wr1.y - p1.y; + impulseDirection.z = wr2.z - wr1.z - p1.z; + if (body2 != null) { + var p2:Vector3 = body2.state.pos; + impulseDirection.x += p2.x; + impulseDirection.y += p2.y; + impulseDirection.z += p2.z; + } + + var len:Number = Math.sqrt(impulseDirection.x*impulseDirection.x + impulseDirection.y*impulseDirection.y + impulseDirection.z*impulseDirection.z); + var delta:Number = len - maxDistance; + if (delta > 0) { + satisfied = false; + if (len < 0.001) { + impulseDirection.x = 1; + } else { + len = 1/len; + impulseDirection.x *= len; + impulseDirection.y *= len; + impulseDirection.z *= len; + } + minClosingVel = delta/(world.penResolutionSteps*dt); + if (minClosingVel > world.maxPenResolutionSpeed) { + minClosingVel = world.maxPenResolutionSpeed; + } + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + var x:Number; + var y:Number; + var z:Number; + var vx:Number; + var vy:Number; + var vz:Number; + velByUnitImpulseN = 0; + if (body1.movable) { +// velByUnitImpulseN += body1.invMass + _v.vCross2(wr1, impulseDirection).vTransformBy3(body1.invInertiaWorld).vCross(wr1).vDot(impulseDirection); + vx = wr1.y*impulseDirection.z - wr1.z*impulseDirection.y; + vy = wr1.z*impulseDirection.x - wr1.x*impulseDirection.z; + vz = wr1.x*impulseDirection.y - wr1.y*impulseDirection.x; + + m = body1.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr1.z - z*wr1.y; + vy = z*wr1.x - x*wr1.z; + vz = x*wr1.y - y*wr1.x; + velByUnitImpulseN += body1.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + if (body2 != null && body2.movable) { +// velByUnitImpulseN += body2.invMass + _v.vCross2(wr2, impulseDirection).vTransformBy3(body2.invInertiaWorld).vCross(wr2).vDot(impulseDirection); + vx = wr2.y*impulseDirection.z - wr2.z*impulseDirection.y; + vy = wr2.z*impulseDirection.x - wr2.x*impulseDirection.z; + vz = wr2.x*impulseDirection.y - wr2.y*impulseDirection.x; + + m = body2.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr2.z - z*wr2.y; + vy = z*wr2.x - x*wr2.z; + vz = x*wr2.y - y*wr2.x; + velByUnitImpulseN += body2.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + } else { + satisfied = true; + } + } + + /** + * @param dt + */ + override public function apply(dt:Number):void { + if (satisfied) return; + // Расчитываем проекцию скорости относительной скорости + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 + var vel:Vector3 = body1.state.velocity; + var rot:Vector3 = body1.state.rotation; +// v1Cross2(state.rotation, wr1); + var vx:Number = vel.x + rot.y*wr1.z - rot.z*wr1.y; + var vy:Number = vel.y + rot.z*wr1.x - rot.x*wr1.z; + var vz:Number = vel.z + rot.x*wr1.y - rot.y*wr1.x; + // V2 = V2_c + w2%r2 + if (body2 != null) { + vel = body2.state.velocity; + rot = body2.state.rotation; +// _v2.vCross2(state.rotation, wr2); + vx -= vel.x + rot.y*wr2.z - rot.z*wr2.y; + vy -= vel.y + rot.z*wr2.x - rot.x*wr2.z; + vz -= vel.z + rot.x*wr2.y - rot.y*wr2.x; + } + var closingVel:Number = vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + if (closingVel > minClosingVel) return; + var impulse:Number = (minClosingVel - closingVel)/velByUnitImpulseN; + if (body1.movable) { + body1.applyRelPosWorldImpulse(wr1, impulseDirection, impulse); + } + if (body2 != null && body2.movable) { + body2.applyRelPosWorldImpulse(wr2, impulseDirection, -impulse); + } + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/constraints/Constraint.as b/2.0.1.0-SNAPSHOT/src/alternativa/physics/constraints/Constraint.as new file mode 100644 index 0000000..51a7d71 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/constraints/Constraint.as @@ -0,0 +1,32 @@ +package alternativa.physics.constraints { + + import alternativa.physics.PhysicsScene; + + /** + * + */ + public class Constraint { + + public var satisfied:Boolean; + public var world:PhysicsScene; + + /** + * + */ + public function Constraint() { + } + + /** + * @param dt + */ + public function preProcess(dt:Number):void { + } + + /** + * @param dt + */ + public function apply(dt:Number):void { + } + + } +} \ No newline at end of file diff --git a/2.0.1.0-SNAPSHOT/src/alternativa/physics/constraints/MaxDistanceConstraint.as b/2.0.1.0-SNAPSHOT/src/alternativa/physics/constraints/MaxDistanceConstraint.as new file mode 100644 index 0000000..b106578 --- /dev/null +++ b/2.0.1.0-SNAPSHOT/src/alternativa/physics/constraints/MaxDistanceConstraint.as @@ -0,0 +1,180 @@ +package alternativa.physics.constraints { + + import alternativa.physics.Body; + import alternativa.math.Matrix3; + import alternativa.math.Vector3; + + /** + * + */ + public class MaxDistanceConstraint extends Constraint { + // Первое тело ограничения + public var body1:Body; + // Второе тело ограничения. Может быть null. + public var body2:Body; + // Первая точка крепления в системе координат первого тела + public var r1:Vector3; + // Вторая точка привязки в системе координат второго тела. В случае отсутствия второго тела, координаты задаются в мировой системе. + public var r2:Vector3; + // Максимально допустимое расстояние между точками крепления + public var maxDistance:Number; + // Радиус-вектор первой точки крепления, трансформированный без смещения в мировую систему координат + public var wr1:Vector3 = new Vector3(); + // Радиус-вектор второй точки крепления, трансформированный без смещения в мировую систему координат + public var wr2:Vector3 = new Vector3(); + // Минимальная скорость сближения для удовлетворения ограничения + private var minClosingVel:Number; + // Изменение относительной скорости на единицу импульса + private var velByUnitImpulseN:Number; + // Направление корректирующего импульса + private var impulseDirection:Vector3 = new Vector3(); + + // Вспомогательные переменные +// private static var _v:Vector3 = new Vector3(); +// private static var _v1:Vector3 = new Vector3(); +// private static var _v2:Vector3 = new Vector3(); + + /** + * @param body1 + * @param body2 + * @param r1 + * @param r2 + * @param maxDistance + */ + public function MaxDistanceConstraint(body1:Body, body2:Body, r1:Vector3, r2:Vector3, maxDistance:Number) { + super(); + this.body1 = body1; + this.body2 = body2; + this.r1 = r1.clone(); + this.r2 = r2.clone(); + this.maxDistance = maxDistance; + } + + /** + * @param dt + */ + override public function preProcess(dt:Number):void { + // Вычислим расстояние между точками + var m:Matrix3 = body1.baseMatrix; + wr1.x = m.a*r1.x + m.b*r1.y + m.c*r1.z; + wr1.y = m.e*r1.x + m.f*r1.y + m.g*r1.z; + wr1.z = m.i*r1.x + m.j*r1.y + m.k*r1.z; + if (body2 != null) { + m = body2.baseMatrix; + wr2.x = m.a*r2.x + m.b*r2.y + m.c*r2.z; + wr2.y = m.e*r2.x + m.f*r2.y + m.g*r2.z; + wr2.z = m.i*r2.x + m.j*r2.y + m.k*r2.z; + } else { + wr2.x = r2.x; + wr2.y = r2.y; + wr2.z = r2.z; + } + var p1:Vector3 = body1.state.pos; + impulseDirection.x = wr2.x - wr1.x - p1.x; + impulseDirection.y = wr2.y - wr1.y - p1.y; + impulseDirection.z = wr2.z - wr1.z - p1.z; + if (body2 != null) { + var p2:Vector3 = body2.state.pos; + impulseDirection.x += p2.x; + impulseDirection.y += p2.y; + impulseDirection.z += p2.z; + } + + var len:Number = Math.sqrt(impulseDirection.x*impulseDirection.x + impulseDirection.y*impulseDirection.y + impulseDirection.z*impulseDirection.z); + var delta:Number = len - maxDistance; + if (delta > 0) { + satisfied = false; + if (len < 0.001) { + impulseDirection.x = 1; + } else { + len = 1/len; + impulseDirection.x *= len; + impulseDirection.y *= len; + impulseDirection.z *= len; + } + minClosingVel = delta/(world.penResolutionSteps*dt); + if (minClosingVel > world.maxPenResolutionSpeed) { + minClosingVel = world.maxPenResolutionSpeed; + } + // Расчитываем изменение нормальной скорости на единицу нормального импульса + // dV = b.invMass + ((invI * (r % n)) % r) * n + var x:Number; + var y:Number; + var z:Number; + var vx:Number; + var vy:Number; + var vz:Number; + velByUnitImpulseN = 0; + if (body1.movable) { +// velByUnitImpulseN += body1.invMass + _v.vCross2(wr1, impulseDirection).vTransformBy3(body1.invInertiaWorld).vCross(wr1).vDot(impulseDirection); + vx = wr1.y*impulseDirection.z - wr1.z*impulseDirection.y; + vy = wr1.z*impulseDirection.x - wr1.x*impulseDirection.z; + vz = wr1.x*impulseDirection.y - wr1.y*impulseDirection.x; + + m = body1.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr1.z - z*wr1.y; + vy = z*wr1.x - x*wr1.z; + vz = x*wr1.y - y*wr1.x; + velByUnitImpulseN += body1.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + if (body2 != null && body2.movable) { +// velByUnitImpulseN += body2.invMass + _v.vCross2(wr2, impulseDirection).vTransformBy3(body2.invInertiaWorld).vCross(wr2).vDot(impulseDirection); + vx = wr2.y*impulseDirection.z - wr2.z*impulseDirection.y; + vy = wr2.z*impulseDirection.x - wr2.x*impulseDirection.z; + vz = wr2.x*impulseDirection.y - wr2.y*impulseDirection.x; + + m = body2.invInertiaWorld; + x = m.a*vx + m.b*vy + m.c*vz; + y = m.e*vx + m.f*vy + m.g*vz; + z = m.i*vx + m.j*vy + m.k*vz; + + vx = y*wr2.z - z*wr2.y; + vy = z*wr2.x - x*wr2.z; + vz = x*wr2.y - y*wr2.x; + velByUnitImpulseN += body2.invMass + vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + } + } else { + satisfied = true; + } + } + + /** + * @param dt + */ + override public function apply(dt:Number):void { + if (satisfied) return; + // Расчитываем проекцию скорости относительной скорости + // sepVel = (V1 - V2)*normal + // V1 = V1_c + w1%r1 + var vel:Vector3 = body1.state.velocity; + var rot:Vector3 = body1.state.rotation; +// v1Cross2(state.rotation, wr1); + var vx:Number = vel.x + rot.y*wr1.z - rot.z*wr1.y; + var vy:Number = vel.y + rot.z*wr1.x - rot.x*wr1.z; + var vz:Number = vel.z + rot.x*wr1.y - rot.y*wr1.x; + // V2 = V2_c + w2%r2 + if (body2 != null) { + vel = body2.state.velocity; + rot = body2.state.rotation; +// _v2.vCross2(state.rotation, wr2); + vx -= vel.x + rot.y*wr2.z - rot.z*wr2.y; + vy -= vel.y + rot.z*wr2.x - rot.x*wr2.z; + vz -= vel.z + rot.x*wr2.y - rot.y*wr2.x; + } + var closingVel:Number = vx*impulseDirection.x + vy*impulseDirection.y + vz*impulseDirection.z; + if (closingVel > minClosingVel) return; + var impulse:Number = (minClosingVel - closingVel)/velByUnitImpulseN; + if (body1.movable) { + body1.applyRelPosWorldImpulse(wr1, impulseDirection, impulse); + } + if (body2 != null && body2.movable) { + body2.applyRelPosWorldImpulse(wr2, impulseDirection, -impulse); + } + } + + } +} \ No newline at end of file